i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-09-15 07:40:20 +08:00
Code Issues Packages Projects Releases Wiki Activity

merged master

Browse Source
This commit is contained in:
matlabbe
2023-01-21 16:19:25 -08:00
parent c204ed6142
commit 72298712c9
91 changed files with 1092 additions and 1026 deletions
Download Patch File Download Diff File Expand all files Collapse all files
CMakeLists.txt
+1 -1
View File
@@ -59,7 +59,7 @@ find_package(octomap_msgs)
#find_package(fiducial_msgs)
## System dependencies are found with CMake's conventions
find_package(RTABMap 0.20.22 REQUIRED)
find_package(RTABMap 0.20.23 REQUIRED)
find_package(Boost REQUIRED COMPONENTS system) # dependencies from PCL
find_package(PCL 1.7 REQUIRED COMPONENTS kdtree) #This crashes idl generation if all components are found?! see https://github.com/ros2/rosidl/issues/402#issuecomment-565586908
launch/azimut3/az3_bag_mapping.launch
+9 -8
View File
@@ -1,3 +1,4 @@
<?xml version="1.0"?>
<launch>
@@ -8,22 +9,22 @@
<group ns="rtabmap">
<node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="--delete_db_on_start">
<param name="frame_id" type="string" value="base_footprint"/>
<param name="subscribe_depth" type="bool" value="true"/>
<param name="subscribe_laserScan" type="bool" value="true"/>
<remap from="odom" to="/base_controller/odom"/>
<remap from="scan" to="/base_scan"/>
<remap from="rgb/image" to="/camera/data_throttled_image"/>
<remap from="depth/image" to="/camera/data_throttled_image_depth"/>
<remap from="rgb/camera_info" to="/camera/data_throttled_camera_info"/>
<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"/>
<!-- RTAB-Map's parameters: do "rosrun rtabmap rtabmap (double-dash)params" to see the list of available parameters. -->
<param name="RGBD/ScanMatchingSize" type="string" value="1"/> <!-- Do odometry correction with consecutive laser scans -->
<param name="RGBD/LocalLoopDetectionSpace" type="string" value="true"/> <!-- Local loop closure detection (using estimated position) with locations in WM -->
@@ -39,10 +40,10 @@
<param name="RGBD/LinearUpdate" type="string" value="0.01"/> <!-- Update map only if the robot is moving -->
<param name="Rtabmap/TimeThr" type="string" value="700"/>
<param name="Mem/RehearsalSimilarity" type="string" value="0.45"/>
<param name="Mem/RehearsedNodesKept" type="string" value="false"/>
<param name="Mem/RehearsedNodesKept" type="string" value="false"/>
</node>
</group>
</group>
<!-- send AZIMUT 3 urdf to param server -->
<param name="robot_description" command="$(find xacro)/xacro.py '$(find az3_description)/robots/azimut_3_laser.urdf.xacro'" />
launch/azimut3/az3_bag_record.launch
+6 -5
View File
@@ -1,7 +1,8 @@
<?xml version="1.0"?>
<launch>
<!-- rosbag record camera/data_throttled_image/compressed camera/data_throttled_image_depth/compressedDepth camera/data_throttled_camera_info tf base_scan /base_controller/odom -->
<include file="$(find az3_bringup)/az3_standalone.launch"/>
<!-- To control with only one joystick -->
@@ -17,20 +18,20 @@
<!-- OpenNI -->
<include file="$(find rtabmap_ros)/launch/azimut3/az3_openni.launch"/>
<!-- 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="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"/>
<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"/>
</node>
</node>
</group>
</launch>
launch/azimut3/az3_db_record.launch
+10 -9
View File
@@ -1,3 +1,4 @@
<?xml version="1.0"?>
<launch>
@@ -8,38 +9,38 @@
<!-- OpenNI -->
<include file="$(find rtabmap_ros)/launch/azimut3/az3_openni.launch"/>
<!-- 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="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"/>
<remap from="rgb/camera_info_in" to="rgb/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"/>
</node>
</node>
</group>
<node name="data_recorder" pkg="rtabmap_ros" type="data_recorder" output="screen">
<param name="output_file_name" value="az3_record.db" type="string"/>
<param name="output_file_name" value="az3_record.db" type="string"/>
<param name="frame_id" type="string" value="base_footprint"/>
<param name="subscribe_odometry" type="bool" value="true"/>
<param name="subscribe_depth" type="bool" value="true"/>
<param name="subscribe_laserScan" type="bool" value="true"/>
<remap from="odom" to="/base_controller/odom"/>
<remap from="scan" to="/base_scan"/>
<remap from="rgb/image" to="camera/data_throttled_image"/>
<remap from="depth/image" to="camera/data_throttled_image_depth"/>
<remap from="rgb/camera_info" to="camera/data_throttled_camera_info"/>
<param name="queue_size" type="int" value="10"/>
</node>
launch/azimut3/az3_mapping_client.launch
+4 -3
View File
@@ -1,3 +1,4 @@
<?xml version="1.0"?>
<launch>
@@ -16,9 +17,9 @@
<remap from="mapData" to="rtabmap/mapData_relay"/>
<remap from="grid_map" to="rtabmap/grid_map"/>
</node>
<node pkg="rviz" type="rviz" name="rviz" args="-d $(find rtabmap_ros)/launch/azimut3/config/azimut3.rviz"/>
<!-- Below, construct point cloud of the latest throttled data, disabled for bandwidth efficiency -->
<node pkg="nodelet" type="nodelet" name="standalone_nodelet" args="manager" output="screen"/>
<node pkg="nodelet" type="nodelet" name="points_xyzrgb" args="load rtabmap_ros/point_cloud_xyzrgb standalone_nodelet">
@@ -26,7 +27,7 @@
<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/azimut3/az3_mapping_client_stereo_nav.launch
+1
View File
@@ -1,3 +1,4 @@
<?xml version="1.0"?>
<launch>
launch/azimut3/az3_mapping_robot.launch
+11 -10
View File
@@ -1,10 +1,11 @@
<?xml version="1.0"?>
<launch>
<!-- AZIMUT 3 bringup: launch motors/odometry, laser scan and openni -->
<include file="$(find az3_bringup)/az3_standalone.launch"/>
<!-- <include file="$(find az3_bringup)/joystick.launch"/> -->
<!-- OpenNI -->
<include file="$(find rtabmap_ros)/launch/azimut3/az3_openni.launch"/>
@@ -12,35 +13,35 @@
<group ns="camera">
<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="5.0"/>
<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"/>
</node>
</group>
</node>
</group>
<!-- SLAM (robot side) -->
<!-- args: "delete_db_on_start" and "udebug" -->
<group ns="rtabmap">
<node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="--delete_db_on_start">
<param name="frame_id" type="string" value="base_footprint"/>
<param name="subscribe_depth" type="bool" value="true"/>
<param name="subscribe_laserScan" type="bool" value="true"/>
<remap from="odom" to="/base_controller/odom"/>
<remap from="scan" to="/base_scan"/>
<remap from="rgb/image" to="/camera/data_throttled_image"/>
<remap from="depth/image" to="/camera/data_throttled_image_depth"/>
<remap from="rgb/camera_info" to="/camera/data_throttled_camera_info"/>
<param name="queue_size" type="int" value="10"/>
<!-- RTAB-Map's parameters: do "rosrun rtabmap rtabmap (double-dash)params" to see the list of available parameters. -->
<param name="RGBD/ScanMatchingSize" type="string" value="1"/> <!-- Do odometry correction with consecutive laser scans -->
<param name="RGBD/LocalLoopDetectionSpace" type="string" value="true"/> <!-- Local loop closure detection (using estimated position) with locations in WM -->
@@ -56,7 +57,7 @@
<param name="RGBD/LinearUpdate" type="string" value="0.01"/> <!-- Update map only if the robot is moving -->
<param name="Rtabmap/TimeThr" type="string" value="700"/>
<param name="Mem/RehearsalSimilarity" type="string" value="0.45"/>
<param name="Mem/RehearsedNodesKept" type="string" value="false"/>
<param name="Mem/RehearsedNodesKept" type="string" value="false"/>
</node>
</group>
</launch>
launch/azimut3/az3_mapping_robot_kinect_odom.launch
+10 -9
View File
@@ -1,10 +1,11 @@
<?xml version="1.0"?>
<launch>
<!-- AZIMUT 3 bringup: launch motors/odometry, laser scan and openni -->
<include file="$(find az3_bringup)/az3_standalone.launch"/>
<!-- <include file="$(find az3_bringup)/joystick.launch"/> -->
<!-- OpenNI -->
<include file="$(find rtabmap_ros)/launch/azimut3/az3_openni.launch"/>
@@ -12,16 +13,16 @@
<group ns="camera">
<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="5.0"/>
<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"/>
</node>
</group>
</node>
</group>
<!-- SLAM (robot side) -->
<!-- args: "delete_db_on_start" and "udebug" -->
@@ -29,14 +30,14 @@
<node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="--delete_db_on_start">
<param name="frame_id" type="string" value="base_footprint"/>
<param name="subscribe_depth" type="bool" value="true"/>
<remap from="odom" to="/base_controller/odom"/>
<remap from="rgb/image" to="/camera/data_throttled_image"/>
<remap from="depth/image" to="/camera/data_throttled_image_depth"/>
<remap from="rgb/camera_info" to="/camera/data_throttled_camera_info"/>
<param name="queue_size" type="int" value="10"/>
<!-- RTAB-Map's parameters: do "rosrun rtabmap rtabmap (double-dash)params" to see the list of available parameters. -->
<param name="RGBD/LocalLoopDetectionTime" type="string" value="false"/> <!-- Local loop closure detection with locations in STM -->
<param name="Mem/BadSignaturesIgnored" type="string" value="false"/> <!-- Don't ignore bad images for 3D node creation (e.g. white walls) -->
@@ -44,7 +45,7 @@
<param name="RGBD/LinearUpdate" type="string" value="0.01"/> <!-- Update map only if the robot is moving -->
<param name="Rtabmap/TimeThr" type="string" value="700"/>
<param name="Mem/RehearsalSimilarity" type="string" value="0.45"/>
<param name="Mem/RehearsedNodesKept" type="string" value="false"/>
<param name="Mem/RehearsedNodesKept" type="string" value="false"/>
</node>
</group>
</launch>
launch/azimut3/az3_mapping_robot_kinect_only.launch
+2 -1
View File
@@ -1,3 +1,4 @@
<?xml version="1.0"?>
<launch>
@@ -19,7 +20,7 @@
<remap from="odom" to="/base_controller/odom"/>
<param name="Odom/MinInliers" type="string" value="10"/>
<param name="Odom/InlierDistance" type="string" value="0.01"/>
<param name="Odom/InlierDistance" type="string" value="0.01"/>
<param name="frame_id" type="string" value="base_footprint"/>
</node>
launch/azimut3/az3_mapping_robot_kinect_scan.launch
+14 -13
View File
@@ -1,3 +1,4 @@
<?xml version="1.0"?>
<launch>
@@ -6,7 +7,7 @@
<!-- AZIMUT 3 bringup: launch motors/odometry -->
<include file="$(find az3_bringup)/az3_standalone.launch"/>
<!-- OpenNI -->
<include file="$(find rtabmap_ros)/launch/azimut3/az3_openni.launch"/>
@@ -23,7 +24,7 @@
<remap from="depth/image" to="/camera/depth_registered/image_raw"/>
<remap from="rgb/camera_info" to="/camera/depth_registered/camera_info"/>
<remap from="goal_out" to="current_goal"/>
<remap from="goal_out" to="current_goal"/>
<remap from="move_base" to="/planner/move_base"/>
<remap from="grid_map" to="/map"/>
@@ -31,8 +32,8 @@
<param unless="$(arg localization)" name="Rtabmap/TimeThr" type="string" value="500"/>
<param if="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="false"/>
<param if="$(arg localization)" name="Mem/InitWMWithAllNodes" type="string" value="true"/>
<param name="RGBD/PoseScanMatching" type="string" value="true"/>
<param name="RGBD/LocalRadius" type="string" value="4"/>
<param name="RGBD/PoseScanMatching" type="string" value="true"/>
<param name="RGBD/LocalRadius" type="string" value="4"/>
<param name="Mem/RehearsalSimilarity" type="string" value="0.30"/>
<param name="Rtabmap/DetectionRate" type="string" value="1"/>
<param name="RGBD/OptimizeSlam2d" type="string" value="true"/>
@@ -44,7 +45,7 @@
<param name="LccIcp2/CorrespondenceRatio" type="string" value="0.2"/>
</node>
</group>
<!-- teleop -->
<node name="joy" pkg="joy" type="joy_node"/>
<group ns="teleop">
@@ -59,7 +60,7 @@
<remap from="obstacles_cloud" to="/obstacles_cloud"/>
<remap from="ground_cloud" to="/ground_cloud"/>
<remap from="map" to="/map"/>
<node pkg="move_base" type="move_base" respawn="true" name="move_base" output="screen">
<param name="base_global_planner" value="navfn/NavfnROS"/>
<rosparam file="$(find rtabmap_ros)/launch/azimut3/config/costmap_common_params_2d.yaml" command="load" ns="global_costmap" />
@@ -68,7 +69,7 @@
<rosparam file="$(find rtabmap_ros)/launch/azimut3/config/global_costmap_params.yaml" command="load" ns="global_costmap"/>
<rosparam file="$(find rtabmap_ros)/launch/azimut3/config/base_local_planner_params.yaml" command="load" />
</node>
<param name="cmd_vel/abtr_priority" value="10"/>
</group>
@@ -81,7 +82,7 @@
args="/cmd_eta /teleop/cmd_eta"/>
<node name="register_cmd_vel" pkg="abtr_priority" type="register"
args="/cmd_vel /planner/cmd_vel"/>
<!-- Throttling messages -->
<group ns="camera">
<node pkg="nodelet" type="nodelet" name="data_throttle" args="load rtabmap_ros/data_throttle camera_nodelet_manager">
@@ -90,7 +91,7 @@
<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="throttled_image"/>
<remap from="depth/image_out" to="throttled_image_depth"/>
<remap from="rgb/camera_info_out" to="throttled_camera_info"/>
@@ -106,26 +107,26 @@
<param name="max_depth" type="double" value="4.0"/>
<param name="voxel_size" type="double" value="0.02"/>
</node>
<node pkg="nodelet" type="nodelet" name="obstacles_detection" args="load rtabmap_ros/obstacles_detection obstacle_nodelet_manager">
<remap from="cloud" to="cloudXYZ"/>
<remap from="obstacles" to="/obstacles_cloud"/>
<remap from="ground" to="/ground_cloud"/>
<param name="frame_id" type="string" value="base_footprint"/>
<param name="frame_id" type="string" value="base_footprint"/>
<param name="map_frame_id" type="string" value="map"/>
<param name="wait_for_transform" type="bool" value="true"/>
<param name="min_cluster_size" type="int" value="20"/>
<param name="max_obstacles_height" type="double" value="0.4"/>
<param name="ground_normal_angle" type="double" value="0.1"/>
</node>
<!-- scan from the camera -->
<node pkg="nodelet" type="nodelet" name="depthimage_to_laserscan" args="load depthimage_to_laserscan/DepthImageToLaserScanNodelet camera_nodelet_manager">
<remap from="image" to="depth_registered/image_raw"/>
<remap from="camera_info" to="depth_registered/camera_info"/>
<remap from="scan" to="/kinect_scan"/>
<param name="range_max" type="double" value="4"/>
</node>
</node>
</group>
</launch>
launch/azimut3/az3_mapping_robot_stereo_nav.launch
+19 -18
View File
@@ -1,3 +1,4 @@
<?xml version="1.0"?>
<launch>
<!-- "Disable" wheel odometry from azimut3 -->
@@ -9,7 +10,7 @@
<!-- AZIMUT 3 bringup: launch motors and TF -->
<include file="$(find az3_bringup)/az3_standalone.launch"/>
<node name="joy" pkg="joy" type="joy_node"/>
<group ns="teleop">
<remap from="joy" to="/joy"/>
@@ -23,7 +24,7 @@
<remap from="base_scan" to="/base_scan"/>
<remap from="map" to="/rtabmap/proj_map"/>
<remap from="move_base_simple/goal" to="/planner_goal"/>
<node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
<rosparam file="$(find rtabmap_ros)/launch/azimut3/config/costmap_common_params.yaml" command="load" ns="global_costmap" />
<rosparam file="$(find rtabmap_ros)/launch/azimut3/config/costmap_common_params.yaml" command="load" ns="local_costmap" />
@@ -31,7 +32,7 @@
<rosparam file="$(find rtabmap_ros)/launch/azimut3/config/global_costmap_params.yaml" command="load" ns="global_costmap"/>
<rosparam file="$(find rtabmap_ros)/launch/azimut3/config/base_local_planner_params.yaml" command="load" />
</node>
<param name="cmd_vel/abtr_priority" value="10"/>
</group>
@@ -44,7 +45,7 @@
args="/cmd_eta /teleop/cmd_eta"/>
<node name="register_cmd_vel" pkg="abtr_priority" type="register"
args="/cmd_vel /planner/cmd_vel"/>
<!-- Stereo camera -->
<node pkg="camera1394stereo" type="camera1394stereo_node" name="camera1394stereo_node" output="screen" >
<param name="video_mode" value="format7_mode3" />
@@ -55,15 +56,15 @@
<param name="camera_info_url_left" value="" />
<param name="camera_info_url_right" value="" />
</node>
<!-- TF transforms for the stereo camera -->
<arg name="pi/2" value="1.5707963267948966" />
<arg name="optical_rotate" value="0 0 0 -$(arg pi/2) 0 -$(arg pi/2)" />
<node pkg="tf" type="static_transform_publisher" name="stereo_camera_base_link"
args="$(arg optical_rotate) stereo_camera_base stereo_camera 100" />
args="$(arg optical_rotate) stereo_camera_base stereo_camera 100" />
<node pkg="tf" type="static_transform_publisher" name="base_to_stereo_camera_base_link"
args="0.01 0.06 0.90 0 0.37 0 base_link stereo_camera_base 100" />
args="0.01 0.06 0.90 0 0.37 0 base_link stereo_camera_base 100" />
<!-- Run the ROS package stereo_image_proc for image rectification-->
<group ns="/stereo_camera" >
<node pkg="nodelet" type="nodelet" name="stereo_nodelet" args="manager"/>
@@ -74,11 +75,11 @@
<remap from="right/image" to="right/image_raw"/>
<remap from="left/camera_info" to="left/camera_info"/>
<remap from="right/camera_info" to="right/camera_info"/>
<param name="queue_size" type="int" value="10"/>
<param name="rate" type="double" value="20"/>
</node>
<node pkg="stereo_image_proc" type="stereo_image_proc" name="stereo_image_proc">
<remap from="left/image_raw" to="left/image_raw_throttle"/>
<remap from="left/camera_info" to="left/camera_info_throttle"/>
@@ -86,13 +87,13 @@
<remap from="right/camera_info" to="right/camera_info_throttle"/>
<param name="disparity_range" value="128"/>
</node>
<!-- Create point cloud for the planner -->
<node pkg="nodelet" type="nodelet" name="disparity2cloud" args="load rtabmap_ros/point_cloud_xyz stereo_nodelet">
<remap from="disparity/image" to="disparity"/>
<remap from="disparity/camera_info" to="right/camera_info_throttle"/>
<remap from="cloud" to="cloudXYZ"/>
<param name="voxel_size" type="double" value="0.05"/>
<param name="decimation" type="int" value="4"/>
<param name="max_depth" type="double" value="4"/>
@@ -101,14 +102,14 @@
<remap from="cloud" to="cloudXYZ"/>
<remap from="obstacles" to="/planner_cloud"/>
<param name="frame_id" type="string" value="base_footprint"/>
<param name="frame_id" type="string" value="base_footprint"/>
<param name="map_frame_id" type="string" value="map"/>
<param name="wait_for_transform" type="bool" value="true"/>
<param name="min_cluster_size" type="int" value="20"/>
<param name="max_obstacles_height" type="double" value="0.0"/>
</node>
</group>
<!-- Visual Odometry -->
<node pkg="rtabmap_ros" type="stereo_odometry" name="stereo_odometry" output="screen">
<remap from="left/image_rect" to="/stereo_camera/left/image_rect"/>
@@ -124,12 +125,12 @@
<param name="Odom/MinInliers" type="string" value="10"/>
<param name="Odom/RoiRatios" type="string" value="0.03 0.03 0.04 0.04"/>
<param name="Odom/MaxDepth" type="string" value="10"/>
<param name="GFTT/MaxCorners" type="string" value="500"/>
<param name="GFTT/MinDistance" type="string" value="5"/>
</node>
<group ns="rtabmap">
<group ns="rtabmap">
<!-- Visual SLAM: args: "delete_db_on_start" and "udebug" -->
<node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="--delete_db_on_start">
<param name="frame_id" type="string" value="base_footprint"/>
@@ -148,7 +149,7 @@
<!-- RTAB-Map's parameters -->
<param name="Rtabmap/TimeThr" type="string" value="700"/>
<param name="Rtabmap/DetectionRate" type="string" value="1"/>
<param name="Kp/WordsPerImage" type="string" value="200"/>
<param name="Kp/RoiRatios" type="string" value="0.03 0.03 0.04 0.04"/>
@@ -162,5 +163,5 @@
<param name="LccReextract/MaxWords" type="string" value="500"/>
</node>
</group>
</launch>
launch/azimut3/az3_nav.launch
+22 -21
View File
@@ -1,3 +1,4 @@
<?xml version="1.0"?>
<launch>
@@ -30,24 +31,24 @@
<remap from="depth/image" to="/camera/depth_registered/image_raw"/>
<remap from="rgb/camera_info" to="/camera/depth_registered/camera_info"/>
<remap from="goal_out" to="current_goal"/>
<remap from="goal_out" to="current_goal"/>
<remap from="move_base" to="/planner/move_base"/>
<remap from="grid_map" to="/map"/>
<!-- RTAB-Map's parameters -->
<param name="RGBD/NeighborLinkRefining" type="string" value="true"/>
<param name="RGBD/NeighborLinkRefining" type="string" value="true"/>
<param name="RGBD/ProximityBySpace" type="string" value="true"/>
<param name="Reg/Strategy" type="string" value="1"/>
<param name="RGBD/AngularUpdate" type="string" value="0.1"/>
<param name="RGBD/LinearUpdate" type="string" value="0.1"/>
<param name="Reg/Strategy" type="string" value="1"/>
<param name="RGBD/AngularUpdate" type="string" value="0.1"/>
<param name="RGBD/LinearUpdate" type="string" value="0.1"/>
<param name="RGBD/LocalRadius" type="string" value="5"/>
<param name="Mem/RehearsalSimilarity" type="string" value="0.45"/>
<param name="Mem/NotLinkedNodesKept" type="string" value="false"/>
<param name="Mem/ImagePostDecimation" type="string" value="4"/>
<param name="Rtabmap/StartNewMapOnLoopClosure" type="string" value="false"/>
<param name="Rtabmap/TimeThr" type="string" value="600"/>
<param name="Rtabmap/DetectionRate" type="string" value="1"/>
@@ -56,16 +57,16 @@
<param name="Optimizer/Slam2D" type="string" value="true"/>
<param name="RGBD/OptimizeFromGraphEnd" type="string" value="true"/>
<param name="Optimizer/Strategy" type="string" value="0"/>
<param name="Optimizer/Strategy" type="string" value="0"/>
<param name="Kp/DetectorStrategy" type="string" value="0"/>
<param name="Kp/MaxFeatures" type="string" value="200"/>
<param name="SURF/HessianThreshold" type="string" value="500"/>
<param name="Reg/Force3DoF" type="string" value="true"/>
<param name="Vis/MaxDepth" type="string" value="5"/>
<param name="Vis/MinInliers" type="string" value="5"/>
<param name="Icp/CorrespondenceRatio" type="string" value="0.3"/>
<param name="Icp/CorrespondenceRatio" type="string" value="0.3"/>
<!-- localization mode -->
<param if="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="false"/>
@@ -73,7 +74,7 @@
<param name="Mem/InitWMWithAllNodes" type="string" value="$(arg localization)"/>
</node>
</group>
<!-- teleop -->
<node name="joy" pkg="joy" type="joy_node"/>
<group ns="teleop">
@@ -97,7 +98,7 @@
<rosparam file="$(find rtabmap_ros)/launch/azimut3/config/global_costmap_params.yaml" command="load" ns="global_costmap"/>
<rosparam file="$(find rtabmap_ros)/launch/azimut3/config/base_local_planner_params.yaml" command="load" />
</node>
<param name="cmd_vel/abtr_priority" value="10"/>
</group>
@@ -110,17 +111,17 @@
args="/cmd_eta /teleop/cmd_eta"/>
<node name="register_cmd_vel" pkg="abtr_priority" type="register"
args="/cmd_vel /planner/cmd_vel"/>
<!-- Throttling messages -->
<group ns="camera">
<node pkg="nodelet" type="nodelet" name="data_throttle" args="load rtabmap_ros/data_throttle camera_nodelet_manager">
<param name="rate" type="double" value="5"/>
<param name="decimation" type="int" value="2"/>
<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_resized_image"/>
<remap from="depth/image_out" to="data_resized_image_depth"/>
<remap from="rgb/camera_info_out" to="data_resized_camera_info"/>
@@ -135,17 +136,17 @@
<param name="max_depth" type="double" value="3.0"/>
<param name="voxel_size" type="double" value="0.02"/>
</node>
<node pkg="nodelet" type="nodelet" name="obstacles_detection" args="load rtabmap_ros/obstacles_detection camera_nodelet_manager">
<remap from="cloud" to="cloudXYZ"/>
<remap from="obstacles" to="/obstacles_cloud"/>
<remap from="ground" to="/ground_cloud"/>
<param name="frame_id" type="string" value="base_footprint"/>
<param name="frame_id" type="string" value="base_footprint"/>
<param name="map_frame_id" type="string" value="map"/>
<param name="wait_for_transform" type="bool" value="true"/>
<param name="min_cluster_size" type="int" value="20"/>
<param name="max_obstacles_height" type="double" value="0.4"/>
</node>
</node>
</group>
</launch>
launch/azimut3/az3_nav_client.launch
+8 -7
View File
@@ -1,3 +1,4 @@
<?xml version="1.0"?>
<launch>
@@ -5,7 +6,7 @@
<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"/>
@@ -13,10 +14,10 @@
<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"/>
@@ -24,24 +25,24 @@
<remap from="cloud" to="/voxel_cloud" />
</node>
<group ns="rtabmap">
<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"/>
</launch>
launch/azimut3/az3_nav_kinect-only.launch
+5 -4
View File
@@ -1,11 +1,12 @@
<?xml version="1.0"?>
<launch>
<!-- 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"/>
<arg unless="$(arg localization)" name="rtabmap_args" default="--delete_db_on_start"/>
<!-- "Disable" odometry from azimut3 -->
<group ns="base_controller">
<param name="odom_frame_id" type="string" value="az3_odom"/>
@@ -29,8 +30,8 @@
<param name="Vis/MinInliers" type="string" value="10"/>
<param name="Vis/InlierDistance" type="string" value="0.1"/>
<param name="Vis/MaxDepth" type="string" value="4"/>
<param name="Reg/Force3DoF" type="string" value="true"/>
<param name="Vis/MaxDepth" type="string" value="4"/>
<param name="Reg/Force3DoF" type="string" value="true"/>
<param name="frame_id" type="string" value="base_footprint"/>
</node>
launch/azimut3/az3_nav_kinect_odom.launch
+24 -23
View File
@@ -1,10 +1,11 @@
<?xml version="1.0"?>
<launch>
<!-- 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"/>
<arg unless="$(arg localization)" name="rtabmap_args" default="--delete_db_on_start"/>
<!-- AZIMUT 3 bringup: launch motors/odometry, laser scan and openni -->
<include file="$(find az3_bringup)/az3_standalone.launch"/>
@@ -30,24 +31,24 @@
<remap from="depth/image" to="/camera/depth_registered/image_raw"/>
<remap from="rgb/camera_info" to="/camera/depth_registered/camera_info"/>
<remap from="goal_out" to="current_goal"/>
<remap from="goal_out" to="current_goal"/>
<remap from="move_base" to="/planner/move_base"/>
<remap from="proj_map" to="/map"/>
<!-- RTAB-Map's parameters -->
<param name="RGBD/NeighborLinkRefining" type="string" value="false"/>
<param name="RGBD/NeighborLinkRefining" type="string" value="false"/>
<param name="RGBD/ProximityBySpace" type="string" value="true"/>
<param name="Reg/Strategy" type="string" value="0"/>
<param name="RGBD/AngularUpdate" type="string" value="0.1"/>
<param name="RGBD/LinearUpdate" type="string" value="0.1"/>
<param name="Reg/Strategy" type="string" value="0"/>
<param name="RGBD/AngularUpdate" type="string" value="0.1"/>
<param name="RGBD/LinearUpdate" type="string" value="0.1"/>
<param name="RGBD/LocalRadius" type="string" value="5"/>
<param name="Mem/RehearsalSimilarity" type="string" value="0.45"/>
<param name="Mem/NotLinkedNodesKept" type="string" value="false"/>
<param name="Mem/ImageDecimation" type="string" value="1"/>
<param name="Rtabmap/StartNewMapOnLoopClosure" type="string" value="false"/>
<param name="Rtabmap/TimeThr" type="string" value="600"/>
<param name="Rtabmap/DetectionRate" type="string" value="1"/>
@@ -63,7 +64,7 @@
<param name="Optimizer/Slam2D" type="string" value="true"/>
<param name="Optimizer/Iterations" type="string" value="100"/>
<param name="RGBD/OptimizeFromGraphEnd" type="string" value="false"/>
<param name="Optimizer/Strategy" type="string" value="1"/>
<param name="Optimizer/Strategy" type="string" value="1"/>
<param name="Optimizer/Robust" type="string" value="false"/>
<param name="Optimizer/VarianceIgnored" type="string" value="true"/>
<param name="RGBD/PlanStuckIterations" type="string" value="10"/>
@@ -72,14 +73,14 @@
<param name="Kp/MaxFeatures" type="string" value="300"/>
<param name="SURF/HessianThreshold" type="string" value="500"/>
<!-- 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)"/>
<param name="Mem/InitWMWithAllNodes" type="string" value="$(arg localization)"/>
</node>
</group>
<!-- teleop -->
<node name="joy" pkg="joy" type="joy_node"/>
<group ns="teleop">
@@ -95,8 +96,8 @@
<remap from="ground_cloud" to="/ground_cloud"/>
<remap from="map" to="/map"/>
<remap from="move_base_simple/goal" to="/planner_goal"/>
<arg name="observation_sources" value="point_cloud_sensorA point_cloud_sensorB"/>
<arg name="observation_sources" value="point_cloud_sensorA point_cloud_sensorB"/>
<node pkg="move_base" type="move_base" respawn="true" name="move_base" output="screen">
<param name="base_global_planner" value="navfn/NavfnROS"/>
<rosparam file="$(find rtabmap_ros)/launch/azimut3/config/costmap_common_params_2d.yaml" command="load" ns="global_costmap" />
@@ -107,7 +108,7 @@
<param name="global_costmap/obstacle_layer/observation_sources" value="$(arg observation_sources)"/>
<param name="local_costmap/obstacle_layer/observation_sources" value="$(arg observation_sources)"/>
</node>
<param name="cmd_vel/abtr_priority" value="10"/>
</group>
@@ -120,17 +121,17 @@
args="/cmd_eta /teleop/cmd_eta"/>
<node name="register_cmd_vel" pkg="abtr_priority" type="register"
args="/cmd_vel /planner/cmd_vel"/>
<!-- Throttling messages -->
<group ns="camera">
<node pkg="nodelet" type="nodelet" name="data_throttle" args="load rtabmap_ros/data_throttle camera_nodelet_manager">
<param name="rate" type="double" value="5"/>
<param name="decimation" type="int" value="2"/>
<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_resized_image"/>
<remap from="depth/image_out" to="data_resized_image_depth"/>
<remap from="rgb/camera_info_out" to="data_resized_camera_info"/>
@@ -145,17 +146,17 @@
<param name="max_depth" type="double" value="3.0"/>
<param name="voxel_size" type="double" value="0.02"/>
</node>
<node pkg="nodelet" type="nodelet" name="obstacles_detection" args="load rtabmap_ros/obstacles_detection camera_nodelet_manager">
<remap from="cloud" to="cloudXYZ"/>
<remap from="obstacles" to="/obstacles_cloud"/>
<remap from="ground" to="/ground_cloud"/>
<param name="frame_id" type="string" value="base_footprint"/>
<param name="frame_id" type="string" value="base_footprint"/>
<param name="map_frame_id" type="string" value="map"/>
<param name="wait_for_transform" type="bool" value="true"/>
<param name="min_cluster_size" type="int" value="20"/>
<param name="max_obstacles_height" type="double" value="0.4"/>
</node>
</node>
</group>
</launch>
launch/azimut3/az3_openni.launch
+2 -1
View File
@@ -1,3 +1,4 @@
<?xml version="1.0"?>
<launch>
@@ -14,5 +15,5 @@
<!-- Xtion frame -->
<node pkg="tf" type="static_transform_publisher" name="base_to_camera_tf"
args="0.057 0.087 0.185 0.0 0.0 0.0 /base_link /camera_link 100" />
</launch>
launch/azimut3/az3_remote_find_object.launch
+2 -1
View File
@@ -1,10 +1,11 @@
<?xml version="1.0"?>
<launch>
<node name="find_object_3d" pkg="find_object_2d" type="find_object_2d" output="screen">
<param name="gui" value="true" type="bool"/>
<param name="settings_path" value="$(find rtabmap_ros)/launch/azimut3/config/azimut3_find_object.ini" type="str"/>
<param name="subscribe_depth" value="true" type="bool"/>
<param name="objects_path" value="$(find rtabmap_ros)/launch/data/books" type="str"/>
<remap from="rgb/image_rect_color" to="camera/data_throttled_image_relay"/>
<remap from="depth_registered/image_raw" to="camera/data_throttled_image_depth_relay"/>
<remap from="depth_registered/camera_info" to="camera/data_throttled_camera_info_relay"/>
launch/azimut3/az3_remote_mapping_robot.launch
+6 -5
View File
@@ -1,10 +1,11 @@
<?xml version="1.0"?>
<launch>
<!-- AZIMUT 3 bringup: launch motors/odometry, laser scan and openni -->
<include file="$(find az3_bringup)/az3_standalone.launch"/>
<!-- <include file="$(find az3_bringup)/joystick.launch"/> -->
<!-- OpenNI -->
<include file="$(find rtabmap_ros)/launch/azimut3/az3_openni.launch"/>
@@ -12,16 +13,16 @@
<group ns="camera">
<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="5.0"/>
<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"/>
</node>
</group>
</node>
</group>
<!-- SLAM is done on client side...-->
</launch>
launch/azimut3/az3_remote_mapping_rviz.launch
+13 -12
View File
@@ -1,9 +1,10 @@
<?xml version="1.0"?>
<launch>
<!-- Remote teleop -->
<include file="$(find az3_bringup)/joystick.launch"/>
<include file="$(find az3_bringup)/joystick.launch"/>
<!-- Visualization and SLAM nodes use same data, so just subscribe once and relay messages -->
<node name="mapData_relay" type="relay" pkg="topic_tools" args="/rtabmap/mapData /rtabmap/mapData_relay"/>
<node name="odom_relay" type="relay" pkg="topic_tools" args="/base_controller/odom /base_controller/odom_relay"/>
@@ -11,25 +12,25 @@
<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" />
<!-- SLAM client side -->
<!-- args: "delete_db_on_start" and "udebug" -->
<group ns="rtabmap">
<node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="--delete_db_on_start">
<param name="frame_id" type="string" value="base_footprint"/>
<param name="subscribe_depth" type="bool" value="true"/>
<param name="subscribe_laserScan" type="bool" value="true"/>
<remap from="odom" to="/base_controller/odom_relay"/>
<remap from="scan" to="/base_scan_relay"/>
<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"/>
<param name="queue_size" type="int" value="10"/>
<!-- RTAB-Map's parameters: do "rosrun rtabmap rtabmap (double-dash)params" to see the list of available parameters. -->
<param name="RGBD/ScanMatchingSize" type="string" value="1"/> <!-- Do odometry correction with consecutive laser scans -->
<param name="RGBD/LocalLoopDetectionSpace" type="string" value="true"/> <!-- Local loop closure detection (using estimated position) with locations in WM -->
@@ -45,15 +46,15 @@
<param name="RGBD/LinearUpdate" type="string" value="0.01"/> <!-- Update map only if the robot is moving -->
<param name="Rtabmap/TimeThr" type="string" value="700"/>
<param name="Mem/RehearsalSimilarity" type="string" value="0.45"/>
<param name="Mem/RehearsedNodesKept" type="string" value="false"/>
<param name="Mem/RehearsedNodesKept" type="string" value="false"/>
</node>
<!-- Grid map assembler for rviz -->
<node pkg="rtabmap_ros" type="grid_map_assembler" name="grid_map_assembler" output="screen"/>
</group>
<node pkg="rviz" type="rviz" name="rviz" args="-d $(find rtabmap_ros)/launch/azimut3/config/azimut3.rviz"/>
<!-- Below, construct point cloud of the latest throttled data -->
<node pkg="nodelet" type="nodelet" name="standalone_nodelet" args="manager" output="screen"/>
<node pkg="nodelet" type="nodelet" name="points_xyzrgb" args="load rtabmap_ros/point_cloud_xyzrgb standalone_nodelet">
@@ -61,7 +62,7 @@
<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/azimut3/config/base_local_planner_params.yaml
+3 -3
View File
@@ -5,7 +5,7 @@ TrajectoryPlannerROS:
acc_lim_y: 0.75
acc_lim_theta: 4
# min_vel_x and max_rotational_vel were set to keep the ICR at
# minimal distance of 0.48 m.
# minimal distance of 0.48 m.
# Basically, max_rotational_vel * rho_min <= min_vel_x
max_vel_x: 0.5
min_vel_x: 0.24
@@ -17,7 +17,7 @@ TrajectoryPlannerROS:
xy_goal_tolerance: 0.25
yaw_goal_tolerance: 0.25
latch_xy_goal_tolerance: true
# make sure that the minimum velocity multiplied by the sim_period is less than twice the tolerance on a goal. Otherwise, the robot will prefer to rotate in place just outside of range of its target position rather than moving towards the goal.
sim_time: 1.5 # set between 1 and 2. The higher he value, the smoother the path (though more samples would be required).
sim_granularity: 0.025
@@ -35,7 +35,7 @@ TrajectoryPlannerROS:
#move_base
controller_frequency: 10.0 #The robot can move faster when higher.
#global planner
#global planner
NavfnROS:
allow_unknown: true
visualize_potential: false
launch/azimut3/config/local_costmap_params_2d.yaml
+9 -9
View File
@@ -18,23 +18,23 @@ obstacle_layer:
raytrace_range: 3.0
max_obstacle_height: 0.4
track_unknown_space: true
observation_sources: laser_scan_sensor point_cloud_sensorA point_cloud_sensorB
laser_scan_sensor: {
data_type: LaserScan,
topic: base_scan,
expected_update_rate: 0.2,
marking: true,
data_type: LaserScan,
topic: base_scan,
expected_update_rate: 0.2,
marking: true,
clearing: true
}
point_cloud_sensorA: {
sensor_frame: base_footprint,
data_type: PointCloud2,
topic: obstacles_cloud,
expected_update_rate: 0.5,
marking: true,
data_type: PointCloud2,
topic: obstacles_cloud,
expected_update_rate: 0.5,
marking: true,
clearing: true,
min_obstacle_height: 0.04
}
launch/azimut3/config/local_costmap_params_3d.yaml
+8 -8
View File
@@ -15,19 +15,19 @@ local_costmap:
observation_sources: point_cloud_sensor
laser_scan_sensor: {
data_type: LaserScan,
topic: base_scan,
expected_update_rate: 0.2,
marking: true,
data_type: LaserScan,
topic: base_scan,
expected_update_rate: 0.2,
marking: true,
clearing: true}
# assuming receiving a cloud from rtabmap/obstacles_detection node
point_cloud_sensor: {
sensor_frame: base_footprint,
data_type: PointCloud2,
topic: openni_points,
expected_update_rate: 0.5,
marking: true,
data_type: PointCloud2,
topic: openni_points,
expected_update_rate: 0.5,
marking: true,
clearing: true,
min_obstacle_height: -99999.0,
max_obstacle_height: 0.5}
launch/calibration/kinect2/README.txt
+1 -1
View File
@@ -7,6 +7,6 @@ The number is the Kinect serial shown when launching kinect2_brige:
[Freenect2Impl] 12 usb devices connected
[Freenect2Impl] found valid Kinect v2 @4:3 with serial 506816242542
[Freenect2Impl] found 1 devices
Kinect2 devices found:
Kinect2 devices found:
0: 506816242542 (selected)
...
launch/config/catvehicle_triclops.gazebo
+10 -10
View File
@@ -7,25 +7,25 @@ Hannah Grace Mason, Joe Macinnes, Landon Chase Bentley
Copyright (c) 2015-2018 Arizona Board of Regents
All rights reserved.
Permission is hereby granted, without written agreement and without
Permission is hereby granted, without written agreement and without
license or royalty fees, to use, copy, modify, and distribute this
software and its documentation for any purpose, provided that the
above copyright notice and the following two paragraphs appear in
software and its documentation for any purpose, provided that the
above copyright notice and the following two paragraphs appear in
all copies of this software.
IN NO EVENT SHALL THE ARIZONA BOARD OF REGENTS BE LIABLE TO ANY PARTY
FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES
ARISING OUT OF THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN
IF THE ARIZONA BOARD OF REGENTS HAS BEEN ADVISED OF THE POSSIBILITY OF
IN NO EVENT SHALL THE ARIZONA BOARD OF REGENTS BE LIABLE TO ANY PARTY
FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES
ARISING OUT OF THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN
IF THE ARIZONA BOARD OF REGENTS HAS BEEN ADVISED OF THE POSSIBILITY OF
SUCH DAMAGE.
THE ARIZONA BOARD OF REGENTS SPECIFICALLY DISCLAIMS ANY WARRANTIES,
INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY
THE ARIZONA BOARD OF REGENTS SPECIFICALLY DISCLAIMS ANY WARRANTIES,
INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY
AND FITNESS FOR A PARTICULAR PURPOSE. THE SOFTWARE PROVIDED HEREUNDER
IS ON AN "AS IS" BASIS, AND THE ARIZONA BOARD OF REGENTS HAS NO OBLIGATION
TO PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS.
Summary:
Summary:
This file includes gazebo reference for front stereo camera simulation mounted
on the top of the car. For more information and for the tutorials used to create
this file, see
launch/config/catvehicle_velodyne_points.gazebo
+10 -10
View File
@@ -5,25 +5,25 @@ Author: Jonathan Sprinkle, Sam Taylor, Alex Warren
Copyright (c) 2015 Arizona Board of Regents
All rights reserved.
Permission is hereby granted, without written agreement and without
Permission is hereby granted, without written agreement and without
license or royalty fees, to use, copy, modify, and distribute this
software and its documentation for any purpose, provided that the
above copyright notice and the following two paragraphs appear in
software and its documentation for any purpose, provided that the
above copyright notice and the following two paragraphs appear in
all copies of this software.
IN NO EVENT SHALL THE ARIZONA BOARD OF REGENTS BE LIABLE TO ANY PARTY
FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES
ARISING OUT OF THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN
IF THE ARIZONA BOARD OF REGENTS HAS BEEN ADVISED OF THE POSSIBILITY OF
IN NO EVENT SHALL THE ARIZONA BOARD OF REGENTS BE LIABLE TO ANY PARTY
FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES
ARISING OUT OF THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN
IF THE ARIZONA BOARD OF REGENTS HAS BEEN ADVISED OF THE POSSIBILITY OF
SUCH DAMAGE.
THE ARIZONA BOARD OF REGENTS SPECIFICALLY DISCLAIMS ANY WARRANTIES,
INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY
THE ARIZONA BOARD OF REGENTS SPECIFICALLY DISCLAIMS ANY WARRANTIES,
INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY
AND FITNESS FOR A PARTICULAR PURPOSE. THE SOFTWARE PROVIDED HEREUNDER
IS ON AN "AS IS" BASIS, AND THE ARIZONA BOARD OF REGENTS HAS NO OBLIGATION
TO PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS.
Summary:
Summary:
This file includes the control interfaces for ROS-based control
through Gazebo. For more information and for the tutorials used to create
this file, see
launch/config/husky_velodyne_extra.urdf.xacro
+1 -1
View File
@@ -5,5 +5,5 @@
<xacro:VLP-16 parent="base_laser_mount" name="velodyne" topic="velodyne_points" gpu="true" organize_cloud="true">
<origin xyz="0.025 0 0.175" rpy="0 0 0" />
</xacro:VLP-16>
</robot>
launch/data_recorder.launch
+17 -16
View File
@@ -1,3 +1,4 @@
<?xml version="1.0"?>
<launch>
<!-- arguments -->
@@ -9,17 +10,17 @@
<arg name="subscribe_scan_cloud" default="false"/>
<arg if="$(arg subscribe_stereo)" name="approx_sync" default="false"/>
<arg unless="$(arg subscribe_stereo)" name="approx_sync" default="true"/>
<arg name="frame_id" default="camera_link"/>
<arg name="odom_frame_id" default=""/> <!-- use topic when not set, otherwise use TF if set -->
<arg name="ground_truth_frame_id" default=""/> <!-- e.g., "world" -->
<arg name="ground_truth_base_frame_id" default=""/> <!-- e.g., "tracker", a fake frame matching the frame "frame_id" (but on different TF tree) -->
<arg name="output_path" default="output.db"/>
<arg name="record_in_RAM" default="false"/>
<arg name="queue_size" default="10"/>
<arg name="max_rate" default="0"/> <!-- Record as fast as possible -->
<arg name="max_rate" default="0"/> <!-- Record as fast as possible -->
<arg name="rgb_topic" default="camera/rgb/image_rect_color"/>
<arg name="rgb_info_topic" default="camera/rgb/camera_info"/>
<arg name="depth_topic" default="camera/depth_registered/image_raw"/>
@@ -31,27 +32,27 @@
<arg name="odom_topic" default="odom"/>
<arg name="scan_topic" default="scan"/>
<arg name="scan_cloud_topic" default="scan_cloud"/>
<arg name="rgb_image_transport" default="raw"/>
<arg name="depth_image_transport" default="raw"/>
<node name="data_recorder" pkg="rtabmap_ros" type="rtabmap" output="screen" args="--delete_db_on_start">
<!-- Disable any processing -->
<param name="Mem/RehearsalSimilarity" type="string" value="1.0"/> <!-- deactivate rehearsal -->
<param name="Kp/MaxFeatures" type="string" value="-1"/> <!-- deactivate keypoints extraction -->
<param name="Rtabmap/MaxRetrieved" type="string" value="0"/> <!-- deactivate global retrieval -->
<param name="RGBD/MaxLocalRetrieved" type="string" value="0"/> <!-- deactivate local retrieval -->
<param name="Mem/MapLabelsAdded" type="string" value="false"/> <!-- don't create map labels -->
<param name="Rtabmap/MemoryThr" type="string" value="2"/> <!-- keep the WM empty -->
<param name="Mem/STMSize" type="string" value="1"/> <!-- STM=1 -->
<param name="Mem/MapLabelsAdded" type="string" value="false"/> <!-- don't create map labels -->
<param name="Rtabmap/MemoryThr" type="string" value="2"/> <!-- keep the WM empty -->
<param name="Mem/STMSize" type="string" value="1"/> <!-- STM=1 -->
<param name="publish_tf" type="bool" value="false"/> <!-- don't publish TF -->
<param name="RGBD/ProximityBySpace" type="string" value="false"/>
<param name="RGBD/LinearUpdate" type="string" value="0"/>
<param name="RGBD/ProximityBySpace" type="string" value="false"/>
<param name="RGBD/LinearUpdate" type="string" value="0"/>
<param name="RGBD/AngularUpdate" type="string" value="0"/>
<param name="RGBD/CreateOccupancyGrid" type="string" value="false"/>
<param name="RGBD/CreateOccupancyGrid" type="string" value="false"/>
<param unless="$(arg subscribe_odometry)" name="RGBD/Enabled" type="string" value="false"/>
<param name="Rtabmap/DetectionRate" type="string" value="$(arg max_rate)"/>
<param name="DbSqlite3/InMemory" type="string" value="$(arg record_in_RAM)"/>
<param name="database_path" type="string" value="$(arg output_path)"/>
@@ -64,16 +65,16 @@
<param name="subscribe_rgbd" type="bool" value="$(arg subscribe_rgbd)"/>
<param name="queue_size" type="int" value="$(arg queue_size)"/>
<param name="approx_sync" type="bool" value="$(arg approx_sync)"/>
<!-- Hack to use a fake odom_frame_id=frame_id if subscribe_odometry = false -->
<param if="$(arg subscribe_odometry)" name="odom_frame_id" type="string" value="$(arg odom_frame_id)"/>
<param unless="$(arg subscribe_odometry)" name="odom_frame_id" type="string" value="$(arg frame_id)"/>
<param name="rgb/image_transport" type="string" value="$(arg rgb_image_transport)"/>
<param name="depth/image_transport" type="string" value="$(arg depth_image_transport)"/>
<param name="left/image_transport" type="string" value="$(arg rgb_image_transport)"/>
<param name="right/image_transport" type="string" value="$(arg rgb_image_transport)"/>
<remap from="rgb/image" to="$(arg rgb_topic)"/>
<remap from="rgb/camera_info" to="$(arg rgb_info_topic)"/>
<remap from="depth/image" to="$(arg depth_topic)"/>
launch/demo/demo_appearance_mapping.launch
+12 -11
View File
@@ -1,5 +1,6 @@
<?xml version="1.0"?>
<launch>
<!-- APPEARANCE-BASED LOOP CLOSURE DETECTION VERSION -->
<!-- WARNING : Database is automatically deleted on each startup -->
<!-- See "delete_db_on_start" option below... -->
@@ -12,14 +13,14 @@
<group ns="rtabmap">
<!-- args: "delete_db_on_start" and "udebug" -->
<node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="$(arg rtabmap_args)">
<param name="subscribe_rgb" type="bool" value="false"/> <!-- must be false for appearance-based mode -->
<param name="subscribe_depth" type="bool" value="false"/> <!-- must be false for appearance-based mode -->
<param name="subscribe_odom" type="bool" value="false"/> <!-- must be false for appearance-based mode -->
<param name="queue_size" type="int" value="10"/>
<remap from="rgb/image" to="/image"/> <!-- connect to "image" topic of the camera below -->
<!-- RTAB-Map's parameters: do "rosrun rtabmap rtabmap (double-dash)params" to see the list of available parameters. -->
<param name="RGBD/Enabled" type="string" value="false"/> <!-- False: appearance-based -->
<param name="Rtabmap/ImageBufferSize" type="string" value="0"/> <!-- process all images -->
@@ -31,28 +32,28 @@
<param name="Mem/UseOdomFeatures" type="string" value="false"/>
<param name="Kp/DetectorStrategy" type="string" value="0"/> <!-- use SURF -->
<param name="SURF/HessianThreshold" type="string" value="100"/>
<!-- 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>
<!-- visualization of the "infoEx" topic sent by rtabmap node -->
<node name="rtabmapviz" pkg="rtabmap_ros" type="rtabmapviz" output="screen" args="-d $(find rtabmap_ros)/launch/config/appearance_gui.ini">
<param name="subscribe_odom" type="bool" value="false"/> <!-- must be false for appearance-based mode -->
<!-- This enables the GUI to pause a rtabmap_ros/camera when action "pause" is checked. -->
<!-- NOTE: It is specific to rtabmap_ros/camera. Action "pause" in the GUI will still pause the rtabmap node. -->
<param name="camera_node_name" type="string" value="/camera"/>
<param name="camera_node_name" type="string" value="/camera"/>
</node>
</group>
<!-- When parameter video_or_images_path is set, the camera uses the directory of images or the video file -->
<node name="camera" pkg="rtabmap_ros" type="camera" output="screen">
<remap from="image" to="image"/>
<param name="device_id" value="0" type="int"/>
<param name="video_or_images_path" value="$(find rtabmap_ros)/launch/data/demo_appearance" type="string"/>
<param name="frame_rate" value="2.0" type="double"/>
launch/demo/demo_catvehicle_mapping.launch
+15 -14
View File
@@ -1,12 +1,13 @@
<?xml version="1.0"?>
<?xml version="1.0"?>
<launch>
<!-- 1) Make sure rtabmap is built with libpointmatcher for lidar:=true option (lidar SLAM)
2) Install https://github.com/jmscslgroup/catvehicle
We need a stereo camera and velodyne point cloud looking like the real
sensor (ring-like pattern), so we have to modifiy the default catvehicle
urdf. First we have to install velodyne_simulator package to have the
right gazebo plugin and copy this modified velodyne config:
We need a stereo camera and velodyne point cloud looking like the real
sensor (ring-like pattern), so we have to modifiy the default catvehicle
urdf. First we have to install velodyne_simulator package to have the
right gazebo plugin and copy this modified velodyne config:
* sudo apt install ros-$ROS_DISTRO-velodyne-simulator
* cp ~/catkin_ws/src/rtabmap_ros/launch/config/catvehicle_velodyne_points.gazebo ~/catkin_ws/src/catvehicle/urdf/.
Secondly, we have to modify the triclops config to make it stereo:
@@ -18,14 +19,14 @@
4) roslaunch catvehicle catvehicle_spawn.launch velodyne_points:=true triclops:=true Z:=5
5) roslaunch catvehicle joystick.launch
6) SLAM, 3 choices:
A) LiDAR-SLAM + Visual loop closure detection:
A) LiDAR-SLAM + Visual loop closure detection:
roslaunch rtabmap_ros demo_catvehicle_mapping.launch
B) LiDAR-SLAM without camera:
B) LiDAR-SLAM without camera:
roslaunch rtabmap_ros demo_catvehicle_mapping.launch camera:=false
C) Visual-SLAM without lidar:
C) Visual-SLAM without lidar:
roslaunch rtabmap_ros demo_catvehicle_mapping.launch lidar:=false
Note: Gazebo real-time factor should be equal or below 1. If it is over 1,
Note: Gazebo real-time factor should be equal or below 1. If it is over 1,
in gazebo client, select Physics, then change real time update rate to 500.
-->
<arg name="camera" default="true" />
@@ -34,7 +35,7 @@
<arg name="rtabmapviz" default="true" />
<arg name="rviz" default="true" />
<arg name="light" default="false" /> <!-- Don't record all scans if false -->
<arg name="altitude" default="0" /> <!-- assemble occupancy grids by altitude (radius in meters, 0=disabled) -->
<arg name="altitude" default="0" /> <!-- assemble occupancy grids by altitude (radius in meters, 0=disabled) -->
<arg name="localization" default="false" />
<!-- Common parameters -->
@@ -77,7 +78,7 @@
<!-- LiDAR SLAM parameters -->
<arg if="$(arg lidar)" name="args" value="$(arg clear_db) $(arg common_args)
--Reg/Strategy 1"/>
<!-- Visual SLAM parameters -->
<arg unless="$(arg lidar)" name="args" value="$(arg clear_db) $(arg common_args)
--Reg/Strategy 0
@@ -85,7 +86,7 @@
--Grid/MaxGroundHeight 0.7
--Grid/NoiseFilteringRadius 0.5"/>
<arg name="localization" value="$(arg localization)"/>
<arg name="localization" value="$(arg localization)"/>
<arg name="subscribe_scan_cloud" value="$(arg lidar)"/>
<arg name="stereo" value="$(arg camera)"/>
<arg unless="$(arg camera)" name="depth" value="false"/>
@@ -95,13 +96,13 @@
<arg if="$(arg lidar)" name="odom_topic" value="lidar_odom"/>
<arg unless="$(arg lidar)" name="odom_topic" value="visual_odom"/>
<arg name="odom_guess_frame_id" value="catvehicle/odom"/>
<arg name="frame_id" value="catvehicle/base_link"/>
<arg name="map_frame_id" value="catvehicle/map"/>
<arg if="$(arg lidar)" name="vo_frame_id" value="catvehicle/lidar_odom"/>
<arg unless="$(arg lidar)" name="vo_frame_id" value="catvehicle/visual_odom"/>
<arg name="stereo_namespace" value="/catvehicle/triclops"/>
<arg name="right_image_topic" value="/catvehicle/triclops/right/image_rect_color"/>
<arg name="scan_cloud_topic" value="/catvehicle/velodyne_points"/>
@@ -121,4 +122,4 @@
</include>
<node if="$(arg rviz)" pkg="rviz" type="rviz" name="rviz" args="-d $(find rtabmap_ros)/launch/config/catvehicle.rviz"/>
</launch>
</launch>
launch/demo/demo_data_recorder.launch
+7 -6
View File
@@ -1,3 +1,4 @@
<?xml version="1.0"?>
<launch>
@@ -5,26 +6,26 @@
<!-- This demo works with demo_mapping.bag -->
<param name="use_sim_time" type="bool" value="True"/>
<include file="$(find rtabmap_ros)/launch/data_recorder.launch">
<arg name="subscribe_odometry" value="true"/>
<arg name="subscribe_depth" value="true"/>
<arg name="subscribe_stereo" value="false"/>
<arg name="subscribe_scan" value="true"/>
<arg name="frame_id" value="base_footprint"/>
<arg name="odom_frame_id" value=""/> <!-- use topic when not set, otherwise use TF if set -->
<arg name="output_path" value="output.db"/>
<arg name="record_in_RAM" value="false"/>
<arg name="queue_size" value="10"/>
<arg name="max_rate" value="0"/>
<arg name="odom_topic" value="/az3/base_controller/odom"/>
<arg name="scan_topic" value="/jn0/base_scan"/>
</include>
<!-- uncompress image data -->
<!-- uncompress image data -->
<node name="cameraInfo_relay" type="relay" pkg="topic_tools" args="/data_throttled_camera_info /camera/rgb/camera_info"/>
<node name="republish_rgb" type="republish" pkg="image_transport" args="compressed in:=/data_throttled_image raw out:=/camera/rgb/image_rect_color" />
<node name="republish_depth" type="republish" pkg="image_transport" args="compressedDepth in:=/data_throttled_image_depth raw out:=/camera/depth_registered/image_raw" />
launch/demo/demo_find_object.launch
+23 -22
View File
@@ -1,9 +1,10 @@
<?xml version="1.0"?>
<launch>
<!-- Choose visualization -->
<arg name="rviz" default="true" />
<arg name="rtabmapviz" default="false" />
<arg name="rtabmapviz" default="false" />
<arg name="save_objects" default="false"/>
<arg name="localization" default="false"/>
<arg name="save_objects_as_landmarks" default="false"/> <!-- apriltag_ros package should be installed and rtabmap_ros built with it -->
@@ -11,16 +12,16 @@
<arg unless="$(arg localization)" name="rtabmap_args" default="--delete_db_on_start"/>
<param name="use_sim_time" type="bool" value="True"/>
<!-- SLAM (robot side) -->
<!-- args: "delete_db_on_start" and "udebug" -->
<group ns="rtabmap">
<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="subscribe_depth" type="bool" value="true"/>
<param name="subscribe_scan" type="bool" value="true"/>
<remap from="odom" to="/base_controller/odom"/>
<remap from="scan" to="/base_scan"/>
<remap if="$(arg save_objects)" from="user_data_async" to="/objectsData"/>
@@ -28,14 +29,14 @@
<remap from="rgb/image" to="/camera/data_throttled_image"/>
<remap from="depth/image" to="/camera/data_throttled_image_depth"/>
<remap from="rgb/camera_info" to="/camera/data_throttled_camera_info"/>
<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="landmark_linear_variance" type="double" value="0.1"/>
<param name="landmark_angular_variance" type="double" value="0.5"/>
<!-- 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"/> <!-- Proximity detection (using estimated position) with locations in WM -->
@@ -53,45 +54,45 @@
<param name="Mem/RehearsalSimilarity" type="string" value="0.45"/>
<param name="Mem/NotLinkedNodesKept" type="string" value="false"/>
<param name="Optimizer/Slam2D" type="string" value="true"/>
<param name="Reg/Force3DoF" type="string" value="true"/>
<param name="Reg/Force3DoF" type="string" value="true"/>
<param if="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="false"/>
</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"/>
<remap from="rgb/image" to="/camera/data_throttled_image"/>
<remap from="depth/image" to="/camera/data_throttled_image_depth"/>
<remap from="rgb/camera_info" to="/camera/data_throttled_camera_info"/>
<remap from="scan" to="/base_scan"/>
<remap from="odom" to="/base_controller/odom"/>
<param name="rgb/image_transport" type="string" value="compressed"/>
<param name="depth/image_transport" type="string" value="compressedDepth"/>
</node>
</group>
</group>
<!-- send AZIMUT 3 urdf to param server -->
<!--
<!--
<param name="robot_description" command="$(find xacro)/xacro.py '$(find az3_description)/robots/azimut_3_laser.urdf.xacro'" />
-->
<!-- Visualisation -->
<node if="$(arg rviz)" pkg="rviz" type="rviz" name="rviz" args="-d $(find rtabmap_ros)/launch/config/demo_find_object.rviz" output="screen"/>
<node pkg="nodelet" type="nodelet" name="points_xyzrgb" args="standalone rtabmap_ros/point_cloud_xyzrgb">
<remap from="rgb/image" to="/camera/data_throttled_image"/>
<remap from="depth/image" to="/camera/data_throttled_image_depth"/>
<remap from="rgb/camera_info" to="/camera/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>
@@ -102,7 +103,7 @@
<param name="settings_path" value="$(find rtabmap_ros)/launch/config/find_object.ini" type="str"/>
<param name="subscribe_depth" value="true" type="bool"/>
<param name="objects_path" value="$(find rtabmap_ros)/launch/data/books" type="str"/>
<remap from="rgb/image_rect_color" to="/camera/data_throttled_image"/>
<remap from="depth_registered/image_raw" to="/camera/data_throttled_image_depth"/>
<remap from="depth_registered/camera_info" to="/camera/data_throttled_camera_info"/>
@@ -110,13 +111,13 @@
<param name="rgb/image_transport" type="string" value="compressed"/>
<param name="depth_registered/image_transport" type="string" value="compressedDepth"/>
</node>
<!-- Save objects to database example -->
<node if="$(arg save_objects)" name="save_objects_example" pkg="rtabmap_ros" type="save_objects_example" output="screen">
<remap from="mapData" to="/rtabmap/mapData"/>
<param name="frame_id" value="base_footprint"/>
</node>
<!-- Convert objects to tags -->
<node if="$(arg save_objects_as_landmarks)" name="objects_to_tags" pkg="rtabmap_ros" type="objects_to_tags.py" output="screen">
<remap from="tag_detections" to="/rtabmap/tag_detections"/>
launch/demo/demo_hector_mapping.launch
+23 -22
View File
@@ -1,3 +1,4 @@
<?xml version="1.0"?>
<!-- -->
<launch>
@@ -6,11 +7,11 @@
<!-- rosbag filter demo_mapping.bag demo_mapping_no_odom.bag 'topic != "/tf" or topic == "/tf" and m.transforms[0].header.frame_id != "/odom"' -->
<!-- WARNING : Database is automatically deleted on each startup -->
<!-- See "delete_db_on_start" option below... -->
<!-- Choose visualization -->
<arg name="rviz" default="true" />
<arg name="rtabmapviz" default="false" />
<!-- Choose hector_slam or icp_odometry for odometry -->
<arg name="hector" default="true" />
@@ -21,19 +22,19 @@
<!-- Example with camera or not -->
<arg name="camera" default="true" />
<!-- Limit lidar range if > 0 (has effect only when hector:=false) -->
<arg name="max_range" default="0" />
<!-- Point to Plane ICP? (has effect only when hector:=false) -->
<arg name="p2n" default="true" />
<!-- Use libpointmatcher for ICP? (has effect only when hector:=false) -->
<arg name="pm" default="true" />
<param name="use_sim_time" type="bool" value="True"/>
<node if="$(arg hector)" pkg="tf" type="static_transform_publisher" name="scanmatcher_to_base_footprint"
<node if="$(arg hector)" pkg="tf" type="static_transform_publisher" name="scanmatcher_to_base_footprint"
args="0.0 0.0 0.0 0.0 0.0 0.0 /scanmatcher_frame /base_footprint 100" />
<!-- Odometry from laser scans -->
@@ -44,36 +45,36 @@
<param name="map_frame" value="hector_map" />
<param name="base_frame" value="base_footprint" />
<param name="odom_frame" value="odom" />
<!-- Tf use -->
<param name="pub_map_odom_transform" value="false"/>
<param name="pub_map_scanmatch_transform" value="true"/>
<param name="pub_odometry" value="true"/>
<!-- Map size / start point -->
<param name="map_resolution" value="0.050"/>
<param name="map_size" value="2048"/>
<param name="map_multi_res_levels" value="2" />
<!-- Map update parameters -->
<param name="map_update_angle_thresh" value="0.06" />
<!-- Advertising config -->
<!-- Advertising config -->
<param name="scan_topic" value="/jn0/base_scan"/>
</node>
<!-- If argument "hector" is false, we use rtabmap's icp odometry to generate odometry for us -->
<node unless="$(arg hector)" pkg="rtabmap_ros" type="icp_odometry" name="icp_odometry" output="screen" >
<remap from="scan" to="/jn0/base_scan"/>
<remap from="odom" to="/scanmatch_odom"/>
<remap from="odom_info" to="/rtabmap/odom_info"/>
<param name="frame_id" type="string" value="base_footprint"/>
<param name="deskewing" type="string" value="true"/>
<param if="$(arg odom_guess)" name="odom_frame_id" type="string" value="icp_odom"/>
<param if="$(arg odom_guess)" name="guess_frame_id" type="string" value="odom"/>
<param name="Icp/VoxelSize" type="string" value="0.05"/>
<param name="Icp/RangeMax" type="string" value="$(arg max_range)"/>
<param name="Icp/Epsilon" type="string" value="0.001"/>
@@ -106,12 +107,12 @@
<!-- args: "delete_db_on_start" and "udebug" -->
<node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="--delete_db_on_start">
<param name="frame_id" type="string" value="base_footprint"/>
<param name="subscribe_rgb" type="bool" value="false"/>
<param name="subscribe_depth" type="bool" value="false"/>
<param name="subscribe_rgbd" type="bool" value="$(arg camera)"/>
<param name="subscribe_scan" type="bool" value="true"/>
<remap from="scan" to="/jn0/base_scan"/>
<!-- As hector doesn't provide compatible covariance in the odometry topic, don't use the topic and fix the covariance -->
@@ -121,7 +122,7 @@
<remap unless="$(arg hector)" from="odom" to="/scanmatch_odom"/>
<param unless="$(arg hector)" name="subscribe_odom_info" type="bool" value="true"/>
<!-- RTAB-Map's parameters -->
<param name="Reg/Strategy" type="string" value="1"/> <!-- 0=Visual, 1=ICP, 2=Visual+ICP -->
<param name="Reg/Force3DoF" type="string" value="true"/>
@@ -131,13 +132,13 @@
<param name="Icp/RangeMax" type="string" value="$(arg max_range)"/>
<param name="Grid/RangeMax" type="string" value="$(arg max_range)"/>
</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_rgbd" type="bool" value="$(arg camera)"/>
<param name="subscribe_laserScan" type="bool" value="true"/>
<param name="frame_id" type="string" value="base_footprint"/>
<remap from="scan" to="/jn0/base_scan"/>
<!-- As hector doesn't provide compatible covariance in the odometry topic -->
@@ -146,9 +147,9 @@
<remap unless="$(arg hector)" from="odom" to="/scanmatch_odom"/>
<param unless="$(arg hector)" name="subscribe_odom_info" type="bool" value="true"/>
</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 if="$(arg camera)" pkg="nodelet" type="nodelet" name="points_xyzrgb" args="standalone rtabmap_ros/point_cloud_xyzrgb">
launch/demo/demo_husky.launch
+47 -46
View File
@@ -1,51 +1,52 @@
<?xml version="1.0"?>
<!-- -->
<launch>
<!-- Bringup the Husky with SICK (2D LiDAR), realsense camera (RGB-D camera) and velodyne (3D LiDAR):
$ export HUSKY_URDF_EXTRAS=$(rospack find rtabmap_ros)/launch/config/husky_velodyne_extra.urdf.xacro
$ roslaunch husky_gazebo husky_playpen.launch realsense_enabled:=true
$ roslaunch husky_viz view_robot.launch
$ roslaunch husky_viz view_robot.launch
For ICP odometry examples, rtabmap should be built with libpointmatcher.
Examples:
1) 6DoF mapping with 3D LiDAR
$ roslaunch rtabmap_ros demo_husky.launch lidar3d:=true slam2d:=false
2) 6DoF mapping with 3D LiDAR and RGB-D camera
$ roslaunch rtabmap_ros demo_husky.launch lidar3d:=true slam2d:=false camera:=true
3) 6DoF mapping with 3D LiDAR and RGB-D camera and ICP odometry (with wheel odometry as guess)
$ roslaunch rtabmap_ros demo_husky.launch lidar3d:=true slam2d:=false camera:=true icp_odometry:=true
4) 3DoF mapping with 3D LiDAR
$ roslaunch rtabmap_ros demo_husky.launch lidar3d:=true slam2d:=true
5) 3DoF mapping with 3D LiDAR and RGB-D camera
$ roslaunch rtabmap_ros demo_husky.launch lidar3d:=true slam2d:=true camera:=true
6) 3DoF mapping with 3D LiDAR and RGB-D camera and ICP odometry (with wheel odometry as guess)
$ roslaunch rtabmap_ros demo_husky.launch lidar3d:=true slam2d:=true camera:=true icp_odometry:=true
7) 3DoF mapping with 2D LiDAR
$ roslaunch rtabmap_ros demo_husky.launch lidar2d:=true slam2d:=true
8) 3DoF mapping with 2D LiDAR and RGB-D camera
$ roslaunch rtabmap_ros demo_husky.launch lidar2d:=true slam2d:=true camera:=true
9) 3DoF mapping with 2D LiDAR and RGB-D camera and ICP odometry (with wheel odometry as guess)
$ roslaunch rtabmap_ros demo_husky.launch lidar2d:=true slam2d:=true camera:=true icp_odometry:=true
10) 6DoF mapping with 3D LiDAR and RGB camera (depth generated by lidar projection) and ICP odometry (with wheel odometry as guess)
$ roslaunch rtabmap_ros demo_husky.launch lidar3d:=true slam2d:=false camera:=true icp_odometry:=true depth_from_lidar:=true rtabmapviz:=true
11) 3DoF mapping with 3D LiDAR and RGB camera (depth generated by lidar projection) and ICP odometry (with wheel odometry as guess)
$ roslaunch rtabmap_ros demo_husky.launch lidar3d:=true slam2d:=true camera:=true icp_odometry:=true depth_from_lidar:=true rtabmapviz:=true
Issues:
When setting icp_odometry:=true with navigation, sending a goal to move_base could cause errors like:
"Extrapolation Error: Lookup would require extrapolation into the future. Requested
time 1340.520000000 but the latest data is at time 1340.500000000, when looking up
"Extrapolation Error: Lookup would require extrapolation into the future. Requested
time 1340.520000000 but the latest data is at time 1340.500000000, when looking up
transform from frame [odom] to frame [map]"
To fix this change "global_frame: odom" to "global_frame: icp_odom" in "husky_navigation/config/costmap_local.yaml"
-->
@@ -60,36 +61,36 @@
<arg name="lidar3d_ray_tracing" default="true"/>
<arg name="slam2d" default="true"/>
<arg name="depth_from_lidar" default="false"/>
<arg if="$(arg lidar3d)" name="cell_size" default="0.3"/>
<arg unless="$(arg lidar3d)" name="cell_size" default="0.05"/>
<arg if="$(arg lidar2d)" name="lidar_args" default="
--Reg/Strategy 1
--RGBD/NeighborLinkRefining true
--Grid/CellSize $(arg cell_size)
--Icp/PointToPlaneRadius 0
--Reg/Strategy 1
--RGBD/NeighborLinkRefining true
--Grid/CellSize $(arg cell_size)
--Icp/PointToPlaneRadius 0
--Icp/MaxTranslation 1"/>
<arg if="$(arg lidar3d)" name="lidar_args" default="
--Reg/Strategy 1
--RGBD/NeighborLinkRefining true
--ICP/PM true
--Icp/PMOutlierRatio 0.7
--Icp/VoxelSize $(arg cell_size)
--Icp/MaxCorrespondenceDistance 1
--Icp/PointToPlaneGroundNormalsUp 0.9
--Icp/Iterations 10
--Icp/Epsilon 0.001
--OdomF2M/ScanSubtractRadius $(arg cell_size)
--OdomF2M/ScanMaxSize 15000
--Grid/ClusterRadius 1
--Grid/RangeMax 20
--Grid/RayTracing $(arg lidar3d_ray_tracing)
--Grid/CellSize $(arg cell_size)
--Icp/PointToPlaneRadius 0
--Icp/PointToPlaneNormalK 10
--Reg/Strategy 1
--RGBD/NeighborLinkRefining true
--ICP/PM true
--Icp/PMOutlierRatio 0.7
--Icp/VoxelSize $(arg cell_size)
--Icp/MaxCorrespondenceDistance 1
--Icp/PointToPlaneGroundNormalsUp 0.9
--Icp/Iterations 10
--Icp/Epsilon 0.001
--OdomF2M/ScanSubtractRadius $(arg cell_size)
--OdomF2M/ScanMaxSize 15000
--Grid/ClusterRadius 1
--Grid/RangeMax 20
--Grid/RayTracing $(arg lidar3d_ray_tracing)
--Grid/CellSize $(arg cell_size)
--Icp/PointToPlaneRadius 0
--Icp/PointToPlaneNormalK 10
--Icp/MaxTranslation 1"/>
<!--- Run rtabmap -->
@@ -105,17 +106,17 @@
<arg name="frame_id" value="base_link" />
<arg name="rtabmapviz" value="$(arg rtabmapviz)" />
<arg name="gps_topic" value="/navsat/fix"/>
<!-- 2D LiDAR -->
<arg name="subscribe_scan" value="$(arg lidar2d)" />
<arg if="$(arg lidar2d)" name="scan_topic" value="/scan" />
<arg unless="$(arg lidar2d)" name="scan_topic" value="/scan_not_used" />
<!-- 3D LiDAR -->
<arg name="subscribe_scan_cloud" value="$(arg lidar3d)" />
<arg if="$(arg lidar3d)" name="scan_cloud_topic" value="/velodyne_points" />
<arg unless="$(arg lidar3d)" name="scan_cloud_topic" value="/scan_cloud_not_used" />
<!-- If camera is used -->
<arg name="depth" value="$(eval camera and not depth_from_lidar)" />
<arg name="subscribe_rgb" value="$(eval camera)" />
@@ -124,14 +125,14 @@
<arg name="camera_info_topic" value="/realsense/color/camera_info" />
<arg name="depth_topic" value="/realsense/depth/image_rect_raw" />
<arg name="approx_rgbd_sync" value="false" />
<!-- If depth generated from lidar projection (in case we have only a single RGB camera with a 3D lidar) -->
<arg name="gen_depth" value="$(arg depth_from_lidar)" />
<arg name="gen_depth_decimation" value="4" />
<arg name="gen_depth_fill_holes_size" value="3" />
<arg name="gen_depth_fill_iterations" value="1" />
<arg name="gen_depth_fill_holes_error" value="0.3" />
<!-- If icp_odometry is used -->
<arg if="$(arg icp_odometry)" name="icp_odometry" value="true" />
<arg if="$(arg icp_odometry)" name="odom_guess_frame_id" value="odom" />
launch/demo/demo_isaac_carter_navigation.launch
+7 -6
View File
@@ -1,3 +1,4 @@
<?xml version="1.0"?>
<!-- -->
<launch>
<!-- For ref: https://docs.omniverse.nvidia.com/app_isaacsim/app_isaacsim/tutorial_ros_navigation.html#isaac-sim-app-tutorial-ros-navigation
@@ -18,7 +19,7 @@
-> verticalFov = 32
-> verticalResolution = 2
-> ROS_lidar -> pointCloudEnabled = True
Isaac Sim 2022 (Action Graph):
To enable all camera streams (right, depth left and depth right):
1. Stage view -> World-> Carter_ROS, right-click on ROS_Cameras -> Open graph
@@ -74,23 +75,23 @@
<arg name="use_sim_time" value="true"/>
<arg name="tag_linear_variance" value="0.1"/>
<arg name="tag_angular_variance" value="0.2"/>
<arg name="scan_cloud_assembling" value="$(arg lidar3d)"/>
<arg name="scan_cloud_assembling_fixed_frame" value="odom"/>
<arg name="scan_cloud_assembling_range_max" value="60"/>
<arg name="scan_cloud_assembling_voxel_size" value="$(arg cell_size)"/>
</include>
<node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
<param name="TrajectoryPlannerROS/max_vel_theta" value="0.3"/>
<param name="TrajectoryPlannerROS/min_vel_theta" value="-0.3"/>
<rosparam file="$(find carter_2dnav)/params/costmap_common_params.yaml" command="load" ns="global_costmap" />
<rosparam file="$(find carter_2dnav)/params/costmap_common_params.yaml" command="load" ns="global_costmap" />
<rosparam file="$(find carter_2dnav)/params/costmap_common_params.yaml" command="load" ns="local_costmap" />
<rosparam file="$(find carter_2dnav)/params/local_costmap_params.yaml" command="load" />
<rosparam file="$(find carter_2dnav)/params/global_costmap_params.yaml" command="load" />
<rosparam file="$(find carter_2dnav)/params/global_costmap_params.yaml" command="load" />
<rosparam file="$(find carter_2dnav)/params/base_local_planner_params.yaml" command="load" />
</node>
<node type="rviz" name="rviz" pkg="rviz" args="-d $(find carter_2dnav)/rviz/carter_2dnav.rviz" />
</launch>
</launch>
launch/demo/demo_multi-session_mapping.launch
+20 -19
View File
@@ -1,8 +1,9 @@
<?xml version="1.0"?>
<launch>
<!-- MULTI-SESSION MAPPING VERSION -->
<!-- Choose visualization -->
<arg name="rviz" default="false" />
<arg name="rtabmapviz" default="true" />
@@ -11,32 +12,32 @@
<param name="use_sim_time" type="bool" value="True"/>
<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="subscribe_depth" type="bool" value="true"/>
<param name="subscribe_scan" type="bool" value="true"/>
<remap from="odom" to="/base_controller/odom"/>
<remap from="scan" to="/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"/>
<param name="queue_size" type="int" value="10"/>
<!-- 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="false"/>
<param name="RGBD/NeighborLinkRefining" type="string" value="false"/>
<param name="RGBD/ProximityBySpace" type="string" value="false"/> <!-- Referred paper did only global loop closure detection -->
<param name="RGBD/ProximityByTime" type="string" value="false"/>
<param name="RGBD/OptimizeFromGraphEnd" type="string" value="true"/>
<param name="RGBD/ProximityByTime" type="string" value="false"/>
<param name="RGBD/OptimizeFromGraphEnd" type="string" value="true"/>
<param name="Reg/Strategy" type="string" value="1"/>
<param name="Icp/Iterations" type="string" value="30"/>
<param name="Icp/VoxelSize" type="string" value="0"/>
@@ -50,33 +51,33 @@
<param name="Bayes/FullPredictionUpdate" type="string" value="true"/>
<param name="Kp/DetectorStrategy" type="string" value="0"/> <!-- use SURF -->
<param name="Kp/MaxFeatures" type="string" value="400"/>
<param name="Reg/Force3DoF" type="string" value="true"/>
<param name="Reg/Force3DoF" type="string" value="true"/>
<param name="RGBD/OptimizeMaxError" type="string" value="0.25"/>
<param name="Optimizer/Strategy" type="string" value="0"/> <!-- TORO is the most stable for multi-session mapping -->
<param name="Optimizer/Iterations" type="string" value="100"/>
<param name="Optimizer/Iterations" type="string" value="100"/>
<param name="Kp/IncrementalFlann" type="string" value="false"/> <!-- Referred paper didn't use incremental FLANN -->
<param name="Grid/FromDepth" type="string" value="false"/>
</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="queue_size" type="int" value="10"/>
<param name="frame_id" type="string" value="base_footprint"/>
<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="/base_scan"/>
<remap from="odom" to="/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"/>
<node pkg="nodelet" type="nodelet" name="points_xyzrgb" args="standalone rtabmap_ros/point_cloud_xyzrgb">
@@ -84,10 +85,10 @@
<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/demo/demo_robot_mapping.launch
+15 -14
View File
@@ -1,3 +1,4 @@
<?xml version="1.0"?>
<launch>
@@ -7,10 +8,10 @@
<!-- Choose visualization -->
<arg name="rviz" default="false" />
<arg name="rtabmapviz" default="true" />
<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=""/>
@@ -22,10 +23,10 @@
<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"/>
@@ -36,10 +37,10 @@
<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 -->
@@ -58,31 +59,31 @@
<param name="Icp/PointToPlane" type="string" value="false"/>
<param name="Icp/MaxCorrespondenceDistance" type="string" value="0.15"/>
<param name="Icp/VoxelSize" type="string" value="0.05"/>
<!-- 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">
@@ -90,10 +91,10 @@
<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/demo/demo_stereo_outdoor.launch
+28 -27
View File
@@ -1,27 +1,28 @@
<?xml version="1.0"?>
<launch>
<!--
Demo of outdoor stereo mapping.
From bag:
$ rosbag record
/stereo_camera/left/image_raw_throttle/compressed
/stereo_camera/right/image_raw_throttle/compressed
/stereo_camera/left/camera_info_throttle
/stereo_camera/right/camera_info_throttle
Demo of outdoor stereo mapping.
From bag:
$ rosbag record
/stereo_camera/left/image_raw_throttle/compressed
/stereo_camera/right/image_raw_throttle/compressed
/stereo_camera/left/camera_info_throttle
/stereo_camera/right/camera_info_throttle
/tf
$ roslaunch rtabmap demo_stereo_outdoor.launch
$ rosbag play -.-clock stereo_oudoorA.bag
-->
<!-- Choose visualization -->
<arg name="rviz" default="true" />
<arg name="rtabmapviz" default="false" />
<arg name="local_bundle" default="true" />
<arg name="stereo_sync" default="false" />
<param name="use_sim_time" type="bool" value="True"/>
<!-- Just to uncompress images for stereo_image_rect -->
<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" />
@@ -35,7 +36,7 @@
<remap from="right/camera_info" to="right/camera_info_throttle"/>
<param name="disparity_range" value="128"/>
</node>
<node if="$(arg stereo_sync)" pkg="nodelet" type="nodelet" name="stereo_sync" args="standalone rtabmap_ros/stereo_sync">
<remap from="left/image_rect" to="left/image_rect_color"/>
<remap from="right/image_rect" to="right/image_rect"/>
@@ -43,10 +44,10 @@
<remap from="right/camera_info" to="right/camera_info_throttle"/>
</node>
</group>
<group ns="rtabmap">
<!-- Stereo Odometry -->
<group ns="rtabmap">
<!-- Stereo Odometry -->
<node pkg="rtabmap_ros" type="stereo_odometry" name="stereo_odometry" output="screen">
<remap from="left/image_rect" to="/stereo_camera/left/image_rect"/>
<remap from="right/image_rect" to="/stereo_camera/right/image_rect"/>
@@ -65,12 +66,12 @@
<param name="Odom/GuessMotion" type="string" value="true"/>
<param name="Vis/MinInliers" type="string" value="10"/>
<param unless="$(arg local_bundle)" name="OdomF2M/BundleAdjustment" type="string" value="0"/>
<param name="OdomF2M/MaxSize" type="string" value="1000"/>
<param name="OdomF2M/MaxSize" type="string" value="1000"/>
<param name="GFTT/MinDistance" type="string" value="10"/>
<param name="GFTT/QualityLevel" type="string" value="0.00001"/>
<param name="GFTT/QualityLevel" type="string" value="0.00001"/>
<param name="GFTT/QualityLevel" type="string" value="0.00001"/>
</node>
<!-- Visual SLAM: args: "delete_db_on_start" and "udebug" -->
<node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="--delete_db_on_start">
<param name="frame_id" type="string" value="base_footprint"/>
@@ -94,12 +95,12 @@
<param name="Grid/DepthDecimation" type="string" value="4"/>
<param name="Grid/FlatObstacleDetected" type="string" value="true"/>
<param name="Kp/MaxDepth" type="string" value="0"/>
<param name="Kp/DetectorStrategy" type="string" value="6"/>
<param name="Kp/DetectorStrategy" type="string" value="6"/>
<param name="Vis/EstimationType" type="string" value="1"/> <!-- 0=3D->3D, 1=3D->2D (PnP) -->
<param name="Vis/MaxDepth" type="string" value="0"/>
<param name="RGBD/CreateOccupancyGrid" type="string" value="true"/>
<param name="RGBD/CreateOccupancyGrid" type="string" value="true"/>
</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 unless="$(arg stereo_sync)" name="subscribe_stereo" type="bool" value="true"/>
@@ -107,7 +108,7 @@
<param name="subscribe_rgbd" type="bool" value="$(arg stereo_sync)"/>
<param name="queue_size" type="int" value="10"/>
<param name="frame_id" type="string" value="base_footprint"/>
<remap from="left/image_rect" to="/stereo_camera/left/image_rect_color"/>
<remap from="right/image_rect" to="/stereo_camera/right/image_rect"/>
<remap from="left/camera_info" to="/stereo_camera/left/camera_info_throttle"/>
@@ -117,10 +118,10 @@
<remap from="odom" to="/stereo_odometry"/>
<remap from="mapData" to="mapData"/>
</node>
</group>
<!-- Visualisation RVIZ -->
<!-- Visualisation RVIZ -->
<node if="$(arg rviz)" pkg="rviz" type="rviz" name="rviz" args="-d $(find rtabmap_ros)/launch/config/demo_stereo_outdoor.rviz"/>
</launch>
launch/demo/demo_stereo_pr2.launch
+15 -14
View File
@@ -1,7 +1,8 @@
<?xml version="1.0"?>
<launch>
<!-- 6DOF stereo demo: download a bag example from http://projects.csail.mit.edu/stata/downloads.php.
You will need to remove the transform /combined_odometry from the /tf messages:
$ rosbag filter 2011-01-20-07-18-45.bag out.bag 'topic != "/tf" or topic == "/tf" and m.transforms[0].header.frame_id != "/odom_combined"'
$ rosbag filter 2011-01-20-07-18-45.bag out.bag 'topic != "/tf" or topic == "/tf" and m.transforms[0].header.frame_id != "/odom_combined"'
Run the example:
$ roslaunch rtabmap demo_stereo.launch
$ rosbag play -.-clock out.bag (replace -.- by double-dashes)
@@ -24,37 +25,37 @@
<param name="odom_frame_id" value="/odom"/>
<param name="ref_frame_change_method" value="1"/>
</node>
<group ns="rtabmap">
<!-- Visual SLAM (robot side) -->
<!-- args: "delete_db_on_start" and "udebug" -->
<node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="--delete_db_on_start">
<param name="subscribe_depth" type="bool" value="true"/>
<param name="subscribe_laserScan" type="bool" value="true"/>
<remap from="rgb/image" to="/wide_stereo/left/image_rect"/>
<remap from="rgb/camera_info" to="/wide_stereo/left/camera_info"/>
<remap from="depth/image" to="/wide_stereo/depth"/>
<remap from="odom" to="/stereo_odometer/odometry"/>
<remap from="scan" to="/base_scan"/>
<param name="frame_id" type="string" value="/base_footprint"/>
<param name="queue_size" type="int" value="30"/>
<param name="Rtabmap/TimeThr" type="string" value="700"/>
<param name="Rtabmap/DetectionRate" type="string" value="1"/>
<param name="SURF/HessianThreshold" type="string" value="600"/>
<param name="LccBow/MaxDepth" type="string" value="0"/>
<param name="RGBD/LocalLoopDetectionSpace" type="string" value="false"/>
<param name="RGBD/LocalLoopDetectionSpace" type="string" value="false"/>
<param name="RGBD/LocalLoopDetectionTime" type="string" value="false"/>
<param name="LccBow/MinInliers" type="string" value="10"/>
<param name="LccBow/InlierDistance" type="string" value="0.05"/>
<param name="Kp/DetectorStrategy" type="string" value="0"/> <!-- use SURF -->
<param name="Kp/NNStrategy" type="string" value="1"/> <!-- kdTree -->
<!-- Uncomment to force 3dof loop closure constraint using -->
<!-- the 2d scans (set ScanMatchingSize=1 to correct odometry with laser) -->
<!--
@@ -65,20 +66,20 @@
-->
</node>
<!-- Visualisation (client side) -->
<node 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_laserScan" type="bool" value="true"/>
<param name="queue_size" type="int" value="30"/>
<remap from="rgb/image" to="/wide_stereo/left/image_rect"/>
<remap from="rgb/camera_info" to="/wide_stereo/left/camera_info"/>
<remap from="depth/image" to="/wide_stereo/depth"/>
<remap from="scan" to="/base_scan"/>
<remap from="odom" to="/stereo_odometer/odometry"/>
</node>
</group>
</launch>
launch/demo/demo_turtlebot3_navigation.launch
+6 -5
View File
@@ -1,3 +1,4 @@
<?xml version="1.0"?>
<launch>
<!--
$ sudo apt install ros-melodic-turtlebot3* ros-melodic-dwa-local-planner
@@ -36,7 +37,7 @@
<remap from="depth/image" to="/camera/depth/image_raw"/>
<remap from="rgb/camera_info" to="/camera/rgb/camera_info"/>
</node>
<node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="$(arg rtabmap_args)">
<param name="database_path" type="string" value="$(arg database_path)"/>
<param name="frame_id" type="string" value="base_footprint"/>
@@ -47,10 +48,10 @@
<param name="subscribe_scan" type="bool" value="true"/>
<param name="approx_sync" type="bool" value="true"/>
<!-- use actionlib to send goals to move_base -->
<!-- use actionlib to send goals to move_base -->
<param name="use_action_for_goal" type="bool" value="true"/>
<remap from="move_base" to="/move_base"/>
<!-- inputs -->
<remap from="scan" to="/scan"/>
<remap from="odom" to="/odom"/>
@@ -58,7 +59,7 @@
<!-- output -->
<remap from="grid_map" to="/map"/>
<!-- RTAB-Map's parameters -->
<param name="Reg/Strategy" type="string" value="1"/>
<param name="Reg/Force3DoF" type="string" value="true"/>
@@ -88,7 +89,7 @@
</include>
<!-- rviz -->
<group if="$(arg open_rviz)">
<group if="$(arg open_rviz)">
<node pkg="rviz" type="rviz" name="rviz" required="true"
args="-d $(find turtlebot3_navigation)/rviz/turtlebot3_navigation.rviz"/>
</group>
launch/demo/demo_turtlebot_mapping.launch
+29 -28
View File
@@ -1,31 +1,32 @@
<?xml version="1.0"?>
<launch>
<!--
<!--
Bringup Turtlebot:
$ roslaunch turtlebot_bringup minimal.launch
Mapping:
$ roslaunch rtabmap_ros demo_turtlebot_mapping.launch
Visualization:
$ roslaunch rtabmap_ros demo_turtlebot_rviz.launch
This launch file is a one to one replacement of the gmapping_demo.launch in the
This launch file is a one to one replacement of the gmapping_demo.launch in the
"SLAM Map Building with TurtleBot" tutorial:
http://wiki.ros.org/turtlebot_navigation/Tutorials/indigo/Build%20a%20map%20with%20SLAM
For localization-only after a mapping session, add argument "localization:=true" to
demo_turtlebot_mapping.launch line above. Move the robot around until it can relocalize in
the previous map, then the 2D map should re-appear again. You can then follow the same steps
demo_turtlebot_mapping.launch line above. Move the robot around until it can relocalize in
the previous map, then the 2D map should re-appear again. You can then follow the same steps
from 3.3.2 of the "Autonomous Navigation of a Known Map with TurtleBot" tutorial:
http://wiki.ros.org/turtlebot_navigation/Tutorials/Autonomously%20navigate%20in%20a%20known%20map
For turtlebot in simulation (Gazebo):
$ roslaunch turtlebot_gazebo turtlebot_world.launch
$ roslaunch rtabmap_ros demo_turtlebot_mapping.launch simulation:=true
$ roslaunch rtabmap_ros demo_turtlebot_rviz.launch
-->
<arg name="database_path" default="rtabmap.db"/>
<arg name="rgbd_odometry" default="false"/>
<arg name="rtabmapviz" default="false"/>
@@ -40,20 +41,20 @@
<arg if="$(arg simulation)" name="depth_topic" default="/camera/depth/image_raw"/>
<arg unless="$(arg simulation)" name="depth_topic" default="/camera/depth_registered/image_raw"/>
<arg name="camera_info_topic" default="/camera/rgb/camera_info"/>
<arg name="wait_for_transform" default="0.2"/>
<!--
robot_state_publisher's publishing frequency in "turtlebot_bringup/launch/includes/robot.launch.xml"
<arg name="wait_for_transform" default="0.2"/>
<!--
robot_state_publisher's publishing frequency in "turtlebot_bringup/launch/includes/robot.launch.xml"
can be increase from 5 to 10 Hz to avoid some TF warnings.
-->
<!-- Navigation stuff (move_base) -->
<include unless="$(arg simulation)" file="$(find turtlebot_bringup)/launch/3dsensor.launch">
<arg if="$(arg sw_registered)" name="depth_registration" value="false"/>
<arg unless="$(arg sw_registered)" name="depth_registration" value="true"/>
</include>
<include file="$(find turtlebot_navigation)/launch/includes/move_base.launch.xml"/>
<!-- Mapping -->
<group ns="rtabmap">
@@ -65,10 +66,10 @@
<param name="subscribe_scan" type="bool" value="true"/>
<param name="map_negative_poses_ignored" type="bool" value="true"/>
<!-- When sending goals on /rtabmap/goal topic, use actionlib to communicate with move_base -->
<!-- When sending goals on /rtabmap/goal topic, use actionlib to communicate with move_base -->
<param name="use_action_for_goal" type="bool" value="true"/>
<remap from="move_base" to="/move_base"/>
<!-- inputs -->
<remap from="scan" to="/scan"/>
<remap from="rgb/image" to="$(arg rgb_topic)"/>
@@ -79,10 +80,10 @@
<param unless="$(arg rgbd_odometry)" name="odom_frame_id" value="odom"/>
<param unless="$(arg rgbd_odometry)" name="odom_tf_linear_variance" value="0.001"/>
<param unless="$(arg rgbd_odometry)" name="odom_tf_angular_variance" value="0.001"/>
<!-- output -->
<remap from="grid_map" to="/map"/>
<!-- RTAB-Map's parameters: do "rosrun rtabmap rtabmap (double-dash)params" to see the list of available parameters. -->
<param name="RGBD/ProximityBySpace" type="string" value="true"/> <!-- Local loop closure detection (using estimated position) with locations in WM -->
<param name="RGBD/OptimizeFromGraphEnd" type="string" value="false"/> <!-- Set to false to generate map correction between /map and /odom -->
@@ -93,43 +94,43 @@
<param name="Vis/InlierDistance" type="string" value="0.1"/> <!-- 3D visual words correspondence distance -->
<param name="RGBD/AngularUpdate" type="string" value="0.1"/> <!-- Update map only if the robot is moving -->
<param name="RGBD/LinearUpdate" type="string" value="0.1"/> <!-- Update map only if the robot is moving -->
<param name="RGBD/ProximityPathMaxNeighbors" type="string" value="0"/>
<param name="RGBD/ProximityPathMaxNeighbors" type="string" value="0"/>
<param name="Rtabmap/TimeThr" type="string" value="0"/>
<param name="Mem/RehearsalSimilarity" type="string" value="0.30"/>
<param name="Reg/Force3DoF" type="string" value="true"/>
<param name="GridGlobal/MinSize" type="string" value="20"/>
<!-- 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)"/>
<param name="Mem/InitWMWithAllNodes" type="string" value="$(arg localization)"/>
</node>
<!-- Odometry : ONLY for testing without the actual robot! /odom TF should not be already published. -->
<node if="$(arg rgbd_odometry)" pkg="rtabmap_ros" type="rgbd_odometry" name="rgbd_odometry" output="screen">
<param name="frame_id" type="string" value="base_footprint"/>
<param name="wait_for_transform_duration" type="double" value="$(arg wait_for_transform)"/>
<param name="Reg/Force3DoF" type="string" value="true"/>
<param name="Vis/InlierDistance" type="string" value="0.05"/>
<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)"/>
</node>
<!-- visualization with rtabmapviz -->
<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_duration" type="double" value="$(arg wait_for_transform)"/>
<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)"/>
<remap from="scan" to="/scan"/>
</node>
</group>
</launch>
launch/demo/demo_turtlebot_rviz.launch
+1
View File
@@ -1,3 +1,4 @@
<?xml version="1.0"?>
<!--
Used for visualising the turtlebot while building a map or navigating with the ros navistack.
-->
launch/demo/demo_turtlebot_tango.launch
+29 -28
View File
@@ -1,50 +1,51 @@
<?xml version="1.0"?>
<launch>
<!--
<!--
Bringup Turtlebot:
$ roslaunch turtlebot_bringup minimal.launch
Mapping:
$ roslaunch rtabmap_ros demo_turtlebot_tango.launch
Visualization:
$ roslaunch rtabmap_ros demo_turtlebot_rviz.launch
This launch file is a one to one replacement of the gmapping_demo.launch in the
This launch file is a one to one replacement of the gmapping_demo.launch in the
"SLAM Map Building with TurtleBot" tutorial:
http://wiki.ros.org/turtlebot_navigation/Tutorials/indigo/Build%20a%20map%20with%20SLAM
For localization-only after a mapping session, add argument "localization:=true" to
demo_turtlebot_mapping.launch line above. Move the robot around until it can relocalize in
the previous map, then the 2D map should re-appear again. You can then follow the same steps
demo_turtlebot_mapping.launch line above. Move the robot around until it can relocalize in
the previous map, then the 2D map should re-appear again. You can then follow the same steps
from 3.3.2 of the "Autonomous Navigation of a Known Map with TurtleBot" tutorial:
http://wiki.ros.org/turtlebot_navigation/Tutorials/Autonomously%20navigate%20in%20a%20known%20map
-->
<arg name="database_path" default="rtabmap.db"/>
<arg name="tango_odometry" default="true"/>
<arg name="localization" default="false"/>
<arg name="rtabmapviz" default="false"/>
<arg if="$(arg localization)" name="args" default=""/>
<arg unless="$(arg localization)" name="args" default="--delete_db_on_start"/>
<arg name="wait_for_transform" default="0.2"/>
<!--
robot_state_publisher's publishing frequency in "turtlebot_bringup/launch/includes/robot.launch.xml"
<arg name="wait_for_transform" default="0.2"/>
<!--
robot_state_publisher's publishing frequency in "turtlebot_bringup/launch/includes/robot.launch.xml"
can be increase from 5 to 10 Hz to avoid some TF warnings.
-->
<!-- Adjust these transforms on where Tango is fixed on the robot (should be landscape to get laser scans right). When Tango odometry is used, we need to offset /start_of_service to actual camera pose, then set /base_footprint relative to /device 1.5707963267948966 -->
<node if="$(arg tango_odometry)" pkg="tf" type="static_transform_publisher" name="device_base_link" args="-0.3 0 0 0 1.5707963267948966 0 device base_footprint 100" />
<node if="$(arg tango_odometry)" pkg="tf" type="static_transform_publisher" name="odom_start_of_service_link" args="0 0 0.3 0 0 0 odom start_of_service 100" />
<node unless="$(arg tango_odometry)" pkg="tf" type="static_transform_publisher" name="base_device_link" args="0 0 0.3 0 0 0 device base_link 100" />
<node if="$(arg tango_odometry)" pkg="tf" type="static_transform_publisher" name="device_base_link" args="-0.3 0 0 0 1.5707963267948966 0 device base_footprint 100" />
<node if="$(arg tango_odometry)" pkg="tf" type="static_transform_publisher" name="odom_start_of_service_link" args="0 0 0.3 0 0 0 odom start_of_service 100" />
<node unless="$(arg tango_odometry)" pkg="tf" type="static_transform_publisher" name="base_device_link" args="0 0 0.3 0 0 0 device base_link 100" />
<!-- Generate registered depth image -->
<node name="pointcloud_to_depthimage" pkg="rtabmap_ros" type="pointcloud_to_depthimage">
<remap from="cloud" to="/tango/point_cloud"/>
<remap from="image" to="/tango/registered_depth"/>
<remap from="camera_info" to="/tango/camera/color_1/camera_info"/>
<param name="fixed_frame_id" type="string" value="odom"/>
<param name="decimation" type="int" value="8"/>
<param name="fill_holes_size" type="int" value="5"/>
@@ -53,7 +54,7 @@
<!-- Navigation stuff (move_base) -->
<node name="scan_relay" type="relay" pkg="topic_tools" args="/tango/laser_scan /scan"/>
<include file="$(find turtlebot_navigation)/launch/includes/move_base.launch.xml"/>
<!-- relay to avoid subscribing multiple times to rgb image -->
<node name="republish_rgb" type="republish" pkg="image_transport" args="compressed in:=/tango/camera/color_1/image_raw raw out:=/tango/camera/color_1/image_raw/relay" />
@@ -68,16 +69,16 @@
<param name="subscribe_depth" type="bool" value="true"/>
<param name="subscribe_scan" type="bool" value="true"/>
<param name="approx_sync" type="bool" value="true"/>
<!-- inputs -->
<remap from="scan" to="/scan"/>
<remap from="rgb/image" to="/tango/camera/color_1/image_raw/relay"/>
<remap from="depth/image" to="/tango/registered_depth"/>
<remap from="rgb/camera_info" to="/tango/camera/color_1/camera_info"/>
<!-- output -->
<remap from="grid_map" to="/map"/>
<!-- RTAB-Map's parameters: do "rosrun rtabmap rtabmap (double-dash)params" to see the list of available parameters. -->
<param name="RGBD/ProximityBySpace" type="string" value="true"/> <!-- Local loop closure detection (using estimated position) with locations in WM -->
<param name="RGBD/OptimizeFromGraphEnd" type="string" value="false"/> <!-- Set to false to generate map correction between /map and /odom -->
@@ -88,33 +89,33 @@
<param name="Vis/InlierDistance" type="string" value="0.1"/> <!-- 3D visual words correspondence distance -->
<param name="RGBD/AngularUpdate" type="string" value="0.1"/> <!-- Update map only if the robot is moving -->
<param name="RGBD/LinearUpdate" type="string" value="0.1"/> <!-- Update map only if the robot is moving -->
<param name="RGBD/ProximityPathMaxNeighbors" type="string" value="0"/>
<param name="RGBD/ProximityPathMaxNeighbors" type="string" value="0"/>
<param name="Rtabmap/TimeThr" type="string" value="700"/>
<param name="Mem/RehearsalSimilarity" type="string" value="0.30"/>
<param name="Optimizer/Slam2D" type="string" value="false"/> <!-- cannot do 2D slam as Tango is 6DoF -->
<param name="Reg/Force3DoF" type="string" value="false"/> <!-- cannot do 2D slam as Tango is 6DoF -->
<param name="Mem/ImagePreDecimation" type="string" value="2"/>
<param name="Mem/ImagePostDecimation" type="string" value="2"/>
<param name="Mem/ImagePostDecimation" type="string" value="2"/>
<!-- 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)"/>
<param name="Mem/InitWMWithAllNodes" type="string" value="$(arg localization)"/>
</node>
<!-- visualization with rtabmapviz -->
<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_duration" type="double" value="$(arg wait_for_transform)"/>
<remap from="rgb/image" to="/tango/camera/color_1/image_raw/relay"/>
<remap from="depth/image" to="/tango/registered_depth"/>
<remap from="rgb/camera_info" to="/tango/camera/color_1/camera_info"/>
<remap from="scan" to="/scan"/>
</node>
</group>
</launch>
launch/demo/demo_two_kinects.launch
+32 -31
View File
@@ -1,8 +1,9 @@
<?xml version="1.0"?>
<launch>
<!-- Multi-cameras demo with 2 Kinects -->
<!-- Cameras -->
<include file="$(find freenect_launch)/launch/freenect.launch">
<arg name="depth_registration" value="True" />
@@ -20,21 +21,21 @@
args="0.0 0.0 0.0 0.0 0.0 0.0 /base_link /camera1_link 100" />
<node pkg="tf" type="static_transform_publisher" name="base_to_camera2_tf"
args="-0.1325 -0.1975 0.0 -1.570796327 0.0 0.0 /base_link /camera2_link 100" />
<!-- Choose visualization -->
<arg name="rviz" default="false" />
<arg name="rtabmapviz" default="true" />
<!-- ODOMETRY MAIN ARGUMENTS:
<arg name="rtabmapviz" default="true" />
<!-- ODOMETRY MAIN ARGUMENTS:
-"strategy" : Strategy: 0=Frame-to-Map 1=Frame-to-Frame
-"feature" : Feature type: 0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK
-"nn" : Nearest neighbor strategy : 0=Linear, 1=FLANN_KDTREE, 2=FLANN_LSH, 3=BRUTEFORCE
Set to 1 for float descriptor like SIFT/SURF
Set to 3 for binary descriptor like ORB/FREAK/BRIEF/BRISK
-"max_depth" : Maximum features depth (m)
-"min_inliers" : Minimum visual correspondences to accept a transformation (m)
-"inlier_distance" : RANSAC maximum inliers distance (m)
-"local_map" : Local map size: number of unique features to keep track
-"nn" : Nearest neighbor strategy : 0=Linear, 1=FLANN_KDTREE, 2=FLANN_LSH, 3=BRUTEFORCE
Set to 1 for float descriptor like SIFT/SURF
Set to 3 for binary descriptor like ORB/FREAK/BRIEF/BRISK
-"max_depth" : Maximum features depth (m)
-"min_inliers" : Minimum visual correspondences to accept a transformation (m)
-"inlier_distance" : RANSAC maximum inliers distance (m)
-"local_map" : Local map size: number of unique features to keep track
-"odom_info_data" : Fill odometry info messages with inliers/outliers data.
-->
<arg name="strategy" default="0" />
@@ -46,7 +47,7 @@
<arg name="local_map" default="1000" />
<arg name="odom_info_data" default="true" />
<arg name="wait_for_transform" default="true" />
<!-- sync rgb/depth images per camera -->
<group ns="camera1">
<node pkg="nodelet" type="nodelet" name="rgbd_sync" args="load rtabmap_ros/rgbd_sync camera1_nodelet_manager">
@@ -62,31 +63,31 @@
<remap from="rgb/camera_info" to="rgb/camera_info"/>
</node>
</group>
<group ns="rtabmap">
<!-- Odometry -->
<node pkg="rtabmap_ros" type="rgbd_odometry" name="rgbd_odometry" output="screen">
<remap from="rgbd_image0" to="/camera1/rgbd_image"/>
<remap from="rgbd_image1" to="/camera2/rgbd_image"/>
<param name="subscribe_rgbd" type="bool" value="true"/>
<param name="frame_id" type="string" value="base_link"/>
<param name="rgbd_cameras" type="int" value="2"/>
<param name="wait_for_transform" type="bool" value="$(arg wait_for_transform)"/>
<param name="Odom/Strategy" type="string" value="$(arg strategy)"/>
<param name="Odom/Strategy" type="string" value="$(arg strategy)"/>
<param name="OdomF2M/BundleAdjustment" type="string" value="0"/> <!-- should be 0 for multi-cameras -->
<param name="Vis/EstimationType" type="string" value="0"/> <!-- should be 0 for multi-cameras -->
<param name="Vis/FeatureType" type="string" value="$(arg feature)"/>
<param name="Vis/CorGuessWinSize" type="string" value="0"/>
<param name="Vis/FeatureType" type="string" value="$(arg feature)"/>
<param name="Vis/CorGuessWinSize" type="string" value="0"/>
<param name="Vis/CorNNType" type="string" value="$(arg nn)"/>
<param name="Vis/MaxDepth" type="string" value="$(arg max_depth)"/>
<param name="Vis/MinInliers" type="string" value="$(arg min_inliers)"/>
<param name="Vis/InlierDistance" type="string" value="$(arg inlier_distance)"/>
<param name="OdomF2M/MaxSize" type="string" value="$(arg local_map)"/>
<param name="Odom/FillInfoData" type="string" value="$(arg odom_info_data)"/>
<param name="Vis/MaxDepth" type="string" value="$(arg max_depth)"/>
<param name="Vis/MinInliers" type="string" value="$(arg min_inliers)"/>
<param name="Vis/InlierDistance" type="string" value="$(arg inlier_distance)"/>
<param name="OdomF2M/MaxSize" type="string" value="$(arg local_map)"/>
<param name="Odom/FillInfoData" type="string" value="$(arg odom_info_data)"/>
</node>
<!-- Visual SLAM (robot side) -->
<!-- args: "delete_db_on_start" and "udebug" -->
<node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="--delete_db_on_start">
@@ -98,7 +99,7 @@
<param name="wait_for_transform" type="bool" value="$(arg wait_for_transform)"/>
<param name="map_negative_poses_ignored" type="bool" value="false"/> <!-- refresh grid map even if we are not moving-->
<param name="map_negative_scan_empty_ray_tracing" type="bool" value="false"/> <!-- don't fill empty space between the generated scans-->
<remap from="rgbd_image0" to="/camera1/rgbd_image"/>
<remap from="rgbd_image1" to="/camera2/rgbd_image"/>
@@ -107,7 +108,7 @@
<param name="Vis/MinInliers" type="string" value="10"/>
<param name="Vis/InlierDistance" type="string" value="$(arg inlier_distance)"/>
</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="false"/>
@@ -116,13 +117,13 @@
<param name="frame_id" type="string" value="base_link"/>
<param name="rgbd_cameras" type="int" value="2"/>
<param name="wait_for_transform" type="bool" value="$(arg wait_for_transform)"/>
<remap from="rgbd_image0" to="/camera1/rgbd_image"/>
<remap from="rgbd_image1" to="/camera2/rgbd_image"/>
</node>
</group>
<!-- Visualization RVIZ -->
<node if="$(arg rviz)" pkg="rviz" type="rviz" name="rviz" args="-d $(find rtabmap_ros)/launch/config/rgbd.rviz"/>
launch/jfr2018/associate.py
+20 -20
View File
@@ -31,7 +31,7 @@
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
# Requirements:
# Requirements:
# sudo apt-get install python-argparse
"""
@@ -48,31 +48,31 @@ import numpy
def read_file_list(filename):
"""
Reads a trajectory from a text file.
Reads a trajectory from a text file.
File format:
The file format is "stamp d1 d2 d3 ...", where stamp denotes the time stamp (to be matched)
and "d1 d2 d3.." is arbitary data (e.g., a 3D position and 3D orientation) associated to this timestamp.
and "d1 d2 d3.." is arbitary data (e.g., a 3D position and 3D orientation) associated to this timestamp.
Input:
filename -- File name
Output:
dict -- dictionary of (stamp,data) tuples
"""
file = open(filename)
data = file.read()
lines = data.replace(","," ").replace("\t"," ").split("\n")
lines = data.replace(","," ").replace("\t"," ").split("\n")
list = [[v.strip() for v in line.split(" ") if v.strip()!=""] for line in lines if len(line)>0 and line[0]!="#"]
list = [(float(l[0]),l[1:]) for l in list if len(l)>1]
return dict(list)
def associate(first_list, second_list,offset,max_difference):
"""
Associate two dictionaries of (stamp,data). As the time stamps never match exactly, we aim
Associate two dictionaries of (stamp,data). As the time stamps never match exactly, we aim
to find the closest match for every input tuple.
Input:
first_list -- first dictionary of (stamp,data) tuples
second_list -- second dictionary of (stamp,data) tuples
@@ -81,13 +81,13 @@ def associate(first_list, second_list,offset,max_difference):
Output:
matches -- list of matched tuples ((stamp1,data1),(stamp2,data2))
"""
first_keys = first_list.keys()
second_keys = second_list.keys()
potential_matches = [(abs(a - (b + offset)), a, b)
for a in first_keys
for b in second_keys
potential_matches = [(abs(a - (b + offset)), a, b)
for a in first_keys
for b in second_keys
if abs(a - (b + offset)) < max_difference]
potential_matches.sort()
matches = []
@@ -96,15 +96,15 @@ def associate(first_list, second_list,offset,max_difference):
first_keys.remove(a)
second_keys.remove(b)
matches.append((a, b))
matches.sort()
return matches
if __name__ == '__main__':
# parse command line
parser = argparse.ArgumentParser(description='''
This script takes two data files with timestamps and associates them
This script takes two data files with timestamps and associates them
''')
parser.add_argument('first_file', help='first text file (format: timestamp data)')
parser.add_argument('second_file', help='second text file (format: timestamp data)')
@@ -116,7 +116,7 @@ if __name__ == '__main__':
first_list = read_file_list(args.first_file)
second_list = read_file_list(args.second_file)
matches = associate(first_list, second_list,float(args.offset),float(args.max_difference))
matches = associate(first_list, second_list,float(args.offset),float(args.max_difference))
if args.first_only:
for a,b in matches:
@@ -124,5 +124,5 @@ if __name__ == '__main__':
else:
for a,b in matches:
print("%f %s %f %s"%(a," ".join(first_list[a]),b-float(args.offset)," ".join(second_list[b])))
launch/jfr2018/cartographer.launch
+2 -1
View File
@@ -1,3 +1,4 @@
<?xml version="1.0"?>
<!--
Copyright 2016 The Cartographer Authors
@@ -22,7 +23,7 @@
type="cartographer_node" args="
-configuration_directory
$(find cartographer_ros)/configuration_files
-configuration_basename pr2.lua"
-configuration_basename pr2.lua"
output="screen">
<remap from="scan" to="/base_scan_t_filtered" /> <!-- /base_scan_t /base_scan_t_filtered /camera_scan -->
<remap from="odom" to="/odom_combined" />
launch/jfr2018/cartographer_sync_markers_gt.py
+1 -1
View File
@@ -1,4 +1,4 @@
#!/usr/bin/env python
#!/usr/bin/env python
import roslib
import rospy
import os
launch/jfr2018/ethzasl_icp_mapper_sync_gt.py
+2 -2
View File
@@ -1,4 +1,4 @@
#!/usr/bin/env python
#!/usr/bin/env python
import roslib
import rospy
import os
@@ -15,7 +15,7 @@ def callback(data):
global listener
global rmse
global lastTime
if rospy.get_time() - lastTime < 1:
return
lastTime = rospy.get_time()
launch/jfr2018/ethzasl_icp_mapping.patch
+4 -4
View File
@@ -9,17 +9,17 @@ index 67820e7..aec1839 100644
+ maxDist: 1
knn: 5
epsilon: 3.16
outlierFilters:
- TrimmedDistOutlierFilter:
- ratio: 0.85
+ ratio: 0.95
- SurfaceNormalOutlierFilter:
maxAngle: 0.42
@@ -25,7 +25,11 @@ transformationCheckers:
maxTranslationNorm: 5.00
inspector:
-# VTKFileInspector
+# VTKFileInspector:
@@ -28,7 +28,7 @@ index 67820e7..aec1839 100644
+# dumpReading : 1
+# dumpReference : 1
NullInspector
logger:
diff --git a/libpointmatcher_ros/src/point_cloud.cpp b/libpointmatcher_ros/src/point_cloud.cpp
index b77651d..8eb1f6c 100644
launch/jfr2018/evaluate_ate.py
+23 -23
View File
@@ -31,7 +31,7 @@
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
# Requirements:
# Requirements:
# sudo apt-get install python-argparse
"""
@@ -46,21 +46,21 @@ import associate
def align(model,data):
"""Align two trajectories using the method of Horn (closed-form).
Input:
model -- first trajectory (3xn)
data -- second trajectory (3xn)
Output:
rot -- rotation matrix (3x3)
trans -- translation vector (3x1)
trans_error -- translational error per point (1xn)
"""
numpy.set_printoptions(precision=3,suppress=True)
model_zerocentered = model - model.mean(1)
data_zerocentered = data - data.mean(1)
W = numpy.zeros( (3,3) )
for column in range(model.shape[1]):
W += numpy.outer(model_zerocentered[:,column],data_zerocentered[:,column])
@@ -70,18 +70,18 @@ def align(model,data):
S[2,2] = -1
rot = U*S*Vh
trans = data.mean(1) - rot * model.mean(1)
model_aligned = rot * model + trans
alignment_error = model_aligned - data
trans_error = numpy.sqrt(numpy.sum(numpy.multiply(alignment_error,alignment_error),0)).A[0]
return rot,trans,trans_error
def plot_traj(ax,stamps,traj,style,color,label):
"""
Plot a trajectory using matplotlib.
Plot a trajectory using matplotlib.
Input:
ax -- the plot
stamps -- time stamps (1xn)
@@ -89,7 +89,7 @@ def plot_traj(ax,stamps,traj,style,color,label):
style -- line style
color -- line color
label -- plot legend
"""
stamps.sort()
interval = numpy.median([s-t for s,t in zip(stamps[1:],stamps[:-1])])
@@ -108,12 +108,12 @@ def plot_traj(ax,stamps,traj,style,color,label):
last= stamps[i]
if len(x)>0:
ax.plot(x,y,style,color=color,label=label)
if __name__=="__main__":
# parse command line
parser = argparse.ArgumentParser(description='''
This script computes the absolute trajectory error from the ground truth trajectory and the estimated trajectory.
This script computes the absolute trajectory error from the ground truth trajectory and the estimated trajectory.
''')
parser.add_argument('first_file', help='ground truth trajectory (format: timestamp tx ty tz qx qy qz qw)')
parser.add_argument('second_file', help='estimated trajectory (format: timestamp tx ty tz qx qy qz qw)')
@@ -129,7 +129,7 @@ if __name__=="__main__":
first_list = associate.read_file_list(args.first_file)
second_list = associate.read_file_list(args.second_file)
matches = associate.associate(first_list, second_list,float(args.offset),float(args.max_difference))
matches = associate.associate(first_list, second_list,float(args.offset),float(args.max_difference))
if len(matches)<2:
sys.exit("Couldn't find matching timestamp pairs between groundtruth and estimated trajectory! Did you choose the correct sequence?")
@@ -137,18 +137,18 @@ if __name__=="__main__":
first_xyz = numpy.matrix([[float(value) for value in first_list[a][0:3]] for a,b in matches]).transpose()
second_xyz = numpy.matrix([[float(value)*float(args.scale) for value in second_list[b][0:3]] for a,b in matches]).transpose()
rot,trans,trans_error = align(second_xyz,first_xyz)
second_xyz_aligned = rot * second_xyz + trans
first_stamps = first_list.keys()
first_stamps.sort()
first_xyz_full = numpy.matrix([[float(value) for value in first_list[b][0:3]] for b in first_stamps]).transpose()
second_stamps = second_list.keys()
second_stamps.sort()
second_xyz_full = numpy.matrix([[float(value)*float(args.scale) for value in second_list[b][0:3]] for b in second_stamps]).transpose()
second_xyz_full_aligned = rot * second_xyz_full + trans
if args.verbose:
print "compared_pose_pairs %d pairs"%(len(trans_error))
@@ -160,12 +160,12 @@ if __name__=="__main__":
print "absolute_translational_error.max %f m"%numpy.max(trans_error)
else:
print "%f"%numpy.sqrt(numpy.dot(trans_error,trans_error) / len(trans_error))
if args.save_associations:
file = open(args.save_associations,"w")
file.write("\n".join(["%f %f %f %f %f %f %f %f"%(a,x1,y1,z1,b,x2,y2,z2) for (a,b),(x1,y1,z1),(x2,y2,z2) in zip(matches,first_xyz.transpose().A,second_xyz_aligned.transpose().A)]))
file.close()
if args.save:
file = open(args.save,"w")
file.write("\n".join(["%f "%stamp+" ".join(["%f"%d for d in line]) for stamp,line in zip(second_stamps,second_xyz_full_aligned.transpose().A)]))
@@ -186,10 +186,10 @@ if __name__=="__main__":
#for (a,b),(x1,y1,z1),(x2,y2,z2) in zip(matches,first_xyz.transpose().A,second_xyz_aligned.transpose().A):
# ax.plot([x1,x2],[y1,y2],'-',color="red",label=label)
# label=""
ax.legend()
ax.set_xlabel('x [m]')
ax.set_ylabel('y [m]')
plt.savefig(args.plot,dpi=300, format='pdf')
launch/jfr2018/gt_tf_broadcaster.py
+3 -3
View File
@@ -1,4 +1,4 @@
#!/usr/bin/env python
#!/usr/bin/env python
import roslib
import rospy
import os
@@ -14,7 +14,7 @@ if __name__ == '__main__':
offset_y = rospy.get_param('~offset_y', 0.0)
offset_theta = rospy.get_param('~offset_theta', 0.0)
gtFile = rospy.get_param('~file', 'groundtruth.txt')
gtFile = os.path.expanduser(gtFile)
gtFile = os.path.expanduser(gtFile)
br = tf.TransformBroadcaster()
init_x = 0
@@ -34,7 +34,7 @@ if __name__ == '__main__':
init_x = x
init_y = y
init = True
x -= init_x
y -= init_y
launch/jfr2018/launch_lidar
+3 -3
View File
@@ -15,7 +15,7 @@ odom_frame_id:="odom_combined" odom_tf_angular_variance:=0.0001 odom_tf_linear_v
//neighbor link refine
--RGBD/NeighborLinkRefining true --RGBD/OptimizeMaxError 1.5
// odom frame to frame
--Odom/Strategy 1
--Odom/Strategy 1
$ rosparam load scan_filter.yaml scan_to_scan_filter_chain
$ rosrun laser_filters scan_to_scan_filter_chain scan:=/base_scan_t scan_filtered:=/base_scan_t_filtered
@@ -23,7 +23,7 @@ $ rosrun laser_filters scan_to_scan_filter_chain scan:=/base_scan_t scan_filtere
$ ./republish_scan.py _offset:=82.2
//stereo
$ ./republish_camera_info.py camera_info_in:=/wide_stereo/right/camera_info camera_info_out:=/wide_stereo/right/camera_info_scaled
$ ./republish_camera_info.py camera_info_in:=/wide_stereo/right/camera_info camera_info_out:=/wide_stereo/right/camera_info_scaled
$ export ROS_NAMESPACE=wide_stereo
$ rosrun stereo_image_proc stereo_image_proc left/image_raw:=left/image_raw right/image_raw:=right/image_raw left/camera_info:=left/camera_info right/camera_info:=right/camera_info_scaled
@@ -66,7 +66,7 @@ $ rosbag play --clock --pause ./stata-mit/2012-01-25-12-14-25.bag
//2012-01-25-12-33-29
$ rosbag play --clock --pause ./stata-mit/2012-01-25-12-33-29.bag
$ ./gt_tf_broadcaster.py _file:=./stata-mit/2012-01-25-12-33-29_part1_floor2.gt.laser.poses _frame_id:=scan_gt _fixed_frame_id:=world _offset_time:=82.2 _offset_x:=-0.275
// for short-lidar
// for short-lidar
$ rosparam load scan_filter.yaml scan_to_scan_filter_chain
$ rosrun laser_filters scan_to_scan_filter_chain scan:=/base_scan_t scan_filtered:=/base_scan_t_filtered
// for fake lidar kinect
launch/jfr2018/launch_rgbd
+2 -2
View File
@@ -1,8 +1,8 @@
// rgbd_odometry
$ roslaunch rtabmap_ros rtabmap.launch args:="-d --Rtabmap/PublishRAMUsage true --Rtabmap/StartNewMapOnLoopClosure true --Reg/Force3DoF false --RGBD/ProximityPathMaxNeighbors 0 --Mem/STMSize 15 --Mem/BinDataKept false --Kp/FlannRebalancingFactor 1.0 --RGBD/LinearUpdate 0 --RGBD/ProximityBySpace true --RGBD/OptimizeMaxError 0.5 --FAST/Threshold 7" odom_args:="--Odom/Strategy 0 --Vis/CorType 0 --Odom/KeyFrameThr 0.3 --OdomF2M/MaxSize 2000 --OdomORBSLAM2/VocPath /home/mathieu/workspace/ORB_SLAM2/Vocabulary/ORBvoc.txt --OdomORBSLAM2/Fps 15" rgbd_sync:=true depth_scale:=1.043 frame_id:=base_footprint ground_truth_frame_id:=world ground_truth_base_frame_id:=scan_gt use_sim_time:=true odom_topic:=odom rgb_topic:=/camera/rgb/image_raw_throttle depth_topic:=/camera/depth/image_raw_throttle camera_info_topic:=/camera/rgb/camera_info_throttle approx_sync:=false database_path:=/media/mathieu/5B60E7B25BDFCB79/bags/rtabmap.db odom_guess_frame_id:=odom_combined
$ roslaunch rtabmap_ros rtabmap.launch args:="-d --Rtabmap/PublishRAMUsage true --Rtabmap/StartNewMapOnLoopClosure true --Reg/Force3DoF false --RGBD/ProximityPathMaxNeighbors 0 --Mem/STMSize 15 --Mem/BinDataKept false --Kp/FlannRebalancingFactor 1.0 --RGBD/LinearUpdate 0 --RGBD/ProximityBySpace true --RGBD/OptimizeMaxError 0.5 --FAST/Threshold 7" odom_args:="--Odom/Strategy 0 --Vis/CorType 0 --Odom/KeyFrameThr 0.3 --OdomF2M/MaxSize 2000 --OdomORBSLAM2/VocPath /home/mathieu/workspace/ORB_SLAM2/Vocabulary/ORBvoc.txt --OdomORBSLAM2/Fps 15" rgbd_sync:=true depth_scale:=1.043 frame_id:=base_footprint ground_truth_frame_id:=world ground_truth_base_frame_id:=scan_gt use_sim_time:=true odom_topic:=odom rgb_topic:=/camera/rgb/image_raw_throttle depth_topic:=/camera/depth/image_raw_throttle camera_info_topic:=/camera/rgb/camera_info_throttle approx_sync:=false database_path:=/media/mathieu/5B60E7B25BDFCB79/bags/rtabmap.db odom_guess_frame_id:=odom_combined
// use wheel odom
odom_frame_id:=odom_combined odom_tf_angular_variance:=0.0001 odom_tf_linear_variance:=0.0001 visual_odometry:=false
// flow
// flow
--Vis/CorType 1 --Odom/KeyFrameThr 0.6 --Vis/BundleAdjustment 0
// robot_localization
odom_topic:=/odometry/filtered visual_odometry:=false approx_sync:=true
launch/jfr2018/launch_stereo
+3 -3
View File
@@ -1,11 +1,11 @@
// stereo_odometry
$ roslaunch rtabmap_ros rtabmap.launch args:="-d --Rtabmap/PublishRAMUsage true --Rtabmap/StartNewMapOnLoopClosure true --Reg/Force3DoF false --RGBD/ProximityPathMaxNeighbors 0 --Mem/STMSize 15 --Mem/BinDataKept false --Kp/FlannRebalancingFactor 1.0 --RGBD/LinearUpdate 0 --RGBD/ProximityBySpace true --Odom/KeyFrameThr 0.3 --Odom/Strategy 0 --OdomF2M/MaxSize 2000 --RGBD/OptimizeMaxError 0.5 --OdomORBSLAM2/VocPath /home/mathieu/workspace/ORB_SLAM2/Vocabulary/ORBvoc.txt --OdomORBSLAM2/Fps 15 --FAST/Threshold 7" odom_args:="--Vis/CorType 0" frame_id:=base_footprint ground_truth_frame_id:=world ground_truth_base_frame_id:=scan_gt use_sim_time:=true stereo:=true stereo_namespace:=/wide_stereo left_camera_info_topic:=/wide_stereo/left/camera_info_throttle right_camera_info_topic:=/wide_stereo/right/camera_info_scaled odom_topic:=odom approx_sync:=false database_path:=/media/mathieu/5B60E7B25BDFCB79/bags/rtabmap.db odom_guess_frame_id:=odom_combined
$ roslaunch rtabmap_ros rtabmap.launch args:="-d --Rtabmap/PublishRAMUsage true --Rtabmap/StartNewMapOnLoopClosure true --Reg/Force3DoF false --RGBD/ProximityPathMaxNeighbors 0 --Mem/STMSize 15 --Mem/BinDataKept false --Kp/FlannRebalancingFactor 1.0 --RGBD/LinearUpdate 0 --RGBD/ProximityBySpace true --Odom/KeyFrameThr 0.3 --Odom/Strategy 0 --OdomF2M/MaxSize 2000 --RGBD/OptimizeMaxError 0.5 --OdomORBSLAM2/VocPath /home/mathieu/workspace/ORB_SLAM2/Vocabulary/ORBvoc.txt --OdomORBSLAM2/Fps 15 --FAST/Threshold 7" odom_args:="--Vis/CorType 0" frame_id:=base_footprint ground_truth_frame_id:=world ground_truth_base_frame_id:=scan_gt use_sim_time:=true stereo:=true stereo_namespace:=/wide_stereo left_camera_info_topic:=/wide_stereo/left/camera_info_throttle right_camera_info_topic:=/wide_stereo/right/camera_info_scaled odom_topic:=odom approx_sync:=false database_path:=/media/mathieu/5B60E7B25BDFCB79/bags/rtabmap.db odom_guess_frame_id:=odom_combined
// with wheel odom
odom_frame_id:=odom_combined odom_tf_angular_variance:=0.0001 odom_tf_linear_variance:=0.0001 visual_odometry:=false
// flow
// flow
--Vis/CorType 1 --Odom/KeyFrameThr 0.6
$ ./republish_camera_info.py camera_info_in:=/wide_stereo/right/camera_info_throttle camera_info_out:=/wide_stereo/right/camera_info_scaled
$ ./republish_camera_info.py camera_info_in:=/wide_stereo/right/camera_info_throttle camera_info_out:=/wide_stereo/right/camera_info_scaled
$ export ROS_NAMESPACE=wide_stereo
$ rosrun stereo_image_proc stereo_image_proc left/image_raw:=left/image_raw_throttle right/image_raw:=right/image_raw_throttle left/camera_info:=left/camera_info_throttle right/camera_info:=right/camera_info_scaled
launch/jfr2018/slam_gmapping.patch
+9 -9
View File
@@ -7,7 +7,7 @@ index bd9977c..b4f9382 100644
#include "ros/console.h"
#include "nav_msgs/MapMetaData.h"
+#include <nav_msgs/Path.h>
#include "gmapping/sensor/sensor_range/rangesensor.h"
#include "gmapping/sensor/sensor_odometry/odometrysensor.h"
@@ -258,6 +259,7 @@ void SlamGMapping::startLiveSlam()
@@ -24,12 +24,12 @@ index bd9977c..b4f9382 100644
sstm_ = node_.advertise<nav_msgs::MapMetaData>("map_metadata", 1, true);
+ pathPub_ = node_.advertise<nav_msgs::Path>("map_path", 1, true);
ss_ = node_.advertiseService("dynamic_map", &SlamGMapping::mapCallback, this);
rosbag::Bag bag;
@@ -410,6 +413,18 @@ SlamGMapping::initMapper(const sensor_msgs::LaserScan& scan)
return false;
}
+ try
+ {
+ tf_.lookupTransform(laser_frame_, base_frame_, scan.header.stamp, scan_to_base_);
@@ -47,7 +47,7 @@ index bd9977c..b4f9382 100644
v.setValue(0, 0, 1 + laser_pose.getOrigin().z());
@@ -617,6 +632,8 @@ SlamGMapping::laserCallback(const sensor_msgs::LaserScan::ConstPtr& scan)
ROS_DEBUG("scan processed");
GMapping::OrientedPoint mpose = gsp_->getParticles()[gsp_->getBestParticleIndex()].pose;
+ GMapping::GridSlamProcessor::TNode * node = gsp_->getParticles()[gsp_->getBestParticleIndex()].node;
+
@@ -56,7 +56,7 @@ index bd9977c..b4f9382 100644
ROS_DEBUG("correction: %.3f %.3f %.3f", mpose.x - odom_pose.x, mpose.y - odom_pose.y, mpose.theta - odom_pose.theta);
@@ -699,6 +716,23 @@ SlamGMapping::updateMap(const sensor_msgs::LaserScan& scan)
delta_);
ROS_DEBUG("Trajectory tree:");
+ nav_msgs::Path path;
+ int count = 0;
@@ -95,15 +95,15 @@ index bd9977c..b4f9382 100644
@@ -764,8 +805,11 @@ SlamGMapping::updateMap(const sensor_msgs::LaserScan& scan)
map_.map.header.stamp = ros::Time::now();
map_.map.header.frame_id = tf_.resolve( map_frame_ );
+ path.header = map_.map.header;
+
sst_.publish(map_.map);
sstm_.publish(map_.map.info);
+ pathPub_.publish(path);
}
bool
bool
diff --git a/gmapping/src/slam_gmapping.h b/gmapping/src/slam_gmapping.h
index ae622b9..8d84645 100644
--- a/gmapping/src/slam_gmapping.h
@@ -119,7 +119,7 @@ index ae622b9..8d84645 100644
@@ -92,6 +93,8 @@ class SlamGMapping
std::string map_frame_;
std::string odom_frame_;
+ tf::StampedTransform scan_to_base_;
+
void updateMap(const sensor_msgs::LaserScan& scan);
launch/jfr2018/slam_karto.patch
+18 -18
View File
@@ -8,19 +8,19 @@ index 712a9ca..0c0d885 100644
void publishLoop(double transform_publish_period);
- void publishGraphVisualization();
+ void publishGraphVisualization(const ros::Time & stamp);
// ROS handles
ros::NodeHandle node_;
@@ -435,16 +435,22 @@ SlamKarto::getOdomPose(karto::Pose2& karto_pose, const ros::Time& t)
}
void
-SlamKarto::publishGraphVisualization()
+SlamKarto::publishGraphVisualization(const ros::Time & stamp)
{
std::vector<float> graph;
solver_->getGraph(graph);
+ std::vector<karto::LocalizedRangeScan*> scans = mapper_->GetAllProcessedScans();
+
+ if(scans.empty())
@@ -28,7 +28,7 @@ index 712a9ca..0c0d885 100644
+ return;
+ }
visualization_msgs::MarkerArray marray;
visualization_msgs::Marker m;
m.header.frame_id = "map";
- m.header.stamp = ros::Time::now();
@@ -37,7 +37,7 @@ index 712a9ca..0c0d885 100644
m.ns = "karto";
m.type = visualization_msgs::Marker::SPHERE;
@@ -462,7 +468,7 @@ SlamKarto::publishGraphVisualization()
visualization_msgs::Marker edge;
edge.header.frame_id = "map";
- edge.header.stamp = ros::Time::now();
@@ -46,10 +46,10 @@ index 712a9ca..0c0d885 100644
edge.ns = "karto";
edge.id = 0;
@@ -477,14 +483,14 @@ SlamKarto::publishGraphVisualization()
m.action = visualization_msgs::Marker::ADD;
uint id = 0;
- for (uint i=0; i<graph.size()/2; i++)
- for (uint i=0; i<graph.size()/2; i++)
+ for (uint i=0; i<scans.size(); i++)
{
m.id = id;
@@ -65,7 +65,7 @@ index 712a9ca..0c0d885 100644
{
edge.points.clear();
@@ -500,15 +506,15 @@ SlamKarto::publishGraphVisualization()
marray.markers.push_back(visualization_msgs::Marker(edge));
id++;
- }
@@ -74,40 +74,40 @@ index 712a9ca..0c0d885 100644
-
+/*
m.action = visualization_msgs::Marker::DELETE;
for (; id < marker_count_; id++)
for (; id < marker_count_; id++)
{
m.id = id;
marray.markers.push_back(visualization_msgs::Marker(m));
- }
+ }*/
marker_count_ = marray.markers.size();
@@ -537,12 +543,14 @@ SlamKarto::laserCallback(const sensor_msgs::LaserScan::ConstPtr& scan)
karto::Pose2 odom_pose;
if(addScan(laser, scan, odom_pose))
{
- ROS_DEBUG("added scan at pose: %.3f %.3f %.3f",
+ ROS_INFO("added scan at pose: %.3f %.3f %.3f",
- ROS_DEBUG("added scan at pose: %.3f %.3f %.3f",
+ ROS_INFO("added scan at pose: %.3f %.3f %.3f",
odom_pose.GetX(),
odom_pose.GetY(),
odom_pose.GetHeading());
- publishGraphVisualization();
+ publishGraphVisualization(scan->header.stamp);
+
+ ROS_INFO("published markers");
if(!got_map_ ||
if(!got_map_ ||
(scan->header.stamp - last_map_update) > map_update_interval_)
diff --git a/src/spa_solver.cpp b/src/spa_solver.cpp
index 5d9a962..6a65211 100644
--- a/src/spa_solver.cpp
+++ b/src/spa_solver.cpp
@@ -46,9 +46,9 @@ void SpaSolver::Compute()
typedef std::vector<sba::Node2d, Eigen::aligned_allocator<sba::Node2d> > NodeVector;
- ROS_INFO("Calling doSPA for loop closure");
+ //ROS_INFO("Calling doSPA for loop closure");
m_Spa.doSPA(40);
launch/jfr2018/slam_karto_sync_markers_gt.py
+3 -3
View File
@@ -1,4 +1,4 @@
#!/usr/bin/env python
#!/usr/bin/env python
import roslib
import rospy
import os
@@ -16,11 +16,11 @@ def callback(data):
global lastSize
global slamPosesInd
global rmse
point_markers = []
for m in data.markers:
if m.type==2:
point_markers.append(m)
point_markers.append(m)
if len(point_markers) > 0 and lastSize != len(point_markers):
t = rospy.Time(point_markers[0].header.stamp.secs, point_markers[0].header.stamp.nsecs)
launch/jfr2018/sync_path_gt.py
+1 -1
View File
@@ -1,4 +1,4 @@
#!/usr/bin/env python
#!/usr/bin/env python
import roslib
import rospy
import os
launch/jfr2018/throttle_bag.launch
+6 -5
View File
@@ -1,3 +1,4 @@
<?xml version="1.0"?>
<launch>
<param name="use_sim_time" value="true"/>
@@ -7,7 +8,7 @@
<remap from="right/image" to="right/image_raw"/>
<remap from="left/camera_info" to="left/camera_info"/>
<remap from="right/camera_info" to="right/camera_info"/>
<param name="queue_size" type="int" value="10"/>
<param name="rate" type="double" value="15"/>
</node>
@@ -16,14 +17,14 @@
<group ns="camera">
<node pkg="nodelet" type="nodelet" name="data_throttle" args="standalone rtabmap_ros/data_throttle">
<param name="rate" type="double" value="15.0"/>
<remap from="rgb/image_in" to="rgb/image_raw"/>
<remap from="depth/image_in" to="depth/image_raw"/>
<remap from="rgb/camera_info_in" to="rgb/camera_info"/>
<remap from="rgb/image_out" to="rgb/image_raw_throttle"/>
<remap from="depth/image_out" to="depth/image_raw_throttle"/>
<remap from="rgb/camera_info_out" to="rgb/camera_info_throttle"/>
</node>
</group>
</node>
</group>
</launch>
launch/rgbd_mapping.launch
+25 -24
View File
@@ -1,76 +1,77 @@
<?xml version="1.0"?>
<launch>
<!-- Backward compatibility launch file, use rtabmap.launch instead -->
<!-- Your RGB-D sensor should be already started with "depth_registration:=true".
Examples:
$ roslaunch freenect_launch freenect.launch depth_registration:=true
$ roslaunch freenect_launch freenect.launch depth_registration:=true
$ roslaunch openni2_launch openni2.launch depth_registration:=true -->
<!-- Choose visualization -->
<arg name="rviz" default="false" />
<arg name="rtabmapviz" default="true" />
<arg name="rtabmapviz" default="true" />
<!-- Localization-only mode -->
<arg name="localization" default="false"/>
<!-- Corresponding config files -->
<arg name="rtabmapviz_cfg" default="~/.ros/rtabmap_gui.ini" />
<arg name="rviz_cfg" default="$(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="database_path" default="~/.ros/rtabmap.db"/>
<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="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="depth_registered_topic" default="/camera/depth_registered/image_raw" />
<arg name="camera_info_topic" default="/camera/rgb/camera_info" />
<arg name="compressed" default="false"/>
<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="subscribe_scan_cloud" default="false"/> <!-- Assuming 3D scan if set -->
<arg name="scan_cloud_topic" default="/scan_cloud"/>
<arg name="visual_odometry" default="true"/> <!-- Generate visual odometry -->
<arg name="odom_topic" default="/odom"/> <!-- Odometry topic used if visual_odometry is false -->
<arg name="odom_frame_id" default=""/> <!-- If set, TF is used to get odometry instead of the topic -->
<arg name="namespace" default="rtabmap"/>
<arg name="wait_for_transform" default="0.2"/>
<include file="$(find rtabmap_ros)/launch/rtabmap.launch">
<arg name="rtabmapviz" value="$(arg rtabmapviz)" />
<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)" />
<arg name="frame_id" value="$(arg frame_id)"/>
<arg name="namespace" value="$(arg namespace)"/>
<arg name="database_path" value="$(arg database_path)"/>
<arg name="wait_for_transform" value="$(arg wait_for_transform)"/>
<arg name="rtabmap_args" value="$(arg rtabmap_args)"/>
<arg name="launch_prefix" value="$(arg launch_prefix)"/>
<arg name="rtabmap_args" value="$(arg rtabmap_args)"/>
<arg name="launch_prefix" value="$(arg launch_prefix)"/>
<arg name="approx_sync" value="$(arg approx_sync)"/>
<arg name="rgb_topic" value="$(arg rgb_topic)" />
<arg name="depth_topic" value="$(arg depth_registered_topic)" />
<arg name="camera_info_topic" value="$(arg camera_info_topic)" />
<arg name="compressed" value="$(arg compressed)"/>
<arg name="compressed" value="$(arg compressed)"/>
<arg name="subscribe_scan" value="$(arg subscribe_scan)"/>
<arg name="scan_topic" value="$(arg scan_topic)"/>
<arg name="subscribe_scan_cloud" value="$(arg subscribe_scan_cloud)"/>
<arg name="scan_cloud_topic" value="$(arg scan_cloud_topic)"/>
<arg name="visual_odometry" value="$(arg visual_odometry)"/>
<arg name="odom_topic" value="$(arg odom_topic)"/>
<arg name="odom_frame_id" value="$(arg odom_frame_id)"/>
<arg name="visual_odometry" value="$(arg visual_odometry)"/>
<arg name="odom_topic" value="$(arg odom_topic)"/>
<arg name="odom_frame_id" value="$(arg odom_frame_id)"/>
<arg name="odom_args" value="$(arg rtabmap_args)"/>
</include>
launch/rgbd_mapping_kinect2.launch
+22 -21
View File
@@ -1,6 +1,7 @@
<?xml version="1.0"?>
<launch>
<!-- Kinect 2
Install Kinect2 : Follow ALL directives at https://github.com/code-iai/iai_kinect2
Make sure it is calibrated!
@@ -11,30 +12,30 @@
<!-- Which image resolution to process in rtabmap: sd, qhd, hd -->
<arg name="resolution" default="qhd" />
<!-- Fixed frame id, you may set "base_link" or "base_footprint" if they are published -->
<arg name="frame_id" default="kinect2_base_link"/>
<!-- Rotate the camera -->
<arg name="pi/2" value="1.5707963267948966"/>
<arg name="optical_rotate" value="0 0 0 -$(arg pi/2) 0 -$(arg pi/2)" />
<node pkg="tf" type="static_transform_publisher" name="kinect2_base_link"
args="$(arg optical_rotate) kinect2_base_link kinect2_link 100" />
args="$(arg optical_rotate) kinect2_base_link kinect2_link 100" />
<!-- Choose visualization -->
<arg name="rviz" default="false" />
<arg name="rtabmapviz" default="true" />
<arg name="rtabmapviz" default="true" />
<!-- Corresponding config files -->
<arg name="rtabmapviz_cfg" default="-d $(find rtabmap_ros)/launch/config/rgbd_gui.ini" />
<arg name="rviz_cfg" default="-d $(find rtabmap_ros)/launch/config/rgbd.rviz" />
<!-- slightly increase default parameters for larger images (qhd=720p) -->
<arg name="gftt_block_size" default="5" />
<arg name="gftt_min_distance" default="5" />
<arg name="gftt_block_size" default="5" />
<arg name="gftt_min_distance" default="5" />
<group ns="rtabmap">
<!-- Odometry -->
<node pkg="rtabmap_ros" type="rgbd_odometry" name="visual_odometry" output="screen">
<remap from="rgb/image" to="/kinect2/$(arg resolution)/image_color_rect"/>
@@ -43,41 +44,41 @@
<param name="frame_id" type="string" value="$(arg frame_id)"/>
<param name="approx_sync" type="bool" value="false"/>
<param name="GFTT/BlockSize" type="string" value="$(arg gftt_block_size)"/>
<param name="GFTT/MinDistance" type="string" value="$(arg gftt_min_distance)"/>
</node>
<!-- Visual SLAM (robot side) -->
<!-- args: "delete_db_on_start" and "udebug" -->
<node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="--delete_db_on_start">
<param name="subscribe_depth" type="bool" value="true"/>
<param name="frame_id" type="string" value="$(arg frame_id)"/>
<remap from="rgb/image" to="/kinect2/$(arg resolution)/image_color_rect"/>
<remap from="depth/image" to="/kinect2/$(arg resolution)/image_depth_rect"/>
<remap from="rgb/camera_info" to="/kinect2/$(arg resolution)/camera_info"/>
<param name="approx_sync" type="bool" value="false"/>
<param name="GFTT/BlockSize" type="string" value="$(arg gftt_block_size)"/>
<param name="GFTT/MinDistance" type="string" value="$(arg gftt_min_distance)"/>
</node>
<!-- Visualisation RTAB-Map -->
<node if="$(arg rtabmapviz)" pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" args="$(arg rtabmapviz_cfg)" output="screen">
<param name="subscribe_depth" type="bool" value="true"/>
<param name="subscribe_odom_info" type="bool" value="true"/>
<param name="approx_sync" type="bool" value="false"/>
<param name="frame_id" type="string" value="$(arg frame_id)"/>
<remap from="rgb/image" to="/kinect2/$(arg resolution)/image_color_rect"/>
<remap from="depth/image" to="/kinect2/$(arg resolution)/image_depth_rect"/>
<remap from="rgb/camera_info" to="/kinect2/$(arg resolution)/camera_info"/>
</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) -->
@@ -90,7 +91,7 @@
<remap from="odom_in" to="rtabmap/odom"/>
<param name="approx_sync" type="bool" value="false"/>
<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"/>
launch/rtabmap.launch
+50 -49
View File
@@ -1,3 +1,4 @@
<?xml version="1.0"?>
<!-- -->
<launch>
<!-- Convenience launch file to launch odometry, rtabmap and rtabmapviz nodes at once -->
@@ -5,38 +6,38 @@
<!-- For stereo:=false
Your RGB-D sensor should be already started with "depth_registration:=true".
Examples:
$ roslaunch freenect_launch freenect.launch depth_registration:=true
$ roslaunch freenect_launch freenect.launch depth_registration:=true
$ roslaunch openni2_launch openni2.launch depth_registration:=true -->
<!-- For stereo:=true
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.
Example:
$ roslaunch rtabmap_ros bumblebee.launch -->
<!-- Choose between depth and stereo, set both to false to do only scan -->
<!-- Choose between depth and stereo, set both to false to do only scan -->
<arg name="stereo" default="false"/>
<arg if="$(arg stereo)" name="depth" default="false"/>
<arg unless="$(arg stereo)" name="depth" default="true"/>
<arg name="subscribe_rgb" default="$(arg depth)"/>
<!-- Choose visualization -->
<arg name="rtabmapviz" default="true" />
<arg name="rtabmapviz" default="true" />
<arg name="rviz" default="false" />
<!-- Localization-only mode -->
<arg name="localization" default="false"/>
<arg name="initial_pose" default=""/> <!-- Format: "x y z roll pitch yaw" or "x y z qx qy qz qw". Default: see "RGBD/StartAtOrigin" doc -->
<!-- sim time for convenience, if playing a rosbag -->
<arg name="use_sim_time" default="false"/>
<param if="$(arg use_sim_time)" name="use_sim_time" value="true"/>
<!-- Corresponding config files -->
<arg name="cfg" default="" /> <!-- To change RTAB-Map's parameters, set the path of config file (*.ini) generated by the standalone app -->
<arg name="gui_cfg" default="~/.ros/rtabmap_gui.ini" />
<arg name="rviz_cfg" default="$(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="odom_frame_id" default=""/> <!-- If set, TF is used to get odometry instead of the topic -->
<arg name="odom_frame_id_init" default=""/> <!-- If set, TF map->odom is published even if no odometry topic has been received yet. The frame id should match the one in the topic. -->
@@ -58,22 +59,22 @@
<!-- if timestamps of the input topics are synchronized using approximate or exact time policy-->
<arg if="$(arg stereo)" name="approx_sync" default="false"/>
<arg unless="$(arg stereo)" name="approx_sync" default="$(arg depth)"/>
<arg unless="$(arg stereo)" name="approx_sync" default="$(arg depth)"/>
<arg name="approx_sync_max_interval" default="0"/> <!-- (sec) 0 means infinite interval duration (used with approx_sync=true) -->
<!-- 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" />
<arg name="depth_camera_info_topic" default="$(arg camera_info_topic)" />
<!-- stereo related topics -->
<arg name="stereo_namespace" default="/stereo_camera"/>
<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="left_camera_info_topic" default="$(arg stereo_namespace)/left/camera_info" />
<arg name="right_camera_info_topic" default="$(arg stereo_namespace)/right/camera_info" />
<!-- Already synchronized RGB-D related topic, with rtabmap_ros/rgbd_sync nodelet -->
<arg name="rgbd_sync" default="false"/> <!-- pre-sync rgb_topic, depth_topic, camera_info_topic -->
<arg name="approx_rgbd_sync" default="true"/> <!-- false=exact synchronization -->
@@ -82,15 +83,15 @@
<arg name="depth_scale" default="1.0" /> <!-- Deprecated, use rgbd_depth_scale instead -->
<arg name="rgbd_depth_scale" default="$(arg depth_scale)" />
<arg name="rgbd_decimation" default="1" />
<arg name="compressed" default="false"/> <!-- If you want to subscribe to compressed image topics -->
<arg name="rgb_image_transport" default="compressed"/> <!-- Common types: compressed, theora (see "rosrun image_transport list_transports") -->
<arg name="depth_image_transport" default="compressedDepth"/> <!-- Depth compatible types: compressedDepth (see "rosrun image_transport list_transports") -->
<arg name="gen_cloud" default="false"/> <!-- only works with depth image and if not subscribing to scan_cloud topic-->
<arg name="gen_cloud_decimation" default="4"/>
<arg name="gen_cloud_voxel" default="0.05"/>
<arg name="subscribe_scan" default="false"/>
<arg name="scan_topic" default="/scan"/>
<arg name="subscribe_scan_cloud" default="$(arg gen_cloud)"/>
@@ -102,13 +103,13 @@
<arg name="scan_cloud_max_points" default="0"/>
<arg name="scan_cloud_filtered" default="$(arg scan_deskewing)"/> <!-- use filtered cloud from icp_odometry for mapping -->
<arg name="gen_scan" default="false"/> <!-- only works with depth image and if not subscribing to scan topic-->
<arg name="gen_depth" default="false" /> <!-- Generate depth image from scan_cloud -->
<arg name="gen_depth_decimation" default="1" />
<arg name="gen_depth_fill_holes_size" default="0" />
<arg name="gen_depth_fill_iterations" default="1" />
<arg name="gen_depth_fill_holes_error" default="0.1" />
<arg name="visual_odometry" default="true"/> <!-- Launch rtabmap visual odometry node -->
<arg name="icp_odometry" default="false"/> <!-- Launch rtabmap icp odometry node -->
<arg name="odom_topic" default="odom"/> <!-- Odometry topic name -->
@@ -126,7 +127,7 @@
<arg name="imu_topic" default="/imu/data"/> <!-- only used with VIO approaches -->
<arg name="wait_imu_to_init" default="false"/>
<arg name="use_odom_features" default="false"/>
<arg name="scan_cloud_assembling" default="false"/>
<arg name="scan_cloud_assembling_time" default="1"/> <!-- max_clouds and time should not be set at the same time -->
<arg name="scan_cloud_assembling_max_clouds" default="0"/> <!-- max_clouds and time should not be set at the same time -->
@@ -136,18 +137,18 @@
<arg name="scan_cloud_assembling_range_max" default="0.0"/> <!-- 0=disabled -->
<arg name="scan_cloud_assembling_noise_radius" default="0.0"/> <!-- 0=disabled -->
<arg name="scan_cloud_assembling_noise_min_neighbors" default="5"/>
<arg name="subscribe_user_data" default="false"/> <!-- user data synchronized subscription -->
<arg name="user_data_topic" default="/user_data"/>
<arg name="user_data_async_topic" default="/user_data_async" /> <!-- user data async subscription (rate should be lower than map update rate) -->
<arg name="gps_topic" default="/gps/fix" /> <!-- gps async subscription -->
<arg name="tag_topic" default="/tag_detections" /> <!-- apriltags async subscription -->
<arg name="tag_linear_variance" default="0.0001" />
<arg name="tag_angular_variance" default="9999" /> <!-- >=9999 means ignore rotation in optimization, when rotation estimation of the tag is not reliable -->
<arg name="fiducial_topic" default="/fiducial_transforms" /> <!-- aruco_detect async subscription, use tag_linear_variance and tag_angular_variance to set covriance -->
<!-- These arguments should not be modified directly, see referred topics without "_relay" suffix above -->
<arg if="$(arg compressed)" name="rgb_topic_relay" default="$(arg rgb_topic)_relay"/>
<arg unless="$(arg compressed)" name="rgb_topic_relay" default="$(arg rgb_topic)"/>
@@ -162,7 +163,7 @@
<!-- Nodes -->
<group ns="$(arg namespace)">
<!-- relays -->
<group if="$(arg depth)">
<group unless="$(arg subscribe_rgbd)">
@@ -205,7 +206,7 @@
</node>
</group>
</group>
<group unless="$(arg rgbd_sync)">
<group if="$(arg subscribe_rgbd)">
<node name="republish_rgbd_image" type="rgbd_relay" pkg="rtabmap_ros" clear_params="$(arg clear_params)">
@@ -216,7 +217,7 @@
</node>
</group>
</group>
<node if="$(arg gen_cloud)" pkg="nodelet" type="nodelet" name="gen_cloud_from_depth" args="standalone rtabmap_ros/point_cloud_xyz" clear_params="$(arg clear_params)" output="$(arg output)">
<remap from="depth/image" to="$(arg depth_topic_relay)"/>
<remap from="depth/camera_info" to="$(arg camera_info_topic)"/>
@@ -231,7 +232,7 @@
<!-- Visual odometry -->
<group unless="$(arg icp_odometry)">
<group if="$(arg visual_odometry)">
<!-- RGB-D Odometry -->
<node unless="$(arg stereo)" pkg="rtabmap_ros" type="rgbd_odometry" name="rgbd_odometry" clear_params="$(arg clear_params)" output="$(arg output)" args="$(arg rtabmap_args) $(arg odom_args)" launch-prefix="$(arg launch_prefix)">
<remap from="rgb/image" to="$(arg rgb_topic_relay)"/>
@@ -240,7 +241,7 @@
<remap from="rgbd_image" to="$(arg rgbd_topic_relay)"/>
<remap from="odom" to="$(arg odom_topic)"/>
<remap from="imu" to="$(arg imu_topic)"/>
<param name="frame_id" type="string" value="$(arg frame_id)"/>
<param name="odom_frame_id" type="string" value="$(arg vo_frame_id)"/>
<param name="publish_tf" type="bool" value="$(arg publish_tf_odom)"/>
@@ -270,7 +271,7 @@
<remap from="rgbd_image" to="$(arg rgbd_topic_relay)"/>
<remap from="odom" to="$(arg odom_topic)"/>
<remap from="imu" to="$(arg imu_topic)"/>
<param name="frame_id" type="string" value="$(arg frame_id)"/>
<param name="odom_frame_id" type="string" value="$(arg vo_frame_id)"/>
<param name="publish_tf" type="bool" value="$(arg publish_tf_odom)"/>
@@ -292,14 +293,14 @@
</node>
</group>
</group>
<!-- ICP Odometry -->
<node if="$(arg icp_odometry)" pkg="rtabmap_ros" type="icp_odometry" name="icp_odometry" clear_params="$(arg clear_params)" output="$(arg output)" args="$(arg rtabmap_args) $(arg odom_args)" launch-prefix="$(arg launch_prefix)">
<remap from="scan" to="$(arg scan_topic)"/>
<remap from="scan_cloud" to="$(arg scan_cloud_topic)"/>
<remap from="odom" to="$(arg odom_topic)"/>
<remap from="imu" to="$(arg imu_topic)"/>
<param name="frame_id" type="string" value="$(arg frame_id)"/>
<param name="odom_frame_id" type="string" value="$(arg vo_frame_id)"/>
<param name="publish_tf" type="bool" value="$(arg publish_tf_odom)"/>
@@ -318,7 +319,7 @@
<param name="deskewing" type="bool" value="$(arg scan_deskewing)"/>
<param name="deskewing_slerp" type="bool" value="$(arg scan_deskewing_slerp)"/>
</node>
<node if="$(eval not icp_odometry and scan_deskewing and subscribe_scan_cloud)" pkg="rtabmap_ros" type="lidar_deskewing" name="lidar_deskewing" clear_params="$(arg clear_params)" output="$(arg output)">
<param name="wait_for_transform" value="$(arg wait_for_transform)"/>
<param if="$(arg visual_odometry)" name="fixed_frame_id" value="$(arg vo_frame_id)"/>
@@ -327,7 +328,7 @@
<remap from="input_cloud" to="$(arg scan_cloud_topic)"/>
<remap from="$(arg scan_cloud_topic)/deskewed" to="odom_filtered_input_scan"/>
</node>
<node if="$(arg scan_cloud_assembling)" pkg="rtabmap_ros" type="point_cloud_assembler" name="point_cloud_assembler" clear_params="$(arg clear_params)" output="$(arg output)">
<remap if="$(arg scan_cloud_filtered)" from="cloud" to="odom_filtered_input_scan"/>
<remap unless="$(arg scan_cloud_filtered)" from="cloud" to="$(arg scan_cloud_topic)"/>
@@ -343,7 +344,7 @@
<param name="noise_min_neighbors" type="int" value="$(arg scan_cloud_assembling_noise_min_neighbors)"/>
<param name="wait_for_transform_duration" type="double" value="$(arg wait_for_transform)"/>
</node>
<!-- Visual SLAM (robot side) -->
<!-- args: "delete_db_on_start" and "udebug" -->
<node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" clear_params="$(arg clear_params)" output="$(arg output)" args="$(arg rtabmap_args)" launch-prefix="$(arg launch_prefix)">
@@ -377,25 +378,25 @@
<param name="queue_size" type="int" value="$(arg queue_size)"/>
<param if="$(eval not scan_cloud_filtered and not scan_cloud_assembling)" name="scan_cloud_max_points" type="int" value="$(arg scan_cloud_max_points)"/>
<param name="landmark_linear_variance" type="double" value="$(arg tag_linear_variance)"/>
<param name="landmark_angular_variance" type="double" value="$(arg tag_angular_variance)"/>
<param name="landmark_angular_variance" type="double" value="$(arg tag_angular_variance)"/>
<param name="gen_depth" type="bool" value="$(arg gen_depth)" />
<param name="gen_depth_decimation" type="int" value="$(arg gen_depth_decimation)" />
<param name="gen_depth_fill_holes_size" type="int" value="$(arg gen_depth_fill_holes_size)" />
<param name="gen_depth_fill_iterations" type="int" value="$(arg gen_depth_fill_iterations)" />
<param name="gen_depth_fill_holes_error" type="double" value="$(arg gen_depth_fill_holes_error)" />
<remap from="rgb/image" to="$(arg rgb_topic_relay)"/>
<remap from="depth/image" to="$(arg depth_topic_relay)"/>
<remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
<remap if="$(arg use_odom_features)" from="rgbd_image" to="odom_rgbd_image"/>
<remap unless="$(arg use_odom_features)" from="rgbd_image" to="$(arg rgbd_topic_relay)"/>
<remap from="left/image_rect" to="$(arg left_image_topic_relay)"/>
<remap from="right/image_rect" to="$(arg right_image_topic_relay)"/>
<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 if="$(eval scan_cloud_assembling)" from="scan_cloud" to="assembled_cloud"/>
<remap if="$(eval scan_cloud_filtered and not scan_cloud_assembling)" from="scan_cloud" to="odom_filtered_input_scan"/>
@@ -408,13 +409,13 @@
<remap from="fiducial_transforms" to="$(arg fiducial_topic)"/>
<remap from="odom" to="$(arg odom_topic)"/>
<remap from="imu" to="$(arg imu_topic)"/>
<!-- 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 $(arg gui_cfg)" clear_params="$(arg clear_params)" output="$(arg output)" launch-prefix="$(arg launch_prefix)">
<param if="$(arg stereo)" name="subscribe_depth" type="bool" value="false"/>
@@ -433,28 +434,28 @@
<param name="wait_for_transform_duration" type="double" value="$(arg wait_for_transform)"/>
<param name="queue_size" type="int" value="$(arg queue_size)"/>
<param name="approx_sync" type="bool" value="$(eval approx_sync and not use_odom_features)"/>
<remap from="rgb/image" to="$(arg rgb_topic_relay)"/>
<remap from="depth/image" to="$(arg depth_topic_relay)"/>
<remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
<remap if="$(arg use_odom_features)" from="rgbd_image" to="odom_rgbd_image"/>
<remap unless="$(arg use_odom_features)" from="rgbd_image" to="$(arg rgbd_topic_relay)"/>
<remap from="left/image_rect" to="$(arg left_image_topic_relay)"/>
<remap from="right/image_rect" to="$(arg right_image_topic_relay)"/>
<remap from="left/camera_info" to="$(arg left_camera_info_topic)"/>
<remap from="right/camera_info" to="$(arg right_camera_info_topic)"/>
<remap unless="$(arg icp_odometry)" from="scan" to="$(arg scan_topic)"/>
<remap if="$(eval icp_odometry or scan_cloud_filtered)" from="scan_cloud" to="odom_filtered_input_scan"/>
<remap unless="$(eval icp_odometry or scan_cloud_filtered)" from="scan_cloud" to="$(arg scan_cloud_topic)"/>
<remap from="scan_descriptor" to="$(arg scan_descriptor_topic)"/>
<remap from="odom" to="$(arg odom_topic)"/>
</node>
</group>
<!-- Visualization RVIZ -->
<node if="$(arg rviz)" pkg="rviz" type="rviz" name="rviz" args="-d $(arg rviz_cfg)"/>
<node if="$(arg rviz)" pkg="nodelet" type="nodelet" name="points_xyzrgb" args="standalone rtabmap_ros/point_cloud_xyzrgb" clear_params="$(arg clear_params)" output="$(arg output)">
launch/stereo_mapping.launch
+25 -24
View File
@@ -1,63 +1,64 @@
<?xml version="1.0"?>
<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.
Example:
$ roslaunch rtabmap_ros bumblebee.launch -->
<!-- Choose visualization -->
<arg name="rtabmapviz" default="true" />
<arg name="rtabmapviz" default="true" />
<arg name="rviz" default="false" />
<!-- Localization-only mode -->
<arg name="localization" default="false"/>
<!-- Corresponding config files -->
<arg name="rtabmapviz_cfg" default="$(find rtabmap_ros)/launch/config/rgbd_gui.ini" />
<arg name="rviz_cfg" default="$(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="database_path" default="~/.ros/rtabmap.db"/>
<arg name="rtabmap_args" default=""/> <!-- delete_db_on_start, udebug -->
<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="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="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="compressed" default="false"/>
<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="subscribe_scan_cloud" default="false"/> <!-- Assuming 3D scan if set -->
<arg name="scan_cloud_topic" default="/scan_cloud"/>
<arg name="visual_odometry" default="true"/> <!-- Generate visual odometry -->
<arg name="odom_topic" default="/odom"/> <!-- Odometry topic used if visual_odometry is false -->
<arg name="odom_frame_id" default=""/> <!-- If set, TF is used to get odometry instead of the topic -->
<arg name="namespace" default="rtabmap"/>
<arg name="wait_for_transform" default="0.2"/>
<include file="$(find rtabmap_ros)/launch/rtabmap.launch">
<arg name="stereo" value="true"/>
<arg name="rtabmapviz" value="$(arg rtabmapviz)" />
<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)" />
<arg name="frame_id" value="$(arg frame_id)"/>
<arg name="namespace" value="$(arg namespace)"/>
<arg name="database_path" value="$(arg database_path)"/>
<arg name="wait_for_transform" value="$(arg wait_for_transform)"/>
<arg name="rtabmap_args" value="$(arg rtabmap_args)"/>
<arg name="launch_prefix" value="$(arg launch_prefix)"/>
<arg name="rtabmap_args" value="$(arg rtabmap_args)"/>
<arg name="launch_prefix" value="$(arg launch_prefix)"/>
<arg name="approx_sync" value="$(arg approx_sync)"/>
<arg name="stereo_namespace" value="$(arg stereo_namespace)"/>
@@ -65,18 +66,18 @@
<arg name="right_image_topic" value="$(arg right_image_topic)" />
<arg name="left_camera_info_topic" value="$(arg left_camera_info_topic)" />
<arg name="right_camera_info_topic" value="$(arg right_camera_info_topic)" />
<arg name="compressed" value="$(arg compressed)"/>
<arg name="compressed" value="$(arg compressed)"/>
<arg name="subscribe_scan" value="$(arg subscribe_scan)"/>
<arg name="scan_topic" value="$(arg scan_topic)"/>
<arg name="subscribe_scan_cloud" value="$(arg subscribe_scan_cloud)"/>
<arg name="scan_cloud_topic" value="$(arg scan_cloud_topic)"/>
<arg name="visual_odometry" value="$(arg visual_odometry)"/>
<arg name="odom_topic" value="$(arg odom_topic)"/>
<arg name="odom_frame_id" value="$(arg odom_frame_id)"/>
<arg name="visual_odometry" value="$(arg visual_odometry)"/>
<arg name="odom_topic" value="$(arg odom_topic)"/>
<arg name="odom_frame_id" value="$(arg odom_frame_id)"/>
<arg name="odom_args" value="$(arg rtabmap_args)"/>
</include>
launch/tests/bumblebee.launch
+7 -6
View File
@@ -1,5 +1,6 @@
<?xml version="1.0"?>
<launch>
<node pkg="camera1394stereo" type="camera1394stereo_node" name="camera1394stereo_node" output="screen" >
<param name="video_mode" value="format7_mode3" />
<param name="format7_color_coding" value="raw16" />
@@ -9,13 +10,13 @@
<param name="camera_info_url_left" value="" />
<param name="camera_info_url_right" value="" />
</node>
<arg name="gen_depth" default="false"/>
<arg name="pi/2" value="1.5707963267948966" />
<arg name="optical_rotate" value="0 0 0 -$(arg pi/2) 0 -$(arg pi/2)" />
<node pkg="tf" type="static_transform_publisher" name="camera_base_link"
args="$(arg optical_rotate) base_link stereo_camera 100" />
args="$(arg optical_rotate) base_link stereo_camera 100" />
<!-- Run the ROS package stereo_image_proc (throttle to 10 Hz to avoid rectifying all images) -->
<group ns="/stereo_camera" >
<node pkg="nodelet" type="nodelet" name="stereo_throttle" args="standalone rtabmap_ros/stereo_throttle">
@@ -23,11 +24,11 @@
<remap from="right/image" to="right/image_raw"/>
<remap from="left/camera_info" to="left/camera_info"/>
<remap from="right/camera_info" to="right/camera_info"/>
<param name="queue_size" type="int" value="10"/>
<param name="rate" type="double" value="10"/>
</node>
<node pkg="stereo_image_proc" type="stereo_image_proc" name="stereo_image_proc">
<remap from="left/image_raw" to="left/image_raw_throttle"/>
<remap from="left/camera_info" to="left/camera_info_throttle"/>
launch/tests/euroc_datasets.launch
+5 -4
View File
@@ -1,6 +1,7 @@
<?xml version="1.0"?>
<launch>
<!--
<!--
Examples:
F2M (default VO):
$ roslaunch rtabmap_ros euroc_datasets.launch
@@ -72,10 +73,10 @@ Examples:
<!-- TF frames -->
<node pkg="tf" type="static_transform_publisher" name="imu_base_link" args="0 0 0 3.1415926 -1.570796 0 base_link imu4 5"/>
<node pkg="tf" type="static_transform_publisher" name="cam0_imu_link" args="-0.021640 -0.064677 0.009811 1.555925 0.025777 0.003757 imu4 cam0 50"/>
<node pkg="tf" type="static_transform_publisher" name="cam1_imu_link" args="-0.019844 0.045369 0.007862 1.558237 0.025393 0.017907 imu4 cam1 50"/>
<node pkg="tf" type="static_transform_publisher" name="cam1_imu_link" args="-0.019844 0.045369 0.007862 1.558237 0.025393 0.017907 imu4 cam1 50"/>
<!-- For MH sequences, /leica/position doesn't give the orientation, so minimal ground truth error could be as high as 12 cm -->
<node if="$(arg MH_seq)" pkg="tf" type="static_transform_publisher" name="leica_base_link" args="0.120209 -0.0184772 -0.0748903 0 0 0 leica base_link_gt 100"/>
<node if="$(arg MH_seq)" pkg="tf" type="static_transform_publisher" name="leica_base_link" args="0.120209 -0.0184772 -0.0748903 0 0 0 leica base_link_gt 100"/>
<node unless="$(arg MH_seq)" pkg="tf" type="static_transform_publisher" name="vicon_base_link" args="0.12395 -0.02781 -0.06901 0 0 0 vicon/firefly_sbx/firefly_sbx base_link_gt 100"/>
<node if="$(arg MH_seq)" pkg="rtabmap_ros" type="point_to_tf.py" name="point_to_tf">
@@ -95,7 +96,7 @@ Examples:
<param name="use_mag" value="false"/>
<param name="world_frame" value="enu"/>
<param name="publish_tf" value="false"/>
</node>
</node>
<!-- RTAB-Map -->
<include file="$(find rtabmap_ros)/launch/rtabmap.launch">
launch/tests/rgbdslam_datasets.launch
+22 -21
View File
@@ -1,6 +1,7 @@
<?xml version="1.0"?>
<launch>
<!-- Example to run rgbd datasets:
$ wget http://vision.in.tum.de/rgbd/dataset/freiburg3/rgbd_dataset_freiburg3_long_office_household.bag
$ rosbag decompress rgbd_dataset_freiburg3_long_office_household.bag
@@ -10,28 +11,28 @@
$ roslaunch rtabmap_ros rgbdslam_datasets.launch
$ rosbag play -.-clock rgbd_dataset_freiburg3_long_office_household.bag
-->
<param name="use_sim_time" type="bool" value="True"/>
<!-- Choose visualization -->
<arg name="rviz" default="true" />
<arg name="rtabmapviz" default="false" />
<arg name="rtabmapviz" default="false" />
<!-- TF FRAMES -->
<node pkg="tf" type="static_transform_publisher" name="world_to_map"
<node pkg="tf" type="static_transform_publisher" name="world_to_map"
args="0.0 0.0 0.0 0.0 0.0 0.0 /world /map 100" />
<group ns="rtabmap">
<!-- Odometry -->
<node pkg="rtabmap_ros" type="rgbd_odometry" name="rgbd_odometry" output="screen">
<remap from="rgb/image" to="/camera/rgb/image_color"/>
<remap from="depth/image" to="/camera/depth/image"/>
<remap from="rgb/camera_info" to="/camera/rgb/camera_info"/>
<param name="Odom/Strategy" type="string" value="0"/> <!-- 0=Frame-to-Map, 1=Frame-to-KeyFrame -->
<param name="Odom/ResetCountdown" type="string" value="15"/>
<param name="Odom/ResetCountdown" type="string" value="15"/>
<param name="Odom/GuessSmoothingDelay" type="string" value="0"/>
<param name="frame_id" type="string" value="kinect"/>
@@ -40,12 +41,12 @@
<param name="ground_truth_frame_id" type="string" value="world"/>
<param name="ground_truth_base_frame_id" type="string" value="kinect_gt"/>
</node>
<!-- Visual SLAM -->
<!-- args: "delete_db_on_start" and "udebug" -->
<node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="--delete_db_on_start">
<param name="subscribe_depth" type="bool" value="true"/>
<param name="Rtabmap/StartNewMapOnLoopClosure" type="string" value="true"/>
<param name="RGBD/CreateOccupancyGrid" type="string" value="false"/>
<param name="Rtabmap/CreateIntermediateNodes" type="string" value="true"/>
@@ -53,38 +54,38 @@
<param name="RGBD/AngularUpdate" type="string" value="0"/>
<param name="frame_id" type="string" value="kinect"/>
<param name="ground_truth_frame_id" type="string" value="world"/>
<param name="ground_truth_frame_id" type="string" value="world"/>
<param name="ground_truth_base_frame_id" type="string" value="kinect_gt"/>
<remap from="rgb/image" to="/camera/rgb/image_color"/>
<remap from="depth/image" to="/camera/depth/image"/>
<remap from="rgb/camera_info" to="/camera/rgb/camera_info"/>
<param name="queue_size" type="int" value="10"/>
</node>
<!-- Visualisation -->
<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_odom_info" type="bool" value="true"/>
<param name="queue_size" type="int" value="30"/>
<param name="frame_id" type="string" value="kinect"/>
<remap from="rgb/image" to="/camera/rgb/image_color"/>
<remap from="depth/image" to="/camera/depth/image"/>
<remap from="rgb/camera_info" to="/camera/rgb/camera_info"/>
</node>
</group>
<node if="$(arg rviz)" pkg="rviz" type="rviz" name="rviz" args="-d $(find rtabmap_ros)/launch/config/rgbdslam_datasets.rviz"/>
<node if="$(arg rviz)" pkg="nodelet" type="nodelet" name="points_xyzrgb" args="standalone rtabmap_ros/point_cloud_xyzrgb">
<remap from="rgb/image" to="/camera/rgb/image_color"/>
<remap from="depth/image" to="/camera/depth/image"/>
<remap from="rgb/camera_info" to="/camera/rgb/camera_info"/>
<remap from="cloud" to="voxel_cloud" />
<param name="queue_size" type="int" value="10"/>
<param name="decimation" type="double" value="4"/>
</node>
launch/tests/sensor_fusion.launch
+15 -14
View File
@@ -1,9 +1,10 @@
<launch>
<?xml version="1.0"?>
<launch>
<!-- This launch assumes that you have already
<!-- This launch assumes that you have already
started you preferred RGB-D sensor and your IMU.
TF between frame_id and the sensors should already be set too. -->
<arg name="frame_id" default="base_link" />
<arg name="rgb_topic" default="/camera/rgb/image_rect_color" />
<arg name="depth_topic" default="/camera/depth_registered/image_raw" />
@@ -15,8 +16,8 @@
<!-- 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"/>
<arg unless="$(arg localization)" name="rtabmap_args" default="--delete_db_on_start"/>
<group ns="rtabmap">
<!-- Visual Odometry -->
<node pkg="rtabmap_ros" type="rgbd_odometry" name="visual_odometry" output="screen" args="$(arg rtabmap_args)">
@@ -32,26 +33,26 @@
<param name="Odom/FillInfoData" type="string" value="true"/>
<param name="Odom/ResetCountdown" type="string" value="1"/>
<param name="Vis/FeatureType" type="string" value="6"/>
<param name="Vis/FeatureType" type="string" value="6"/>
<param name="OdomF2M/MaxSize" type="string" value="1000"/>
</node>
<!-- SLAM -->
<node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="$(arg rtabmap_args)">
<param name="frame_id" type="string" value="$(arg frame_id)"/>
<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)"/>
<remap from="odom" to="/odometry/filtered"/>
<param name="Kp/DetectorStrategy" type="string" value="6"/> <!-- use same features as odom -->
<!-- localization mode -->
<param name="Mem/InitWMWithAllNodes" type="string" value="$(arg localization)"/>
<param name="Mem/InitWMWithAllNodes" type="string" value="$(arg localization)"/>
<param if="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="false"/>
<param unless="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="true"/>
</node>
</group>
@@ -69,7 +70,7 @@
<param name="transform_time_offset" value="0.0"/>
<param name="odom0" value="/vo"/>
<param name="imu0" value="$(arg imu_topic)"/>
<param name="imu0" value="$(arg imu_topic)"/>
<!-- The order of the values is x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. -->
<rosparam param="odom0_config">[true, true, true,
@@ -89,8 +90,8 @@
true, true, true,
false, false, false,
true, true, true,
true, true, true] </rosparam>
true, true, true] </rosparam>
<param name="odom0_differential" value="false"/>
<param name="imu0_differential" value="false"/>
@@ -103,7 +104,7 @@
<!-- ======== ADVANCED PARAMETERS ======== -->
<param name="odom0_queue_size" value="5"/>
<param name="imu0_queue_size" value="50"/>
<param name="imu0_queue_size" value="50"/>
<!-- The values are ordered as x, y, z, roll, pitch, yaw, vx, vy, vz,
vroll, vpitch, vyaw, ax, ay, az. -->
launch/tests/sensor_fusion_kinect_brick.launch
+6 -5
View File
@@ -1,3 +1,4 @@
<?xml version="1.0"?>
<launch>
<arg name="localization" default="false"/>
@@ -8,7 +9,7 @@
<arg name="depth_registration" value="true" />
<arg name="publish_tf" value="false" />
</include>
<!-- IMU Sensor: -->
<node pkg="imu_brick" type="imu_brick_node" name="imu_brick">
<param name="frame_id" value="imu_link"/>
@@ -19,20 +20,20 @@
<param name="cov_acceleration" type="double" value="0.1"/>
<param name="remove_gravitational_acceleration" type="bool" value="true"/>
</node>
<!-- IMU frame: just over the RGB camera -->
<node pkg="tf" type="static_transform_publisher" name="rgb_to_imu_tf"
args="-0.032 0.0 0.032 0.0 0.0 0.0 /camera_rgb_frame /imu_link 100" />
<arg name="pi/2" value="1.5707963267948966" />
<arg name="optical_rotate" value="0 0 0 -$(arg pi/2) 0 -$(arg pi/2)" />
<node pkg="tf" type="static_transform_publisher" name="optical_rotation"
args="$(arg optical_rotate) /camera_rgb_frame /camera_rgb_optical_frame 100" />
args="$(arg optical_rotate) /camera_rgb_frame /camera_rgb_optical_frame 100" />
<include file="$(find rtabmap_ros)/launch/tests/sensor_fusion.launch">
<arg name="frame_id" value="camera_rgb_frame"/>
<arg name="localization" value="$(arg localization)"/>
<arg name="imu_remove_gravitational_acceleration" value="false"/>
</include>
</launch>
launch/tests/tags.yaml
+1 -1
View File
@@ -47,7 +47,7 @@ tag_bundles:
name: 'tag_bundle1',
layout:
[
# for a pixel = ~8mm
# for a pixel = ~8mm
{id: 1, size: 0.0620, x: 0.0000, y: 0.0000, z: 0, qw: 1, qx: 0, qy: 0, qz: 0},
{id: 2, size: 0.0620, x: 0.0770, y: 0.0000, z: 0, qw: 1, qx: 0, qy: 0, qz: 0},
{id: 25, size: 0.0620, x: 0.0000, y: -0.0770, z: 0, qw: 1, qx: 0, qy: 0, qz: 0},
launch/tests/test_apriltag_ros.launch
+2 -1
View File
@@ -1,3 +1,4 @@
<?xml version="1.0"?>
<launch>
<!-- Print on the file tag_bundle1.png (with GIMP: set size in Image Settings to 160x240mm) -->
<!-- Definition of the tag bundle is tags.yaml, make sure the camera is calibrated or adjust the size of the tag if needed -->
@@ -15,7 +16,7 @@
<!-- Set parameters -->
<rosparam command="load" file="$(find rtabmap_ros)/launch/tests/tag_settings.yaml" ns="apriltag_ros_continuous_node" />
<rosparam command="load" file="$(find rtabmap_ros)/launch/tests/tags.yaml" ns="apriltag_ros_continuous_node" />
<node pkg="apriltag_ros" type="apriltag_ros_continuous_node" name="apriltag_ros_continuous_node" clear_params="true" output="screen">
<remap from="image_rect" to="$(arg rgb_topic)" />
<remap from="camera_info" to="$(arg camera_info_topic)" />
launch/tests/test_d435i_vio.launch
+3 -2
View File
@@ -1,7 +1,8 @@
<?xml version="1.0"?>
<launch>
<!-- Example usage of RTAB-Map with VINS-Fusion support for realsense D435i.
Make sure to disable the IR emitter or put a tape on the IR emitter to
Make sure to disable the IR emitter or put a tape on the IR emitter to
avoid VINS tracking the fixed IR points (that would cause large drifts) -->
<arg name="rtabmapviz" default="true"/>
@@ -12,7 +13,7 @@
<include file="$(find realsense2_camera)/launch/rs_camera.launch">
<arg name="align_depth" value="$(arg depth_mode)"/>
<arg name="unite_imu_method" value="$(arg unite_imu_method)"/>
<arg name="unite_imu_method" value="$(arg unite_imu_method)"/>
<arg name="enable_gyro" value="true"/>
<arg name="enable_accel" value="true"/>
<arg name="enable_infra1" value="true"/>
launch/tests/test_icp_odometry.launch
+27 -26
View File
@@ -1,18 +1,19 @@
<?xml version="1.0"?>
<launch>
<!-- We test here ICP odometry using a guess from visual odometry -->
<arg name="rgbd" default="false"/>
<arg name="pm" default="false"/>
<arg name="nodelet" default="false"/>
<include file="$(find freenect_launch)/launch/freenect.launch" >
<arg name="depth_registration" value="true"/>
<arg name="data_skip" value="3"/>
</include>
<group ns="camera">
<group ns="camera">
<node pkg="nodelet" type="nodelet" name="points_xyz" args="load rtabmap_ros/point_cloud_xyz camera_nodelet_manager">
<remap from="depth/image" to="depth_registered/image_raw"/>
<remap from="depth/camera_info" to="depth_registered/camera_info"/>
@@ -20,7 +21,7 @@
<param name="voxel_size" type="double" value="0.05"/>
<param name="decimation" type="int" value="8"/>
<param name="Odom/AlignWithGround" type="string" value="true"/>
</node>
@@ -30,11 +31,11 @@
<remap from="depth/image" to="depth_registered/image_raw"/>
<remap from="rgb/camera_info" to="rgb/camera_info"/>
<remap from="rgb/image" to="rgb/image_rect_mono"/>
<param name="frame_id" type="string" value="camera_link"/>
<param name="frame_id" type="string" value="camera_link"/>
<param name="scan_normal_k" type="int" value="10"/>
<param name="subscribe_scan_cloud" type="bool" value="true"/>
<param name="subscribe_scan_cloud" type="bool" value="true"/>
<param name="Icp/PointToPlane" type="string" value="true"/>
<param name="Icp/VoxelSize" type="string" value="0"/>
<param name="Icp/PM" type="string" value="$(arg pm)"/>
@@ -42,10 +43,10 @@
</node>
<node unless="$(arg rgbd)" pkg="nodelet" type="nodelet" name="icp_odometry" args="load rtabmap_ros/icp_odometry camera_nodelet_manager" output="screen">
<remap from="scan_cloud" to="/voxel_cloud"/>
<param name="frame_id" type="string" value="camera_link"/>
<param name="scan_normal_k" type="int" value="10"/>
<param name="frame_id" type="string" value="camera_link"/>
<param name="scan_normal_k" type="int" value="10"/>
<param name="Icp/PointToPlane" type="string" value="true"/>
<param name="Icp/VoxelSize" type="string" value="0"/>
<param name="Icp/PM" type="string" value="$(arg pm)"/>
@@ -54,18 +55,18 @@
<param name="Odom/ResetCountdown" type="string" value="1"/>
</node>
</group>
<group unless="$(arg nodelet)">
<node if="$(arg rgbd)" pkg="rtabmap_ros" type="rgbdicp_odometry" name="rgbdicp_odometry" output="screen">
<remap from="scan_cloud" to="/voxel_cloud"/>
<remap from="depth/image" to="depth_registered/image_raw"/>
<remap from="rgb/camera_info" to="rgb/camera_info"/>
<remap from="rgb/image" to="rgb/image_rect_mono"/>
<param name="frame_id" type="string" value="camera_link"/>
<param name="frame_id" type="string" value="camera_link"/>
<param name="scan_normal_k" type="int" value="10"/>
<param name="subscribe_scan_cloud" type="bool" value="true"/>
<param name="subscribe_scan_cloud" type="bool" value="true"/>
<param name="Icp/PointToPlane" type="string" value="true"/>
<param name="Icp/VoxelSize" type="string" value="0"/>
<param name="Icp/PM" type="string" value="$(arg pm)"/>
@@ -73,10 +74,10 @@
</node>
<node unless="$(arg rgbd)" pkg="rtabmap_ros" type="icp_odometry" name="icp_odometry" output="screen">
<remap from="scan_cloud" to="/voxel_cloud"/>
<param name="frame_id" type="string" value="camera_link"/>
<param name="scan_normal_k" type="int" value="10"/>
<param name="frame_id" type="string" value="camera_link"/>
<param name="scan_normal_k" type="int" value="10"/>
<param name="Icp/PointToPlane" type="string" value="true"/>
<param name="Icp/VoxelSize" type="string" value="0"/>
<param name="Icp/PM" type="string" value="$(arg pm)"/>
@@ -87,12 +88,12 @@
</group>
</group>
<!-- We just use odometry without rtabmap node, so set a static /map->/odom
<!-- We just use odometry without rtabmap node, so set a static /map->/odom
transform so that rviz config below works out-of-the-box -->
<node pkg="tf" type="static_transform_publisher" name="map_odom"
args="0 0 0 0 0 0 map odom 100" />
args="0 0 0 0 0 0 map odom 100" />
<!-- Visualization RVIZ -->
<node pkg="rviz" type="rviz" name="rviz" args="-d $(find rtabmap_ros)/launch/config/rgbd.rviz"/>
</launch>
launch/tests/test_k4a_cloud_only.launch
+20 -19
View File
@@ -1,15 +1,16 @@
<?xml version="1.0"?>
<launch>
<arg name="rtabmapviz" default="true"/>
<include file="$(find azure_kinect_ros_driver)/launch/driver.launch">
<arg name="point_cloud" value="false"/>
<arg name="rgb_point_cloud" value="false"/>
<arg name="fps" value="15"/>
</include>
<group ns="rtabmap">
<node pkg="nodelet" type="nodelet" name="points_xyz" args="standalone rtabmap_ros/point_cloud_xyz" output="screen">
<remap from="depth/image" to="/depth_to_rgb/image_raw"/>
<remap from="depth/camera_info" to="/depth_to_rgb/camera_info"/>
@@ -21,38 +22,38 @@
<param name="normal_k" type="int" value="10"/>
<param name="approx_sync" type="bool" value="false"/>
</node>
<node pkg="rtabmap_ros" type="icp_odometry" name="icp_odometry" output="screen">
<remap from="scan_cloud" to="voxel_cloud"/>
<param name="frame_id" type="string" value="camera_base"/>
<param name="frame_id" type="string" value="camera_base"/>
<!-- ICP parameters -->
<param name="Icp/PointToPlane" type="string" value="true"/>
<param name="Icp/PM" type="string" value="true"/>
<param name="Icp/PMOutlierRatio" type="string" value="0.7"/>
<param name="Icp/PM" type="string" value="true"/>
<param name="Icp/PMOutlierRatio" type="string" value="0.7"/>
<!-- Odom parameters -->
<!-- Odom parameters -->
<param name="OdomF2M/ScanSubtractRadius" type="string" value="0.05"/>
<param name="OdomF2M/ScanMaxSize" type="string" value="5000"/>
<param name="OdomF2M/ScanMaxSize" type="string" value="5000"/>
</node>
<node pkg="rtabmap_ros" type="rtabmap" name="rtabmap" output="screen" args="-d">
<param name="frame_id" type="string" value="camera_base"/>
<node pkg="rtabmap_ros" type="rtabmap" name="rtabmap" output="screen" args="-d">
<param name="frame_id" type="string" value="camera_base"/>
<param name="subscribe_depth" type="bool" value="false"/>
<param name="subscribe_rgb" type="bool" value="false"/>
<param name="subscribe_scan_cloud" type="bool" value="true"/>
<param name="approx_sync" type="bool" value="false"/>
<remap from="scan_cloud" to="voxel_cloud"/>
<param name="RGBD/ProximityPathMaxNeighbors" type="string" value="0"/>
<param name="RGBD/ProximityOdomGuess" type="string" value="true"/>
<param name="RGBD/ProximityPathMaxNeighbors" type="string" value="0"/>
<param name="RGBD/ProximityOdomGuess" type="string" value="true"/>
<!-- ICP parameters -->
<param name="Icp/PointToPlane" type="string" value="true"/>
<param name="Icp/PM" type="string" value="true"/>
<param name="Icp/PM" type="string" value="true"/>
<param name="Icp/PMOutlierRatio" type="string" value="0.7"/>
<param name="Icp/MaxTranslation" type="string" value="0.5"/>
<param name="Icp/MaxTranslation" type="string" value="0.5"/>
</node>
<node if="$(arg rtabmapviz)" name="rtabmapviz" pkg="rtabmap_ros" type="rtabmapviz" output="screen">
launch/tests/test_laser_assembler.launch
+2 -1
View File
@@ -1,3 +1,4 @@
<?xml version="1.0"?>
<launch>
<!-- Build laser_assembler package from source to have periodic_snapshotter node -->
@@ -39,7 +40,7 @@
<arg name="rgb_topic" value="$(arg rgb_topic)" />
<arg name="depth_topic" value="$(arg depth_topic)" />
<arg name="camera_info_topic" value="$(arg camera_info_topic)" />
<arg name="subscribe_scan_cloud" value="true"/>
<arg name="scan_cloud_topic" value="/assembled_cloud2"/>
launch/tests/test_map_assembler.launch
+4 -3
View File
@@ -1,14 +1,15 @@
<?xml version="1.0"?>
<launch>
<!-- Example to assemble 3D point clouds from depth when rtabmap is using scan for 2d occupancy grid :
$ roslaunch rtabmap_ros demo_robot_mapping.launch rviz:=true rtabmapviz:=false
$ roslaunch rtabmap_ros test_map_assembler.launch
$ rosbag play -clock demo_mapping.bag
-->
-->
<group ns="rtabmap">
<node pkg="rtabmap_ros" type="map_assembler" name="map_assembler">
<remap from="mapData" to="mapData"/>
<param name="regenerate_local_grids" value="true"/>
</node>
</group>
</group>
</launch>
launch/tests/test_map_optimizer.launch
+7 -6
View File
@@ -1,24 +1,25 @@
<?xml version="1.0"?>
<launch>
<include file="$(find rtabmap_ros)/launch/rtabmap.launch">
<arg name="rtabmap_args" value="
--delete_db_on_start
--RGBD/OptimizeMaxError 0
--delete_db_on_start
--RGBD/OptimizeMaxError 0
--Optimizer/Iterations 0
--RGBD/ProximityBySpace false"/>
<arg name="rtabmapviz" value="false"/>
</include>
<group ns="rtabmap">
<node pkg="rtabmap_ros" type="map_optimizer" name="map_optimizer"/>
<node pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" output="screen">
<remap from="mapData" to="mapData_optimized"/>
<param name="frame_id" value="camera_link"/>
<param name="subscribe_depth" value="false"/>
</node>
<node pkg="rtabmap_ros" type="map_assembler" name="map_assembler">
<remap from="mapData" to="mapData_optimized"/>
</node>
</group>
</group>
</launch>
launch/tests/test_obstacles_detection.launch
+4 -3
View File
@@ -1,3 +1,4 @@
<?xml version="1.0"?>
<launch>
<!-- Use stereo_outdoorA.bag for testing -->
@@ -9,16 +10,16 @@
<remap from="disparity/image" to="disparity"/>
<remap from="disparity/camera_info" to="right/camera_info_throttle"/>
<remap from="cloud" to="cloudXYZ"/>
<param name="voxel_size" type="double" value="0.05"/>
<param name="decimation" type="int" value="4"/>
<param name="max_depth" type="double" value="4"/>
</node>
<node pkg="nodelet" type="nodelet" name="obstacles_detection" args="load rtabmap_ros/obstacles_detection obstacles_manager">
<remap from="cloud" to="cloudXYZ"/>
<param name="frame_id" type="string" value="base_footprint"/>
<param name="frame_id" type="string" value="base_footprint"/>
<param name="wait_for_transform" type="bool" value="true"/>
<param name="min_cluster_size" type="int" value="20"/>
<param name="max_obstacles_height" type="double" value="0.0"/>
launch/tests/test_ouster.launch
+21 -20
View File
@@ -1,14 +1,15 @@
<?xml version="1.0"?>
<launch>
<!--
<!--
Hand-held 3D lidar mapping example using only a Ouster OS-1 (no camera).
Prerequisities: rtabmap should be built with libpointmatcher
Example:
$ roslaunch rtabmap_ros test_ouster.launch os1_hostname:=os1-XXXXXXXXXXXX.local os1_udp_dest:=192.168.1.XXX
$ rosrun rviz rviz -f map
$ Show TF and /rtabmap/cloud_map topics
ISSUE: You may have to reset odometry after receiving the first cloud if the map looks tilted. The problem seems
ISSUE: You may have to reset odometry after receiving the first cloud if the map looks tilted. The problem seems
coming from the first cloud sent by os1_cloud_node, which may be poorly synchronized with IMU data.
-->
@@ -22,7 +23,7 @@
<arg name="rtabmapviz" default="true"/>
<arg name="scan_20_hz" default="true"/>
<param if="$(arg use_sim_time)" name="use_sim_time" value="true"/>
<!-- Ouster -->
<remap unless="$(arg use_sim_time)" from="/os1_cloud_node/imu" to="/os1_cloud_node/imu/data_raw"/>
<include unless="$(arg use_sim_time)" file="$(find ouster_ros)/os1.launch">
@@ -40,17 +41,17 @@
<param name="use_mag" value="false"/>
<param name="world_frame" value="enu"/>
<param name="publish_tf" value="false"/>
</node>
</node>
<node pkg="nodelet" type="nodelet" name="imu_to_tf" args="load rtabmap_ros/imu_to_tf imu_nodelet_manager">
<remap from="imu/data" to="/os1_cloud_node/imu/data"/>
<param name="fixed_frame_id" value="$(arg frame_id)_stabilized"/>
<param name="base_frame_id" value="$(arg frame_id)"/>
</node>
</node>
<group ns="rtabmap">
<node pkg="rtabmap_ros" type="icp_odometry" name="icp_odometry" output="screen">
<remap from="scan_cloud" to="/os1_cloud_node/points"/>
<param name="frame_id" type="string" value="$(arg frame_id)"/>
<param name="frame_id" type="string" value="$(arg frame_id)"/>
<param name="odom_frame_id" type="string" value="odom"/>
<param if="$(arg scan_20_hz)" name="expected_update_rate" type="double" value="25"/>
<param unless="$(arg scan_20_hz)" name="expected_update_rate" type="double" value="15"/>
@@ -58,7 +59,7 @@
<remap from="imu" to="/os1_cloud_node/imu/data"/>
<param name="guess_frame_id" type="string" value="$(arg frame_id)_stabilized"/>
<param name="wait_imu_to_init" type="bool" value="true"/>
<!-- ICP parameters -->
<param name="Icp/PointToPlane" type="string" value="true"/>
<param name="Icp/Iterations" type="string" value="10"/>
@@ -69,29 +70,29 @@
<param name="Icp/PointToPlaneRadius" type="string" value="0"/>
<param name="Icp/MaxTranslation" type="string" value="2"/>
<param name="Icp/MaxCorrespondenceDistance" type="string" value="1"/>
<param name="Icp/PM" type="string" value="true"/>
<param name="Icp/PM" type="string" value="true"/>
<param name="Icp/PMOutlierRatio" type="string" value="0.1"/>
<param name="Icp/CorrespondenceRatio" type="string" value="0.01"/>
<param name="Icp/CorrespondenceRatio" type="string" value="0.01"/>
<!-- Odom parameters -->
<!-- Odom parameters -->
<param name="Odom/ScanKeyFrameThr" type="string" value="0.95"/>
<param name="Odom/Strategy" type="string" value="0"/>
<param name="OdomF2M/ScanSubtractRadius" type="string" value="0.2"/>
<param name="OdomF2M/ScanMaxSize" type="string" value="15000"/>
<param name="OdomF2M/ScanMaxSize" type="string" value="15000"/>
</node>
<node pkg="rtabmap_ros" type="rtabmap" name="rtabmap" output="screen" args="-d">
<param name="frame_id" type="string" value="$(arg frame_id)"/>
<node pkg="rtabmap_ros" type="rtabmap" name="rtabmap" output="screen" args="-d">
<param name="frame_id" type="string" value="$(arg frame_id)"/>
<param name="subscribe_depth" type="bool" value="false"/>
<param name="subscribe_rgb" type="bool" value="false"/>
<param name="subscribe_scan_cloud" type="bool" value="true"/>
<param name="approx_sync" type="bool" value="false"/>
<remap from="scan_cloud" to="/os1_cloud_node/points"/>
<remap from="imu" to="/os1_cloud_node/imu/data"/>
<!-- RTAB-Map's parameters -->
<param name="Rtabmap/DetectionRate" type="string" value="1"/>
<param name="Rtabmap/DetectionRate" type="string" value="1"/>
<param name="RGBD/NeighborLinkRefining" type="string" value="false"/>
<param name="RGBD/ProximityBySpace" type="string" value="true"/>
<param name="RGBD/ProximityMaxGraphDepth" type="string" value="0"/>
@@ -103,8 +104,8 @@
<!-- param name="Mem/LaserScanVoxelSize" type="string" value="0.1"/ -->
<!-- param name="Mem/LaserScanNormalK" type="string" value="10"/ -->
<!-- param name="Mem/LaserScanRadius" type="string" value="0"/ -->
<param name="Reg/Strategy" type="string" value="1"/>
<param name="Reg/Strategy" type="string" value="1"/>
<param name="Grid/CellSize" type="string" value="0.1"/>
<param name="Grid/RangeMax" type="string" value="20"/>
<param name="Grid/ClusterRadius" type="string" value="1"/>
@@ -120,7 +121,7 @@
<param name="Icp/Epsilon" type="string" value="0.001"/>
<param name="Icp/MaxTranslation" type="string" value="3"/>
<param name="Icp/MaxCorrespondenceDistance" type="string" value="1"/>
<param name="Icp/PM" type="string" value="true"/>
<param name="Icp/PM" type="string" value="true"/>
<param name="Icp/PMOutlierRatio" type="string" value="0.7"/>
<param name="Icp/CorrespondenceRatio" type="string" value="0.4"/>
</node>
launch/tests/test_ouster_gen2.launch
+34 -33
View File
@@ -1,35 +1,36 @@
<?xml version="1.0"?>
<launch>
<!--
<!--
Hand-held 3D lidar mapping example using only a Ouster GEN2 (no camera).
Prerequisities: rtabmap should be built with libpointmatcher
Example:
$ roslaunch rtabmap_ros test_ouster_gen2.launch sensor_hostname:=os-XXXXXXXXXXXX.local udp_dest:=192.168.1.XXX
$ rosrun rviz rviz -f map
RVIZ: Show TF and /rtabmap/cloud_map topics
ISSUE: You may have to reset odometry after receiving the first cloud if the map looks tilted. The problem seems
ISSUE: You may have to reset odometry after receiving the first cloud if the map looks tilted. The problem seems
coming from the first cloud sent by os_cloud_node, which may be poorly synchronized with IMU data.
PTP mode (synchronize timestamp with host computer time)
* Install:
$ sudo apt install linuxptp httpie
$ printf "[global]\ntx_timestamp_timeout 10\n" >> ~/os.conf
* Running:
(replace "XXXXXXXXXXXX" by your ouster serial, as well as XXX by its IP address)
(replace "eth0" by the network interface used to communicate with ouster)
$ http PUT http://os-XXXXXXXXXXXX.local/api/v1/time/ptp/profile <<< '"default-relaxed"'
$ sudo ptp4l -i eth0 -m -f ~/os.conf -S
$ roslaunch rtabmap_ros test_ouster_gen2.launch sensor_hostname:=os-XXXXXXXXXXXX.local udp_dest:=192.168.1.XXX ptp:=true
-->
<arg name="use_sim_time" default="false"/>
@@ -43,14 +44,14 @@
<arg name="deskewing" default="true"/>
<arg name="slerp" default="false"/>
<arg name="scan_20_hz" default="true"/>
<arg name="voxel_size" default="0.15"/> <!-- indoor: 0.1 to 0.3, outdoor: 0.3 to 0.5 -->
<arg name="voxel_size" default="0.15"/> <!-- indoor: 0.1 to 0.3, outdoor: 0.3 to 0.5 -->
<arg name="assemble" default="false"/>
<arg name="ptp" default="false"/> <!-- See comments in header to start before launching the launch -->
<arg name="imu_topic" default="/os_cloud_node/imu"/>
<arg name="scan_topic" default="/os_cloud_node/points"/>
<param if="$(arg use_sim_time)" name="use_sim_time" value="true"/>
<!-- Ouster -->
<include unless="$(arg use_sim_time)" file="$(find ouster_ros)/ouster.launch">
<arg name="sensor_hostname" value="$(arg sensor_hostname)"/>
@@ -75,8 +76,8 @@
<remap from="imu/data" to="$(arg imu_topic)/filtered"/>
<param name="fixed_frame_id" value="$(arg frame_id)_stabilized"/>
<param name="base_frame_id" value="$(arg frame_id)"/>
</node>
</node>
<!-- Lidar Deskewing -->
<node if="$(arg deskewing)" pkg="nodelet" type="nodelet" name="lidar_deskewing" args="standalone rtabmap_ros/lidar_deskewing" output="screen">
<param name="wait_for_transform" value="0.01"/>
@@ -92,14 +93,14 @@
<node pkg="rtabmap_ros" type="icp_odometry" name="icp_odometry" output="screen">
<remap from="scan_cloud" to="$(arg scan_topic_deskewed)"/>
<remap from="imu" to="$(arg imu_topic)/filtered"/>
<param name="frame_id" type="string" value="$(arg frame_id)"/>
<param name="frame_id" type="string" value="$(arg frame_id)"/>
<param name="odom_frame_id" type="string" value="odom"/>
<param if="$(arg scan_20_hz)" name="expected_update_rate" type="double" value="25"/>
<param unless="$(arg scan_20_hz)" name="expected_update_rate" type="double" value="15"/>
<param name="guess_frame_id" type="string" value="$(arg frame_id)_stabilized"/>
<param name="wait_imu_to_init" type="bool" value="true"/>
<!-- ICP parameters -->
<param name="Icp/PointToPlane" type="string" value="true"/>
<param name="Icp/Iterations" type="string" value="10"/>
@@ -110,31 +111,31 @@
<param name="Icp/PointToPlaneRadius" type="string" value="0"/>
<param name="Icp/MaxTranslation" type="string" value="2"/>
<param name="Icp/MaxCorrespondenceDistance" type="string" value="1"/>
<param name="Icp/PM" type="string" value="true"/>
<param name="Icp/PM" type="string" value="true"/>
<param name="Icp/PMOutlierRatio" type="string" value="0.1"/>
<param name="Icp/CorrespondenceRatio" type="string" value="0.01"/>
<!-- Odom parameters -->
<!-- Odom parameters -->
<param name="Odom/ScanKeyFrameThr" type="string" value="0.95"/>
<param name="Odom/Strategy" type="string" value="0"/>
<param name="OdomF2M/ScanSubtractRadius" type="string" value="$(arg voxel_size)"/>
<param name="OdomF2M/ScanMaxSize" type="string" value="15000"/>
<param name="OdomF2M/ScanMaxSize" type="string" value="15000"/>
</node>
<node pkg="rtabmap_ros" type="rtabmap" name="rtabmap" output="screen" args="-d">
<param name="frame_id" type="string" value="$(arg frame_id)"/>
<node pkg="rtabmap_ros" type="rtabmap" name="rtabmap" output="screen" args="-d">
<param name="frame_id" type="string" value="$(arg frame_id)"/>
<param name="subscribe_depth" type="bool" value="false"/>
<param name="subscribe_rgb" type="bool" value="false"/>
<param name="subscribe_scan_cloud" type="bool" value="true"/>
<param name="approx_sync" type="bool" value="false"/>
<remap if="$(arg assemble)" from="scan_cloud" to="assembled_cloud"/>
<remap unless="$(arg assemble)" from="scan_cloud" to="$(arg scan_topic_deskewed)"/>
<remap from="imu" to="$(arg imu_topic)/filtered"/>
<!-- RTAB-Map's parameters -->
<param if="$(arg assemble)" name="Rtabmap/DetectionRate" type="string" value="0"/> <!-- already set 1 Hz in point_cloud_assembler -->
<param unless="$(arg assemble)" name="Rtabmap/DetectionRate" type="string" value="1"/>
<param unless="$(arg assemble)" name="Rtabmap/DetectionRate" type="string" value="1"/>
<param name="RGBD/NeighborLinkRefining" type="string" value="false"/>
<param name="RGBD/ProximityBySpace" type="string" value="true"/>
<param name="RGBD/ProximityMaxGraphDepth" type="string" value="0"/>
@@ -147,10 +148,10 @@
<!-- param name="Mem/LaserScanVoxelSize" type="string" value="0.1"/ -->
<!-- param name="Mem/LaserScanNormalK" type="string" value="10"/ -->
<!-- param name="Mem/LaserScanRadius" type="string" value="0"/ -->
<param name="Reg/Strategy" type="string" value="1"/>
<param name="Reg/Strategy" type="string" value="1"/>
<param name="Optimizer/GravitySigma" type="string" value="0.5"/>
<param name="Optimizer/Strategy" type="string" value="1"/>
<param name="Optimizer/Strategy" type="string" value="1"/>
<param name="Grid/CellSize" type="string" value="0.1"/>
<param name="Grid/RangeMax" type="string" value="20"/>
<param name="Grid/ClusterRadius" type="string" value="1"/>
@@ -165,11 +166,11 @@
<param name="Icp/Epsilon" type="string" value="0.001"/>
<param name="Icp/MaxTranslation" type="string" value="3"/>
<param name="Icp/MaxCorrespondenceDistance" type="string" value="1"/>
<param name="Icp/PM" type="string" value="true"/>
<param name="Icp/PM" type="string" value="true"/>
<param name="Icp/PMOutlierRatio" type="string" value="0.7"/>
<param name="Icp/CorrespondenceRatio" type="string" value="0.2"/>
</node>
<node if="$(arg assemble)" pkg="rtabmap_ros" type="point_cloud_assembler" name="point_cloud_assembler" output="screen">
<remap from="cloud" to="$(arg scan_topic_deskewed)"/>
<remap from="odom" to="odom"/>
launch/tests/test_pointxyzrgb.launch
+11 -10
View File
@@ -1,20 +1,21 @@
<?xml version="1.0"?>
<launch>
<include file="$(find freenect_launch)/launch/freenect.launch" >
<arg name="depth_registration" value="true"/>
<arg name="data_skip" value="3"/>
</include>
<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="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"/>
@@ -26,7 +27,7 @@
<param name="noise_filter_radius" type="double" value="0.05"/>
<param name="normal_k" type="int" value="6"/>
</node>
<node pkg="nodelet" type="nodelet" name="points_xyz" args="load rtabmap_ros/point_cloud_xyz camera_nodelet_manager">
<remap from="depth/image" to="depth/image_out"/>
<remap from="depth/camera_info" to="rgb/camera_info_out"/>
@@ -42,10 +43,10 @@
<!-- 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"/>
@@ -55,18 +56,18 @@
<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"/>
launch/tests/test_prior.launch
+23 -22
View File
@@ -1,44 +1,45 @@
<?xml version="1.0"?>
<launch>
<!-- Example with rgbd datasets:
$ wget http://vision.in.tum.de/rgbd/dataset/freiburg3/rgbd_dataset_freiburg3_long_office_household.bag
$ rosbag decompress rgbd_dataset_freiburg3_long_office_household.bag
$ chmod +x test_prior_rename_kinect_bag_tf.py
$ ./test_prior_rename_kinect_bag_tf.py
We simulate an external "global_pose" by republishing ground truth TF (VICON) with some
covariance, normally you should not have to use test_prior_tf_to_pose.py as the node
We simulate an external "global_pose" by republishing ground truth TF (VICON) with some
covariance, normally you should not have to use test_prior_tf_to_pose.py as the node
publishing the global pose would give it directly as a pose with correct covariance.
Rename all child_frame_id "/kinect" to "/kinect_gt" Tf in the bag
Rename all child_frame_id "/kinect" to "/kinect_gt" Tf in the bag
using test_prior_rename_kinect_bag_tf.py in this directory!
$ roslaunch rtabmap_ros test_prior.launch
$ roslaunch rtabmap_ros test_prior.launch
$ chmod +x test_prior_tf_to_pose.py
$ ./test_prior_tf_to_pose.py
$ rosbag play -.-clock rgbd_dataset_freiburg3_long_office_household_tf_renamed.bag
-->
<param name="use_sim_time" type="bool" value="True"/>
<!-- Choose visualization -->
<arg name="rviz" default="true" />
<arg name="rtabmapviz" default="false" />
<arg name="rtabmapviz" default="false" />
<!-- TF FRAMES -->
<node pkg="tf" type="static_transform_publisher" name="world_to_map"
<node pkg="tf" type="static_transform_publisher" name="world_to_map"
args="0.0 0.0 0.0 0.0 0.0 0.0 /world /map 100" />
<group ns="rtabmap">
<!-- Odometry -->
<node pkg="rtabmap_ros" type="rgbd_odometry" name="rgbd_odometry" output="screen">
<remap from="rgb/image" to="/camera/rgb/image_color"/>
<remap from="depth/image" to="/camera/depth/image"/>
<remap from="rgb/camera_info" to="/camera/rgb/camera_info"/>
<remap from="odom" to="vis_odom"/>
<param name="odom_frame_id" type="string" value="vis_odom"/>
<param name="frame_id" type="string" value="kinect"/>
</node>
@@ -47,11 +48,11 @@
<node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="--delete_db_on_start">
<param name="subscribe_depth" type="bool" value="true"/>
<param name="frame_id" type="string" value="kinect"/>
<param name="RGBD/CreateOccupancyGrid" type="string" value="false"/>
<param name="Optimizer/PriorsIgnored" type="string" value="false"/>
<param name="Optimizer/Strategy" type="string" value="1"/>
<remap from="rgb/image" to="/camera/rgb/image_color"/>
<remap from="depth/image" to="/camera/depth/image"/>
<remap from="rgb/camera_info" to="/camera/rgb/camera_info"/>
@@ -60,24 +61,24 @@
<param name="ground_truth_frame_id" type="string" value="world"/>
<param name="ground_truth_base_frame_id" type="string" value="kinect_gt"/>
</node>
<!-- Visualisation -->
<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_laserScan" type="bool" value="false"/>
<param name="subscribe_odom_info" type="bool" value="true"/>
<param name="queue_size" type="int" value="30"/>
<param name="frame_id" type="string" value="kinect"/>
<remap from="rgb/image" to="/camera/rgb/image_color"/>
<remap from="depth/image" to="/camera/depth/image"/>
<remap from="rgb/camera_info" to="/camera/rgb/camera_info"/>
<remap from="odom" to="vis_odom"/>
</node>
</group>
<node if="$(arg rviz)" pkg="rviz" type="rviz" name="rviz" args="-d $(find rtabmap_ros)/launch/config/rgbdslam_datasets.rviz"/>
</launch>
launch/tests/test_prior_rename_kinect_bag_tf.py
+1 -1
View File
@@ -1,4 +1,4 @@
#!/usr/bin/env python
#!/usr/bin/env python
import rosbag
from tf.msg import tfMessage
with rosbag.Bag('rgbd_dataset_freiburg3_long_office_household_tf_renamed.bag', 'w') as outbag:
launch/tests/test_prior_tf_to_pose.py
+4 -4
View File
@@ -1,4 +1,4 @@
#!/usr/bin/env python
#!/usr/bin/env python
import rospy
import tf
import numpy
@@ -14,9 +14,9 @@ if __name__ == '__main__':
cov = rospy.get_param('~cov', 1)
rateParam = rospy.get_param('~rate', 30) # 10hz
pub = rospy.Publisher('global_pose', PoseWithCovarianceStamped, queue_size=1)
print 'start loop!'
rate = rospy.Rate(rateParam)
rate = rospy.Rate(rateParam)
while not rospy.is_shutdown():
poseOut = PoseWithCovarianceStamped()
try:
@@ -27,7 +27,7 @@ if __name__ == '__main__':
print str(e)
rate.sleep()
continue
poseOut.header.stamp.nsecs = now.nsecs
poseOut.header.stamp.secs = now.secs
poseOut.header.frame_id = outputFrame
launch/tests/test_rgbd_image.launch
+7 -6
View File
@@ -1,5 +1,6 @@
<?xml version="1.0"?>
<launch>
<launch>
<!-- Kinect: -->
<include file="$(find freenect_launch)/launch/freenect.launch">
@@ -20,17 +21,17 @@
<!-- Nodes -->
<group ns="rtabmap">
<!-- RGB-D Odometry -->
<node pkg="rtabmap_ros" type="rgbd_odometry" name="rgbd_odometry" output="screen">
<param name="subscribe_rgbd" type="bool" value="true"/>
<remap if="$(arg compressed)" from="rgbd_image" to="/camera/rgbd_image/compressed"/>
<remap unless="$(arg compressed)" from="rgbd_image" to="/camera/rgbd_image"/>
<param name="Odom/AlignWithGround" type="string" value="true"/>
<param name="frame_id" type="string" value="camera_link"/>
</node>
<node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="--delete_db_on_start">
<param name="subscribe_depth" type="bool" value="false"/>
<param name="subscribe_rgbd" type="bool" value="true"/>
@@ -39,7 +40,7 @@
<remap if="$(arg compressed)" from="rgbd_image" to="/camera/rgbd_image/compressed"/>
<remap unless="$(arg compressed)" from="rgbd_image" to="/camera/rgbd_image"/>
</node>
<node pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" output="screen">
<param name="subscribe_depth" type="bool" value="false"/>
<param name="subscribe_rgbd" type="bool" value="true"/>
@@ -49,7 +50,7 @@
<remap unless="$(arg compressed)" from="rgbd_image" to="/camera/rgbd_image"/>
<param name="subscribe_odom_info" type="bool" value="true"/>
</node>
</group>
</launch>
launch/tests/test_rtabmap_nodelets.launch
+12 -11
View File
@@ -1,3 +1,4 @@
<?xml version="1.0"?>
<launch>
@@ -5,32 +6,32 @@
<include file="$(find freenect_launch)/launch/freenect.launch">
<arg name="depth_registration" value="true" />
</include>
<arg name="frame_id" default="camera_link"/>
<arg name="rtabmap_args" default="--delete_db_on_start"/> <!-- delete_db_on_start, udebug -->
<arg name="frame_id" default="camera_link"/>
<arg name="rtabmap_args" default="--delete_db_on_start"/> <!-- delete_db_on_start, udebug -->
<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" />
<group ns="camera">
<group ns="camera">
<!-- Use RGBD synchronization -->
<node pkg="nodelet" type="nodelet" name="rgbd_sync" args="load rtabmap_ros/rgbd_sync camera_nodelet_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)"/>
</node>
<!-- RGB-D Odometry -->
<node pkg="nodelet" type="nodelet" name="rgbd_odometry" args="load rtabmap_ros/rgbd_odometry camera_nodelet_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"/>
</node>
<!-- RTAB-Map -->
<node pkg="nodelet" type="nodelet" name="rtabmap" args="load rtabmap_ros/rtabmap camera_nodelet_manager $(arg rtabmap_args)">
<param name="frame_id" type="string" value="$(arg frame_id)"/>
@@ -38,7 +39,7 @@
<param name="subscribe_rgbd" type="bool" value="true"/>
<param name="approx_sync" type="bool" value="false"/>
</node>
<!-- Visualisation -->
<node pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" output="screen">
<param name="frame_id" type="string" value="$(arg frame_id)"/>
@@ -46,7 +47,7 @@
<param name="subscribe_odom_info" type="bool" value="true"/>
<param name="approx_sync" type="bool" value="false"/>
</node>
</group>
</launch>
launch/tests/test_two_kinects_one_map.launch
+6 -5
View File
@@ -1,15 +1,16 @@
<?xml version="1.0"?>
<launch>
<!-- Testing 2 Kinects localizing in the same map at the same time -->
<!-- Prerequisities: the default ~/.ros/rtabmap.db should be
<!-- Prerequisities: the default ~/.ros/rtabmap.db should be
already created with one of the Kinect using mapping tutorial:
http://wiki.ros.org/rtabmap_ros/Tutorials/HandHeldMapping
http://wiki.ros.org/rtabmap_ros/Tutorials/HandHeldMapping
$ roslaunch freenect_launch freenect.launch depth_registration:=true
$ roslaunch rtabmap_ros rtabmap.launch args:="-d"
-->
<!-- Cameras -->
<include file="$(find freenect_launch)/launch/freenect.launch">
<arg name="depth_registration" value="True" />
launch/tests/test_undistort_depth.launch
+5 -4
View File
@@ -1,3 +1,4 @@
<?xml version="1.0"?>
<launch>
@@ -5,16 +6,16 @@
<include file="$(find openni2_launch)/launch/openni2.launch">
<arg name="depth_registration" value="true" />
</include>
<arg name="depth" default="/camera/depth_registered/image_raw" />
<arg name="model" default="$(find rtabmap_ros)/launch/calibration/distortion_model_PS1080.bin" /> <!-- XTION Live Pro -->
<group ns="camera">
<group ns="camera">
<!-- Undistort depth image -->
<node pkg="nodelet" type="nodelet" name="undistort" args="load rtabmap_ros/undistort_depth camera_nodelet_manager">
<remap from="depth" to="$(arg depth)"/>
<param name="model" value="$(arg model)"/>
</node>
</node>
</group>
</launch>
launch/tests/test_use_odom_features.launch
+12 -11
View File
@@ -1,3 +1,4 @@
<?xml version="1.0"?>
<launch>
@@ -5,25 +6,25 @@
<include file="$(find freenect_launch)/launch/freenect.launch">
<arg name="depth_registration" value="true" />
</include>
<arg name="frame_id" default="camera_link"/>
<arg name="rtabmap_args" default="--delete_db_on_start"/> <!-- delete_db_on_start, udebug -->
<arg name="frame_id" default="camera_link"/>
<arg name="rtabmap_args" default="--delete_db_on_start"/> <!-- delete_db_on_start, udebug -->
<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" />
<group ns="camera">
<group ns="camera">
<!-- Use RGBD synchronization -->
<node pkg="nodelet" type="nodelet" name="rgbd_sync" args="load rtabmap_ros/rgbd_sync camera_nodelet_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)"/>
</node>
<!-- RGB-D Odometry -->
<node pkg="nodelet" type="nodelet" name="rgbd_odometry" args="load rtabmap_ros/rgbd_odometry camera_nodelet_manager $(arg odom_args)">
<param name="frame_id" type="string" value="$(arg frame_id)"/>
@@ -31,7 +32,7 @@
<param name="subscribe_rgbd" type="bool" value="true"/>
<param name="keep_color" type="bool" value="true"/>
</node>
<!-- RTAB-Map -->
<node pkg="rtabmap_ros" type="rtabmap" name="rtabmap" args="$(arg rtabmap_args)" output="screen">
<param name="frame_id" type="string" value="$(arg frame_id)"/>
@@ -39,7 +40,7 @@
<param name="approx_sync" type="bool" value="false"/>
<remap from="rgbd_image" to="odom_rgbd_image"/>
</node>
<!-- Visualisation -->
<node pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" output="screen">
<param name="frame_id" type="string" value="$(arg frame_id)"/>
@@ -47,7 +48,7 @@
<param name="subscribe_odom_info" type="bool" value="true"/>
<param name="approx_sync" type="bool" value="false"/>
</node>
</group>
</launch>
launch/tests/test_velodyne.launch
+23 -22
View File
@@ -1,7 +1,8 @@
<?xml version="1.0"?>
<!-- -->
<launch>
<!--
<!--
Hand-held 3D lidar mapping example using only a Velodyne PUCK (no camera).
Prerequisities: rtabmap should be built with libpointmatcher
Example:
@@ -28,26 +29,26 @@
<arg name="resolution" default="0.05"/> <!-- set 0.05-0.3 for indoor, set 0.3-0.5 for outdoor (0.4 for kitti) -->
<arg name="iterations" default="10"/>
<!-- Grid parameters -->
<arg name="ground_is_obstacle" default="true"/>
<arg name="grid_max_range" default="20"/>
<!-- For F2M Odometry -->
<arg name="ground_normals_up" default="false"/> <!-- set to true when velodyne is always horizontal to ground (ground robot, car, kitti) -->
<arg name="local_map_size" default="15000"/>
<arg name="key_frame_thr" default="0.6"/>
<!-- For FLOAM Odometry -->
<arg name="floam" default="false"/> <!-- RTAB-Map should be built with FLOAM http://official-rtab-map-forum.206.s1.nabble.com/icp-odometry-with-LOAM-crash-tp8261p8563.html -->
<arg name="floam_sensor" default="0"/> <!-- 0=16 rings (VLP16), 1=32 rings, 2=64 rings (kitti dataset) -->
<include unless="$(arg use_sim_time)" file="$(find velodyne_pointcloud)/launch/VLP16_points.launch">
<arg if="$(arg scan_20_hz)" name="rpm" value="1200"/>
<arg unless="$(arg scan_20_hz)" name="rpm" value="600"/>
<arg name="organize_cloud" value="$(arg organize_cloud)"/>
</include>
<!-- IMU orientation estimation and publish tf -->
<node if="$(arg use_imu)" pkg="nodelet" type="nodelet" name="imu_nodelet_manager" args="manager">
<remap from="imu/data_raw" to="$(arg imu_topic)"/>
@@ -62,12 +63,12 @@
<remap from="imu/data" to="$(arg imu_topic)/filtered"/>
<param name="fixed_frame_id" value="$(arg frame_id)_stabilized"/>
<param name="base_frame_id" value="$(arg frame_id)"/>
</node>
</node>
<group ns="rtabmap">
<node pkg="rtabmap_ros" type="icp_odometry" name="icp_odometry" output="screen">
<remap from="scan_cloud" to="$(arg scan_topic)"/>
<param name="frame_id" type="string" value="$(arg frame_id)"/>
<param name="frame_id" type="string" value="$(arg frame_id)"/>
<param name="odom_frame_id" type="string" value="odom"/>
<param name="deskewing" type="bool" value="$(arg deskewing)"/>
<param name="deskewing_slerp" type="bool" value="$(arg slerp)"/>
@@ -79,7 +80,7 @@
<remap if="$(arg use_imu)" from="imu" to="$(arg imu_topic)/filtered"/>
<param if="$(arg use_imu)" name="guess_frame_id" type="string" value="$(arg frame_id)_stabilized"/>
<param if="$(arg use_imu)" name="wait_imu_to_init" type="bool" value="true"/>
<!-- ICP parameters -->
<param name="Icp/PointToPlane" type="string" value="true"/>
<param name="Icp/Iterations" type="string" value="$(arg iterations)"/>
@@ -92,37 +93,37 @@
<param name="Icp/PointToPlaneRadius" type="string" value="0"/>
<param name="Icp/MaxTranslation" type="string" value="2"/>
<param name="Icp/MaxCorrespondenceDistance" type="string" value="$(eval resolution*10)"/>
<param name="Icp/PM" type="string" value="true"/>
<param name="Icp/PM" type="string" value="true"/>
<param name="Icp/PMOutlierRatio" type="string" value="0.7"/>
<param name="Icp/CorrespondenceRatio" type="string" value="0.01"/>
<param name="Icp/CorrespondenceRatio" type="string" value="0.01"/>
<param if="$(arg ground_normals_up)" name="Icp/PointToPlaneGroundNormalsUp" type="string" value="0.8"/>
<!-- Odom parameters -->
<!-- Odom parameters -->
<param name="Odom/ScanKeyFrameThr" type="string" value="$(arg key_frame_thr)"/>
<param if="$(arg floam)" name="Odom/Strategy" type="string" value="11"/>
<param unless="$(arg floam)" name="Odom/Strategy" type="string" value="0"/>
<param name="OdomF2M/ScanSubtractRadius" type="string" value="$(arg resolution)"/>
<param name="OdomF2M/ScanMaxSize" type="string" value="$(arg local_map_size)"/>
<param name="OdomLOAM/Sensor" type="string" value="$(arg floam_sensor)"/>
<param name="OdomLOAM/Sensor" type="string" value="$(arg floam_sensor)"/>
<param name="OdomLOAM/Resolution" type="string" value="$(arg resolution)"/>
<param if="$(eval not deskewing and scan_20_hz)" name="OdomLOAM/ScanPeriod" type="string" value="0.05"/>
<param if="$(eval not deskewing and not scan_20_hz)" name="OdomLOAM/ScanPeriod" type="string" value="0.1"/>
<param if="$(arg deskewing)" name="OdomLOAM/ScanPeriod" type="string" value="0"/>
</node>
<node pkg="rtabmap_ros" type="rtabmap" name="rtabmap" output="screen" args="-d">
<param name="frame_id" type="string" value="$(arg frame_id)"/>
<node pkg="rtabmap_ros" type="rtabmap" name="rtabmap" output="screen" args="-d">
<param name="frame_id" type="string" value="$(arg frame_id)"/>
<param name="subscribe_depth" type="bool" value="false"/>
<param name="subscribe_rgb" type="bool" value="false"/>
<param name="subscribe_scan_cloud" type="bool" value="true"/>
<param name="approx_sync" type="bool" value="false"/>
<param name="wait_for_transform_duration" type="double" value="0.2"/>
<remap from="scan_cloud" to="assembled_cloud"/>
<remap from="imu" to="$(arg imu_topic)/filtered"/>
<!-- RTAB-Map's parameters -->
<param name="Rtabmap/DetectionRate" type="string" value="1"/>
<param name="Rtabmap/DetectionRate" type="string" value="1"/>
<param name="RGBD/NeighborLinkRefining" type="string" value="false"/>
<param name="RGBD/ProximityBySpace" type="string" value="true"/>
<param name="RGBD/ProximityMaxGraphDepth" type="string" value="0"/>
@@ -132,8 +133,8 @@
<param name="Mem/NotLinkedNodesKept" type="string" value="false"/>
<param name="Mem/STMSize" type="string" value="30"/>
<param name="Mem/LaserScanNormalK" type="string" value="20"/>
<param name="Reg/Strategy" type="string" value="1"/>
<param name="Reg/Strategy" type="string" value="1"/>
<param name="Grid/CellSize" type="string" value="$(arg resolution)"/>
<param name="Grid/RangeMax" type="string" value="$(arg grid_max_range)"/>
<param name="Grid/ClusterRadius" type="string" value="1"/>
@@ -149,7 +150,7 @@
<param name="Icp/Epsilon" type="string" value="0.001"/>
<param name="Icp/MaxTranslation" type="string" value="3"/>
<param name="Icp/MaxCorrespondenceDistance" type="string" value="$(eval resolution*10)"/>
<param name="Icp/PM" type="string" value="true"/>
<param name="Icp/PM" type="string" value="true"/>
<param name="Icp/PMOutlierRatio" type="string" value="0.7"/>
<param name="Icp/CorrespondenceRatio" type="string" value="$(arg loop_ratio)"/>
</node>
launch/tests/test_velodyne_d435i_deskewing.launch
+25 -24
View File
@@ -1,7 +1,8 @@
<?xml version="1.0"?>
<!-- -->
<launch>
<!--
<!--
Hand-held 3D lidar mapping example using a Velodyne PUCK, an external IMU and color camera (using D435i as example).
Prerequisities: rtabmap should be built with libpointmatcher
We use D435i imu only for lidar deskewing and icp_odometry guess in this example.
@@ -27,30 +28,30 @@
<arg name="resolution" default="0.05"/> <!-- set 0.05-0.3 for indoor, set 0.3-0.5 for outdoor -->
<arg name="iterations" default="10"/>
<!-- Grid parameters -->
<arg name="ground_is_obstacle" default="true"/>
<arg name="grid_max_range" default="20"/>
<!-- For F2M Odometry -->
<arg name="ground_normals_up" default="false"/> <!-- set to true when velodyne is always horizontal to ground (ground robot, car) -->
<arg name="local_map_size" default="15000"/>
<arg name="key_frame_thr" default="0.6"/>
<!-- For FLOAM Odometry -->
<arg name="floam" default="false"/> <!-- RTAB-Map should be built with FLOAM http://official-rtab-map-forum.206.s1.nabble.com/icp-odometry-with-LOAM-crash-tp8261p8563.html -->
<arg name="floam_sensor" default="0"/> <!-- 0=16 rings (VLP16), 1=32 rings, 2=64 rings -->
<!-- Static transform between velodyne and D435i: TODO: Adjust with real position/orientation!!! -->
<node unless="$(arg use_sim_time)" pkg="tf" type="static_transform_publisher" name="velodyne_to_camera_tf" args="0.03 0.064 -0.055 0 -0.02 0 velodyne camera_link 100"/>
<!-- Velodyne sensor VLP16 -->
<include unless="$(arg use_sim_time)" file="$(find velodyne_pointcloud)/launch/VLP16_points.launch">
<arg if="$(arg scan_20_hz)" name="rpm" value="1200"/>
<arg unless="$(arg scan_20_hz)" name="rpm" value="600"/>
<arg name="organize_cloud" value="true"/> <!-- should be organized for deskewing -->
</include>
<!-- D435i -->
<group unless="$(arg use_sim_time)">
<include file="$(find realsense2_camera)/launch/rs_camera.launch">
@@ -59,7 +60,7 @@
<arg name="enable_accel" value="true"/>
</include>
</group>
<!-- IMU orientation estimation and publish tf accordingly to os_sensor frame -->
<node pkg="nodelet" type="nodelet" name="imu_nodelet_manager" args="manager">
<remap from="imu/data_raw" to="$(arg imu_topic)"/>
@@ -74,8 +75,8 @@
<remap from="imu/data" to="$(arg imu_topic)/filtered"/>
<param name="fixed_frame_id" value="$(arg frame_id)_stabilized"/>
<param name="base_frame_id" value="$(arg frame_id)"/>
</node>
</node>
<!-- Lidar Deskewing -->
<node if="$(arg deskewing)" pkg="nodelet" type="nodelet" name="lidar_deskewing" args="standalone rtabmap_ros/lidar_deskewing" output="screen">
<param name="wait_for_transform" value="0.01"/>
@@ -99,7 +100,7 @@
<param name="wait_for_transform_duration" type="double" value="0.2"/>
<param if="$(arg scan_20_hz)" name="expected_update_rate" type="double" value="25"/>
<param unless="$(arg scan_20_hz)" name="expected_update_rate" type="double" value="15"/>
<!-- ICP parameters -->
<param name="Icp/PointToPlane" type="string" value="true"/>
<param name="Icp/Iterations" type="string" value="$(arg iterations)"/>
@@ -112,39 +113,39 @@
<param name="Icp/PointToPlaneRadius" type="string" value="0"/>
<param name="Icp/MaxTranslation" type="string" value="2"/>
<param name="Icp/MaxCorrespondenceDistance" type="string" value="$(eval resolution*10)"/>
<param name="Icp/PM" type="string" value="true"/>
<param name="Icp/PM" type="string" value="true"/>
<param name="Icp/PMOutlierRatio" type="string" value="0.7"/>
<param name="Icp/CorrespondenceRatio" type="string" value="0.01"/>
<param name="Icp/CorrespondenceRatio" type="string" value="0.01"/>
<param if="$(arg ground_normals_up)" name="Icp/PointToPlaneGroundNormalsUp" type="string" value="0.8"/>
<!-- Odom parameters -->
<!-- Odom parameters -->
<param name="Odom/ScanKeyFrameThr" type="string" value="$(arg key_frame_thr)"/>
<param if="$(arg floam)" name="Odom/Strategy" type="string" value="11"/>
<param unless="$(arg floam)" name="Odom/Strategy" type="string" value="0"/>
<param name="OdomF2M/ScanSubtractRadius" type="string" value="$(arg resolution)"/>
<param name="OdomF2M/ScanMaxSize" type="string" value="$(arg local_map_size)"/>
<param name="OdomLOAM/Sensor" type="string" value="$(arg floam_sensor)"/>
<param name="OdomLOAM/Sensor" type="string" value="$(arg floam_sensor)"/>
<param name="OdomLOAM/Resolution" type="string" value="$(arg resolution)"/>
<param if="$(eval not deskewing and scan_20_hz)" name="OdomLOAM/ScanPeriod" type="string" value="0.05"/>
<param if="$(eval not deskewing and not scan_20_hz)" name="OdomLOAM/ScanPeriod" type="string" value="0.1"/>
<param if="$(arg deskewing)" name="OdomLOAM/ScanPeriod" type="string" value="0"/>
</node>
<node pkg="rtabmap_ros" type="rtabmap" name="rtabmap" output="screen" args="-d">
<param name="frame_id" type="string" value="$(arg frame_id)"/>
<node pkg="rtabmap_ros" type="rtabmap" name="rtabmap" output="screen" args="-d">
<param name="frame_id" type="string" value="$(arg frame_id)"/>
<param name="subscribe_depth" type="bool" value="false"/>
<param name="subscribe_rgb" type="bool" value="true"/>
<param name="subscribe_scan_cloud" type="bool" value="true"/>
<param name="approx_sync" type="bool" value="true"/>
<param name="wait_for_transform_duration" type="double" value="0.2"/>
<remap from="scan_cloud" to="assembled_cloud"/>
<remap from="rgb/image" to="/camera/color/image_raw"/>
<remap from="rgb/camera_info" to="/camera/color/camera_info"/>
<remap from="imu" to="$(arg imu_topic)/filtered"/>
<!-- RTAB-Map's parameters -->
<param name="Rtabmap/DetectionRate" type="string" value="1"/>
<param name="Rtabmap/DetectionRate" type="string" value="1"/>
<param name="RGBD/NeighborLinkRefining" type="string" value="false"/>
<param name="RGBD/ProximityBySpace" type="string" value="true"/>
<param name="RGBD/ProximityMaxGraphDepth" type="string" value="0"/>
@@ -154,8 +155,8 @@
<param name="Mem/NotLinkedNodesKept" type="string" value="false"/>
<param name="Mem/STMSize" type="string" value="30"/>
<param name="Mem/LaserScanNormalK" type="string" value="20"/>
<param name="Reg/Strategy" type="string" value="1"/>
<param name="Reg/Strategy" type="string" value="1"/>
<param name="Grid/CellSize" type="string" value="$(arg resolution)"/>
<param name="Grid/RangeMax" type="string" value="$(arg grid_max_range)"/>
<param name="Grid/ClusterRadius" type="string" value="1"/>
@@ -171,7 +172,7 @@
<param name="Icp/Epsilon" type="string" value="0.001"/>
<param name="Icp/MaxTranslation" type="string" value="3"/>
<param name="Icp/MaxCorrespondenceDistance" type="string" value="$(eval resolution*10)"/>
<param name="Icp/PM" type="string" value="true"/>
<param name="Icp/PM" type="string" value="true"/>
<param name="Icp/PMOutlierRatio" type="string" value="0.7"/>
<param name="Icp/CorrespondenceRatio" type="string" value="$(arg loop_ratio)"/>
</node>
launch/tests/test_velodyne_t265_deskewing.launch
+23 -22
View File
@@ -1,7 +1,8 @@
<?xml version="1.0"?>
<!-- -->
<launch>
<!--
<!--
Hand-held 3D lidar mapping example using only a Velodyne PUCK and external odometry (using t265 as example).
Prerequisities: rtabmap should be built with libpointmatcher
We use T265 only for lidar deskewing and icp_odometry guess in this example.
@@ -27,30 +28,30 @@
<arg name="resolution" default="0.05"/> <!-- set 0.05-0.3 for indoor, set 0.3-0.5 for outdoor -->
<arg name="iterations" default="10"/>
<!-- Grid parameters -->
<arg name="ground_is_obstacle" default="true"/>
<arg name="grid_max_range" default="20"/>
<!-- For F2M Odometry -->
<arg name="ground_normals_up" default="false"/> <!-- set to true when velodyne is always horizontal to ground (ground robot, car) -->
<arg name="local_map_size" default="15000"/>
<arg name="key_frame_thr" default="0.6"/>
<!-- For FLOAM Odometry -->
<arg name="floam" default="false"/> <!-- RTAB-Map should be built with FLOAM http://official-rtab-map-forum.206.s1.nabble.com/icp-odometry-with-LOAM-crash-tp8261p8563.html -->
<arg name="floam_sensor" default="0"/> <!-- 0=16 rings (VLP16), 1=32 rings, 2=64 rings -->
<!-- Static transform between velodyne and T265: TODO: Adjust with real position/orientation!!! -->
<node unless="$(arg use_sim_time)" pkg="tf" type="static_transform_publisher" name="T265_to_velodyne_tf" args="-0.01 0 0.055 0 0 0 t265_link velodyne 100"/>
<!-- Velodyne sensor VLP16 -->
<include unless="$(arg use_sim_time)" file="$(find velodyne_pointcloud)/launch/VLP16_points.launch">
<arg if="$(arg scan_20_hz)" name="rpm" value="1200"/>
<arg unless="$(arg scan_20_hz)" name="rpm" value="600"/>
<arg name="organize_cloud" value="true"/> <!-- should be organized for deskewing -->
</include>
<!-- T265 -->
<group unless="$(arg use_sim_time)" ns="t265">
<include file="$(find realsense2_camera)/launch/includes/nodelet.launch.xml">
@@ -65,7 +66,7 @@
<arg name="enable_pose" value="true"/>
</include>
</group>
<!-- Lidar Deskewing -->
<node if="$(arg deskewing)" pkg="nodelet" type="nodelet" name="lidar_deskewing" args="standalone rtabmap_ros/lidar_deskewing" output="screen">
<param name="wait_for_transform" value="0.01"/>
@@ -87,7 +88,7 @@
<param name="wait_for_transform_duration" type="double" value="0.2"/>
<param if="$(arg scan_20_hz)" name="expected_update_rate" type="double" value="25"/>
<param unless="$(arg scan_20_hz)" name="expected_update_rate" type="double" value="15"/>
<!-- ICP parameters -->
<param name="Icp/PointToPlane" type="string" value="true"/>
<param name="Icp/Iterations" type="string" value="$(arg iterations)"/>
@@ -100,36 +101,36 @@
<param name="Icp/PointToPlaneRadius" type="string" value="0"/>
<param name="Icp/MaxTranslation" type="string" value="2"/>
<param name="Icp/MaxCorrespondenceDistance" type="string" value="$(eval resolution*10)"/>
<param name="Icp/PM" type="string" value="true"/>
<param name="Icp/PM" type="string" value="true"/>
<param name="Icp/PMOutlierRatio" type="string" value="0.7"/>
<param name="Icp/CorrespondenceRatio" type="string" value="0.01"/>
<param name="Icp/CorrespondenceRatio" type="string" value="0.01"/>
<param if="$(arg ground_normals_up)" name="Icp/PointToPlaneGroundNormalsUp" type="string" value="0.8"/>
<!-- Odom parameters -->
<!-- Odom parameters -->
<param name="Odom/ScanKeyFrameThr" type="string" value="$(arg key_frame_thr)"/>
<param if="$(arg floam)" name="Odom/Strategy" type="string" value="11"/>
<param unless="$(arg floam)" name="Odom/Strategy" type="string" value="0"/>
<param name="OdomF2M/ScanSubtractRadius" type="string" value="$(arg resolution)"/>
<param name="OdomF2M/ScanMaxSize" type="string" value="$(arg local_map_size)"/>
<param name="OdomLOAM/Sensor" type="string" value="$(arg floam_sensor)"/>
<param name="OdomLOAM/Sensor" type="string" value="$(arg floam_sensor)"/>
<param name="OdomLOAM/Resolution" type="string" value="$(arg resolution)"/>
<param if="$(eval not deskewing and scan_20_hz)" name="OdomLOAM/ScanPeriod" type="string" value="0.05"/>
<param if="$(eval not deskewing and not scan_20_hz)" name="OdomLOAM/ScanPeriod" type="string" value="0.1"/>
<param if="$(arg deskewing)" name="OdomLOAM/ScanPeriod" type="string" value="0"/>
</node>
<node pkg="rtabmap_ros" type="rtabmap" name="rtabmap" output="screen" args="-d">
<param name="frame_id" type="string" value="$(arg frame_id)"/>
<node pkg="rtabmap_ros" type="rtabmap" name="rtabmap" output="screen" args="-d">
<param name="frame_id" type="string" value="$(arg frame_id)"/>
<param name="subscribe_depth" type="bool" value="false"/>
<param name="subscribe_rgb" type="bool" value="false"/>
<param name="subscribe_scan_cloud" type="bool" value="true"/>
<param name="approx_sync" type="bool" value="false"/>
<param name="wait_for_transform_duration" type="double" value="0.2"/>
<remap from="scan_cloud" to="assembled_cloud"/>
<!-- RTAB-Map's parameters -->
<param name="Rtabmap/DetectionRate" type="string" value="1"/>
<param name="Rtabmap/DetectionRate" type="string" value="1"/>
<param name="RGBD/NeighborLinkRefining" type="string" value="false"/>
<param name="RGBD/ProximityBySpace" type="string" value="true"/>
<param name="RGBD/ProximityMaxGraphDepth" type="string" value="0"/>
@@ -139,8 +140,8 @@
<param name="Mem/NotLinkedNodesKept" type="string" value="false"/>
<param name="Mem/STMSize" type="string" value="30"/>
<param name="Mem/LaserScanNormalK" type="string" value="20"/>
<param name="Reg/Strategy" type="string" value="1"/>
<param name="Reg/Strategy" type="string" value="1"/>
<param name="Grid/CellSize" type="string" value="$(arg resolution)"/>
<param name="Grid/RangeMax" type="string" value="$(arg grid_max_range)"/>
<param name="Grid/ClusterRadius" type="string" value="1"/>
@@ -156,7 +157,7 @@
<param name="Icp/Epsilon" type="string" value="0.001"/>
<param name="Icp/MaxTranslation" type="string" value="3"/>
<param name="Icp/MaxCorrespondenceDistance" type="string" value="$(eval resolution*10)"/>
<param name="Icp/PM" type="string" value="true"/>
<param name="Icp/PM" type="string" value="true"/>
<param name="Icp/PMOutlierRatio" type="string" value="0.7"/>
<param name="Icp/CorrespondenceRatio" type="string" value="$(arg loop_ratio)"/>
</node>
package.xml
+1 -1
View File
@@ -2,7 +2,7 @@
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>rtabmap_ros</name>
<version>0.20.22</version>
<version>0.20.23</version>
<description>RTAB-Map's ros-pkg. RTAB-Map is a RGB-D SLAM approach with real-time constraints.</description>
<maintainer email="matlabbe@gmail.com">Mathieu Labbe</maintainer>
<author>Mathieu Labbe</author>