Added demo_husky.launch

2026-10-04 00:37:46 +08:00 · 2021-03-04 19:42:32 -05:00
parent 16d0a00992
commit 0119f3e1fa
2 changed files with 103 additions and 0 deletions
@@ -0,0 +1,9 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://ros.org/wiki/xacro">
+
+  <xacro:include filename="$(find velodyne_description)/urdf/VLP-16.urdf.xacro"/>
+  <VLP-16 parent="base_laser_mount" name="velodyne" topic="velodyne_points" gpu="true">
+    <origin xyz="0.025 0 0.175" rpy="0 0 0" />
+  </VLP-16>
+  
+</robot>
@@ -0,0 +1,94 @@
+
+<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 
+       
+       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 2D LiDAR
+            $ roslaunch rtabmap_ros demo_husky.launch lidar2d:=true slam2d:=true
+         
+         5) 3DoF mapping with 2D LiDAR and RGB-D camera
+            $ roslaunch rtabmap_ros demo_husky.launch lidar2d:=true slam2d:=true camera:=true
+            
+         6) 6DoF mapping with 3D 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
+            
+       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 
+             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"
+       -->
+
+  <arg name="navigation"   default="true"/>
+  <arg name="localization" default="false"/>
+  <arg name="icp_odometry" default="false"/>
+  <arg name="rtabmapviz"   default="false"/>
+  <arg name="camera"       default="false"/>
+  <arg name="lidar2d"      default="false"/>
+  <arg name="lidar3d"      default="false"/>
+  <arg name="lidar3d_ray_tracing" default="true"/>
+  <arg name="slam2d"       default="true"/>
+  
+  
+  <arg     if="$(arg lidar3d)" name="cell_size" default="0.2"/>
+  <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"/>
+  <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"/>
+
+  <!--- Run rtabmap -->
+  <remap from="/rtabmap/grid_map" to="/map"/>
+  <include file="$(find rtabmap_ros)/launch/rtabmap.launch">
+    <arg     if="$(arg localization)" name="args" value="--Reg/Force3DoF $(arg slam2d) $(arg lidar_args)" />
+    <arg unless="$(arg localization)" name="args" value="--Reg/Force3DoF $(arg slam2d) $(arg lidar_args) -d" /> <!-- create new map -->
+    <arg name="localization"    value="$(arg localization)" />
+    <arg name="visual_odometry" value="false" />
+    <arg name="approx_sync"     value="$(eval camera or not icp_odometry)" />
+    <arg name="imu_topic"       value="/imu/data" />
+    <arg unless="$(arg icp_odometry)" name="odom_topic" value="/odometry/filtered" />
+    <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 name="scan_topic"      value="/scan" />
+    
+    <!-- 3D LiDAR -->
+    <arg name="subscribe_scan_cloud"  value="$(arg lidar3d)" />
+    <arg name="scan_cloud_topic"      value="/velodyne_points" />
+    
+    <!-- If camera is used -->
+    <arg name="depth"             value="$(arg camera)" />
+    <arg name="rgbd_sync"         value="$(arg camera)" />
+    <arg name="rgb_topic"         value="/realsense/color/image_raw" />
+    <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 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" />
+    <arg if="$(arg icp_odometry)" name="vo_frame_id" value="icp_odom" />
+    <arg unless="$(arg slam2d)"   name="wait_imu_to_init" value="true" />
+    <arg if="$(arg lidar3d)"      name="odom_args" value="--Icp/CorrespondenceRatio 0.01"/>
+  </include>
+
+  <!--- Run Move Base -->
+  <include if="$(arg navigation)" file="$(find husky_navigation)/launch/move_base.launch" />
+
+</launch>