mirror of
https://github.com/introlab/rtabmap_ros.git
synced 2026-10-04 16:57:46 +08:00
Add rtabmap_msgs/SensorData (#1055)
* Add rtabmap_msgs/SensorData * After testing fixes * Updated output topic name * added explicit --logconsole for nodelets * fixed intermediate nodes not generated * Refactored SyncDiagnostic usage to handle nodelet name * SyncDiagnostic: added TimeStampStatus. Odom: added new status when data not received yet * rtabmap: Fixed parameters not updated when using nodelet
This commit is contained in:
matlabbe
@@ -0,0 +1,191 @@
|
||||
<?xml version="1.0"?>
|
||||
|
||||
<!--
|
||||
Examples with OAK-D camera.
|
||||
$ roslaunch depthai_examples stereo_inertial_node.launch depth_aligned:=false
|
||||
|
||||
$ rosrun imu_filter_madgwick imu_filter_node \
|
||||
imu/data_raw:=/stereo_inertial_publisher/imu \
|
||||
imu/data:=/stereo_inertial_publisher/imu/data \
|
||||
_use_mag:=false \
|
||||
_publish_tf:=false
|
||||
|
||||
Stereo:
|
||||
$ roslaunch rtabmap_examples test_sensor_data.launch \
|
||||
stereo:=true \
|
||||
left_topic:=/stereo_inertial_publisher/left/image_rect \
|
||||
right_topic:=/stereo_inertial_publisher/right/image_rect \
|
||||
left_info_topic:=/stereo_inertial_publisher/left/camera_info \
|
||||
right_info_topic:=/stereo_inertial_publisher/right/camera_info \
|
||||
imu_topic:=/stereo_inertial_publisher/imu/data \
|
||||
frame_id:=oak-d_frame \
|
||||
approx_sync:=true \
|
||||
approx_sync_max_interval:=0.001 \
|
||||
wait_imu_to_init:=true
|
||||
|
||||
RGB-D:
|
||||
$ roslaunch rtabmap_examples test_sensor_data.launch \
|
||||
rgb_topic:=/stereo_inertial_publisher/right/image_rect \
|
||||
depth_topic:=/stereo_inertial_publisher/stereo/depth \
|
||||
camera_info_topic:=/stereo_inertial_publisher/right/camera_info \
|
||||
imu_topic:=/stereo_inertial_publisher/imu/data \
|
||||
frame_id:=oak-d_frame \
|
||||
approx_sync:=true \
|
||||
approx_sync_max_interval:=0.001 \
|
||||
wait_imu_to_init:=true
|
||||
-->
|
||||
|
||||
<launch>
|
||||
|
||||
<arg name="features_only" default="false"/>
|
||||
<arg name="compression_format" default=".jpg"/> <!-- ".jpg" or ".png" -->
|
||||
<arg name="parallel_compression" default="true"/>
|
||||
|
||||
<arg name="use_nodelets" default="true"/>
|
||||
|
||||
<arg name="stereo" default="false"/>
|
||||
|
||||
<arg name="frame_id" default="camera_link"/>
|
||||
<arg name="rtabmap_args" default="--delete_db_on_start --logconsole"/> <!-- delete_db_on_start -->
|
||||
<arg name="odom_args" default="$(arg rtabmap_args)"/>
|
||||
|
||||
<!-- RGB-D related topics -->
|
||||
<arg name="rgb_topic" default="/camera/rgb/image_rect_color" />
|
||||
<arg name="depth_topic" default="/camera/depth_registered/image_raw" />
|
||||
<arg name="camera_info_topic" default="/camera/rgb/camera_info" />
|
||||
|
||||
<!-- Stereo related topics -->
|
||||
<arg name="left_topic" default="/camera/left/image_rect" />
|
||||
<arg name="right_topic" default="/camera/right/image_rect" />
|
||||
<arg name="left_info_topic" default="/camera/left/camera_info" />
|
||||
<arg name="right_info_topic" default="/camera/right/camera_info" />
|
||||
|
||||
<arg name="imu_topic" default="/imu/data" />
|
||||
<arg name="wait_imu_to_init" default="false" />
|
||||
|
||||
<arg name="approx_sync" default="$(eval not stereo)" />
|
||||
<arg name="approx_sync_max_interval" default="0" />
|
||||
|
||||
<group ns="rtabmap">
|
||||
|
||||
<!-- WITH NODELETS -->
|
||||
<group if="$(arg use_nodelets)">
|
||||
<node pkg="nodelet" type="nodelet" name="manager" args="manager" output="screen"/>
|
||||
|
||||
<!-- Use Stereo synchronization -->
|
||||
<node if="$(arg stereo)" pkg="nodelet" type="nodelet" name="stereo_sync" args="load rtabmap_sync/stereo_sync manager">
|
||||
<remap from="left/image_rect" to="$(arg left_topic)"/>
|
||||
<remap from="right/image_rect" to="$(arg right_topic)"/>
|
||||
<remap from="left/camera_info" to="$(arg left_info_topic)"/>
|
||||
<remap from="right/camera_info" to="$(arg right_info_topic)"/>
|
||||
<param name="approx_sync" value="$(arg approx_sync)"/>
|
||||
<param name="approx_sync_max_interval" value="$(arg approx_sync_max_interval)"/>
|
||||
</node>
|
||||
<!-- Use RGBD synchronization -->
|
||||
<node unless="$(arg stereo)" pkg="nodelet" type="nodelet" name="rgbd_sync" args="load rtabmap_sync/rgbd_sync manager">
|
||||
<remap from="rgb/image" to="$(arg rgb_topic)"/>
|
||||
<remap from="depth/image" to="$(arg depth_topic)"/>
|
||||
<remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
|
||||
<param name="approx_sync" value="$(arg approx_sync)"/>
|
||||
<param name="approx_sync_max_interval" value="$(arg approx_sync_max_interval)"/>
|
||||
</node>
|
||||
|
||||
<!-- Stereo Odometry -->
|
||||
<node if="$(arg stereo)" pkg="nodelet" type="nodelet" name="stereo_odometry" args="load rtabmap_odom/stereo_odometry manager $(arg odom_args)">
|
||||
<param name="frame_id" type="string" value="$(arg frame_id)"/>
|
||||
<param name="subscribe_depth" type="bool" value="false"/>
|
||||
<param name="subscribe_rgbd" type="bool" value="true"/>
|
||||
<param name="keep_color" type="bool" value="true"/>
|
||||
<param name="wait_imu_to_init" type="bool" value="$(arg wait_imu_to_init)"/>
|
||||
<remap from="imu" to="$(arg imu_topic)"/>
|
||||
<param name="sensor_data_compression_format" value="$(arg compression_format)"/>
|
||||
<param name="sensor_data_parallel_compression" value="$(arg parallel_compression)"/>
|
||||
</node>
|
||||
<!-- RGB-D Odometry -->
|
||||
<node unless="$(arg stereo)" pkg="nodelet" type="nodelet" name="rgbd_odometry" args="load rtabmap_odom/rgbd_odometry manager $(arg odom_args)">
|
||||
<param name="frame_id" type="string" value="$(arg frame_id)"/>
|
||||
<param name="subscribe_depth" type="bool" value="false"/>
|
||||
<param name="subscribe_rgbd" type="bool" value="true"/>
|
||||
<param name="keep_color" type="bool" value="true"/>
|
||||
<param name="wait_imu_to_init" type="bool" value="$(arg wait_imu_to_init)"/>
|
||||
<remap from="imu" to="$(arg imu_topic)"/>
|
||||
<param name="sensor_data_compression_format" value="$(arg compression_format)"/>
|
||||
<param name="sensor_data_parallel_compression" value="$(arg parallel_compression)"/>
|
||||
</node>
|
||||
|
||||
<!-- RTAB-Map -->
|
||||
<node pkg="nodelet" type="nodelet" name="rtabmap" args="load rtabmap_slam/rtabmap manager $(arg rtabmap_args)">
|
||||
<param name="frame_id" type="string" value="$(arg frame_id)"/>
|
||||
<param name="subscribe_sensor_data" type="bool" value="true"/>
|
||||
<param name="approx_sync" type="bool" value="false"/>
|
||||
<remap if="$(arg features_only)" from="sensor_data" to="odom_sensor_data/features"/>
|
||||
<remap unless="$(arg features_only)" from="sensor_data" to="odom_sensor_data/raw"/>
|
||||
</node>
|
||||
</group>
|
||||
|
||||
<!-- WITHOUT NODELETS -->
|
||||
<group unless="$(arg use_nodelets)">
|
||||
<!-- Use Stereo synchronization -->
|
||||
<node if="$(arg stereo)" pkg="rtabmap_sync" type="stereo_sync" name="stereo_sync" output="screen">
|
||||
<remap from="left/image_rect" to="$(arg left_topic)"/>
|
||||
<remap from="right/image_rect" to="$(arg right_topic)"/>
|
||||
<remap from="left/camera_info" to="$(arg left_info_topic)"/>
|
||||
<remap from="right/camera_info" to="$(arg right_info_topic)"/>
|
||||
<param name="approx_sync" value="$(arg approx_sync)"/>
|
||||
<param name="approx_sync_max_interval" value="$(arg approx_sync_max_interval)"/>
|
||||
</node>
|
||||
<!-- Use RGBD synchronization -->
|
||||
<node unless="$(arg stereo)" pkg="rtabmap_sync" type="rgbd_sync" name="rgbd_sync" output="screen">
|
||||
<remap from="rgb/image" to="$(arg rgb_topic)"/>
|
||||
<remap from="depth/image" to="$(arg depth_topic)"/>
|
||||
<remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
|
||||
<param name="approx_sync" value="$(arg approx_sync)"/>
|
||||
<param name="approx_sync_max_interval" value="$(arg approx_sync_max_interval)"/>
|
||||
</node>
|
||||
|
||||
<!-- Stereo Odometry -->
|
||||
<node if="$(arg stereo)" pkg="rtabmap_odom" type="stereo_odometry" name="stereo_odometry" args="$(arg odom_args)" output="screen">
|
||||
<param name="frame_id" type="string" value="$(arg frame_id)"/>
|
||||
<param name="subscribe_depth" type="bool" value="false"/>
|
||||
<param name="subscribe_rgbd" type="bool" value="true"/>
|
||||
<param name="keep_color" type="bool" value="true"/>
|
||||
<param name="wait_imu_to_init" type="bool" value="$(arg wait_imu_to_init)"/>
|
||||
<remap from="imu" to="$(arg imu_topic)"/>
|
||||
<param name="sensor_data_compression_format" value="$(arg compression_format)"/>
|
||||
<param name="sensor_data_parallel_compression" value="$(arg parallel_compression)"/>
|
||||
</node>
|
||||
<!-- RGB-D Odometry -->
|
||||
<node unless="$(arg stereo)" pkg="rtabmap_odom" type="rgbd_odometry" name="rgbd_odometry" args="$(arg odom_args)" output="screen">
|
||||
<param name="frame_id" type="string" value="$(arg frame_id)"/>
|
||||
<param name="subscribe_depth" type="bool" value="false"/>
|
||||
<param name="subscribe_rgbd" type="bool" value="true"/>
|
||||
<param name="keep_color" type="bool" value="true"/>
|
||||
<param name="wait_imu_to_init" type="bool" value="$(arg wait_imu_to_init)"/>
|
||||
<remap from="imu" to="$(arg imu_topic)"/>
|
||||
<param name="sensor_data_compression_format" value="$(arg compression_format)"/>
|
||||
<param name="sensor_data_parallel_compression" value="$(arg parallel_compression)"/>
|
||||
</node>
|
||||
|
||||
<!-- RTAB-Map -->
|
||||
<node pkg="rtabmap_slam" type="rtabmap" name="rtabmap" args="$(arg rtabmap_args)" output="screen">
|
||||
<param name="frame_id" type="string" value="$(arg frame_id)"/>
|
||||
<param name="subscribe_sensor_data" type="bool" value="true"/>
|
||||
<param name="approx_sync" type="bool" value="false"/>
|
||||
<remap if="$(arg features_only)" from="sensor_data" to="odom_sensor_data/features"/>
|
||||
<remap unless="$(arg features_only)" from="sensor_data" to="odom_sensor_data/raw"/>
|
||||
</node>
|
||||
</group>
|
||||
|
||||
<!-- Visualization -->
|
||||
<node pkg="rtabmap_viz" type="rtabmap_viz" name="rtabmap_viz" output="screen">
|
||||
<param name="frame_id" type="string" value="$(arg frame_id)"/>
|
||||
<param name="subscribe_sensor_data" type="bool" value="true"/>
|
||||
<param name="subscribe_odom_info" type="bool" value="true"/>
|
||||
<param name="approx_sync" type="bool" value="false"/>
|
||||
<remap if="$(arg features_only)" from="sensor_data" to="odom_sensor_data/features"/>
|
||||
<remap unless="$(arg features_only)" from="sensor_data" to="odom_sensor_data/raw"/>
|
||||
</node>
|
||||
|
||||
</group>
|
||||
|
||||
</launch>
|
||||
Reference in New Issue
Block a user