Skip to content

Commit cb2349b

Browse files
mohammedarimohammedari
authored
Merge pull request #9 from mohammedari/mbzirc2020
Merging recent works
2 parents b7310a5 + 98c0663 commit cb2349b

32 files changed

+2344
-72
lines changed

‎.vscode/settings.json‎

Lines changed: 3 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,6 +1,8 @@
11
{
22
"files.associations": {
3-
"stdexcept": "cpp"
3+
"stdexcept": "cpp",
4+
"*.tcc": "cpp",
5+
"thread": "cpp"
46
},
57
"python.autoComplete.extraPaths": [
68
"/opt/ros/kinetic/lib/python2.7/dist-packages",

‎opengl_ros/CMakeLists.txt‎

Lines changed: 14 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -14,12 +14,14 @@ find_package(catkin REQUIRED COMPONENTS
1414
image_transport
1515
cv_bridge
1616
opengl_ros_lib
17+
tf
1718
dynamic_reconfigure
1819
)
1920

2021
## System dependencies are found with CMake's conventions
2122
# find_package(Boost REQUIRED COMPONENTS system)
2223
find_package(OpenCV REQUIRED)
24+
find_package(Eigen3 REQUIRED)
2325

2426
## Uncomment this if the package has a setup.py. This macro ensures
2527
## modules and global scripts declared therein get installed
@@ -111,7 +113,7 @@ generate_dynamic_reconfigure_options(
111113
catkin_package(
112114
# INCLUDE_DIRS include
113115
# LIBRARIES opengl_ros
114-
CATKIN_DEPENDS opengl_ros_lib
116+
CATKIN_DEPENDS roscpp std_msgs sensor_msgs cgs_nav_msgs tf image_transport cv_bridge opengl_ros_lib
115117
# DEPENDS system_lib
116118
)
117119

@@ -124,6 +126,7 @@ catkin_package(
124126
include_directories(
125127
${catkin_INCLUDE_DIRS}
126128
${opengl_ros_lib_INCLUDE_DIRS}
129+
${EIGEN3_INCLUDE_DIRS}
127130
)
128131

129132
## Declare a C++ library
@@ -139,6 +142,8 @@ include_directories(
139142
## Declare a C++ executable
140143
add_executable(simple_renderer src/simple_renderer_node.cpp src/simple_renderer_nodecore.cpp)
141144
add_executable(color_extraction_cpu src/color_extraction_cpu_node.cpp src/color_extraction_cpu_nodecore.cpp)
145+
add_executable(depth_image_projector src/depth_image_projector_node.cpp src/depth_image_projector_nodecore.cpp)
146+
add_executable(object_position_extractor src/object_position_extractor_node.cpp src/object_position_extractor_nodecore.cpp)
142147

143148
## Add cmake target dependencies of the executable
144149
## same as for the library above
@@ -152,6 +157,14 @@ target_link_libraries(simple_renderer
152157
target_link_libraries(color_extraction_cpu
153158
${catkin_LIBRARIES}
154159
)
160+
target_link_libraries(depth_image_projector
161+
${catkin_LIBRARIES}
162+
${opengl_ros_lib_LIBRARIES}
163+
)
164+
target_link_libraries(object_position_extractor
165+
${catkin_LIBRARIES}
166+
${opengl_ros_lib_LIBRARIES}
167+
)
155168

156169
#############
157170
## Install ##

‎opengl_ros/launch/depth_image_projection.launch‎

Lines changed: 49 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,49 @@
1+
<?xml version="1.0"?>
2+
<launch>
3+
<arg name="bag_prefix" default="/mnt/log/d435"/>
4+
<arg name="depth_width" default="640"/>
5+
<arg name="depth_height" default="360"/>
6+
<arg name="depth_min" default="0.3"/>
7+
<arg name="depth_max" default="10"/>
8+
<arg name="depth_hit_threshold" default="0.1"/>
9+
<arg name="depth_unknown_color" default="0"/> <!-- 0 to 255 -->
10+
<arg name="grid_map_width" default="200"/>
11+
<arg name="grid_map_height" default="200"/>
12+
<arg name="grid_map_resolution" default="0.1"/>
13+
<arg name="grid_map_layer_height" default="2.0"/>
14+
<arg name="grid_map_accumulation_weight" default="0.001"/>
15+
<arg name="grid_map_decay" default="0.95"/>
16+
<arg name="frame_rate" default="30"/>
17+
18+
<node pkg="opengl_ros" type="depth_image_projector"
19+
name="depth_image_projector" output="screen">
20+
<param name="depth_frame_id" value="d435_depth_optical"/>
21+
<param name="map_frame_id" value="base_link_hor"/>
22+
<param name="fixed_frame_id" value="map"/>
23+
<param name="tf_wait_duration" value="0.1"/>
24+
25+
<param name="depth_width" value="$(arg depth_width)"/>
26+
<param name="depth_height" value="$(arg depth_height)"/>
27+
28+
<param name="min_depth" value="$(arg depth_min)"/>
29+
<param name="max_depth" value="$(arg depth_max)"/>
30+
<param name="depth_hit_threshold" value="$(arg depth_hit_threshold)"/>
31+
<param name="unknown_depth_color" value="$(arg depth_unknown_color)"/>
32+
33+
<param name="grid_map_width" value="$(arg grid_map_width)"/>
34+
<param name="grid_map_height" value="$(arg grid_map_height)"/>
35+
<param name="grid_map_resolution" value="$(arg grid_map_resolution)"/>
36+
<param name="grid_map_layer_height" value="$(arg grid_map_layer_height)"/>
37+
<param name="grid_map_accumulation_weight" value="$(arg grid_map_accumulation_weight)"/>
38+
<param name="grid_map_decay" value="$(arg grid_map_decay)"/>
39+
40+
<param name="vertex_shader" value="$(find opengl_ros_lib)/shader/vs_depth_image_projection.glsl"/>
41+
<param name="geometry_shader" value="$(find opengl_ros_lib)/shader/gs_depth_image_projection.glsl"/>
42+
<param name="fragment_shader" value="$(find opengl_ros_lib)/shader/fs_depth_image_projection.glsl"/>
43+
<param name="vertex_shader_scaling" value="$(find opengl_ros_lib)/shader/vs_passthrough.glsl"/>
44+
<param name="fragment_shader_scaling" value="$(find opengl_ros_lib)/shader/fs_rgb_to_occupancy_grid.glsl"/>
45+
46+
<remap from="depth_in" to="/d435/depth/image_rect_raw"/>
47+
<remap from="~map" to="/occupancy_grid2"/>
48+
</node>
49+
</launch>

‎opengl_ros/launch/object_position_extraction.launch‎

Lines changed: 134 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,134 @@
1+
<?xml version="1.0"?>
2+
<launch>
3+
<arg name="bag_prefix" default="/mnt/log/d435"/>
4+
<arg name="depth_width" default="640"/>
5+
<arg name="depth_height" default="360"/>
6+
<arg name="frame_rate" default="30"/>
7+
8+
<!-- use small size image to accumulate in CPU -->
9+
<arg name="output_width" default="160"/>
10+
<arg name="output_height" default="90"/>
11+
12+
<!-- valid depth range of D435 -->
13+
<arg name="depth_min" default="0.105"/>
14+
<arg name="depth_max" default="10"/>
15+
16+
<!-- color extraction parameter -->
17+
<arg name="threshold_l" default="50"/>
18+
<arg name="svm_coef_a" default="0.181817179054"/>
19+
<arg name="svm_coef_b" default="-0.0205828687538"/>
20+
<arg name="svm_intercept" default="-27.8266303282"/>
21+
22+
<!-- object position extraction parameter -->
23+
<arg name="target_pixel_count_threshold" default="50"/> <!-- specify accumulated count in input resolution -->
24+
<arg name="target_pixel_count_max" default="500"/> <!-- restrict the point for DBSCAN to finish the process in a reasonable time -->
25+
<arg name="dbscan_epsilon" default="0.5"/>
26+
<arg name="dbscan_min_points" default="5"/>
27+
<arg name="sigma_coefficient" default="1.0"/>
28+
<arg name="object_size_min_x" default="0.1"/>
29+
<arg name="object_size_max_x" default="0.5"/>
30+
<arg name="object_size_min_y" default="0.1"/>
31+
<arg name="object_size_max_y" default="0.5"/>
32+
<arg name="object_candidate_lifetime" default="0.5"/>
33+
<arg name="object_separation_distance" default="2.0"/>
34+
35+
<arg name="d435" default="false"/>
36+
<arg name="viewer" default="false"/>
37+
<arg name="record" default="false"/>
38+
39+
<group ns="d435" if="$(arg d435)">
40+
<node pkg="nodelet" type="nodelet" name="realsense2_camera_manager" args="manager" output="screen"/>
41+
<node pkg="nodelet" type="nodelet" name="realsense2_camera" args="load realsense2_camera/RealSenseNodeFactory realsense2_camera_manager">
42+
<param name="enable_pointcloud" type="bool" value="false"/>
43+
<param name="enable_sync" type="bool" value="false"/>
44+
<param name="align_depth" type="bool" value="false"/>
45+
<param name="enable_fisheye" type="bool" value="false"/>
46+
<param name="enable_imu" type="bool" value="false"/>
47+
48+
<param name="enable_depth" type="bool" value="true"/>
49+
<param name="enable_infra1" type="bool" value="false"/>
50+
<param name="enable_infra2" type="bool" value="false"/>
51+
<param name="enable_color" type="bool" value="true"/>
52+
53+
<param name="depth_width" type="int" value="$(arg depth_width)"/>
54+
<param name="depth_height" type="int" value="$(arg depth_height)"/>
55+
<param name="depth_fps" type="int" value="$(arg frame_rate)"/>
56+
<param name="color_width" type="int" value="$(arg depth_width)"/>
57+
<param name="color_height" type="int" value="$(arg depth_height)"/>
58+
<param name="color_fps" type="int" value="$(arg frame_rate)"/>
59+
<param name="base_frame_id" type="str" value="d435_link"/>
60+
<param name="depth_frame_id" type="str" value="d435_depth_link"/>
61+
<param name="depth_optical_frame_id" type="str" value="d435_depth_optical"/>
62+
</node>
63+
</group>
64+
65+
<node pkg="opengl_ros" type="object_position_extractor"
66+
name="object_position_extractor" output="screen">
67+
<param name="color_frame_id" value="d435_color_optical"/>
68+
<param name="depth_frame_id" value="d435_depth_optical"/>
69+
<param name="fixed_frame_id" value="map"/>
70+
<param name="tf_wait_duration" value="0.1"/>
71+
72+
<param name="color_width" value="$(arg depth_width)"/>
73+
<param name="color_height" value="$(arg depth_height)"/>
74+
<param name="depth_width" value="$(arg depth_width)"/>
75+
<param name="depth_height" value="$(arg depth_height)"/>
76+
<param name="output_width" value="$(arg output_width)"/>
77+
<param name="output_height" value="$(arg output_height)"/>
78+
79+
<param name="min_depth" value="$(arg depth_min)"/>
80+
<param name="max_depth" value="$(arg depth_max)"/>
81+
82+
<param name="threshold_l" value="$(arg threshold_l)"/>
83+
<param name="svm_coef_a" value="$(arg svm_coef_a)"/>
84+
<param name="svm_coef_b" value="$(arg svm_coef_b)"/>
85+
<param name="svm_intercept" value="$(arg svm_intercept)"/>
86+
87+
<param name="object_separation_distance" value="$(arg object_separation_distance)"/>
88+
<param name="target_pixel_count_threshold" value="$(arg target_pixel_count_threshold)"/>
89+
<param name="target_pixel_count_max" value="$(arg target_pixel_count_max)"/>
90+
<param name="sigma_coefficient" value="$(arg sigma_coefficient)"/>
91+
<param name="object_size_min_x" value="$(arg object_size_min_x)"/>
92+
<param name="object_size_max_x" value="$(arg object_size_max_x)"/>
93+
<param name="object_size_min_y" value="$(arg object_size_min_y)"/>
94+
<param name="object_size_max_y" value="$(arg object_size_max_y)"/>
95+
<param name="object_candidate_lifetime" value="$(arg object_candidate_lifetime)"/>
96+
<param name="dbscan_epsilon" value="$(arg dbscan_epsilon)"/>
97+
<param name="dbscan_min_points" value="$(arg dbscan_min_points)"/>
98+
99+
<param name="vertex_shader" value="$(find opengl_ros_lib)/shader/vs_object_position_extraction.glsl"/>
100+
<param name="fragment_shader" value="$(find opengl_ros_lib)/shader/fs_object_position_extraction.glsl"/>
101+
102+
<remap from="color_in" to="/d435/color/image_raw"/>
103+
<remap from="depth_in" to="/d435/depth/image_rect_raw"/>
104+
<remap from="image_out" to="/object_extraction"/>
105+
<remap from="~objects" to="/objects"/>
106+
</node>
107+
108+
<group ns="viewer" if="$(arg viewer)">
109+
<node pkg="image_view" type="image_view" name="viewer_depth">
110+
<remap from="image" to="/d435/depth/image_rect_raw"/>
111+
</node>
112+
113+
<node pkg="image_view" type="image_view" name="viewer_color">
114+
<remap from="image" to="/d435/color/image_raw"/>
115+
</node>
116+
117+
<node pkg="image_view" type="image_view" name="viewer_out">
118+
<remap from="image" to="/object_extraction"/>
119+
</node>
120+
</group>
121+
122+
<node pkg="rosbag" type="record" name="logger" if="$(arg record)"
123+
args="-o $(arg bag_prefix)
124+
--lz4
125+
-a
126+
-x /(d435|object_extraction|viewer_depth|viewer_color|viewer_out)/(.*)
127+
/d435/color/camera_info
128+
/d435/color/image_raw
129+
/d435/depth/camera_info
130+
/d435/depth/image_rect_raw
131+
/d435/extrinsics/depth_to_color
132+
/object_extraction
133+
"/>
134+
</launch>

‎opengl_ros/package.xml‎

Lines changed: 10 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -41,13 +41,22 @@
4141
<!-- <test_depend>gtest</test_depend> -->
4242
<buildtool_depend>catkin</buildtool_depend>
4343
<build_depend>roscpp</build_depend>
44+
<build_depend>std_msgs</build_depend>
4445
<build_depend>sensor_msgs</build_depend>
46+
<build_depend>cgs_nav_msgs</build_depend>
47+
<build_depend>tf</build_depend>
4548
<build_depend>image_transport</build_depend>
4649
<build_depend>cv_bridge</build_depend>
4750
<build_depend>opengl_ros_lib</build_depend>
51+
<run_depend>roscpp</run_depend>
52+
<run_depend>std_msgs</run_depend>
53+
<run_depend>sensor_msgs</run_depend>
54+
<run_depend>cgs_nav_msgs</run_depend>
55+
<run_depend>tf</run_depend>
56+
<run_depend>image_transport</run_depend>
57+
<run_depend>cv_bridge</run_depend>
4858
<run_depend>opengl_ros_lib</run_depend>
4959

50-
5160
<!-- The export tag contains other, unspecified, tags -->
5261
<export>
5362
<!-- Other tools can request additional information be placed here -->

‎opengl_ros/scripts/test_frame_broadcaster.py‎

Lines changed: 78 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,78 @@
1+
#!/usr/bin/env python
2+
import roslib
3+
import rospy
4+
import tf
5+
6+
#Using GUI interface for capturing key inputs
7+
import curses
8+
import os
9+
10+
import threading
11+
12+
#TODO refrain to use global variables
13+
roll = 0.0
14+
pitch = 0.0
15+
yaw = 0.0
16+
17+
def publisher(frame, parent_frame):
18+
global roll
19+
global pitch
20+
global yaw
21+
22+
br = tf.TransformBroadcaster()
23+
r = rospy.Rate(10)
24+
25+
while not rospy.is_shutdown():
26+
br.sendTransform(
27+
(0, 0, 0),
28+
tf.transformations.quaternion_from_euler(
29+
roll,
30+
pitch,
31+
yaw),
32+
rospy.Time.now(),
33+
frame,
34+
parent_frame)
35+
r.sleep()
36+
37+
def main(win):
38+
global roll
39+
global pitch
40+
global yaw
41+
42+
win.clear()
43+
win.addstr("Launched test_frame_broadcaster ros node\n")
44+
win.addstr("- W : Pitch Up\n")
45+
win.addstr("- S : Pitch Down\n")
46+
win.addstr("- A : Yaw Up\n")
47+
win.addstr("- D : Yaw Down\n")
48+
win.addstr("- E : Roll Up\n")
49+
win.addstr("- Q : Roll Down\n")
50+
51+
rospy.init_node('test_frame_broadcaster')
52+
frame = rospy.get_param('frame', 'base_link')
53+
parent_frame = rospy.get_param('parent_frame', 'map')
54+
step = float(rospy.get_param('step', '0.1'))
55+
56+
threading.Thread(target=publisher, args=(frame, parent_frame)).start()
57+
58+
while not rospy.is_shutdown():
59+
try:
60+
key = str(win.getkey())
61+
62+
if key == 'w':
63+
pitch += step
64+
if key == 's':
65+
pitch -= step
66+
if key == 'a':
67+
yaw += step
68+
if key == 'd':
69+
yaw -= step
70+
if key == 'e':
71+
roll += step
72+
if key == 'q':
73+
roll -= step
74+
75+
except Exception as e:
76+
pass
77+
78+
curses.wrapper(main)

0 commit comments

Comments
 (0)