Skip to content
Permalink
Branch: master
Find file Copy path
Find file Copy path
Fetching contributors…
Cannot retrieve contributors at this time
98 lines (77 sloc) 5.76 KB
<launch>
<!-- ROBOT MAPPING VERSION: use this with ROS bag demo_mapping.bag -->
<!-- WARNING : Database is automatically deleted on each startup -->
<!-- See "delete_db_on_start" option below... -->
<!-- Choose visualization -->
<arg name="rviz" default="false" />
<arg name="rtabmapviz" default="true" />
<param name="use_sim_time" type="bool" value="True"/>
<!-- Localization-only mode -->
<arg name="localization" default="false"/>
<arg if="$(arg localization)" name="rtabmap_args" default=""/>
<arg unless="$(arg localization)" name="rtabmap_args" default="--delete_db_on_start"/>
<group ns="rtabmap">
<!-- SLAM (robot side) -->
<!-- args: "delete_db_on_start" and "udebug" -->
<node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="$(arg rtabmap_args)">
<param name="frame_id" type="string" value="base_footprint"/>
<param name="wait_for_transform" type="bool" value="true"/>
<param name="subscribe_depth" type="bool" value="true"/>
<param name="subscribe_scan" type="bool" value="true"/>
<!-- As /az3/base_controller/odom topic doesn't provide covariances, we use TF to get odom and we fix the covariance -->
<param name="odom_frame_id" type="string" value="odom"/>
<param name="odom_tf_linear_variance" type="double" value="0.001"/>
<param name="odom_tf_angular_variance" type="double" value="0.001"/>
<remap from="scan" to="/jn0/base_scan"/>
<remap from="rgb/image" to="/data_throttled_image"/>
<remap from="depth/image" to="/data_throttled_image_depth"/>
<remap from="rgb/camera_info" to="/data_throttled_camera_info"/>
<param name="rgb/image_transport" type="string" value="compressed"/>
<param name="depth/image_transport" type="string" value="compressedDepth"/>
<!-- RTAB-Map's parameters: do "rosrun rtabmap rtabmap (double-dash)params" to see the list of available parameters. -->
<param name="RGBD/NeighborLinkRefining" type="string" value="true"/> <!-- Do odometry correction with consecutive laser scans -->
<param name="RGBD/ProximityBySpace" type="string" value="true"/> <!-- Local loop closure detection (using estimated position) with locations in WM -->
<param name="RGBD/ProximityByTime" type="string" value="false"/> <!-- Local loop closure detection with locations in STM -->
<param name="RGBD/ProximityPathMaxNeighbors" type="string" value="10"/> <!-- Do also proximity detection by space by merging close scans together. -->
<param name="Reg/Strategy" type="string" value="1"/> <!-- 0=Visual, 1=ICP, 2=Visual+ICP -->
<param name="Vis/MinInliers" type="string" value="12"/> <!-- 3D visual words correspondence distance -->
<param name="RGBD/OptimizeFromGraphEnd" type="string" value="false"/> <!-- Optimize graph from initial node so /map -> /odom transform will be generated -->
<param name="RGBD/OptimizeMaxError" type="string" value="3"/> <!-- Reject any loop closure causing large errors (>3x link's covariance) in the map -->
<param name="Reg/Force3DoF" type="string" value="true"/> <!-- 2D SLAM -->
<param name="Grid/FromDepth" type="string" value="false"/> <!-- Create 2D occupancy grid from laser scan -->
<param name="Mem/STMSize" type="string" value="30"/> <!-- increased to 30 to avoid adding too many loop closures on just seen locations -->
<param name="RGBD/LocalRadius" type="string" value="5"/> <!-- limit length of proximity detections -->
<param name="Icp/CorrespondenceRatio" type="string" value="0.4"/> <!-- minimum scan overlap to accept loop closure -->
<!-- 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)"/>
</node>
<!-- 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="subscribe_depth" type="bool" value="true"/>
<param name="subscribe_scan" type="bool" value="true"/>
<param name="frame_id" type="string" value="base_footprint"/>
<param name="wait_for_transform" type="bool" value="true"/>
<remap from="rgb/image" to="/data_throttled_image"/>
<remap from="depth/image" to="/data_throttled_image_depth"/>
<remap from="rgb/camera_info" to="/data_throttled_camera_info"/>
<remap from="scan" to="/jn0/base_scan"/>
<remap from="odom" to="/az3/base_controller/odom"/>
<param name="rgb/image_transport" type="string" value="compressed"/>
<param name="depth/image_transport" type="string" value="compressedDepth"/>
</node>
</group>
<!-- Visualisation RVIZ -->
<node if="$(arg rviz)" pkg="rviz" type="rviz" name="rviz" args="-d $(find rtabmap_ros)/launch/config/demo_robot_mapping.rviz" output="screen"/>
<node pkg="nodelet" type="nodelet" name="points_xyzrgb" args="standalone rtabmap_ros/point_cloud_xyzrgb">
<remap from="rgb/image" to="/data_throttled_image"/>
<remap from="depth/image" to="/data_throttled_image_depth"/>
<remap from="rgb/camera_info" to="/data_throttled_camera_info"/>
<remap from="cloud" to="voxel_cloud" />
<param name="rgb/image_transport" type="string" value="compressed"/>
<param name="depth/image_transport" type="string" value="compressedDepth"/>
<param name="queue_size" type="int" value="10"/>
<param name="voxel_size" type="double" value="0.01"/>
</node>
</launch>
You can’t perform that action at this time.