i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-04 08:47:45 +08:00
Code Issues Packages Projects Releases Wiki Activity

Added "decimation" parameter to data_throttle and stereo_throttle nodelets. Added cloud processing parameters from point_cloud_xyz to point_cloud_xyzrgb nodelet. Added stereo inputs to point_cloud_xyzrgb.

Browse Source
This commit is contained in:
Mathieu Labbe
2015-02-16 16:26:44 -05:00
parent 4ac2eb09b0
commit 4314246d1e
20 changed files with 491 additions and 158 deletions
Download Patch File Download Diff File Expand all files Collapse all files
launch/azimut3/az3_bag_record.launch
+2 -2
View File
@@ -20,8 +20,8 @@
<!-- Throttling messages --> <!-- Throttling messages -->
<group ns="camera"> <group ns="camera">
<node pkg="nodelet" type="nodelet" name="data_throttle" args="load rtabmap/data_throttle camera_nodelet_manager" output="screen"> <node pkg="nodelet" type="nodelet" name="data_throttle" args="load rtabmap_ros/data_throttle camera_nodelet_manager" output="screen">
<param name="max_rate" type="double" value="10.0"/> <param name="rate" type="double" value="10.0"/>
<remap from="rgb/image_in" to="rgb/image_rect_color"/> <remap from="rgb/image_in" to="rgb/image_rect_color"/>
<remap from="depth/image_in" to="depth_registered/image_raw"/> <remap from="depth/image_in" to="depth_registered/image_raw"/>
launch/azimut3/az3_db_record.launch
+2 -2
View File
@@ -11,8 +11,8 @@
<!-- Throttling messages --> <!-- Throttling messages -->
<group ns="camera"> <group ns="camera">
<node pkg="nodelet" type="nodelet" name="data_throttle" args="load rtabmap/data_throttle camera_nodelet_manager" output="screen"> <node pkg="nodelet" type="nodelet" name="data_throttle" args="load rtabmap_ros/data_throttle camera_nodelet_manager" output="screen">
<param name="max_rate" type="double" value="10.0"/> <param name="rate" type="double" value="10.0"/>
<remap from="rgb/image_in" to="rgb/image_rect_color"/> <remap from="rgb/image_in" to="rgb/image_rect_color"/>
<remap from="depth/image_in" to="depth_registered/image_raw"/> <remap from="depth/image_in" to="depth_registered/image_raw"/>
launch/azimut3/az3_mapping_client_nav.launch
+28 -5
View File
@@ -4,6 +4,25 @@
<!-- Choose visualization --> <!-- Choose visualization -->
<arg name="rviz" default="true" /> <arg name="rviz" default="true" />
<arg name="rtabmapviz" default="false" /> <arg name="rtabmapviz" default="false" />
<arg name="sub_data" default="false"/>
<!-- use a relay on this machine -->
<node name="mapData_relay" type="relay" pkg="topic_tools" args="/rtabmap/mapData /rtabmap/mapData_relay">
<param name="lazy" type="bool" value="true"/>
</node>
<node if="$(arg sub_data)" name="scan_relay" type="relay" pkg="topic_tools" args="/base_scan /base_scan_relay">
<param name="lazy" type="bool" value="true"/>
</node>
<node if="$(arg sub_data)" name="republish_rgb" type="republish" pkg="image_transport" args="theora in:=/camera/data_resized_image raw out:=/camera/data_resized_image_relay" />
<node if="$(arg sub_data)" name="republish_depth" type="republish" pkg="image_transport" args="compressedDepth in:=/camera/data_resized_image_depth raw out:=/camera/data_resized_image_depth_relay" />
<node if="$(arg sub_data)" pkg="nodelet" type="nodelet" name="points_xyzrgb" args="standalone rtabmap_ros/point_cloud_xyzrgb">
<remap from="rgb/image" to="/camera/data_resized_image_relay"/>
<remap from="depth/image" to="/camera/data_resized_image_depth_relay"/>
<remap from="rgb/camera_info" to="/camera/data_resized_camera_info"/>
<remap from="cloud" to="/voxel_cloud" />
</node>
<group ns="rtabmap"> <group ns="rtabmap">
<!-- Visualisation RTAB-Map --> <!-- Visualisation RTAB-Map -->
@@ -11,14 +30,18 @@
<param name="queue_size" type="int" value="10"/> <param name="queue_size" type="int" value="10"/>
<param name="frame_id" type="string" value="base_footprint"/> <param name="frame_id" type="string" value="base_footprint"/>
<remap from="odom" to="/base_controller/odom"/> <remap from="odom" to="/base_controller/odom"/>
<remap from="mapData" to="mapData_relay"/>
<param name="subscribe_depth" type="bool" value="$(arg sub_data)"/>
<remap from="rgb/image" to="/camera/data_resized_image_relay"/>
<remap from="depth/image" to="/camera/data_resized_image_depth_relay"/>
<remap from="rgb/camera_info" to="/camera/data_resized_camera_info"/>
<param name="subscribe_laserScan" type="bool" value="$(arg sub_data)"/>
<remap from="scan" to="/base_scan_relay"/>
</node> </node>
</group> </group>
<!-- Visualisation RVIZ --> <!-- Visualisation RVIZ -->
<node if="$(arg rviz)" pkg="rviz" type="rviz" name="rviz" args="-d $(find rtabmap_ros)/launch/azimut3/config/azimut3_nav.rviz"/> <node if="$(arg rviz)" pkg="rviz" type="rviz" name="rviz" args="-d $(find rtabmap_ros)/launch/azimut3/config/azimut3_nav.rviz"/>
<!-- use a relay on this machine, same for images -->
<node if="$(arg rviz)" name="mapData_relay" type="relay" pkg="topic_tools" args="/rtabmap/mapData /rtabmap/mapData_relay">
<param name="lazy" type="bool" value="true"/>
</node>
</launch> </launch>
launch/azimut3/az3_mapping_client_nav.launch~
-23
View File
@@ -1,23 +0,0 @@
<launch>
<!-- We have two nodes (grid_map_assembler and rviz) subscribing to /rtabmap/mapData, so use -->
<!-- a relay on this machine, same for images -->
<node name="mapData_relay" type="relay" pkg="topic_tools" args="/rtabmap/mapData_optimized /rtabmap/mapData_relay"/>
<node name="camera_info_relay" type="relay" pkg="topic_tools" args="/camera/data_throttled_camera_info /camera/data_throttled_camera_info_relay"/>
<node name="republish_rgb" type="republish" pkg="image_transport" args="theora in:=/camera/data_throttled_image raw out:=/camera/data_throttled_image_relay" />
<node name="republish_depth" type="republish" pkg="image_transport" args="compressedDepth in:=/camera/data_throttled_image_depth raw out:=/camera/data_throttled_image_depth_relay" />
<node pkg="rviz" type="rviz" name="rviz" args="-d $(find rtabmap_ros)/launch/azimut3/config/azimut3_nav.rviz"/>
<!-- Below, construct point cloud of the latest throttled data, disabled for bandwidth efficiency -->
<node pkg="nodelet" type="nodelet" name="points_xyzrgb" args="standalone rtabmap_ros/point_cloud_xyzrgb">
<remap from="rgb/image" to="/camera/data_throttled_image_relay"/>
<remap from="depth/image" to="/camera/data_throttled_image_depth_relay"/>
<remap from="rgb/camera_info" to="/camera/data_throttled_camera_info_relay"/>
<remap from="cloud" to="voxel_cloud" />
<param name="queue_size" type="int" value="10"/>
<param name="voxel_size" type="double" value="0.01"/>
</node>
</launch>
launch/azimut3/az3_mapping_robot.launch
+1 -1
View File
@@ -11,7 +11,7 @@
<!-- Throttling messages --> <!-- Throttling messages -->
<group ns="camera"> <group ns="camera">
<node pkg="nodelet" type="nodelet" name="data_throttle" args="load rtabmap_ros/data_throttle camera_nodelet_manager" output="screen"> <node pkg="nodelet" type="nodelet" name="data_throttle" args="load rtabmap_ros/data_throttle camera_nodelet_manager" output="screen">
<param name="max_rate" type="double" value="5.0"/> <param name="rate" type="double" value="5.0"/>
<remap from="rgb/image_in" to="rgb/image_rect_color"/> <remap from="rgb/image_in" to="rgb/image_rect_color"/>
<remap from="depth/image_in" to="depth_registered/image_raw"/> <remap from="depth/image_in" to="depth_registered/image_raw"/>
launch/azimut3/az3_mapping_robot_kinect_odom.launch
+1 -1
View File
@@ -11,7 +11,7 @@
<!-- Throttling messages --> <!-- Throttling messages -->
<group ns="camera"> <group ns="camera">
<node pkg="nodelet" type="nodelet" name="data_throttle" args="load rtabmap_ros/data_throttle camera_nodelet_manager" output="screen"> <node pkg="nodelet" type="nodelet" name="data_throttle" args="load rtabmap_ros/data_throttle camera_nodelet_manager" output="screen">
<param name="max_rate" type="double" value="5.0"/> <param name="rate" type="double" value="5.0"/>
<remap from="rgb/image_in" to="rgb/image_rect_color"/> <remap from="rgb/image_in" to="rgb/image_rect_color"/>
<remap from="depth/image_in" to="depth_registered/image_raw"/> <remap from="depth/image_in" to="depth_registered/image_raw"/>
launch/azimut3/az3_mapping_robot_kinect_scan.launch
+1 -1
View File
@@ -11,7 +11,7 @@
<!-- Throttling messages --> <!-- Throttling messages -->
<group ns="camera"> <group ns="camera">
<node pkg="nodelet" type="nodelet" name="data_throttle" args="load rtabmap_ros/data_throttle camera_nodelet_manager" output="screen"> <node pkg="nodelet" type="nodelet" name="data_throttle" args="load rtabmap_ros/data_throttle camera_nodelet_manager" output="screen">
<param name="max_rate" type="double" value="5.0"/> <param name="rate" type="double" value="5.0"/>
<remap from="rgb/image_in" to="rgb/image_rect_color"/> <remap from="rgb/image_in" to="rgb/image_rect_color"/>
<remap from="depth/image_in" to="depth_registered/image_raw"/> <remap from="depth/image_in" to="depth_registered/image_raw"/>
launch/azimut3/az3_mapping_robot_nav.launch
+11 -8
View File
@@ -8,7 +8,9 @@
<include file="$(find az3_bringup)/az3_standalone.launch"/> <include file="$(find az3_bringup)/az3_standalone.launch"/>
<!-- OpenNI --> <!-- OpenNI -->
<include file="$(find rtabmap_ros)/launch/azimut3/az3_openni.launch"/> <include file="$(find rtabmap_ros)/launch/azimut3/az3_openni.launch">
<arg name="data_skip" value="5" />
</include>
<!-- SLAM (robot side) --> <!-- SLAM (robot side) -->
<group ns="rtabmap"> <group ns="rtabmap">
@@ -108,23 +110,24 @@
<!-- Throttling messages --> <!-- Throttling messages -->
<group ns="camera"> <group ns="camera">
<node pkg="nodelet" type="nodelet" name="data_throttle" args="load rtabmap_ros/data_throttle camera_nodelet_manager" output="screen"> <node pkg="nodelet" type="nodelet" name="data_throttle" args="load rtabmap_ros/data_throttle camera_nodelet_manager" output="screen">
<param name="max_rate" type="double" value="4.0"/> <param name="rate" type="double" value="0"/> <!-- set by data_skip above -->
<param name="decimation" type="int" value="4"/>
<remap from="rgb/image_in" to="rgb/image_rect_color"/> <remap from="rgb/image_in" to="rgb/image_rect_color"/>
<remap from="depth/image_in" to="depth_registered/image_raw"/> <remap from="depth/image_in" to="depth_registered/image_raw"/>
<remap from="rgb/camera_info_in" to="depth_registered/camera_info"/> <remap from="rgb/camera_info_in" to="depth_registered/camera_info"/>
<remap from="rgb/image_out" to="data_throttled_image"/> <remap from="rgb/image_out" to="data_resized_image"/>
<remap from="depth/image_out" to="data_throttled_image_depth"/> <remap from="depth/image_out" to="data_resized_image_depth"/>
<remap from="rgb/camera_info_out" to="data_throttled_camera_info"/> <remap from="rgb/camera_info_out" to="data_resized_camera_info"/>
</node> </node>
<!-- for the planner --> <!-- for the planner -->
<node pkg="nodelet" type="nodelet" name="points_xyz_planner" args="load rtabmap_ros/point_cloud_xyz camera_nodelet_manager"> <node pkg="nodelet" type="nodelet" name="points_xyz_planner" args="load rtabmap_ros/point_cloud_xyz camera_nodelet_manager">
<remap from="depth/image" to="/camera/data_throttled_image_depth"/> <remap from="depth/image" to="data_resized_image_depth"/>
<remap from="depth/camera_info" to="/camera/data_throttled_camera_info"/> <remap from="depth/camera_info" to="data_resized_camera_info"/>
<remap from="cloud" to="/local_costmap_cloud" /> <remap from="cloud" to="/local_costmap_cloud" />
<param name="decimation" type="int" value="4"/> <param name="decimation" type="int" value="1"/>
<param name="max_depth" type="double" value="6.0"/> <param name="max_depth" type="double" value="6.0"/>
<param name="noise_filter_radius" type="double" value="0.05"/> <param name="noise_filter_radius" type="double" value="0.05"/>
<param name="noise_filter_min_neighbors" type="int" value="10"/> <param name="noise_filter_min_neighbors" type="int" value="10"/>
launch/azimut3/az3_openni.launch
+3
View File
@@ -1,9 +1,12 @@
<launch> <launch>
<arg name="data_skip" default="0" />
<!-- Xtion --> <!-- Xtion -->
<include file="$(find openni2_launch)/launch/openni2.launch"> <include file="$(find openni2_launch)/launch/openni2.launch">
<arg name="depth_registration" value="True" /> <arg name="depth_registration" value="True" />
<arg name="data_skip" value="$(arg data_skip)" />
<arg name="rgb_camera_info_url" <arg name="rgb_camera_info_url"
value="file://$(find rtabmap_ros)/launch/calibration/rgb_PS1080_PrimeSense.yaml" /> value="file://$(find rtabmap_ros)/launch/calibration/rgb_PS1080_PrimeSense.yaml" />
<arg name="depth_camera_info_url" <arg name="depth_camera_info_url"
launch/azimut3/az3_remote_mapping_robot.launch
+1 -1
View File
@@ -11,7 +11,7 @@
<!-- Throttling messages --> <!-- Throttling messages -->
<group ns="camera"> <group ns="camera">
<node pkg="nodelet" type="nodelet" name="data_throttle" args="load rtabmap_ros/data_throttle camera_nodelet_manager" output="screen"> <node pkg="nodelet" type="nodelet" name="data_throttle" args="load rtabmap_ros/data_throttle camera_nodelet_manager" output="screen">
<param name="max_rate" type="double" value="5.0"/> <param name="rate" type="double" value="5.0"/>
<remap from="rgb/image_in" to="rgb/image_rect_color"/> <remap from="rgb/image_in" to="rgb/image_rect_color"/>
<remap from="depth/image_in" to="depth_registered/image_raw"/> <remap from="depth/image_in" to="depth_registered/image_raw"/>
launch/azimut3/config/azimut3_nav.rviz
+42 -42
View File
@@ -7,7 +7,7 @@ Panels:
- /Global Options1 - /Global Options1
- /TF1/Frames1 - /TF1/Frames1
Splitter Ratio: 0.601881 Splitter Ratio: 0.601881
Tree Height: 371 Tree Height: 187
- Class: rviz/Selection - Class: rviz/Selection
Name: Selection Name: Selection
- Class: rviz/Views - Class: rviz/Views
@@ -73,7 +73,7 @@ Visualization Manager:
Color: 255; 255; 255 Color: 255; 255; 255
Color Transformer: AxisColor Color Transformer: AxisColor
Decay Time: 0 Decay Time: 0
Enabled: false Enabled: true
Invert Rainbow: false Invert Rainbow: false
Max Color: 255; 255; 255 Max Color: 255; 255; 255
Max Intensity: 4096 Max Intensity: 4096
@@ -86,10 +86,10 @@ Visualization Manager:
Size (Pixels): 5 Size (Pixels): 5
Size (m): 0.01 Size (m): 0.01
Style: Points Style: Points
Topic: /base_scan Topic: /base_scan_relay
Use Fixed Frame: false Use Fixed Frame: false
Use rainbow: true Use rainbow: true
Value: false Value: true
- Alpha: 0.7 - Alpha: 0.7
Class: rviz/Map Class: rviz/Map
Color Scheme: map Color Scheme: map
@@ -131,8 +131,8 @@ Visualization Manager:
Value: true Value: true
Visual Enabled: true Visual Enabled: true
- Class: rviz/Image - Class: rviz/Image
Enabled: false Enabled: true
Image Topic: /camera/data_throttled_image Image Topic: /camera/data_resized_image_relay
Max Value: 1 Max Value: 1
Median window: 5 Median window: 5
Min Value: 0 Min Value: 0
@@ -140,7 +140,7 @@ Visualization Manager:
Normalize Range: true Normalize Range: true
Queue Size: 2 Queue Size: 2
Transport Hint: theora Transport Hint: theora
Value: false Value: true
- Alpha: 1 - Alpha: 1
Autocompute Intensity Bounds: true Autocompute Intensity Bounds: true
Autocompute Value Bounds: Autocompute Value Bounds:
@@ -158,7 +158,7 @@ Visualization Manager:
Download graph: false Download graph: false
Download map: false Download map: false
Enabled: true Enabled: true
Filter floor (m): 0.1 Filter floor (m): 0.07
Invert Rainbow: false Invert Rainbow: false
Max Color: 255; 255; 255 Max Color: 255; 255; 255
Max Intensity: 4096 Max Intensity: 4096
@@ -180,26 +180,12 @@ Visualization Manager:
Name: Info Name: Info
Topic: /rtabmap/info Topic: /rtabmap/info
Value: true Value: true
- Alpha: 1
Axes Length: 1
Axes Radius: 0.1
Class: rviz/Pose
Color: 255; 25; 0
Enabled: true
Head Length: 0.3
Head Radius: 0.1
Name: Pose
Shaft Length: 1
Shaft Radius: 0.05
Shape: Arrow
Topic: /rtabmap/goal
Value: true
- Alpha: 1 - Alpha: 1
Buffer Length: 1 Buffer Length: 1
Class: rviz/Path Class: rviz/Path
Color: 255; 149; 57 Color: 255; 149; 57
Enabled: true Enabled: true
Name: Path Name: move_base global plan
Topic: /planner/move_base/NavfnROS/plan Topic: /planner/move_base/NavfnROS/plan
Value: true Value: true
- Alpha: 1 - Alpha: 1
@@ -207,7 +193,7 @@ Visualization Manager:
Class: rviz/Path Class: rviz/Path
Color: 2; 14; 255 Color: 2; 14; 255
Enabled: true Enabled: true
Name: Path Name: move_base local plan
Topic: /planner/move_base/TrajectoryPlannerROS/local_plan Topic: /planner/move_base/TrajectoryPlannerROS/local_plan
Value: true Value: true
- Alpha: 1 - Alpha: 1
@@ -222,36 +208,36 @@ Visualization Manager:
Color: 85; 255; 0 Color: 85; 255; 0
Color Transformer: RGB8 Color Transformer: RGB8
Decay Time: 0 Decay Time: 0
Enabled: false Enabled: true
Invert Rainbow: false Invert Rainbow: false
Max Color: 255; 255; 255 Max Color: 255; 255; 255
Max Intensity: 4096 Max Intensity: 4096
Min Color: 0; 0; 0 Min Color: 0; 0; 0
Min Intensity: 0 Min Intensity: 0
Name: PointCloud2 Name: Voxel_cloud
Position Transformer: XYZ Position Transformer: XYZ
Queue Size: 10 Queue Size: 10
Selectable: true Selectable: true
Size (Pixels): 3 Size (Pixels): 3
Size (m): 0.01 Size (m): 0.01
Style: Points Style: Points
Topic: /rtabmap/cloud_map Topic: /voxel_cloud
Use Fixed Frame: true Use Fixed Frame: true
Use rainbow: true Use rainbow: true
Value: false Value: true
- Alpha: 1 - Alpha: 1
Class: rtabmap_ros/MapGraph Class: rtabmap_ros/MapGraph
Color: 0; 0; 255 Color: 0; 0; 255
Enabled: false Enabled: true
Name: MapGraph Name: MapGraph
Topic: /rtabmap/mapData_relay Topic: /rtabmap/mapData_relay
Value: false Value: true
- Alpha: 1 - Alpha: 1
Buffer Length: 1 Buffer Length: 1
Class: rviz/Path Class: rviz/Path
Color: 255; 0; 255 Color: 255; 0; 255
Enabled: true Enabled: true
Name: Path Name: Rtabmap global path
Topic: /rtabmap/global_path Topic: /rtabmap/global_path
Value: true Value: true
- Alpha: 1 - Alpha: 1
@@ -259,7 +245,7 @@ Visualization Manager:
Class: rviz/Path Class: rviz/Path
Color: 85; 255; 255 Color: 85; 255; 255
Enabled: true Enabled: true
Name: Path Name: Rtabmap local path
Topic: /rtabmap/local_path Topic: /rtabmap/local_path
Value: true Value: true
- Alpha: 1 - Alpha: 1
@@ -270,12 +256,26 @@ Visualization Manager:
Enabled: true Enabled: true
Head Length: 0.3 Head Length: 0.3
Head Radius: 0.1 Head Radius: 0.1
Name: Pose Name: Current goal
Shaft Length: 1 Shaft Length: 1
Shaft Radius: 0.05 Shaft Radius: 0.05
Shape: Arrow Shape: Arrow
Topic: /rtabmap/current_goal Topic: /rtabmap/current_goal
Value: true Value: true
- Alpha: 1
Axes Length: 1
Axes Radius: 0.1
Class: rviz/Pose
Color: 255; 25; 0
Enabled: true
Head Length: 0.3
Head Radius: 0.1
Name: Global goal
Shaft Length: 1
Shaft Radius: 0.05
Shape: Arrow
Topic: /rtabmap/goal
Value: true
Enabled: true Enabled: true
Global Options: Global Options:
Background Color: 48; 48; 48 Background Color: 48; 48; 48
@@ -296,22 +296,22 @@ Visualization Manager:
Views: Views:
Current: Current:
Class: rviz/Orbit Class: rviz/Orbit
Distance: 17.1662 Distance: 13.3786
Enable Stereo Rendering: Enable Stereo Rendering:
Stereo Eye Separation: 0.06 Stereo Eye Separation: 0.06
Stereo Focal Distance: 1 Stereo Focal Distance: 1
Swap Stereo Eyes: false Swap Stereo Eyes: false
Value: false Value: false
Focal Point: Focal Point:
X: -0.0852849 X: -1.4347
Y: 0.0904403 Y: 1.38269
Z: -0.189729 Z: -0.0856586
Name: Current View Name: Current View
Near Clip Distance: 0.01 Near Clip Distance: 0.01
Pitch: 1.2048 Pitch: 0.884797
Target Frame: base_footprint Target Frame: base_footprint
Value: Orbit (rviz) Value: Orbit (rviz)
Yaw: 3.50041 Yaw: 3.81544
Saved: ~ Saved: ~
Window Geometry: Window Geometry:
Displays: Displays:
@@ -321,7 +321,7 @@ Window Geometry:
Hide Right Dock: false Hide Right Dock: false
Image: Image:
collapsed: false collapsed: false
QMainWindow State: 000000ff00000000fd000000040000000000000151000002dafc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000001b4000000dd00fffffffb0000000a0049006d00610067006500000001290000012d0000001600fffffffb0000000a0049006d0061006700650000000184000000490000000000000000fb0000000a0049006d006100670065010000027d000000fa0000000000000000fb0000001e0054006f006f006c002000500072006f007000650072007400690065007301000001e2000001200000006400ffffff000000010000010f000001b2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000001b2000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004a00000003efc0100000002fb0000000800540069006d00650000000000000004a0000002f600fffffffb0000000800540069006d0065010000000000000450000000000000000000000375000002da00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 QMainWindow State: 000000ff00000000fd000000040000000000000151000002dafc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000000fc000000dd00fffffffb0000000a0049006d006100670065010000012a0000012d0000001600fffffffb0000000a0049006d0061006700650000000184000000490000000000000000fb0000000a0049006d006100670065010000027d000000fa0000000000000000fb0000001e0054006f006f006c002000500072006f0070006500720074006900650073010000025d000000a50000006400ffffff000000010000010f000001b2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000001b2000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004a00000003efc0100000002fb0000000800540069006d00650000000000000004a0000002f600fffffffb0000000800540069006d0065010000000000000450000000000000000000000375000002da00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Selection: Selection:
collapsed: false collapsed: false
Time: Time:
@@ -331,5 +331,5 @@ Window Geometry:
Views: Views:
collapsed: false collapsed: false
Width: 1228 Width: 1228
X: 372 X: 406
Y: 32 Y: 154
launch/data_recorder.launch
+1 -1
View File
@@ -22,7 +22,7 @@
<!-- Throttling messages --> <!-- Throttling messages -->
<node pkg="nodelet" type="nodelet" name="standalone_nodelet" args="manager"/> <node pkg="nodelet" type="nodelet" name="standalone_nodelet" args="manager"/>
<node pkg="nodelet" type="nodelet" name="data_throttle" args="load rtabmap_ros/data_throttle standalone_nodelet" output="screen"> <node pkg="nodelet" type="nodelet" name="data_throttle" args="load rtabmap_ros/data_throttle standalone_nodelet" output="screen">
<param name="max_rate" type="double" value="10.0"/> <param name="rate" type="double" value="10.0"/>
<remap from="rgb/image_in" to="/camera/rgb/image_rect_color"/> <remap from="rgb/image_in" to="/camera/rgb/image_rect_color"/>
<remap from="depth/image_in" to="/camera/depth_registered/image_raw"/> <remap from="depth/image_in" to="/camera/depth_registered/image_raw"/>
launch/demo/demo_multi-session_mapping.launch
+1
View File
@@ -51,6 +51,7 @@
<param name="Bayes/FullPredictionUpdate" type="string" value="true"/> <param name="Bayes/FullPredictionUpdate" type="string" value="true"/>
<param name="Kp/DetectorStrategy" type="string" value="0"/> <!-- use SURF --> <param name="Kp/DetectorStrategy" type="string" value="0"/> <!-- use SURF -->
<param name="Kp/NNStrategy" type="string" value="1"/> <!-- kdTree --> <param name="Kp/NNStrategy" type="string" value="1"/> <!-- kdTree -->
<param name="Kp/WordsPerImage" type="string" value="400"/>
</node> </node>
<!-- Visualisation RTAB-Map --> <!-- Visualisation RTAB-Map -->
launch/tests/bumblebee.launch
+1 -1
View File
@@ -18,7 +18,7 @@
<!-- Run the ROS package stereo_image_proc --> <!-- Run the ROS package stereo_image_proc -->
<group ns="/stereo_camera" > <group ns="/stereo_camera" >
<node pkg="stereo_image_proc" type="stereo_image_proc" name="stereo_image_proc"/> <node pkg="stereo_image_proc" type="stereo_image_proc" name="stereo_image_proc"/>
<node pkg="nodelet" type="nodelet" name="disparity2depth" args="standalone rtabmap/disparity_to_depth"/> <node pkg="nodelet" type="nodelet" name="disparity2depth" args="standalone rtabmap_ros/disparity_to_depth"/>
</group> </group>
launch/tests/test_odometry.launch
+2 -2
View File
@@ -19,7 +19,7 @@
<node pkg="nodelet" type="nodelet" name="standalone_nodelet" args="manager" output="screen"/> <node pkg="nodelet" type="nodelet" name="standalone_nodelet" args="manager" output="screen"/>
<!-- sync cloud with odometry and voxelize the point cloud (for fast visualization in rviz) --> <!-- sync cloud with odometry and voxelize the point cloud (for fast visualization in rviz) -->
<node pkg="nodelet" type="nodelet" name="data_odom_sync" args="load rtabmap/data_odom_sync standalone_nodelet"> <node pkg="nodelet" type="nodelet" name="data_odom_sync" args="load rtabmap_ros/data_odom_sync standalone_nodelet">
<remap from="rgb/image_in" to="camera/rgb/image_rect_color"/> <remap from="rgb/image_in" to="camera/rgb/image_rect_color"/>
<remap from="depth/image_in" to="camera/depth_registered/image_raw"/> <remap from="depth/image_in" to="camera/depth_registered/image_raw"/>
<remap from="rgb/camera_info_in" to="camera/depth_registered/camera_info"/> <remap from="rgb/camera_info_in" to="camera/depth_registered/camera_info"/>
@@ -32,7 +32,7 @@
<param name="queue_size" type="int" value="30"/> <param name="queue_size" type="int" value="30"/>
</node> </node>
<node pkg="nodelet" type="nodelet" name="points_xyzrgb" args="load rtabmap/point_cloud_xyzrgb standalone_nodelet"> <node pkg="nodelet" type="nodelet" name="points_xyzrgb" args="load rtabmap_ros/point_cloud_xyzrgb standalone_nodelet">
<remap from="rgb/image" to="data_odom_sync/image"/> <remap from="rgb/image" to="data_odom_sync/image"/>
<remap from="depth/image" to="data_odom_sync/depth"/> <remap from="depth/image" to="data_odom_sync/depth"/>
<remap from="rgb/camera_info" to="data_odom_sync/camera_info"/> <remap from="rgb/camera_info" to="data_odom_sync/camera_info"/>
launch/tests/test_pointxyzrgb.launch
+60 -11
View File
@@ -1,20 +1,69 @@
<launch> <launch>
<include file="$(find freenect_launch)/launch/freenect.launch" >
<arg name="depth_registration" value="true"/>
<arg name="data_skip" value="3"/>
</include>
<!-- sync cloud with odometry and voxelize the point cloud (for fast visualization in rviz) --> <group ns="camera">
<node pkg="nodelet" type="nodelet" name="points_xyzrgb" args="standalone rtabmap/point_cloud_xyzrgb"> <node pkg="nodelet" type="nodelet" name="data_throttle" args="load rtabmap_ros/data_throttle camera_nodelet_manager">
<remap from="rgb/image" to="camera/rgb/image_rect_color"/> <remap from="rgb/image_in" to="rgb/image_rect_color"/>
<remap from="depth/image" to="camera/depth_registered/image_raw"/> <remap from="depth/image_in" to="depth_registered/image_raw"/>
<remap from="rgb/camera_info" to="camera/depth_registered/camera_info"/> <remap from="rgb/camera_info_in" to="depth_registered/camera_info"/>
<remap from="cloud" to="/voxel_cloud" />
<param name="queue_size" type="int" value="30"/>
<param name="voxel_size" type="double" value="0.02"/>
<param name="decimation" type="int" value="2"/> <param name="decimation" type="int" value="2"/>
<param name="rgb/image_transport" type="string" value="compressed"/>
<param name="depth/image_transport" type="string" value="compressedDepth"/>
</node> </node>
<node pkg="nodelet" type="nodelet" name="points_xyzrgb" args="load rtabmap_ros/point_cloud_xyzrgb camera_nodelet_manager">
<remap from="rgb/image" to="rgb/image_out"/>
<remap from="depth/image" to="depth/image_out"/>
<remap from="rgb/camera_info" to="rgb/camera_info_out"/>
<remap from="cloud" to="/voxel_cloud" />
<param name="voxel_size" type="double" value="0.02"/>
<param name="decimation" type="int" value="2"/>
</node>
</group>
<!-- stereo test with stereo_outdoorA.bag -->
<!--
<param name="use_sim_time" value="true"/>
<node name="republish_left" type="republish" pkg="image_transport" args="compressed in:=/stereo_camera/left/image_raw_throttle raw out:=/stereo_camera/left/image_raw_throttle_relay" />
<node name="republish_right" type="republish" pkg="image_transport" args="compressed in:=/stereo_camera/right/image_raw_throttle raw out:=/stereo_camera/right/image_raw_throttle_relay" />
<group ns="/stereo_camera" >
<node pkg="stereo_image_proc" type="stereo_image_proc" name="stereo_image_proc">
<remap from="left/image_raw" to="left/image_raw_throttle_relay"/>
<remap from="left/camera_info" to="left/camera_info_throttle"/>
<remap from="right/image_raw" to="right/image_raw_throttle_relay"/>
<remap from="right/camera_info" to="right/camera_info_throttle"/>
<param name="disparity_range" value="128"/>
</node>
</group>
<node pkg="nodelet" type="nodelet" name="standalone_nodelet" args="manager" output="screen"/>
<node pkg="nodelet" type="nodelet" name="stereo_throttle" args="load rtabmap_ros/stereo_throttle standalone_nodelet">
<remap from="left/image" to="/stereo_camera/left/image_rect_color"/>
<remap from="right/image" to="/stereo_camera/right/image_rect"/>
<remap from="left/camera_info" to="/stereo_camera/left/camera_info_throttle"/>
<remap from="right/camera_info" to="/stereo_camera/right/camera_info_throttle"/>
<param name="decimation" type="int" value="2"/>
<param name="approx_sync" type="bool" value="false"/>
</node>
<node pkg="nodelet" type="nodelet" name="points_xyzrgb" args="load rtabmap_ros/point_cloud_xyzrgb standalone_nodelet">
<remap from="left/image" to="/stereo_camera/left/image_rect_color_throttle"/>
<remap from="right/image" to="/stereo_camera/right/image_rect_throttle"/>
<remap from="left/camera_info" to="/stereo_camera/left/camera_info_throttle_throttle"/>
<remap from="right/camera_info" to="/stereo_camera/right/camera_info_throttle_throttle"/>
<remap from="cloud" to="/voxel_cloud" />
<param name="decimation" type="int" value="2"/>
<param name="approx_sync" type="bool" value="false"/>
</node>
-->
</launch> </launch>
src/nodelets/data_throttle.cpp
+73 -14
View File
@@ -39,6 +39,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <sensor_msgs/CameraInfo.h> #include <sensor_msgs/CameraInfo.h>
#include <cv_bridge/cv_bridge.h>
#include <rtabmap/core/util3d.h>
namespace rtabmap_ros namespace rtabmap_ros
{ {
@@ -47,9 +51,10 @@ class DataThrottleNodelet : public nodelet::Nodelet
public: public:
//Constructor //Constructor
DataThrottleNodelet(): DataThrottleNodelet():
max_update_rate_(0), rate_(0),
approxSync_(0), approxSync_(0),
exactSync_(0) exactSync_(0),
decimation_(1)
{ {
} }
@@ -67,7 +72,7 @@ public:
private: private:
ros::Time last_update_; ros::Time last_update_;
double max_update_rate_; double rate_;
virtual void onInit() virtual void onInit()
{ {
ros::NodeHandle& nh = getNodeHandle(); ros::NodeHandle& nh = getNodeHandle();
@@ -84,9 +89,17 @@ private:
int queueSize = 10; int queueSize = 10;
bool approxSync = true; bool approxSync = true;
private_nh.param("max_rate", max_update_rate_, max_update_rate_); if(private_nh.getParam("max_rate", rate_))
{
ROS_WARN("\"max_rate\" is now known as \"rate\".");
}
private_nh.param("rate", rate_, rate_);
private_nh.param("queue_size", queueSize, queueSize); private_nh.param("queue_size", queueSize, queueSize);
private_nh.param("approx_sync", approxSync, approxSync); private_nh.param("approx_sync", approxSync, approxSync);
private_nh.param("decimation", decimation_, decimation_);
ROS_ASSERT(decimation_ >= 1);
ROS_INFO("Rate=%f Hz", rate_);
ROS_INFO("Decimation=%d", decimation_);
ROS_INFO("Approximate time sync = %s", approxSync?"true":"false"); ROS_INFO("Approximate time sync = %s", approxSync?"true":"false");
if(approxSync) if(approxSync)
@@ -104,40 +117,84 @@ private:
image_depth_sub_.subscribe(depth_it, depth_nh.resolveName("image_in"), 1, hintsDepth); image_depth_sub_.subscribe(depth_it, depth_nh.resolveName("image_in"), 1, hintsDepth);
info_sub_.subscribe(rgb_nh, "camera_info_in", 1); info_sub_.subscribe(rgb_nh, "camera_info_in", 1);
imagePub_ = rgb_it.advertise("image_out", 10); imagePub_ = rgb_it.advertise("image_out", 1);
imageDepthPub_ = depth_it.advertise("image_out", 10); imageDepthPub_ = depth_it.advertise("image_out", 1);
infoPub_ = rgb_nh.advertise<sensor_msgs::CameraInfo>("camera_info_out", 10); infoPub_ = rgb_nh.advertise<sensor_msgs::CameraInfo>("camera_info_out", 1);
}; };
void callback(const sensor_msgs::ImageConstPtr& image, void callback(const sensor_msgs::ImageConstPtr& image,
const sensor_msgs::ImageConstPtr& imageDepth, const sensor_msgs::ImageConstPtr& imageDepth,
const sensor_msgs::CameraInfoConstPtr& camInfo) const sensor_msgs::CameraInfoConstPtr& camInfo)
{ {
if (max_update_rate_ > 0.0) if (rate_ > 0.0)
{ {
NODELET_DEBUG("update set to %f", max_update_rate_); NODELET_DEBUG("update set to %f", rate_);
if ( last_update_ + ros::Duration(1.0/max_update_rate_) > ros::Time::now()) if ( last_update_ + ros::Duration(1.0/rate_) > ros::Time::now())
{ {
NODELET_DEBUG("throttle last update at %f skipping", last_update_.toSec()); NODELET_DEBUG("throttle last update at %f skipping", last_update_.toSec());
return; return;
} }
} }
else else
NODELET_DEBUG("update_rate unset continuing"); NODELET_DEBUG("rate unset continuing");
last_update_ = ros::Time::now(); last_update_ = ros::Time::now();
if(imagePub_.getNumSubscribers()) if(imagePub_.getNumSubscribers())
{ {
imagePub_.publish(image); if(decimation_ > 1)
{
cv_bridge::CvImageConstPtr imagePtr = cv_bridge::toCvShare(image);
cv_bridge::CvImage out;
out.header = imagePtr->header;
out.encoding = imagePtr->encoding;
out.image = rtabmap::util3d::decimate(imagePtr->image, decimation_);
imagePub_.publish(out.toImageMsg());
}
else
{
imagePub_.publish(image);
}
} }
if(imageDepthPub_.getNumSubscribers()) if(imageDepthPub_.getNumSubscribers())
{ {
imageDepthPub_.publish(imageDepth); if(decimation_ > 1)
{
cv_bridge::CvImageConstPtr imagePtr = cv_bridge::toCvShare(imageDepth);
cv_bridge::CvImage out;
out.header = imagePtr->header;
out.encoding = imagePtr->encoding;
out.image = rtabmap::util3d::decimate(imagePtr->image, decimation_);
imageDepthPub_.publish(out.toImageMsg());
}
else
{
imageDepthPub_.publish(imageDepth);
}
} }
if(infoPub_.getNumSubscribers()) if(infoPub_.getNumSubscribers())
{ {
infoPub_.publish(camInfo); if(decimation_ > 1)
{
sensor_msgs::CameraInfo info = *camInfo;
info.height /= decimation_;
info.width /= decimation_;
info.roi.height /= decimation_;
info.roi.width /= decimation_;
info.K[2]/=float(decimation_); // cx
info.K[5]/=float(decimation_); // cy
info.K[0]/=float(decimation_); // fx
info.K[4]/=float(decimation_); // fy
info.P[2]/=float(decimation_); // cx
info.P[6]/=float(decimation_); // cy
info.P[0]/=float(decimation_); // fx
info.P[5]/=float(decimation_); // fy
infoPub_.publish(info);
}
else
{
infoPub_.publish(camInfo);
}
} }
} }
@@ -154,6 +211,8 @@ private:
typedef message_filters::sync_policies::ExactTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo> MyExactSyncPolicy; typedef message_filters::sync_policies::ExactTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo> MyExactSyncPolicy;
message_filters::Synchronizer<MyExactSyncPolicy> * exactSync_; message_filters::Synchronizer<MyExactSyncPolicy> * exactSync_;
int decimation_;
}; };
src/nodelets/point_cloud_xyz.cpp
+6 -6
View File
@@ -105,7 +105,7 @@ private:
approxSyncDepth_ = new message_filters::Synchronizer<MyApproxSyncDepthPolicy>(MyApproxSyncDepthPolicy(queueSize), imageDepthSub_, cameraInfoSub_); approxSyncDepth_ = new message_filters::Synchronizer<MyApproxSyncDepthPolicy>(MyApproxSyncDepthPolicy(queueSize), imageDepthSub_, cameraInfoSub_);
approxSyncDepth_->registerCallback(boost::bind(&PointCloudXYZ::callback, this, _1, _2)); approxSyncDepth_->registerCallback(boost::bind(&PointCloudXYZ::callback, this, _1, _2));
approxSyncDisparity_ = new message_filters::Synchronizer<MyApproxSyncDisparityDispPolicy>(MyApproxSyncDisparityDispPolicy(queueSize), disparitySub_, disparityCameraInfoSub_); approxSyncDisparity_ = new message_filters::Synchronizer<MyApproxSyncDisparityPolicy>(MyApproxSyncDisparityPolicy(queueSize), disparitySub_, disparityCameraInfoSub_);
approxSyncDisparity_->registerCallback(boost::bind(&PointCloudXYZ::callbackDisparity, this, _1, _2)); approxSyncDisparity_->registerCallback(boost::bind(&PointCloudXYZ::callbackDisparity, this, _1, _2));
} }
else else
@@ -113,7 +113,7 @@ private:
exactSyncDepth_ = new message_filters::Synchronizer<MyExactSyncDepthPolicy>(MyExactSyncDepthPolicy(queueSize), imageDepthSub_, cameraInfoSub_); exactSyncDepth_ = new message_filters::Synchronizer<MyExactSyncDepthPolicy>(MyExactSyncDepthPolicy(queueSize), imageDepthSub_, cameraInfoSub_);
exactSyncDepth_->registerCallback(boost::bind(&PointCloudXYZ::callback, this, _1, _2)); exactSyncDepth_->registerCallback(boost::bind(&PointCloudXYZ::callback, this, _1, _2));
exactSyncDisparity_ = new message_filters::Synchronizer<MyExactSyncDisparityDispPolicy>(MyExactSyncDisparityDispPolicy(queueSize), disparitySub_, disparityCameraInfoSub_); exactSyncDisparity_ = new message_filters::Synchronizer<MyExactSyncDisparityPolicy>(MyExactSyncDisparityPolicy(queueSize), disparitySub_, disparityCameraInfoSub_);
exactSyncDisparity_->registerCallback(boost::bind(&PointCloudXYZ::callbackDisparity, this, _1, _2)); exactSyncDisparity_->registerCallback(boost::bind(&PointCloudXYZ::callbackDisparity, this, _1, _2));
} }
@@ -255,14 +255,14 @@ private:
typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::CameraInfo> MyApproxSyncDepthPolicy; typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::CameraInfo> MyApproxSyncDepthPolicy;
message_filters::Synchronizer<MyApproxSyncDepthPolicy> * approxSyncDepth_; message_filters::Synchronizer<MyApproxSyncDepthPolicy> * approxSyncDepth_;
typedef message_filters::sync_policies::ApproximateTime<stereo_msgs::DisparityImage, sensor_msgs::CameraInfo> MyApproxSyncDisparityDispPolicy; typedef message_filters::sync_policies::ApproximateTime<stereo_msgs::DisparityImage, sensor_msgs::CameraInfo> MyApproxSyncDisparityPolicy;
message_filters::Synchronizer<MyApproxSyncDisparityDispPolicy> * approxSyncDisparity_; message_filters::Synchronizer<MyApproxSyncDisparityPolicy> * approxSyncDisparity_;
typedef message_filters::sync_policies::ExactTime<sensor_msgs::Image, sensor_msgs::CameraInfo> MyExactSyncDepthPolicy; typedef message_filters::sync_policies::ExactTime<sensor_msgs::Image, sensor_msgs::CameraInfo> MyExactSyncDepthPolicy;
message_filters::Synchronizer<MyExactSyncDepthPolicy> * exactSyncDepth_; message_filters::Synchronizer<MyExactSyncDepthPolicy> * exactSyncDepth_;
typedef message_filters::sync_policies::ExactTime<stereo_msgs::DisparityImage, sensor_msgs::CameraInfo> MyExactSyncDisparityDispPolicy; typedef message_filters::sync_policies::ExactTime<stereo_msgs::DisparityImage, sensor_msgs::CameraInfo> MyExactSyncDisparityPolicy;
message_filters::Synchronizer<MyExactSyncDisparityDispPolicy> * exactSyncDisparity_; message_filters::Synchronizer<MyExactSyncDisparityPolicy> * exactSyncDisparity_;
}; };
src/nodelets/point_cloud_xyzrgb.cpp
+167 -21
View File
@@ -42,8 +42,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <image_transport/subscriber_filter.h> #include <image_transport/subscriber_filter.h>
#include <image_geometry/pinhole_camera_model.h> #include <image_geometry/pinhole_camera_model.h>
#include <image_geometry/stereo_camera_model.h>
#include <message_filters/sync_policies/approximate_time.h> #include <message_filters/sync_policies/approximate_time.h>
#include <message_filters/sync_policies/exact_time.h>
#include <message_filters/subscriber.h> #include <message_filters/subscriber.h>
#include <cv_bridge/cv_bridge.h> #include <cv_bridge/cv_bridge.h>
@@ -57,11 +59,28 @@ namespace rtabmap_ros
class PointCloudXYZRGB : public nodelet::Nodelet class PointCloudXYZRGB : public nodelet::Nodelet
{ {
public: public:
PointCloudXYZRGB() : voxelSize_(0.0), decimation_(1) {} PointCloudXYZRGB() :
maxDepth_(0.0),
voxelSize_(0.0),
decimation_(1),
noiseFilterRadius_(0.0),
noiseFilterMinNeighbors_(5),
approxSyncDepth_(0),
approxSyncStereo_(0),
exactSyncDepth_(0),
exactSyncStereo_(0)
{}
virtual ~PointCloudXYZRGB() virtual ~PointCloudXYZRGB()
{ {
delete sync_; if(approxSyncDepth_)
delete approxSyncDepth_;
if(approxSyncStereo_)
delete approxSyncStereo_;
if(exactSyncDepth_)
delete exactSyncDepth_;
if(exactSyncStereo_)
delete exactSyncStereo_;
} }
private: private:
@@ -71,14 +90,37 @@ private:
ros::NodeHandle & pnh = getPrivateNodeHandle(); ros::NodeHandle & pnh = getPrivateNodeHandle();
int queueSize = 10; int queueSize = 10;
bool approxSync = true;
pnh.param("approx_sync", approxSync, approxSync);
pnh.param("queue_size", queueSize, queueSize); pnh.param("queue_size", queueSize, queueSize);
pnh.param("max_depth", maxDepth_, maxDepth_);
pnh.param("voxel_size", voxelSize_, voxelSize_); pnh.param("voxel_size", voxelSize_, voxelSize_);
pnh.param("decimation", decimation_, decimation_); pnh.param("decimation", decimation_, decimation_);
pnh.param("noise_filter_radius", noiseFilterRadius_, noiseFilterRadius_);
pnh.param("noise_filter_min_neighbors", noiseFilterMinNeighbors_, noiseFilterMinNeighbors_);
ROS_INFO("Approximate time sync = %s", approxSync?"true":"false");
sync_ = new message_filters::Synchronizer<MySyncPolicy>(MySyncPolicy(queueSize), imageSub_, imageDepthSub_, cameraInfoSub_);
sync_->registerCallback(boost::bind(&PointCloudXYZRGB::callback, this, _1, _2, _3));
cloudPub_ = nh.advertise<sensor_msgs::PointCloud2>("cloud", 1); cloudPub_ = nh.advertise<sensor_msgs::PointCloud2>("cloud", 1);
if(approxSync)
{
approxSyncDepth_ = new message_filters::Synchronizer<MyApproxSyncDepthPolicy>(MyApproxSyncDepthPolicy(queueSize), imageSub_, imageDepthSub_, cameraInfoSub_);
approxSyncDepth_->registerCallback(boost::bind(&PointCloudXYZRGB::depthCallback, this, _1, _2, _3));
approxSyncStereo_ = new message_filters::Synchronizer<MyApproxSyncStereoPolicy>(MyApproxSyncStereoPolicy(queueSize), imageLeft_, imageRight_, cameraInfoLeft_, cameraInfoRight_);
approxSyncStereo_->registerCallback(boost::bind(&PointCloudXYZRGB::stereoCallback, this, _1, _2, _3, _4));
}
else
{
exactSyncDepth_ = new message_filters::Synchronizer<MyExactSyncDepthPolicy>(MyExactSyncDepthPolicy(queueSize), imageSub_, imageDepthSub_, cameraInfoSub_);
exactSyncDepth_->registerCallback(boost::bind(&PointCloudXYZRGB::depthCallback, this, _1, _2, _3));
exactSyncStereo_ = new message_filters::Synchronizer<MyExactSyncStereoPolicy>(MyExactSyncStereoPolicy(queueSize), imageLeft_, imageRight_, cameraInfoLeft_, cameraInfoRight_);
exactSyncStereo_->registerCallback(boost::bind(&PointCloudXYZRGB::stereoCallback, this, _1, _2, _3, _4));
}
ros::NodeHandle rgb_nh(nh, "rgb"); ros::NodeHandle rgb_nh(nh, "rgb");
ros::NodeHandle depth_nh(nh, "depth"); ros::NodeHandle depth_nh(nh, "depth");
ros::NodeHandle rgb_pnh(pnh, "rgb"); ros::NodeHandle rgb_pnh(pnh, "rgb");
@@ -91,11 +133,24 @@ private:
imageSub_.subscribe(rgb_it, rgb_nh.resolveName("image"), 1, hintsRgb); imageSub_.subscribe(rgb_it, rgb_nh.resolveName("image"), 1, hintsRgb);
imageDepthSub_.subscribe(depth_it, depth_nh.resolveName("image"), 1, hintsDepth); imageDepthSub_.subscribe(depth_it, depth_nh.resolveName("image"), 1, hintsDepth);
cameraInfoSub_.subscribe(rgb_nh, "camera_info", 1); cameraInfoSub_.subscribe(rgb_nh, "camera_info", 1);
ros::NodeHandle left_nh(nh, "left");
ros::NodeHandle right_nh(nh, "right");
ros::NodeHandle left_pnh(pnh, "left");
ros::NodeHandle right_pnh(pnh, "right");
image_transport::ImageTransport left_it(left_nh);
image_transport::ImageTransport right_it(right_nh);
image_transport::TransportHints hintsLeft("raw", ros::TransportHints(), left_pnh);
image_transport::TransportHints hintsRight("raw", ros::TransportHints(), right_pnh);
imageLeft_.subscribe(left_it, left_nh.resolveName("image"), 1, hintsLeft);
imageRight_.subscribe(right_it, right_nh.resolveName("image"), 1, hintsRight);
cameraInfoLeft_.subscribe(left_nh, "camera_info", 1);
cameraInfoRight_.subscribe(right_nh, "camera_info", 1);
} }
void depthCallback(
void callback(
const sensor_msgs::ImageConstPtr& image, const sensor_msgs::ImageConstPtr& image,
const sensor_msgs::ImageConstPtr& imageDepth, const sensor_msgs::ImageConstPtr& imageDepth,
const sensor_msgs::CameraInfoConstPtr& cameraInfo) const sensor_msgs::CameraInfoConstPtr& cameraInfo)
@@ -133,33 +188,124 @@ private:
fy, fy,
decimation_); decimation_);
if(voxelSize_ > 0.0)
{
pclCloud = rtabmap::util3d::voxelize<pcl::PointXYZRGB>(pclCloud, voxelSize_);
}
sensor_msgs::PointCloud2 rosCloud; processAndPublish(pclCloud, imagePtr->header);
pcl::toROSMsg(*pclCloud, rosCloud);
rosCloud.header.stamp = image->header.stamp;
rosCloud.header.frame_id = image->header.frame_id;
//publish the message
cloudPub_.publish(rosCloud);
} }
} }
void stereoCallback(const sensor_msgs::ImageConstPtr& imageLeft,
const sensor_msgs::ImageConstPtr& imageRight,
const sensor_msgs::CameraInfoConstPtr& camInfoLeft,
const sensor_msgs::CameraInfoConstPtr& camInfoRight)
{
if(!(imageLeft->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
imageLeft->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0 ||
imageLeft->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
imageLeft->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0) ||
!(imageRight->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
imageRight->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0 ||
imageRight->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
imageRight->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0))
{
ROS_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8 (enc=%s)", imageLeft->encoding.c_str());
return;
}
if(cloudPub_.getNumSubscribers())
{
cv_bridge::CvImageConstPtr ptrLeftImage, ptrRightImage;
if(imageLeft->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
imageLeft->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0)
{
ptrLeftImage = cv_bridge::toCvShare(imageLeft, "mono8");
}
else
{
ptrLeftImage = cv_bridge::toCvShare(imageLeft, "bgr8");
}
ptrRightImage = cv_bridge::toCvShare(imageRight, "mono8");
image_geometry::StereoCameraModel model;
model.fromCameraInfo(*camInfoLeft, *camInfoRight);
float fx = model.left().fx();
float cx = model.left().cx();
float cy = model.left().cy();
float baseline = model.baseline();
pcl::PointCloud<pcl::PointXYZRGB>::Ptr pclCloud;
pclCloud = rtabmap::util3d::cloudFromStereoImages(
ptrLeftImage->image,
ptrRightImage->image,
cx,
cy,
fx,
baseline,
decimation_);
processAndPublish(pclCloud, imageLeft->header);
}
}
void processAndPublish(pcl::PointCloud<pcl::PointXYZRGB>::Ptr & pclCloud, const std_msgs::Header & header)
{
if(pclCloud->size() && maxDepth_ > 0)
{
pclCloud = rtabmap::util3d::passThrough<pcl::PointXYZRGB>(pclCloud, "z", 0, maxDepth_);
}
if(pclCloud->size() && noiseFilterRadius_ > 0.0 && noiseFilterMinNeighbors_ > 0)
{
pcl::IndicesPtr indices = rtabmap::util3d::radiusFiltering<pcl::PointXYZRGB>(pclCloud, noiseFilterRadius_, noiseFilterMinNeighbors_);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr tmp(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::copyPointCloud(*pclCloud, *indices, *tmp);
pclCloud = tmp;
}
if(pclCloud->size() && voxelSize_ > 0.0)
{
pclCloud = rtabmap::util3d::voxelize<pcl::PointXYZRGB>(pclCloud, voxelSize_);
}
sensor_msgs::PointCloud2 rosCloud;
pcl::toROSMsg(*pclCloud, rosCloud);
rosCloud.header.stamp = header.stamp;
rosCloud.header.frame_id = header.frame_id;
//publish the message
cloudPub_.publish(rosCloud);
}
private: private:
double maxDepth_;
double voxelSize_; double voxelSize_;
int decimation_; int decimation_;
double noiseFilterRadius_;
int noiseFilterMinNeighbors_;
ros::Publisher cloudPub_; ros::Publisher cloudPub_;
image_transport::SubscriberFilter imageSub_; image_transport::SubscriberFilter imageSub_;
image_transport::SubscriberFilter imageDepthSub_; image_transport::SubscriberFilter imageDepthSub_;
message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoSub_; message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoSub_;
typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo> MySyncPolicy; image_transport::SubscriberFilter imageLeft_;
message_filters::Synchronizer<MySyncPolicy> * sync_; image_transport::SubscriberFilter imageRight_;
message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoLeft_;
message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoRight_;
typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo> MyApproxSyncDepthPolicy;
message_filters::Synchronizer<MyApproxSyncDepthPolicy> * approxSyncDepth_;
typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::CameraInfo> MyApproxSyncStereoPolicy;
message_filters::Synchronizer<MyApproxSyncStereoPolicy> * approxSyncStereo_;
typedef message_filters::sync_policies::ExactTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo> MyExactSyncDepthPolicy;
message_filters::Synchronizer<MyExactSyncDepthPolicy> * exactSyncDepth_;
typedef message_filters::sync_policies::ExactTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::CameraInfo> MyExactSyncStereoPolicy;
message_filters::Synchronizer<MyExactSyncStereoPolicy> * exactSyncStereo_;
}; };
PLUGINLIB_EXPORT_CLASS(rtabmap_ros::PointCloudXYZRGB, nodelet::Nodelet); PLUGINLIB_EXPORT_CLASS(rtabmap_ros::PointCloudXYZRGB, nodelet::Nodelet);
src/nodelets/stereo_throttle.cpp
+89 -17
View File
@@ -38,6 +38,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <sensor_msgs/CameraInfo.h> #include <sensor_msgs/CameraInfo.h>
#include <cv_bridge/cv_bridge.h>
#include <rtabmap/core/util3d.h>
namespace rtabmap_ros namespace rtabmap_ros
{ {
@@ -46,9 +50,10 @@ class StereoThrottleNodelet : public nodelet::Nodelet
public: public:
//Constructor //Constructor
StereoThrottleNodelet(): StereoThrottleNodelet():
max_update_rate_(0), rate_(0),
approxSync_(0), approxSync_(0),
exactSync_(0) exactSync_(0),
decimation_(1)
{ {
} }
@@ -66,7 +71,7 @@ public:
private: private:
ros::Time last_update_; ros::Time last_update_;
double max_update_rate_; double rate_;
virtual void onInit() virtual void onInit()
{ {
ros::NodeHandle& nh = getNodeHandle(); ros::NodeHandle& nh = getNodeHandle();
@@ -84,15 +89,14 @@ private:
int queueSize = 5; int queueSize = 5;
bool approxSync = false; bool approxSync = false;
pnh.param("approx_sync", approxSync, approxSync); pnh.param("approx_sync", approxSync, approxSync);
pnh.param("rate", max_update_rate_, max_update_rate_); pnh.param("rate", rate_, rate_);
pnh.param("queue_size", queueSize, queueSize); pnh.param("queue_size", queueSize, queueSize);
pnh.param("decimation", decimation_, decimation_);
ROS_ASSERT(decimation_ >= 1);
ROS_INFO("Rate=%f Hz", rate_);
ROS_INFO("Decimation=%d", decimation_);
ROS_INFO("Approximate time sync = %s", approxSync?"true":"false"); ROS_INFO("Approximate time sync = %s", approxSync?"true":"false");
if(max_update_rate_ == 0.0)
{
ROS_WARN("Parameter \"rate\" (Hz) is not set!");
}
if(approxSync) if(approxSync)
{ {
approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize), imageLeft_, imageRight_, cameraInfoLeft_, cameraInfoRight_); approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize), imageLeft_, imageRight_, cameraInfoLeft_, cameraInfoRight_);
@@ -120,35 +124,101 @@ private:
const sensor_msgs::CameraInfoConstPtr& camInfoLeft, const sensor_msgs::CameraInfoConstPtr& camInfoLeft,
const sensor_msgs::CameraInfoConstPtr& camInfoRight) const sensor_msgs::CameraInfoConstPtr& camInfoRight)
{ {
if (max_update_rate_ > 0.0) if (rate_ > 0.0)
{ {
NODELET_DEBUG("update set to %f", max_update_rate_); NODELET_DEBUG("update set to %f", rate_);
if ( last_update_ + ros::Duration(1.0/max_update_rate_) > ros::Time::now()) if ( last_update_ + ros::Duration(1.0/rate_) > ros::Time::now())
{ {
NODELET_DEBUG("throttle last update at %f skipping", last_update_.toSec()); NODELET_DEBUG("throttle last update at %f skipping", last_update_.toSec());
return; return;
} }
} }
else else
NODELET_DEBUG("update_rate unset continuing"); NODELET_DEBUG("rate unset continuing");
last_update_ = ros::Time::now(); last_update_ = ros::Time::now();
if(imageLeftPub_.getNumSubscribers()) if(imageLeftPub_.getNumSubscribers())
{ {
imageLeftPub_.publish(imageLeft); if(decimation_ > 1)
{
cv_bridge::CvImageConstPtr imagePtr = cv_bridge::toCvShare(imageLeft);
cv_bridge::CvImage out;
out.header = imagePtr->header;
out.encoding = imagePtr->encoding;
out.image = rtabmap::util3d::decimate(imagePtr->image, decimation_);
imageLeftPub_.publish(out.toImageMsg());
}
else
{
imageLeftPub_.publish(imageLeft);
}
} }
if(imageRightPub_.getNumSubscribers()) if(imageRightPub_.getNumSubscribers())
{ {
imageRightPub_.publish(imageRight); if(decimation_ > 1)
{
cv_bridge::CvImageConstPtr imagePtr = cv_bridge::toCvShare(imageRight);
cv_bridge::CvImage out;
out.header = imagePtr->header;
out.encoding = imagePtr->encoding;
out.image = rtabmap::util3d::decimate(imagePtr->image, decimation_);
imageRightPub_.publish(out.toImageMsg());
}
else
{
imageRightPub_.publish(imageRight);
}
} }
if(infoLeftPub_.getNumSubscribers()) if(infoLeftPub_.getNumSubscribers())
{ {
infoLeftPub_.publish(camInfoLeft); if(decimation_ > 1)
{
sensor_msgs::CameraInfo info = *camInfoLeft;
info.height /= decimation_;
info.width /= decimation_;
info.roi.height /= decimation_;
info.roi.width /= decimation_;
info.K[2]/=float(decimation_); // cx
info.K[5]/=float(decimation_); // cy
info.K[0]/=float(decimation_); // fx
info.K[4]/=float(decimation_); // fy
info.P[2]/=float(decimation_); // cx
info.P[6]/=float(decimation_); // cy
info.P[0]/=float(decimation_); // fx
info.P[5]/=float(decimation_); // fy
info.P[3]/=float(decimation_); // Tx
infoLeftPub_.publish(info);
}
else
{
infoLeftPub_.publish(camInfoLeft);
}
} }
if(infoRightPub_.getNumSubscribers()) if(infoRightPub_.getNumSubscribers())
{ {
infoRightPub_.publish(camInfoRight); if(decimation_ > 1)
{
sensor_msgs::CameraInfo info = *camInfoRight;
info.height /= decimation_;
info.width /= decimation_;
info.roi.height /= decimation_;
info.roi.width /= decimation_;
info.K[2]/=float(decimation_); // cx
info.K[5]/=float(decimation_); // cy
info.K[0]/=float(decimation_); // fx
info.K[4]/=float(decimation_); // fy
info.P[2]/=float(decimation_); // cx
info.P[6]/=float(decimation_); // cy
info.P[0]/=float(decimation_); // fx
info.P[5]/=float(decimation_); // fy
info.P[3]/=float(decimation_); // Tx
infoRightPub_.publish(info);
}
else
{
infoRightPub_.publish(camInfoRight);
}
} }
} }
@@ -167,6 +237,8 @@ private:
typedef message_filters::sync_policies::ExactTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::CameraInfo> MyExactSyncPolicy; typedef message_filters::sync_policies::ExactTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::CameraInfo> MyExactSyncPolicy;
message_filters::Synchronizer<MyExactSyncPolicy> * exactSync_; message_filters::Synchronizer<MyExactSyncPolicy> * exactSync_;
int decimation_;
}; };