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

Uniformized "approx_sync" parameter name across all nodes. Updated rgbd_mapping.launch and stereo_mapping.launch to include rtabmap.launch instead of redefining all nodes.

Browse Source
This commit is contained in:
matlabbe
2016-06-30 12:49:20 -04:00
parent 88f164e6bf
commit 77faedf9d2
7 changed files with 191 additions and 251 deletions
Download Patch File Download Diff File Expand all files Collapse all files
launch/rgbd_mapping.launch
+30 -97
View File
@@ -1,5 +1,7 @@
<launch> <launch>
<!-- Backward compatibility launch file, use rtabmap.launch instead -->
<!-- Your RGB-D sensor should be already started with "depth_registration:=true". <!-- Your RGB-D sensor should be already started with "depth_registration:=true".
Examples: Examples:
$ roslaunch freenect_launch freenect.launch depth_registration:=true $ roslaunch freenect_launch freenect.launch depth_registration:=true
@@ -17,17 +19,15 @@
<arg name="rviz_cfg" default="-d $(find rtabmap_ros)/launch/config/rgbd.rviz" /> <arg name="rviz_cfg" default="-d $(find rtabmap_ros)/launch/config/rgbd.rviz" />
<arg name="frame_id" default="camera_link"/> <!-- Fixed frame id, you may set "base_link" or "base_footprint" if they are published --> <arg name="frame_id" default="camera_link"/> <!-- Fixed frame id, you may set "base_link" or "base_footprint" if they are published -->
<arg name="time_threshold" default="0"/> <!-- (ms) If not 0 ms, memory management is used to keep processing time on this fixed limit. -->
<arg name="optimize_from_last_node" default="false"/> <!-- Optimize the map from the last node. Should be true on multi-session mapping and when time threshold is set -->
<arg name="database_path" default="~/.ros/rtabmap.db"/> <arg name="database_path" default="~/.ros/rtabmap.db"/>
<arg name="rtabmap_args" default=""/> <!-- delete_db_on_start, udebug --> <arg name="rtabmap_args" default=""/> <!-- delete_db_on_start, udebug -->
<arg name="launch_prefix" default=""/> <!-- for debugging purpose, it fills launch-prefix tag of the nodes --> <arg name="launch_prefix" default=""/> <!-- for debugging purpose, it fills launch-prefix tag of the nodes -->
<arg name="approx_sync" default="true"/> <!-- if timestamps of the input topics are not synchronized -->
<arg name="rgb_topic" default="/camera/rgb/image_rect_color" /> <arg name="rgb_topic" default="/camera/rgb/image_rect_color" />
<arg name="depth_registered_topic" default="/camera/depth_registered/image_raw" /> <arg name="depth_registered_topic" default="/camera/depth_registered/image_raw" />
<arg name="camera_info_topic" default="/camera/rgb/camera_info" /> <arg name="camera_info_topic" default="/camera/rgb/camera_info" />
<arg name="compressed" default="false"/> <arg name="compressed" default="false"/>
<arg name="convert_depth_to_mm" default="true"/>
<arg name="subscribe_scan" default="false"/> <!-- Assuming 2D scan if set, rtabmap will do 3DoF mapping instead of 6DoF --> <arg name="subscribe_scan" default="false"/> <!-- Assuming 2D scan if set, rtabmap will do 3DoF mapping instead of 6DoF -->
<arg name="scan_topic" default="/scan"/> <arg name="scan_topic" default="/scan"/>
@@ -41,102 +41,35 @@
<arg name="namespace" default="rtabmap"/> <arg name="namespace" default="rtabmap"/>
<arg name="wait_for_transform" default="0.2"/> <arg name="wait_for_transform" default="0.2"/>
<!-- Nodes --> <include file="$(find rtabmap_ros)/launch/rtabmap.launch">
<group ns="$(arg namespace)"> <arg name="rtabmapviz" value="$(arg rtabmapviz)" />
<arg name="rviz" value="$(arg rviz)" />
<arg name="localization" value="$(arg localization)"/>
<arg name="gui_cfg" value="$(arg rtabmapviz_cfg)" />
<arg name="rviz_cfg" value="$(arg rviz_cfg)" />
<node if="$(arg compressed)" name="republish_rgb" type="republish" pkg="image_transport" args="compressed in:=$(arg rgb_topic) raw out:=$(arg rgb_topic)" /> <arg name="frame_id" value="$(arg frame_id)"/>
<node if="$(arg compressed)" name="republish_depth" type="republish" pkg="image_transport" args="compressedDepth in:=$(arg depth_registered_topic) raw out:=$(arg depth_registered_topic)" /> <arg name="namespace" value="$(arg namespace)"/>
<arg name="database_path" value="$(arg database_path)"/>
<!-- Odometry --> <arg name="wait_for_transform" value="$(arg wait_for_transform)"/>
<node if="$(arg visual_odometry)" pkg="rtabmap_ros" type="rgbd_odometry" name="rgbd_odometry" output="screen" args="$(arg rtabmap_args)" launch-prefix="$(arg launch_prefix)"> <arg name="rtabmap_args" value="$(arg rtabmap_args)"/>
<remap from="rgb/image" to="$(arg rgb_topic)"/> <arg name="launch_prefix" value="$(arg launch_prefix)"/>
<remap from="depth/image" to="$(arg depth_registered_topic)"/> <arg name="approx_sync" value="$(arg approx_sync)"/>
<remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
<param name="frame_id" type="string" value="$(arg frame_id)"/>
<param name="wait_for_transform_duration" type="double" value="$(arg wait_for_transform)"/>
<param name="Odom/FillInfoData" type="string" value="true"/>
</node>
<!-- Visual SLAM (robot side) -->
<node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="$(arg rtabmap_args)" launch-prefix="$(arg launch_prefix)">
<param name="subscribe_depth" type="bool" value="true"/>
<param name="subscribe_scan" type="bool" value="$(arg subscribe_scan)"/>
<param name="subscribe_scan_cloud" type="bool" value="$(arg subscribe_scan_cloud)"/>
<param name="frame_id" type="string" value="$(arg frame_id)"/>
<param name="wait_for_transform_duration" type="double" value="$(arg wait_for_transform)"/>
<param name="database_path" type="string" value="$(arg database_path)"/>
<remap from="rgb/image" to="$(arg rgb_topic)"/>
<remap from="depth/image" to="$(arg depth_registered_topic)"/>
<remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
<remap from="scan" to="$(arg scan_topic)"/>
<remap from="scan_cloud" to="$(arg scan_cloud_topic)"/>
<remap unless="$(arg visual_odometry)" from="odom" to="$(arg odom_topic)"/>
<param name="Rtabmap/TimeThr" type="string" value="$(arg time_threshold)"/>
<param name="RGBD/OptimizeFromGraphEnd" type="string" value="$(arg optimize_from_last_node)"/>
<param name="Mem/SaveDepth16Format" type="string" value="$(arg convert_depth_to_mm)"/>
<!-- localization mode -->
<param if="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="false"/>
<param unless="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="true"/>
<param name="Mem/InitWMWithAllNodes" type="string" value="$(arg localization)"/>
<!-- when 2D scan is set --> <arg name="rgb_topic" value="$(arg rgb_topic)" />
<param if="$(arg subscribe_scan)" name="Optimizer/Slam2D" type="string" value="true"/> <arg name="depth_topic" value="$(arg depth_registered_topic)" />
<param if="$(arg subscribe_scan)" name="Icp/CorrespondenceRatio" type="string" value="0.25"/> <arg name="camera_info_topic" value="$(arg camera_info_topic)" />
<param if="$(arg subscribe_scan)" name="Reg/Strategy" type="string" value="1"/> <arg name="compressed" value="$(arg compressed)"/>
<param if="$(arg subscribe_scan)" name="Reg/Force3DoF" type="string" value="true"/>
<arg name="subscribe_scan" value="$(arg subscribe_scan)"/>
<!-- when 3D scan is set --> <arg name="scan_topic" value="$(arg scan_topic)"/>
<param if="$(arg subscribe_scan_cloud)" name="Reg/Strategy" type="string" value="1"/>
</node>
<!-- Visualisation RTAB-Map --> <arg name="subscribe_scan_cloud" value="$(arg subscribe_scan_cloud)"/>
<node if="$(arg rtabmapviz)" pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" args="$(arg rtabmapviz_cfg)" output="screen" launch-prefix="$(arg launch_prefix)"> <arg name="scan_cloud_topic" value="$(arg scan_cloud_topic)"/>
<param name="subscribe_depth" type="bool" value="true"/>
<param name="subscribe_scan" type="bool" value="$(arg subscribe_scan)"/> <arg name="visual_odometry" value="$(arg visual_odometry)"/>
<param name="subscribe_scan_cloud" type="bool" value="$(arg subscribe_scan_cloud)"/> <arg name="odom_topic" value="$(arg odom_topic)"/>
<param name="subscribe_odom_info" type="bool" value="$(arg visual_odometry)"/> <arg name="odom_args" value="$(arg rtabmap_args)"/>
<param name="frame_id" type="string" value="$(arg frame_id)"/> </include>
<param name="wait_for_transform_duration" type="double" value="$(arg wait_for_transform)"/>
<remap from="rgb/image" to="$(arg rgb_topic)"/>
<remap from="depth/image" to="$(arg depth_registered_topic)"/>
<remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
<remap from="scan" to="$(arg scan_topic)"/>
<remap from="scan_cloud" to="$(arg scan_cloud_topic)"/>
<remap unless="$(arg visual_odometry)" from="odom" to="$(arg odom_topic)"/>
</node>
</group>
<!-- Visualization RVIZ -->
<node if="$(arg rviz)" pkg="rviz" type="rviz" name="rviz" args="$(arg rviz_cfg)"/>
<!-- sync cloud with odometry and voxelize the point cloud (for fast visualization in rviz) -->
<node if="$(arg rviz)" pkg="nodelet" type="nodelet" name="standalone_nodelet" args="manager" output="screen"/>
<node if="$(arg rviz)" pkg="nodelet" type="nodelet" name="data_odom_sync" args="load rtabmap_ros/data_odom_sync standalone_nodelet">
<remap from="rgb/image_in" to="$(arg rgb_topic)"/>
<remap from="depth/image_in" to="$(arg depth_registered_topic)"/>
<remap from="rgb/camera_info_in" to="$(arg camera_info_topic)"/>
<remap if="$(arg visual_odometry)" from="odom_in" to="rtabmap/odom"/>
<remap unless="$(arg visual_odometry)" from="odom_in" to="$(arg odom_topic)"/>
<remap from="rgb/image_out" to="data_odom_sync/image"/>
<remap from="depth/image_out" to="data_odom_sync/depth"/>
<remap from="rgb/camera_info_out" to="data_odom_sync/camera_info"/>
<remap from="odom_out" to="odom_sync"/>
</node>
<node if="$(arg rviz)" 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"/>
<remap from="cloud" to="voxel_cloud" />
<param name="decimation" type="double" value="2"/>
<param name="voxel_size" type="double" value="0.02"/>
</node>
</launch> </launch>
launch/rtabmap.launch
+5 -4
View File
@@ -37,7 +37,8 @@
<arg name="wait_for_transform" default="0.2"/> <arg name="wait_for_transform" default="0.2"/>
<arg name="rtabmap_args" default=""/> <!-- delete_db_on_start, udebug --> <arg name="rtabmap_args" default=""/> <!-- delete_db_on_start, udebug -->
<arg name="launch_prefix" default=""/> <!-- for debugging purpose, it fills launch-prefix tag of the nodes --> <arg name="launch_prefix" default=""/> <!-- for debugging purpose, it fills launch-prefix tag of the nodes -->
<arg name="approx_sync" default="true"/> <!-- if timestamps of the input topics are not synchronized -->
<!-- RGB-D related topics --> <!-- RGB-D related topics -->
<arg name="rgb_topic" default="/camera/rgb/image_rect_color" /> <arg name="rgb_topic" default="/camera/rgb/image_rect_color" />
<arg name="depth_topic" default="/camera/depth_registered/image_raw" /> <arg name="depth_topic" default="/camera/depth_registered/image_raw" />
@@ -49,7 +50,6 @@
<arg name="right_image_topic" default="$(arg stereo_namespace)/right/image_rect" /> <!-- using grayscale image for efficiency --> <arg name="right_image_topic" default="$(arg stereo_namespace)/right/image_rect" /> <!-- using grayscale image for efficiency -->
<arg name="left_camera_info_topic" default="$(arg stereo_namespace)/left/camera_info" /> <arg name="left_camera_info_topic" default="$(arg stereo_namespace)/left/camera_info" />
<arg name="right_camera_info_topic" default="$(arg stereo_namespace)/right/camera_info" /> <arg name="right_camera_info_topic" default="$(arg stereo_namespace)/right/camera_info" />
<arg name="approx_sync" default="false"/> <!-- if timestamps of the stereo images are not synchronized -->
<arg name="compressed" default="false"/> <!-- If you want to subscribe to compressed image topics --> <arg name="compressed" default="false"/> <!-- If you want to subscribe to compressed image topics -->
<!-- For depth_topic, "compressedDepth" image_transport is used. --> <!-- For depth_topic, "compressedDepth" image_transport is used. -->
@@ -91,6 +91,7 @@
<param name="frame_id" type="string" value="$(arg frame_id)"/> <param name="frame_id" type="string" value="$(arg frame_id)"/>
<param name="wait_for_transform_duration" type="double" value="$(arg wait_for_transform)"/> <param name="wait_for_transform_duration" type="double" value="$(arg wait_for_transform)"/>
<param name="approx_sync" type="bool" value="$(arg approx_sync)"/>
<param name="config_path" type="string" value="$(arg cfg)"/> <param name="config_path" type="string" value="$(arg cfg)"/>
<param name="queue_size" type="int" value="$(arg queue_size)"/> <param name="queue_size" type="int" value="$(arg queue_size)"/>
</node> </node>
@@ -125,7 +126,7 @@
<param name="frame_id" type="string" value="$(arg frame_id)"/> <param name="frame_id" type="string" value="$(arg frame_id)"/>
<param name="wait_for_transform_duration" type="double" value="$(arg wait_for_transform)"/> <param name="wait_for_transform_duration" type="double" value="$(arg wait_for_transform)"/>
<param name="database_path" type="string" value="$(arg database_path)"/> <param name="database_path" type="string" value="$(arg database_path)"/>
<param name="stereo_approx_sync" type="bool" value="$(arg approx_sync)"/> <param name="approx_sync" type="bool" value="$(arg approx_sync)"/>
<param name="config_path" type="string" value="$(arg cfg)"/> <param name="config_path" type="string" value="$(arg cfg)"/>
<param name="queue_size" type="int" value="$(arg queue_size)"/> <param name="queue_size" type="int" value="$(arg queue_size)"/>
@@ -189,7 +190,7 @@
<param name="decimation" type="double" value="2"/> <param name="decimation" type="double" value="2"/>
<param name="voxel_size" type="double" value="0.02"/> <param name="voxel_size" type="double" value="0.02"/>
<param if="$(arg stereo)" name="approx_sync" type="bool" value="$(arg approx_sync)"/> <param name="approx_sync" type="bool" value="$(arg approx_sync)"/>
</node> </node>
</launch> </launch>
launch/stereo_mapping.launch
+35 -94
View File
@@ -1,5 +1,7 @@
<launch> <launch>
<!-- Backward compatibility launch file, use "rtabmap.launch rgbd:=false stereo:=true" instead -->
<!-- Your camera should be calibrated and publishing rectified left and right <!-- Your camera should be calibrated and publishing rectified left and right
images + corresponding camera_info msgs. You can use stereo_image_proc for image rectification. images + corresponding camera_info msgs. You can use stereo_image_proc for image rectification.
Example: Example:
@@ -17,20 +19,17 @@
<arg name="rviz_cfg" default="-d $(find rtabmap_ros)/launch/config/rgbd.rviz" /> <arg name="rviz_cfg" default="-d $(find rtabmap_ros)/launch/config/rgbd.rviz" />
<arg name="frame_id" default="base_link"/> <!-- Fixed frame id, you may set "base_link" or "base_footprint" if they are published --> <arg name="frame_id" default="base_link"/> <!-- Fixed frame id, you may set "base_link" or "base_footprint" if they are published -->
<arg name="time_threshold" default="0"/> <!-- (ms) If not 0 ms, memory management is used to keep processing time on this fixed limit. -->
<arg name="optimize_from_last_node" default="false"/> <!-- Optimize the map from the last node. Should be true on multi-session mapping and when time threshold is set -->
<arg name="database_path" default="~/.ros/rtabmap.db"/> <arg name="database_path" default="~/.ros/rtabmap.db"/>
<arg name="rtabmap_args" default=""/> <!-- delete_db_on_start, udebug --> <arg name="rtabmap_args" default=""/> <!-- delete_db_on_start, udebug -->
<arg name="launch_prefix" default=""/> <arg name="launch_prefix" default=""/>
<arg name="approx_sync" default="false"/> <!-- if timestamps of the input topics are not synchronized -->
<arg name="stereo_namespace" default="/stereo_camera"/> <arg name="stereo_namespace" default="/stereo_camera"/>
<arg name="left_image_topic" default="$(arg stereo_namespace)/left/image_rect_color" /> <arg name="left_image_topic" default="$(arg stereo_namespace)/left/image_rect_color" />
<arg name="right_image_topic" default="$(arg stereo_namespace)/right/image_rect" /> <!-- using grayscale image for efficiency --> <arg name="right_image_topic" default="$(arg stereo_namespace)/right/image_rect" /> <!-- using grayscale image for efficiency -->
<arg name="left_camera_info_topic" default="$(arg stereo_namespace)/left/camera_info" /> <arg name="left_camera_info_topic" default="$(arg stereo_namespace)/left/camera_info" />
<arg name="right_camera_info_topic" default="$(arg stereo_namespace)/right/camera_info" /> <arg name="right_camera_info_topic" default="$(arg stereo_namespace)/right/camera_info" />
<arg name="approximate_sync" default="false"/> <!-- if timestamps of the stereo images are not synchronized -->
<arg name="compressed" default="false"/> <arg name="compressed" default="false"/>
<arg name="convert_depth_to_mm" default="true"/>
<arg name="subscribe_scan" default="false"/> <!-- Assuming 2D scan if set, rtabmap will do 3DoF mapping instead of 6DoF --> <arg name="subscribe_scan" default="false"/> <!-- Assuming 2D scan if set, rtabmap will do 3DoF mapping instead of 6DoF -->
<arg name="scan_topic" default="/scan"/> <arg name="scan_topic" default="/scan"/>
@@ -44,98 +43,40 @@
<arg name="namespace" default="rtabmap"/> <arg name="namespace" default="rtabmap"/>
<arg name="wait_for_transform" default="0.2"/> <arg name="wait_for_transform" default="0.2"/>
<!-- Nodes --> <include file="$(find rtabmap_ros)/launch/rtabmap.launch">
<group ns="$(arg namespace)"> <arg name="rgbd" value="false"/>
<arg name="stereo" value="true"/>
<arg name="rtabmapviz" value="$(arg rtabmapviz)" />
<arg name="rviz" value="$(arg rviz)" />
<arg name="localization" value="$(arg localization)"/>
<arg name="gui_cfg" value="$(arg rtabmapviz_cfg)" />
<arg name="rviz_cfg" value="$(arg rviz_cfg)" />
<node if="$(arg compressed)" name="republish_left" type="republish" pkg="image_transport" args="compressed in:=$(arg left_image_topic) raw out:=$(arg left_image_topic)" /> <arg name="frame_id" value="$(arg frame_id)"/>
<node if="$(arg compressed)" name="republish_right" type="republish" pkg="image_transport" args="compressed in:=$(arg right_image_topic) raw out:=$(arg right_image_topic)" /> <arg name="namespace" value="$(arg namespace)"/>
<arg name="database_path" value="$(arg database_path)"/>
<!-- Odometry --> <arg name="wait_for_transform" value="$(arg wait_for_transform)"/>
<node if="$(arg visual_odometry)" pkg="rtabmap_ros" type="stereo_odometry" name="stereo_odometry" output="screen" args="$(arg rtabmap_args)" launch-prefix="$(arg launch_prefix)"> <arg name="rtabmap_args" value="$(arg rtabmap_args)"/>
<remap from="left/image_rect" to="$(arg left_image_topic)"/> <arg name="launch_prefix" value="$(arg launch_prefix)"/>
<remap from="right/image_rect" to="$(arg right_image_topic)"/> <arg name="approx_sync" value="$(arg approx_sync)"/>
<remap from="left/camera_info" to="$(arg left_camera_info_topic)"/>
<remap from="right/camera_info" to="$(arg right_camera_info_topic)"/>
<param name="frame_id" type="string" value="$(arg frame_id)"/>
<param name="wait_for_transform_duration" type="double" value="$(arg wait_for_transform)"/>
<param name="approx_sync" type="bool" value="$(arg approximate_sync)"/>
<param name="Odom/FillInfoData" type="string" value="true"/>
</node>
<!-- Visual SLAM (robot side) -->
<!-- args: "delete_db_on_start" and "udebug" -->
<node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="$(arg rtabmap_args)" launch-prefix="$(arg launch_prefix)">
<param name="subscribe_depth" type="bool" value="false"/>
<param name="subscribe_stereo" type="bool" value="true"/>
<param name="subscribe_scan" type="bool" value="$(arg subscribe_scan)"/>
<param name="subscribe_scan_cloud" type="bool" value="$(arg subscribe_scan_cloud)"/>
<param name="frame_id" type="string" value="$(arg frame_id)"/>
<param name="wait_for_transform_duration" type="double" value="$(arg wait_for_transform)"/>
<param name="database_path" type="string" value="$(arg database_path)"/>
<param name="stereo_approx_sync" type="bool" value="$(arg approximate_sync)"/>
<remap from="left/image_rect" to="$(arg left_image_topic)"/>
<remap from="right/image_rect" to="$(arg right_image_topic)"/>
<remap from="left/camera_info" to="$(arg left_camera_info_topic)"/>
<remap from="right/camera_info" to="$(arg right_camera_info_topic)"/>
<remap from="scan" to="$(arg scan_topic)"/>
<remap from="scan_cloud" to="$(arg scan_cloud_topic)"/>
<remap unless="$(arg visual_odometry)" from="odom" to="$(arg odom_topic)"/>
<param name="Rtabmap/TimeThr" type="string" value="$(arg time_threshold)"/>
<param name="RGBD/OptimizeFromGraphEnd" type="string" value="$(arg optimize_from_last_node)"/>
<param name="Mem/SaveDepth16Format" type="string" value="$(arg convert_depth_to_mm)"/>
<!-- localization mode -->
<param if="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="false"/>
<param unless="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="true"/>
<param name="Mem/InitWMWithAllNodes" type="string" value="$(arg localization)"/>
<!-- when 2D scan is set --> <arg name="stereo_namespace" value="$(arg stereo_namespace)"/>
<param if="$(arg subscribe_scan)" name="Optimizer/Slam2D" type="string" value="true"/> <arg name="left_image_topic" value="$(arg left_image_topic)" />
<param if="$(arg subscribe_scan)" name="Icp/CorrespondenceRatio" type="string" value="0.25"/> <arg name="right_image_topic" value="$(arg right_image_topic)" />
<param if="$(arg subscribe_scan)" name="Reg/Strategy" type="string" value="1"/> <arg name="left_camera_info_topic" value="$(arg left_camera_info_topic)" />
<param if="$(arg subscribe_scan)" name="Reg/Force3DoF" type="string" value="true"/> <arg name="right_camera_info_topic" value="$(arg right_camera_info_topic)" />
<!-- when 3D scan is set -->
<param if="$(arg subscribe_scan_cloud)" name="Reg/Strategy" type="string" value="1"/>
</node>
<!-- Visualisation RTAB-Map --> <arg name="compressed" value="$(arg compressed)"/>
<node if="$(arg rtabmapviz)" pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" args="$(arg rtabmapviz_cfg)" output="screen" launch-prefix="$(arg launch_prefix)">
<param name="subscribe_depth" type="bool" value="false"/> <arg name="subscribe_scan" value="$(arg subscribe_scan)"/>
<param name="subscribe_stereo" type="bool" value="true"/> <arg name="scan_topic" value="$(arg scan_topic)"/>
<param name="subscribe_scan" type="bool" value="$(arg subscribe_scan)"/>
<param name="subscribe_scan_cloud" type="bool" value="$(arg subscribe_scan_cloud)"/>
<param name="subscribe_odom_info" type="bool" value="$(arg visual_odometry)"/>
<param name="frame_id" type="string" value="$(arg frame_id)"/>
<param name="wait_for_transform_duration" type="double" value="$(arg wait_for_transform)"/>
<remap from="left/image_rect" to="$(arg left_image_topic)"/>
<remap from="right/image_rect" to="$(arg right_image_topic)"/>
<remap from="left/camera_info" to="$(arg left_camera_info_topic)"/>
<remap from="right/camera_info" to="$(arg right_camera_info_topic)"/>
<remap from="scan" to="$(arg scan_topic)"/>
<remap from="scan_cloud" to="$(arg scan_cloud_topic)"/>
<remap unless="$(arg visual_odometry)" from="odom" to="$(arg odom_topic)"/>
</node>
</group> <arg name="subscribe_scan_cloud" value="$(arg subscribe_scan_cloud)"/>
<arg name="scan_cloud_topic" value="$(arg scan_cloud_topic)"/>
<!-- Visualization RVIZ -->
<node if="$(arg rviz)" pkg="rviz" type="rviz" name="rviz" args="$(arg rviz_cfg)"/> <arg name="visual_odometry" value="$(arg visual_odometry)"/>
<node if="$(arg rviz)" pkg="nodelet" type="nodelet" name="points_xyzrgb" args="standalone rtabmap_ros/point_cloud_xyzrgb"> <arg name="odom_topic" value="$(arg odom_topic)"/>
<remap from="left/image" to="$(arg left_image_topic)"/> <arg name="odom_args" value="$(arg rtabmap_args)"/>
<remap from="right/image" to="$(arg right_image_topic)"/> </include>
<remap from="left/camera_info" to="$(arg left_camera_info_topic)"/>
<remap from="right/camera_info" to="$(arg right_camera_info_topic)"/>
<remap from="cloud" to="voxel_cloud" />
<param name="decimation" type="double" value="2"/>
<param name="voxel_size" type="double" value="0.02"/>
<param name="approx_sync" type="bool" value="$(arg approximate_sync)"/>
</node>
</launch> </launch>
src/CoreWrapper.cpp
+63 -35
View File
@@ -132,7 +132,7 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart, const ParametersMap & parameters)
double tfDelay = 0.05; // 20 Hz double tfDelay = 0.05; // 20 Hz
double tfTolerance = 0.1; // 100 ms double tfTolerance = 0.1; // 100 ms
std::string tfPrefix = ""; std::string tfPrefix = "";
bool stereoApproxSync = false; bool approxSync = true;
// ROS related parameters (private) // ROS related parameters (private)
pnh.param("subscribe_depth", subscribeDepth, subscribeDepth); pnh.param("subscribe_depth", subscribeDepth, subscribeDepth);
@@ -171,7 +171,17 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart, const ParametersMap & parameters)
pnh.param("ground_truth_frame_id", groundTruthFrameId_, groundTruthFrameId_); pnh.param("ground_truth_frame_id", groundTruthFrameId_, groundTruthFrameId_);
pnh.param("depth_cameras", depthCameras, depthCameras); pnh.param("depth_cameras", depthCameras, depthCameras);
pnh.param("queue_size", queueSize, queueSize); pnh.param("queue_size", queueSize, queueSize);
pnh.param("stereo_approx_sync", stereoApproxSync, stereoApproxSync); if(pnh.hasParam("stereo_approx_sync") && !pnh.hasParam("approx_sync"))
{
ROS_WARN("Parameter \"stereo_approx_sync\" has been renamed "
"to \"approx_sync\"! Your value is still copied to "
"corresponding parameter.");
pnh.param("stereo_approx_sync", approxSync, approxSync);
}
else
{
pnh.param("approx_sync", approxSync, approxSync);
}
pnh.param("publish_tf", publishTf, publishTf); pnh.param("publish_tf", publishTf, publishTf);
pnh.param("tf_delay", tfDelay, tfDelay); pnh.param("tf_delay", tfDelay, tfDelay);
@@ -227,6 +237,7 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart, const ParametersMap & parameters)
ROS_INFO("rtabmap: tf_delay = %f", tfDelay); ROS_INFO("rtabmap: tf_delay = %f", tfDelay);
ROS_INFO("rtabmap: tf_tolerance = %f", tfTolerance); ROS_INFO("rtabmap: tf_tolerance = %f", tfTolerance);
ROS_INFO("rtabmap: depth_cameras = %d", depthCameras); ROS_INFO("rtabmap: depth_cameras = %d", depthCameras);
ROS_INFO("rtabmap: approx_sync = %s", approxSync?"true":"false");
infoPub_ = nh.advertise<rtabmap_ros::Info>("info", 1); infoPub_ = nh.advertise<rtabmap_ros::Info>("info", 1);
mapDataPub_ = nh.advertise<rtabmap_ros::MapData>("mapData", 1); mapDataPub_ = nh.advertise<rtabmap_ros::MapData>("mapData", 1);
@@ -427,7 +438,7 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart, const ParametersMap & parameters)
setLogWarnSrv_ = pnh.advertiseService("log_warning", &CoreWrapper::setLogWarn, this); setLogWarnSrv_ = pnh.advertiseService("log_warning", &CoreWrapper::setLogWarn, this);
setLogErrorSrv_ = pnh.advertiseService("log_error", &CoreWrapper::setLogError, this); setLogErrorSrv_ = pnh.advertiseService("log_error", &CoreWrapper::setLogError, this);
setupCallbacks(subscribeDepth, subscribeScan2d, subscribeScan3d, subscribeStereo, queueSize, stereoApproxSync, depthCameras); setupCallbacks(subscribeDepth, subscribeScan2d, subscribeScan3d, subscribeStereo, queueSize, approxSync, depthCameras);
int optimizeIterations = 0; int optimizeIterations = 0;
Parameters::parse(parameters_, Parameters::kOptimizerIterations(), optimizeIterations); Parameters::parse(parameters_, Parameters::kOptimizerIterations(), optimizeIterations);
@@ -2634,7 +2645,7 @@ void CoreWrapper::setupCallbacks(
bool subscribeScan3d, bool subscribeScan3d,
bool subscribeStereo, bool subscribeStereo,
int queueSize, int queueSize,
bool stereoApproxSync, bool approxSync,
int depthCameras) int depthCameras)
{ {
ros::NodeHandle nh; // public ros::NodeHandle nh; // public
@@ -2679,7 +2690,6 @@ void CoreWrapper::setupCallbacks(
odomSub_.subscribe(nh, "odom", 1); odomSub_.subscribe(nh, "odom", 1);
if(subscribeScan2d) if(subscribeScan2d)
{ {
ROS_INFO("Registering Depth+LaserScan callback...");
scanSub_.subscribe(nh, "scan", 1); scanSub_.subscribe(nh, "scan", 1);
depthScanSync_ = new message_filters::Synchronizer<MyDepthScanSyncPolicy>( depthScanSync_ = new message_filters::Synchronizer<MyDepthScanSyncPolicy>(
MyDepthScanSyncPolicy(queueSize), MyDepthScanSyncPolicy(queueSize),
@@ -2700,7 +2710,6 @@ void CoreWrapper::setupCallbacks(
} }
else if(subscribeScan3d) else if(subscribeScan3d)
{ {
ROS_INFO("Registering Depth+LaserScan3d callback...");
scan3dSub_.subscribe(nh, "scan_cloud", 1); scan3dSub_.subscribe(nh, "scan_cloud", 1);
depthScan3dSync_ = new message_filters::Synchronizer<MyDepthScan3dSyncPolicy>( depthScan3dSync_ = new message_filters::Synchronizer<MyDepthScan3dSyncPolicy>(
MyDepthScan3dSyncPolicy(queueSize), MyDepthScan3dSyncPolicy(queueSize),
@@ -2723,7 +2732,6 @@ void CoreWrapper::setupCallbacks(
{ {
if(depthCameras > 1) if(depthCameras > 1)
{ {
ROS_INFO("Registering Depth2 callback...");
depth2Sync_ = new message_filters::Synchronizer<MyDepth2SyncPolicy>( depth2Sync_ = new message_filters::Synchronizer<MyDepth2SyncPolicy>(
MyDepth2SyncPolicy(queueSize), MyDepth2SyncPolicy(queueSize),
odomSub_, odomSub_,
@@ -2747,17 +2755,29 @@ void CoreWrapper::setupCallbacks(
} }
else else
{ {
ROS_INFO("Registering Depth callback..."); if(approxSync)
depthSync_ = new message_filters::Synchronizer<MyDepthSyncPolicy>( {
MyDepthSyncPolicy(queueSize), depthSync_ = new message_filters::Synchronizer<MyDepthSyncPolicy>(
*imageSubs_[0], MyDepthSyncPolicy(queueSize),
odomSub_, *imageSubs_[0],
*imageDepthSubs_[0], odomSub_,
*cameraInfoSubs_[0]); *imageDepthSubs_[0],
depthSync_->registerCallback(boost::bind(&CoreWrapper::depthCallback, this, _1, _2, _3, _4)); *cameraInfoSubs_[0]);
depthSync_->registerCallback(boost::bind(&CoreWrapper::depthCallback, this, _1, _2, _3, _4));
ROS_INFO("\n%s subscribed to:\n %s,\n %s,\n %s,\n %s", }
else
{
depthExactSync_ = new message_filters::Synchronizer<MyDepthExactSyncPolicy>(
MyDepthExactSyncPolicy(queueSize),
*imageSubs_[0],
odomSub_,
*imageDepthSubs_[0],
*cameraInfoSubs_[0]);
depthExactSync_->registerCallback(boost::bind(&CoreWrapper::depthCallback, this, _1, _2, _3, _4));
}
ROS_INFO("\n%s subscribed to (%s sync):\n %s,\n %s,\n %s,\n %s",
ros::this_node::getName().c_str(), ros::this_node::getName().c_str(),
approxSync?"approx":"exact",
imageSubs_[0]->getTopic().c_str(), imageSubs_[0]->getTopic().c_str(),
imageDepthSubs_[0]->getTopic().c_str(), imageDepthSubs_[0]->getTopic().c_str(),
cameraInfoSubs_[0]->getTopic().c_str(), cameraInfoSubs_[0]->getTopic().c_str(),
@@ -2806,15 +2826,27 @@ void CoreWrapper::setupCallbacks(
} }
else //!subscribeLaserScan else //!subscribeLaserScan
{ {
depthTFSync_ = new message_filters::Synchronizer<MyDepthTFSyncPolicy>( if(approxSync)
MyDepthTFSyncPolicy(queueSize), {
*imageSubs_[0], depthTFSync_ = new message_filters::Synchronizer<MyDepthTFSyncPolicy>(
*imageDepthSubs_[0], MyDepthTFSyncPolicy(queueSize),
*cameraInfoSubs_[0]); *imageSubs_[0],
depthTFSync_->registerCallback(boost::bind(&CoreWrapper::depthTFCallback, this, _1, _2, _3)); *imageDepthSubs_[0],
*cameraInfoSubs_[0]);
ROS_INFO("\n%s subscribed to:\n %s,\n %s,\n %s", depthTFSync_->registerCallback(boost::bind(&CoreWrapper::depthTFCallback, this, _1, _2, _3));
}
else
{
depthTFExactSync_ = new message_filters::Synchronizer<MyDepthTFExactSyncPolicy>(
MyDepthTFExactSyncPolicy(queueSize),
*imageSubs_[0],
*imageDepthSubs_[0],
*cameraInfoSubs_[0]);
depthTFExactSync_->registerCallback(boost::bind(&CoreWrapper::depthTFCallback, this, _1, _2, _3));
}
ROS_INFO("\n%s subscribed to (%s sync):\n %s,\n %s,\n %s",
ros::this_node::getName().c_str(), ros::this_node::getName().c_str(),
approxSync?"approx":"exact",
imageSubs_[0]->getTopic().c_str(), imageSubs_[0]->getTopic().c_str(),
imageDepthSubs_[0]->getTopic().c_str(), imageDepthSubs_[0]->getTopic().c_str(),
cameraInfoSubs_[0]->getTopic().c_str()); cameraInfoSubs_[0]->getTopic().c_str());
@@ -2886,9 +2918,8 @@ void CoreWrapper::setupCallbacks(
} }
else //!subscribeLaserScan else //!subscribeLaserScan
{ {
if(stereoApproxSync) if(approxSync)
{ {
ROS_INFO("Registering Stereo Approx callback...");
stereoApproxSync_ = new message_filters::Synchronizer<MyStereoApproxSyncPolicy>( stereoApproxSync_ = new message_filters::Synchronizer<MyStereoApproxSyncPolicy>(
MyStereoApproxSyncPolicy(queueSize), MyStereoApproxSyncPolicy(queueSize),
imageRectLeft_, imageRectLeft_,
@@ -2900,7 +2931,6 @@ void CoreWrapper::setupCallbacks(
} }
else else
{ {
ROS_INFO("Registering Stereo Exact callback...");
stereoExactSync_ = new message_filters::Synchronizer<MyStereoExactSyncPolicy>( stereoExactSync_ = new message_filters::Synchronizer<MyStereoExactSyncPolicy>(
MyStereoExactSyncPolicy(queueSize), MyStereoExactSyncPolicy(queueSize),
imageRectLeft_, imageRectLeft_,
@@ -2911,8 +2941,9 @@ void CoreWrapper::setupCallbacks(
stereoExactSync_->registerCallback(boost::bind(&CoreWrapper::stereoCallback, this, _1, _2, _3, _4, _5)); stereoExactSync_->registerCallback(boost::bind(&CoreWrapper::stereoCallback, this, _1, _2, _3, _4, _5));
} }
ROS_INFO("\n%s subscribed to:\n %s,\n %s,\n %s,\n %s,\n %s", ROS_INFO("\n%s subscribed to (%s sync):\n %s,\n %s,\n %s,\n %s,\n %s",
ros::this_node::getName().c_str(), ros::this_node::getName().c_str(),
approxSync?"approx":"exact",
imageRectLeft_.getTopic().c_str(), imageRectLeft_.getTopic().c_str(),
imageRectRight_.getTopic().c_str(), imageRectRight_.getTopic().c_str(),
cameraInfoLeft_.getTopic().c_str(), cameraInfoLeft_.getTopic().c_str(),
@@ -2925,7 +2956,6 @@ void CoreWrapper::setupCallbacks(
// use odom from TF, so subscribe to sensors only // use odom from TF, so subscribe to sensors only
if(subscribeScan2d) if(subscribeScan2d)
{ {
ROS_INFO("Registering Stereo+LaserScan2d+OdomTF callback...");
scanSub_.subscribe(nh, "scan", 1); scanSub_.subscribe(nh, "scan", 1);
stereoScanTFSync_ = new message_filters::Synchronizer<MyStereoScanTFSyncPolicy>( stereoScanTFSync_ = new message_filters::Synchronizer<MyStereoScanTFSyncPolicy>(
MyStereoScanTFSyncPolicy(queueSize), MyStereoScanTFSyncPolicy(queueSize),
@@ -2946,7 +2976,6 @@ void CoreWrapper::setupCallbacks(
} }
else if(subscribeScan3d) else if(subscribeScan3d)
{ {
ROS_INFO("Registering Stereo+LaserScan3d+OdomTF callback...");
scan3dSub_.subscribe(nh, "scan_cloud", 1); scan3dSub_.subscribe(nh, "scan_cloud", 1);
stereoScan3dTFSync_ = new message_filters::Synchronizer<MyStereoScan3dTFSyncPolicy>( stereoScan3dTFSync_ = new message_filters::Synchronizer<MyStereoScan3dTFSyncPolicy>(
MyStereoScan3dTFSyncPolicy(queueSize), MyStereoScan3dTFSyncPolicy(queueSize),
@@ -2967,9 +2996,8 @@ void CoreWrapper::setupCallbacks(
} }
else //!subscribeLaserScan else //!subscribeLaserScan
{ {
if(stereoApproxSync) if(approxSync)
{ {
ROS_INFO("Registering Stereo+OdomTF Approx callback...");
stereoApproxTFSync_ = new message_filters::Synchronizer<MyStereoApproxTFSyncPolicy>( stereoApproxTFSync_ = new message_filters::Synchronizer<MyStereoApproxTFSyncPolicy>(
MyStereoApproxTFSyncPolicy(queueSize), MyStereoApproxTFSyncPolicy(queueSize),
imageRectLeft_, imageRectLeft_,
@@ -2980,7 +3008,6 @@ void CoreWrapper::setupCallbacks(
} }
else else
{ {
ROS_INFO("Registering Stereo+OdomTF Exact callback...");
stereoExactTFSync_ = new message_filters::Synchronizer<MyStereoExactTFSyncPolicy>( stereoExactTFSync_ = new message_filters::Synchronizer<MyStereoExactTFSyncPolicy>(
MyStereoExactTFSyncPolicy(queueSize), MyStereoExactTFSyncPolicy(queueSize),
imageRectLeft_, imageRectLeft_,
@@ -2990,8 +3017,9 @@ void CoreWrapper::setupCallbacks(
stereoExactTFSync_->registerCallback(boost::bind(&CoreWrapper::stereoTFCallback, this, _1, _2, _3, _4)); stereoExactTFSync_->registerCallback(boost::bind(&CoreWrapper::stereoTFCallback, this, _1, _2, _3, _4));
} }
ROS_INFO("\n%s subscribed to:\n %s,\n %s,\n %s,\n %s", ROS_INFO("\n%s subscribed to (%s sync):\n %s,\n %s,\n %s,\n %s",
ros::this_node::getName().c_str(), ros::this_node::getName().c_str(),
approxSync?"approx":"exact",
imageRectLeft_.getTopic().c_str(), imageRectLeft_.getTopic().c_str(),
imageRectRight_.getTopic().c_str(), imageRectRight_.getTopic().c_str(),
cameraInfoLeft_.getTopic().c_str(), cameraInfoLeft_.getTopic().c_str(),
src/CoreWrapper.h
+12 -1
View File
@@ -90,7 +90,7 @@ private:
bool subscribeScan3d, bool subscribeScan3d,
bool subscribeStereo, bool subscribeStereo,
int queueSize, int queueSize,
bool stereoApproxSync, bool approxSync,
int depthCameras); int depthCameras);
void defaultCallback(const sensor_msgs::ImageConstPtr & imageMsg); // no odom void defaultCallback(const sensor_msgs::ImageConstPtr & imageMsg); // no odom
@@ -338,6 +338,12 @@ private:
sensor_msgs::Image, sensor_msgs::Image,
sensor_msgs::CameraInfo> MyDepthSyncPolicy; sensor_msgs::CameraInfo> MyDepthSyncPolicy;
message_filters::Synchronizer<MyDepthSyncPolicy> * depthSync_; message_filters::Synchronizer<MyDepthSyncPolicy> * depthSync_;
typedef message_filters::sync_policies::ExactTime<
sensor_msgs::Image,
nav_msgs::Odometry,
sensor_msgs::Image,
sensor_msgs::CameraInfo> MyDepthExactSyncPolicy;
message_filters::Synchronizer<MyDepthExactSyncPolicy> * depthExactSync_;
typedef message_filters::sync_policies::ApproximateTime< typedef message_filters::sync_policies::ApproximateTime<
sensor_msgs::Image, sensor_msgs::Image,
@@ -403,6 +409,11 @@ private:
sensor_msgs::Image, sensor_msgs::Image,
sensor_msgs::CameraInfo> MyDepthTFSyncPolicy; sensor_msgs::CameraInfo> MyDepthTFSyncPolicy;
message_filters::Synchronizer<MyDepthTFSyncPolicy> * depthTFSync_; message_filters::Synchronizer<MyDepthTFSyncPolicy> * depthTFSync_;
typedef message_filters::sync_policies::ExactTime<
sensor_msgs::Image,
sensor_msgs::Image,
sensor_msgs::CameraInfo> MyDepthTFExactSyncPolicy;
message_filters::Synchronizer<MyDepthTFExactSyncPolicy> * depthTFExactSync_;
typedef message_filters::sync_policies::ApproximateTime< typedef message_filters::sync_policies::ApproximateTime<
sensor_msgs::Image, sensor_msgs::Image,
src/RGBDOdometryNode.cpp
+37 -13
View File
@@ -54,7 +54,8 @@ class RGBDOdometry : public rtabmap_ros::OdometryROS
public: public:
RGBDOdometry(int argc, char * argv[]) : RGBDOdometry(int argc, char * argv[]) :
rtabmap_ros::OdometryROS(argc, argv), rtabmap_ros::OdometryROS(argc, argv),
sync_(0), approxSync_(0),
exactSync_(0),
sync2_(0), sync2_(0),
queueSize_(5) queueSize_(5)
{ {
@@ -62,6 +63,8 @@ public:
ros::NodeHandle pnh("~"); ros::NodeHandle pnh("~");
int depthCameras = 1; int depthCameras = 1;
bool approxSync = true;
pnh.param("approx_sync", approxSync, approxSync);
pnh.param("queue_size", queueSize_, queueSize_); pnh.param("queue_size", queueSize_, queueSize_);
pnh.param("depth_cameras", depthCameras, depthCameras); pnh.param("depth_cameras", depthCameras, depthCameras);
if(depthCameras <= 0) if(depthCameras <= 0)
@@ -135,22 +138,35 @@ public:
image_depth_sub_.subscribe(depth_it, depth_nh.resolveName("image"), 1, hintsDepth); image_depth_sub_.subscribe(depth_it, depth_nh.resolveName("image"), 1, hintsDepth);
info_sub_.subscribe(rgb_nh, "camera_info", 1); info_sub_.subscribe(rgb_nh, "camera_info", 1);
ROS_INFO("\n%s subscribed to:\n %s,\n %s,\n %s", if(approxSync)
{
approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_);
approxSync_->registerCallback(boost::bind(&RGBDOdometry::callback, this, _1, _2, _3));
}
else
{
exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_);
exactSync_->registerCallback(boost::bind(&RGBDOdometry::callback, this, _1, _2, _3));
}
ROS_INFO("\n%s subscribed to (%s sync):\n %s,\n %s,\n %s",
ros::this_node::getName().c_str(), ros::this_node::getName().c_str(),
approxSync?"approx":"exact",
image_mono_sub_.getTopic().c_str(), image_mono_sub_.getTopic().c_str(),
image_depth_sub_.getTopic().c_str(), image_depth_sub_.getTopic().c_str(),
info_sub_.getTopic().c_str()); info_sub_.getTopic().c_str());
sync_ = new message_filters::Synchronizer<MySyncPolicy>(MySyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_);
sync_->registerCallback(boost::bind(&RGBDOdometry::callback, this, _1, _2, _3));
} }
} }
virtual ~RGBDOdometry() virtual ~RGBDOdometry()
{ {
if(sync_) if(approxSync_)
{ {
delete sync_; delete approxSync_;
}
if(exactSync_)
{
delete exactSync_;
} }
if(sync2_) if(sync2_)
{ {
@@ -343,11 +359,17 @@ protected:
virtual void flushCallbacks() virtual void flushCallbacks()
{ {
// flush callbacks // flush callbacks
if(sync_) if(approxSync_)
{ {
delete sync_; delete approxSync_;
sync_ = new message_filters::Synchronizer<MySyncPolicy>(MySyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_); approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_);
sync_->registerCallback(boost::bind(&RGBDOdometry::callback, this, _1, _2, _3)); approxSync_->registerCallback(boost::bind(&RGBDOdometry::callback, this, _1, _2, _3));
}
if(exactSync_)
{
delete exactSync_;
exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_);
exactSync_->registerCallback(boost::bind(&RGBDOdometry::callback, this, _1, _2, _3));
} }
if(sync2_) if(sync2_)
{ {
@@ -371,8 +393,10 @@ private:
image_transport::SubscriberFilter image_mono2_sub_; image_transport::SubscriberFilter image_mono2_sub_;
image_transport::SubscriberFilter image_depth2_sub_; image_transport::SubscriberFilter image_depth2_sub_;
message_filters::Subscriber<sensor_msgs::CameraInfo> info2_sub_; message_filters::Subscriber<sensor_msgs::CameraInfo> info2_sub_;
typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo> MySyncPolicy; typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo> MyApproxSyncPolicy;
message_filters::Synchronizer<MySyncPolicy> * sync_; message_filters::Synchronizer<MyApproxSyncPolicy> * approxSync_;
typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo> MyExactSyncPolicy;
message_filters::Synchronizer<MyExactSyncPolicy> * exactSync_;
typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo> MySync2Policy; typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo> MySync2Policy;
message_filters::Synchronizer<MySync2Policy> * sync2_; message_filters::Synchronizer<MySync2Policy> * sync2_;
int queueSize_; int queueSize_;
src/StereoOdometryNode.cpp
+9 -7
View File
@@ -80,13 +80,6 @@ public:
cameraInfoLeft_.subscribe(left_nh, "camera_info", 1); cameraInfoLeft_.subscribe(left_nh, "camera_info", 1);
cameraInfoRight_.subscribe(right_nh, "camera_info", 1); cameraInfoRight_.subscribe(right_nh, "camera_info", 1);
ROS_INFO("\n%s subscribed to:\n %s,\n %s,\n %s,\n %s",
ros::this_node::getName().c_str(),
imageRectLeft_.getTopic().c_str(),
imageRectRight_.getTopic().c_str(),
cameraInfoLeft_.getTopic().c_str(),
cameraInfoRight_.getTopic().c_str());
if(approxSync) if(approxSync)
{ {
approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize_), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_); approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize_), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
@@ -97,6 +90,15 @@ public:
exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(queueSize_), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_); exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(queueSize_), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
exactSync_->registerCallback(boost::bind(&StereoOdometry::callback, this, _1, _2, _3, _4)); exactSync_->registerCallback(boost::bind(&StereoOdometry::callback, this, _1, _2, _3, _4));
} }
ROS_INFO("\n%s subscribed to (%s sync):\n %s,\n %s,\n %s,\n %s",
ros::this_node::getName().c_str(),
approxSync?"approx":"exact",
imageRectLeft_.getTopic().c_str(),
imageRectRight_.getTopic().c_str(),
cameraInfoLeft_.getTopic().c_str(),
cameraInfoRight_.getTopic().c_str());
} }
virtual ~StereoOdometry() virtual ~StereoOdometry()