mirror of
https://github.com/introlab/rtabmap_ros.git
synced 2026-10-03 16:27:46 +08:00
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.
This commit is contained in:
Mathieu Labbe
@@ -20,8 +20,8 @@
|
||||
|
||||
<!-- Throttling messages -->
|
||||
<group ns="camera">
|
||||
<node pkg="nodelet" type="nodelet" name="data_throttle" args="load rtabmap/data_throttle camera_nodelet_manager" output="screen">
|
||||
<param name="max_rate" type="double" value="10.0"/>
|
||||
<node pkg="nodelet" type="nodelet" name="data_throttle" args="load rtabmap_ros/data_throttle camera_nodelet_manager" output="screen">
|
||||
<param name="rate" type="double" value="10.0"/>
|
||||
|
||||
<remap from="rgb/image_in" to="rgb/image_rect_color"/>
|
||||
<remap from="depth/image_in" to="depth_registered/image_raw"/>
|
||||
|
||||
@@ -11,8 +11,8 @@
|
||||
|
||||
<!-- Throttling messages -->
|
||||
<group ns="camera">
|
||||
<node pkg="nodelet" type="nodelet" name="data_throttle" args="load rtabmap/data_throttle camera_nodelet_manager" output="screen">
|
||||
<param name="max_rate" type="double" value="10.0"/>
|
||||
<node pkg="nodelet" type="nodelet" name="data_throttle" args="load rtabmap_ros/data_throttle camera_nodelet_manager" output="screen">
|
||||
<param name="rate" type="double" value="10.0"/>
|
||||
|
||||
<remap from="rgb/image_in" to="rgb/image_rect_color"/>
|
||||
<remap from="depth/image_in" to="depth_registered/image_raw"/>
|
||||
|
||||
@@ -4,21 +4,44 @@
|
||||
<!-- Choose visualization -->
|
||||
<arg name="rviz" default="true" />
|
||||
<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">
|
||||
<!-- Visualisation RTAB-Map -->
|
||||
<node if="$(arg rtabmapviz)" pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" args="-d $(find rtabmap_ros)/launch/config/rgbd_gui.ini" output="screen">
|
||||
<param name="queue_size" type="int" value="10"/>
|
||||
<param name="frame_id" type="string" value="base_footprint"/>
|
||||
<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>
|
||||
</group>
|
||||
|
||||
<!-- Visualisation 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>
|
||||
|
||||
@@ -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>
|
||||
@@ -11,7 +11,7 @@
|
||||
<!-- Throttling messages -->
|
||||
<group ns="camera">
|
||||
<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="depth/image_in" to="depth_registered/image_raw"/>
|
||||
|
||||
@@ -11,7 +11,7 @@
|
||||
<!-- Throttling messages -->
|
||||
<group ns="camera">
|
||||
<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="depth/image_in" to="depth_registered/image_raw"/>
|
||||
|
||||
@@ -11,7 +11,7 @@
|
||||
<!-- Throttling messages -->
|
||||
<group ns="camera">
|
||||
<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="depth/image_in" to="depth_registered/image_raw"/>
|
||||
|
||||
@@ -8,7 +8,9 @@
|
||||
<include file="$(find az3_bringup)/az3_standalone.launch"/>
|
||||
|
||||
<!-- 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) -->
|
||||
<group ns="rtabmap">
|
||||
@@ -108,23 +110,24 @@
|
||||
<!-- Throttling messages -->
|
||||
<group ns="camera">
|
||||
<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="depth/image_in" to="depth_registered/image_raw"/>
|
||||
<remap from="rgb/camera_info_in" to="depth_registered/camera_info"/>
|
||||
|
||||
<remap from="rgb/image_out" to="data_throttled_image"/>
|
||||
<remap from="depth/image_out" to="data_throttled_image_depth"/>
|
||||
<remap from="rgb/camera_info_out" to="data_throttled_camera_info"/>
|
||||
<remap from="rgb/image_out" to="data_resized_image"/>
|
||||
<remap from="depth/image_out" to="data_resized_image_depth"/>
|
||||
<remap from="rgb/camera_info_out" to="data_resized_camera_info"/>
|
||||
</node>
|
||||
|
||||
<!-- for the planner -->
|
||||
<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/camera_info" to="/camera/data_throttled_camera_info"/>
|
||||
<remap from="depth/image" to="data_resized_image_depth"/>
|
||||
<remap from="depth/camera_info" to="data_resized_camera_info"/>
|
||||
<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="noise_filter_radius" type="double" value="0.05"/>
|
||||
<param name="noise_filter_min_neighbors" type="int" value="10"/>
|
||||
|
||||
@@ -1,9 +1,12 @@
|
||||
|
||||
<launch>
|
||||
|
||||
<arg name="data_skip" default="0" />
|
||||
|
||||
<!-- Xtion -->
|
||||
<include file="$(find openni2_launch)/launch/openni2.launch">
|
||||
<arg name="depth_registration" value="True" />
|
||||
<arg name="data_skip" value="$(arg data_skip)" />
|
||||
<arg name="rgb_camera_info_url"
|
||||
value="file://$(find rtabmap_ros)/launch/calibration/rgb_PS1080_PrimeSense.yaml" />
|
||||
<arg name="depth_camera_info_url"
|
||||
|
||||
@@ -11,7 +11,7 @@
|
||||
<!-- Throttling messages -->
|
||||
<group ns="camera">
|
||||
<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="depth/image_in" to="depth_registered/image_raw"/>
|
||||
|
||||
@@ -7,7 +7,7 @@ Panels:
|
||||
- /Global Options1
|
||||
- /TF1/Frames1
|
||||
Splitter Ratio: 0.601881
|
||||
Tree Height: 371
|
||||
Tree Height: 187
|
||||
- Class: rviz/Selection
|
||||
Name: Selection
|
||||
- Class: rviz/Views
|
||||
@@ -73,7 +73,7 @@ Visualization Manager:
|
||||
Color: 255; 255; 255
|
||||
Color Transformer: AxisColor
|
||||
Decay Time: 0
|
||||
Enabled: false
|
||||
Enabled: true
|
||||
Invert Rainbow: false
|
||||
Max Color: 255; 255; 255
|
||||
Max Intensity: 4096
|
||||
@@ -86,10 +86,10 @@ Visualization Manager:
|
||||
Size (Pixels): 5
|
||||
Size (m): 0.01
|
||||
Style: Points
|
||||
Topic: /base_scan
|
||||
Topic: /base_scan_relay
|
||||
Use Fixed Frame: false
|
||||
Use rainbow: true
|
||||
Value: false
|
||||
Value: true
|
||||
- Alpha: 0.7
|
||||
Class: rviz/Map
|
||||
Color Scheme: map
|
||||
@@ -131,8 +131,8 @@ Visualization Manager:
|
||||
Value: true
|
||||
Visual Enabled: true
|
||||
- Class: rviz/Image
|
||||
Enabled: false
|
||||
Image Topic: /camera/data_throttled_image
|
||||
Enabled: true
|
||||
Image Topic: /camera/data_resized_image_relay
|
||||
Max Value: 1
|
||||
Median window: 5
|
||||
Min Value: 0
|
||||
@@ -140,7 +140,7 @@ Visualization Manager:
|
||||
Normalize Range: true
|
||||
Queue Size: 2
|
||||
Transport Hint: theora
|
||||
Value: false
|
||||
Value: true
|
||||
- Alpha: 1
|
||||
Autocompute Intensity Bounds: true
|
||||
Autocompute Value Bounds:
|
||||
@@ -158,7 +158,7 @@ Visualization Manager:
|
||||
Download graph: false
|
||||
Download map: false
|
||||
Enabled: true
|
||||
Filter floor (m): 0.1
|
||||
Filter floor (m): 0.07
|
||||
Invert Rainbow: false
|
||||
Max Color: 255; 255; 255
|
||||
Max Intensity: 4096
|
||||
@@ -180,26 +180,12 @@ Visualization Manager:
|
||||
Name: Info
|
||||
Topic: /rtabmap/info
|
||||
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
|
||||
Buffer Length: 1
|
||||
Class: rviz/Path
|
||||
Color: 255; 149; 57
|
||||
Enabled: true
|
||||
Name: Path
|
||||
Name: move_base global plan
|
||||
Topic: /planner/move_base/NavfnROS/plan
|
||||
Value: true
|
||||
- Alpha: 1
|
||||
@@ -207,7 +193,7 @@ Visualization Manager:
|
||||
Class: rviz/Path
|
||||
Color: 2; 14; 255
|
||||
Enabled: true
|
||||
Name: Path
|
||||
Name: move_base local plan
|
||||
Topic: /planner/move_base/TrajectoryPlannerROS/local_plan
|
||||
Value: true
|
||||
- Alpha: 1
|
||||
@@ -222,36 +208,36 @@ Visualization Manager:
|
||||
Color: 85; 255; 0
|
||||
Color Transformer: RGB8
|
||||
Decay Time: 0
|
||||
Enabled: false
|
||||
Enabled: true
|
||||
Invert Rainbow: false
|
||||
Max Color: 255; 255; 255
|
||||
Max Intensity: 4096
|
||||
Min Color: 0; 0; 0
|
||||
Min Intensity: 0
|
||||
Name: PointCloud2
|
||||
Name: Voxel_cloud
|
||||
Position Transformer: XYZ
|
||||
Queue Size: 10
|
||||
Selectable: true
|
||||
Size (Pixels): 3
|
||||
Size (m): 0.01
|
||||
Style: Points
|
||||
Topic: /rtabmap/cloud_map
|
||||
Topic: /voxel_cloud
|
||||
Use Fixed Frame: true
|
||||
Use rainbow: true
|
||||
Value: false
|
||||
Value: true
|
||||
- Alpha: 1
|
||||
Class: rtabmap_ros/MapGraph
|
||||
Color: 0; 0; 255
|
||||
Enabled: false
|
||||
Enabled: true
|
||||
Name: MapGraph
|
||||
Topic: /rtabmap/mapData_relay
|
||||
Value: false
|
||||
Value: true
|
||||
- Alpha: 1
|
||||
Buffer Length: 1
|
||||
Class: rviz/Path
|
||||
Color: 255; 0; 255
|
||||
Enabled: true
|
||||
Name: Path
|
||||
Name: Rtabmap global path
|
||||
Topic: /rtabmap/global_path
|
||||
Value: true
|
||||
- Alpha: 1
|
||||
@@ -259,7 +245,7 @@ Visualization Manager:
|
||||
Class: rviz/Path
|
||||
Color: 85; 255; 255
|
||||
Enabled: true
|
||||
Name: Path
|
||||
Name: Rtabmap local path
|
||||
Topic: /rtabmap/local_path
|
||||
Value: true
|
||||
- Alpha: 1
|
||||
@@ -270,12 +256,26 @@ Visualization Manager:
|
||||
Enabled: true
|
||||
Head Length: 0.3
|
||||
Head Radius: 0.1
|
||||
Name: Pose
|
||||
Name: Current goal
|
||||
Shaft Length: 1
|
||||
Shaft Radius: 0.05
|
||||
Shape: Arrow
|
||||
Topic: /rtabmap/current_goal
|
||||
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
|
||||
Global Options:
|
||||
Background Color: 48; 48; 48
|
||||
@@ -296,22 +296,22 @@ Visualization Manager:
|
||||
Views:
|
||||
Current:
|
||||
Class: rviz/Orbit
|
||||
Distance: 17.1662
|
||||
Distance: 13.3786
|
||||
Enable Stereo Rendering:
|
||||
Stereo Eye Separation: 0.06
|
||||
Stereo Focal Distance: 1
|
||||
Swap Stereo Eyes: false
|
||||
Value: false
|
||||
Focal Point:
|
||||
X: -0.0852849
|
||||
Y: 0.0904403
|
||||
Z: -0.189729
|
||||
X: -1.4347
|
||||
Y: 1.38269
|
||||
Z: -0.0856586
|
||||
Name: Current View
|
||||
Near Clip Distance: 0.01
|
||||
Pitch: 1.2048
|
||||
Pitch: 0.884797
|
||||
Target Frame: base_footprint
|
||||
Value: Orbit (rviz)
|
||||
Yaw: 3.50041
|
||||
Yaw: 3.81544
|
||||
Saved: ~
|
||||
Window Geometry:
|
||||
Displays:
|
||||
@@ -321,7 +321,7 @@ Window Geometry:
|
||||
Hide Right Dock: false
|
||||
Image:
|
||||
collapsed: false
|
||||
QMainWindow State: 000000ff00000000fd000000040000000000000151000002dafc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000001b4000000dd00fffffffb0000000a0049006d00610067006500000001290000012d0000001600fffffffb0000000a0049006d0061006700650000000184000000490000000000000000fb0000000a0049006d006100670065010000027d000000fa0000000000000000fb0000001e0054006f006f006c002000500072006f007000650072007400690065007301000001e2000001200000006400ffffff000000010000010f000001b2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000001b2000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004a00000003efc0100000002fb0000000800540069006d00650000000000000004a0000002f600fffffffb0000000800540069006d0065010000000000000450000000000000000000000375000002da00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
|
||||
QMainWindow State: 000000ff00000000fd000000040000000000000151000002dafc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000000fc000000dd00fffffffb0000000a0049006d006100670065010000012a0000012d0000001600fffffffb0000000a0049006d0061006700650000000184000000490000000000000000fb0000000a0049006d006100670065010000027d000000fa0000000000000000fb0000001e0054006f006f006c002000500072006f0070006500720074006900650073010000025d000000a50000006400ffffff000000010000010f000001b2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000001b2000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004a00000003efc0100000002fb0000000800540069006d00650000000000000004a0000002f600fffffffb0000000800540069006d0065010000000000000450000000000000000000000375000002da00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
|
||||
Selection:
|
||||
collapsed: false
|
||||
Time:
|
||||
@@ -331,5 +331,5 @@ Window Geometry:
|
||||
Views:
|
||||
collapsed: false
|
||||
Width: 1228
|
||||
X: 372
|
||||
Y: 32
|
||||
X: 406
|
||||
Y: 154
|
||||
|
||||
@@ -22,7 +22,7 @@
|
||||
<!-- Throttling messages -->
|
||||
<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">
|
||||
<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="depth/image_in" to="/camera/depth_registered/image_raw"/>
|
||||
|
||||
@@ -51,6 +51,7 @@
|
||||
<param name="Bayes/FullPredictionUpdate" type="string" value="true"/>
|
||||
<param name="Kp/DetectorStrategy" type="string" value="0"/> <!-- use SURF -->
|
||||
<param name="Kp/NNStrategy" type="string" value="1"/> <!-- kdTree -->
|
||||
<param name="Kp/WordsPerImage" type="string" value="400"/>
|
||||
</node>
|
||||
|
||||
<!-- Visualisation RTAB-Map -->
|
||||
|
||||
@@ -18,7 +18,7 @@
|
||||
<!-- Run the ROS package stereo_image_proc -->
|
||||
<group ns="/stereo_camera" >
|
||||
<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>
|
||||
|
||||
|
||||
|
||||
@@ -19,7 +19,7 @@
|
||||
<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) -->
|
||||
<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="depth/image_in" to="camera/depth_registered/image_raw"/>
|
||||
<remap from="rgb/camera_info_in" to="camera/depth_registered/camera_info"/>
|
||||
@@ -32,7 +32,7 @@
|
||||
|
||||
<param name="queue_size" type="int" value="30"/>
|
||||
</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="depth/image" to="data_odom_sync/depth"/>
|
||||
<remap from="rgb/camera_info" to="data_odom_sync/camera_info"/>
|
||||
|
||||
@@ -1,20 +1,69 @@
|
||||
|
||||
<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) -->
|
||||
<node pkg="nodelet" type="nodelet" name="points_xyzrgb" args="standalone rtabmap/point_cloud_xyzrgb">
|
||||
<remap from="rgb/image" to="camera/rgb/image_rect_color"/>
|
||||
<remap from="depth/image" to="camera/depth_registered/image_raw"/>
|
||||
<remap from="rgb/camera_info" to="camera/depth_registered/camera_info"/>
|
||||
<remap from="cloud" to="/voxel_cloud" />
|
||||
<group ns="camera">
|
||||
<node pkg="nodelet" type="nodelet" name="data_throttle" args="load rtabmap_ros/data_throttle camera_nodelet_manager">
|
||||
<remap from="rgb/image_in" to="rgb/image_rect_color"/>
|
||||
<remap from="depth/image_in" to="depth_registered/image_raw"/>
|
||||
<remap from="rgb/camera_info_in" to="depth_registered/camera_info"/>
|
||||
|
||||
<param name="queue_size" type="int" value="30"/>
|
||||
<param name="decimation" type="int" value="2"/>
|
||||
</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"/>
|
||||
|
||||
<param name="rgb/image_transport" type="string" value="compressed"/>
|
||||
<param name="depth/image_transport" type="string" value="compressedDepth"/>
|
||||
</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>
|
||||
|
||||
@@ -39,6 +39,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
|
||||
#include <sensor_msgs/CameraInfo.h>
|
||||
|
||||
#include <cv_bridge/cv_bridge.h>
|
||||
|
||||
#include <rtabmap/core/util3d.h>
|
||||
|
||||
namespace rtabmap_ros
|
||||
{
|
||||
|
||||
@@ -47,9 +51,10 @@ class DataThrottleNodelet : public nodelet::Nodelet
|
||||
public:
|
||||
//Constructor
|
||||
DataThrottleNodelet():
|
||||
max_update_rate_(0),
|
||||
rate_(0),
|
||||
approxSync_(0),
|
||||
exactSync_(0)
|
||||
exactSync_(0),
|
||||
decimation_(1)
|
||||
{
|
||||
}
|
||||
|
||||
@@ -67,7 +72,7 @@ public:
|
||||
|
||||
private:
|
||||
ros::Time last_update_;
|
||||
double max_update_rate_;
|
||||
double rate_;
|
||||
virtual void onInit()
|
||||
{
|
||||
ros::NodeHandle& nh = getNodeHandle();
|
||||
@@ -84,9 +89,17 @@ private:
|
||||
|
||||
int queueSize = 10;
|
||||
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("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");
|
||||
|
||||
if(approxSync)
|
||||
@@ -104,40 +117,84 @@ private:
|
||||
image_depth_sub_.subscribe(depth_it, depth_nh.resolveName("image_in"), 1, hintsDepth);
|
||||
info_sub_.subscribe(rgb_nh, "camera_info_in", 1);
|
||||
|
||||
imagePub_ = rgb_it.advertise("image_out", 10);
|
||||
imageDepthPub_ = depth_it.advertise("image_out", 10);
|
||||
infoPub_ = rgb_nh.advertise<sensor_msgs::CameraInfo>("camera_info_out", 10);
|
||||
imagePub_ = rgb_it.advertise("image_out", 1);
|
||||
imageDepthPub_ = depth_it.advertise("image_out", 1);
|
||||
infoPub_ = rgb_nh.advertise<sensor_msgs::CameraInfo>("camera_info_out", 1);
|
||||
};
|
||||
|
||||
void callback(const sensor_msgs::ImageConstPtr& image,
|
||||
const sensor_msgs::ImageConstPtr& imageDepth,
|
||||
const sensor_msgs::CameraInfoConstPtr& camInfo)
|
||||
{
|
||||
if (max_update_rate_ > 0.0)
|
||||
if (rate_ > 0.0)
|
||||
{
|
||||
NODELET_DEBUG("update set to %f", max_update_rate_);
|
||||
if ( last_update_ + ros::Duration(1.0/max_update_rate_) > ros::Time::now())
|
||||
NODELET_DEBUG("update set to %f", rate_);
|
||||
if ( last_update_ + ros::Duration(1.0/rate_) > ros::Time::now())
|
||||
{
|
||||
NODELET_DEBUG("throttle last update at %f skipping", last_update_.toSec());
|
||||
return;
|
||||
}
|
||||
}
|
||||
else
|
||||
NODELET_DEBUG("update_rate unset continuing");
|
||||
NODELET_DEBUG("rate unset continuing");
|
||||
|
||||
last_update_ = ros::Time::now();
|
||||
|
||||
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())
|
||||
{
|
||||
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())
|
||||
{
|
||||
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;
|
||||
message_filters::Synchronizer<MyExactSyncPolicy> * exactSync_;
|
||||
|
||||
int decimation_;
|
||||
|
||||
};
|
||||
|
||||
|
||||
|
||||
@@ -105,7 +105,7 @@ private:
|
||||
approxSyncDepth_ = new message_filters::Synchronizer<MyApproxSyncDepthPolicy>(MyApproxSyncDepthPolicy(queueSize), imageDepthSub_, cameraInfoSub_);
|
||||
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));
|
||||
}
|
||||
else
|
||||
@@ -113,7 +113,7 @@ private:
|
||||
exactSyncDepth_ = new message_filters::Synchronizer<MyExactSyncDepthPolicy>(MyExactSyncDepthPolicy(queueSize), imageDepthSub_, cameraInfoSub_);
|
||||
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));
|
||||
}
|
||||
|
||||
@@ -255,14 +255,14 @@ private:
|
||||
typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::CameraInfo> MyApproxSyncDepthPolicy;
|
||||
message_filters::Synchronizer<MyApproxSyncDepthPolicy> * approxSyncDepth_;
|
||||
|
||||
typedef message_filters::sync_policies::ApproximateTime<stereo_msgs::DisparityImage, sensor_msgs::CameraInfo> MyApproxSyncDisparityDispPolicy;
|
||||
message_filters::Synchronizer<MyApproxSyncDisparityDispPolicy> * approxSyncDisparity_;
|
||||
typedef message_filters::sync_policies::ApproximateTime<stereo_msgs::DisparityImage, sensor_msgs::CameraInfo> MyApproxSyncDisparityPolicy;
|
||||
message_filters::Synchronizer<MyApproxSyncDisparityPolicy> * approxSyncDisparity_;
|
||||
|
||||
typedef message_filters::sync_policies::ExactTime<sensor_msgs::Image, sensor_msgs::CameraInfo> MyExactSyncDepthPolicy;
|
||||
message_filters::Synchronizer<MyExactSyncDepthPolicy> * exactSyncDepth_;
|
||||
|
||||
typedef message_filters::sync_policies::ExactTime<stereo_msgs::DisparityImage, sensor_msgs::CameraInfo> MyExactSyncDisparityDispPolicy;
|
||||
message_filters::Synchronizer<MyExactSyncDisparityDispPolicy> * exactSyncDisparity_;
|
||||
typedef message_filters::sync_policies::ExactTime<stereo_msgs::DisparityImage, sensor_msgs::CameraInfo> MyExactSyncDisparityPolicy;
|
||||
message_filters::Synchronizer<MyExactSyncDisparityPolicy> * exactSyncDisparity_;
|
||||
|
||||
};
|
||||
|
||||
|
||||
@@ -42,8 +42,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
#include <image_transport/subscriber_filter.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/exact_time.h>
|
||||
#include <message_filters/subscriber.h>
|
||||
|
||||
#include <cv_bridge/cv_bridge.h>
|
||||
@@ -57,11 +59,28 @@ namespace rtabmap_ros
|
||||
class PointCloudXYZRGB : public nodelet::Nodelet
|
||||
{
|
||||
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()
|
||||
{
|
||||
delete sync_;
|
||||
if(approxSyncDepth_)
|
||||
delete approxSyncDepth_;
|
||||
if(approxSyncStereo_)
|
||||
delete approxSyncStereo_;
|
||||
if(exactSyncDepth_)
|
||||
delete exactSyncDepth_;
|
||||
if(exactSyncStereo_)
|
||||
delete exactSyncStereo_;
|
||||
}
|
||||
|
||||
private:
|
||||
@@ -71,14 +90,37 @@ private:
|
||||
ros::NodeHandle & pnh = getPrivateNodeHandle();
|
||||
|
||||
int queueSize = 10;
|
||||
bool approxSync = true;
|
||||
pnh.param("approx_sync", approxSync, approxSync);
|
||||
pnh.param("queue_size", queueSize, queueSize);
|
||||
pnh.param("max_depth", maxDepth_, maxDepth_);
|
||||
pnh.param("voxel_size", voxelSize_, voxelSize_);
|
||||
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);
|
||||
|
||||
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 depth_nh(nh, "depth");
|
||||
ros::NodeHandle rgb_pnh(pnh, "rgb");
|
||||
@@ -91,11 +133,24 @@ private:
|
||||
imageSub_.subscribe(rgb_it, rgb_nh.resolveName("image"), 1, hintsRgb);
|
||||
imageDepthSub_.subscribe(depth_it, depth_nh.resolveName("image"), 1, hintsDepth);
|
||||
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 callback(
|
||||
void depthCallback(
|
||||
const sensor_msgs::ImageConstPtr& image,
|
||||
const sensor_msgs::ImageConstPtr& imageDepth,
|
||||
const sensor_msgs::CameraInfoConstPtr& cameraInfo)
|
||||
@@ -133,33 +188,124 @@ private:
|
||||
fy,
|
||||
decimation_);
|
||||
|
||||
if(voxelSize_ > 0.0)
|
||||
{
|
||||
pclCloud = rtabmap::util3d::voxelize<pcl::PointXYZRGB>(pclCloud, voxelSize_);
|
||||
}
|
||||
|
||||
sensor_msgs::PointCloud2 rosCloud;
|
||||
pcl::toROSMsg(*pclCloud, rosCloud);
|
||||
rosCloud.header.stamp = image->header.stamp;
|
||||
rosCloud.header.frame_id = image->header.frame_id;
|
||||
|
||||
//publish the message
|
||||
cloudPub_.publish(rosCloud);
|
||||
processAndPublish(pclCloud, imagePtr->header);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
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:
|
||||
|
||||
double maxDepth_;
|
||||
double voxelSize_;
|
||||
int decimation_;
|
||||
double noiseFilterRadius_;
|
||||
int noiseFilterMinNeighbors_;
|
||||
|
||||
ros::Publisher cloudPub_;
|
||||
|
||||
image_transport::SubscriberFilter imageSub_;
|
||||
image_transport::SubscriberFilter imageDepthSub_;
|
||||
message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoSub_;
|
||||
|
||||
typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo> MySyncPolicy;
|
||||
message_filters::Synchronizer<MySyncPolicy> * sync_;
|
||||
image_transport::SubscriberFilter imageLeft_;
|
||||
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);
|
||||
|
||||
@@ -38,6 +38,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
|
||||
#include <sensor_msgs/CameraInfo.h>
|
||||
|
||||
#include <cv_bridge/cv_bridge.h>
|
||||
|
||||
#include <rtabmap/core/util3d.h>
|
||||
|
||||
namespace rtabmap_ros
|
||||
{
|
||||
|
||||
@@ -46,9 +50,10 @@ class StereoThrottleNodelet : public nodelet::Nodelet
|
||||
public:
|
||||
//Constructor
|
||||
StereoThrottleNodelet():
|
||||
max_update_rate_(0),
|
||||
rate_(0),
|
||||
approxSync_(0),
|
||||
exactSync_(0)
|
||||
exactSync_(0),
|
||||
decimation_(1)
|
||||
{
|
||||
}
|
||||
|
||||
@@ -66,7 +71,7 @@ public:
|
||||
|
||||
private:
|
||||
ros::Time last_update_;
|
||||
double max_update_rate_;
|
||||
double rate_;
|
||||
virtual void onInit()
|
||||
{
|
||||
ros::NodeHandle& nh = getNodeHandle();
|
||||
@@ -84,15 +89,14 @@ private:
|
||||
int queueSize = 5;
|
||||
bool approxSync = false;
|
||||
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("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");
|
||||
|
||||
if(max_update_rate_ == 0.0)
|
||||
{
|
||||
ROS_WARN("Parameter \"rate\" (Hz) is not set!");
|
||||
}
|
||||
|
||||
if(approxSync)
|
||||
{
|
||||
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& camInfoRight)
|
||||
{
|
||||
if (max_update_rate_ > 0.0)
|
||||
if (rate_ > 0.0)
|
||||
{
|
||||
NODELET_DEBUG("update set to %f", max_update_rate_);
|
||||
if ( last_update_ + ros::Duration(1.0/max_update_rate_) > ros::Time::now())
|
||||
NODELET_DEBUG("update set to %f", rate_);
|
||||
if ( last_update_ + ros::Duration(1.0/rate_) > ros::Time::now())
|
||||
{
|
||||
NODELET_DEBUG("throttle last update at %f skipping", last_update_.toSec());
|
||||
return;
|
||||
}
|
||||
}
|
||||
else
|
||||
NODELET_DEBUG("update_rate unset continuing");
|
||||
NODELET_DEBUG("rate unset continuing");
|
||||
|
||||
last_update_ = ros::Time::now();
|
||||
|
||||
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())
|
||||
{
|
||||
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())
|
||||
{
|
||||
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())
|
||||
{
|
||||
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;
|
||||
message_filters::Synchronizer<MyExactSyncPolicy> * exactSync_;
|
||||
|
||||
int decimation_;
|
||||
|
||||
};
|
||||
|
||||
|
||||
|
||||
Reference in New Issue
Block a user