merged master -> ros2

launch/demo/demo_husky.launch

@@ -1,4 +1,4 @@
-
+<!-- -->
 <launch>
    
   <!-- Bringup the Husky with SICK (2D LiDAR), realsense camera (RGB-D camera) and velodyne (3D LiDAR):

launch/demo/demo_isaac_carter_navigation.launch

@@ -0,0 +1,76 @@
+<!-- -->
+<launch>
+    <!-- For ref: https://docs.omniverse.nvidia.com/app_isaacsim/app_isaacsim/tutorial_ros_navigation.html#isaac-sim-app-tutorial-ros-navigation
+
+         1) In Isaac sim, click on menu Isaac Examples - ROS - Navigation
+         2) Terminal:  $ roscore
+         3) To teleop: $ rosrun teleop_twist_keyboard teleop_twist_keyboard.py _speed:=0.15 _turn:=0.20
+         4) Navigation: roslaunch rtabmap_ros demo_isaac_carter_navigation.launch
+         5) Set max depth range to 10m in rtabmapviz for correct visualization (Preferences->3D Rendering under map and odom columns)
+         6) Press Play in Isaac sim
+
+         Note: carter_2dnav package can be copied to your catkin_ws from:
+            $ cp -r ~/.local/share/ov/pkg/isaac_sim-2021.2.0/ros_workspace/src/* ~/catkin_ws/src/.
+
+         For Lidar 3D Mode (lidar3d:=true), make sure in isaac sim that Carter robot is set with VLP16-like parameters (16 rings):
+            Under World -> Carter_ROS -> chassis_link -> carter_lidar -> highLod = True
+                                                                      -> verticalFov = 32
+                                                                      -> verticalResolution = 2
+                                                      -> ROS_lidar    -> pointCloudEnabled = True
+    -->
+
+    <param name="use_sim_time" value="true" />
+    <arg name="localization" default="false" />
+
+    <arg name="lidar3d"             default="false" /> <!-- Best results if rtabmap is built with libpointmatcher -->
+    <arg name="lidar3d_ray_tracing" default="true" />
+    <arg name="lidar3d_grid3d"      default="true" />
+
+    <!-- Load Robot Description -->
+    <arg name="model" default="$(find carter_description)/urdf/carter.urdf"/>
+    <param name="robot_description" textfile="$(arg model)" />
+
+    <!-- Lidar 3D based on demo_husky.launch -->
+    <arg     if="$(arg lidar3d)" name="cell_size" default="0.2" />
+    <arg unless="$(arg lidar3d)" name="cell_size" default="0.05" />
+    <arg     if="$(arg lidar3d)" name="args3d" value="--Icp/Iterations 10 --Icp/PointToPlaneGroundNormalsUp 0.9 --Icp/PointToPlaneRadius 0 --Icp/MaxCorrespondenceDistance 1 --Grid/ClusterRadius 1 --Grid/RangeMax 10 --Grid/RayTracing $(arg lidar3d_ray_tracing) --Grid/CellSize $(arg cell_size) --Mem/LaserScanNormalK --Grid/3D $(arg lidar3d_grid3d)" />
+    <arg unless="$(arg lidar3d)" name="args3d" value="--Grid/RangeMax 10" /> <!-- 2d scan, use default params -->
+
+    <!-- RTAB-Map -->
+    <remap from="/rtabmap/grid_map" to="/map"/>
+    <include file="$(find rtabmap_ros)/launch/rtabmap.launch">
+        <arg     if="$(arg localization)" name="args" value="--Reg/Strategy 1 --Reg/Force3DoF true --RGBD/NeighborLinkRefining true $(arg args3d)"/>
+        <arg unless="$(arg localization)" name="args" value="-d --Reg/Strategy 1 --Reg/Force3DoF true --RGBD/NeighborLinkRefining true $(arg args3d)"/>
+        <arg name="localization"      value="$(arg localization)"/>
+        <arg     if="$(arg lidar3d)" name="subscribe_scan_cloud" value="true"/>
+        <arg unless="$(arg lidar3d)" name="subscribe_scan" value="true"/>
+        <arg name="visual_odometry"   value="false"/>
+        <arg name="odom_topic"        value="/odom"/>
+        <arg name="frame_id"          value="base_link"/>
+        <arg name="depth_topic"       value="/depth_left"/>
+        <arg name="rgb_topic"         value="/rgb_left"/>
+        <arg name="camera_info_topic" value="/camera_info_left"/>
+        <arg name="scan_topic"        value="/scan"/>
+        <arg name="scan_cloud_topic"  value="/point_cloud"/>
+        <arg name="rgbd_sync"         value="true"/>
+        <arg name="approx_rgbd_sync"  value="false"/>
+        <arg name="use_sim_time"      value="true"/>
+
+        <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="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/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/ros2/rtabmap.launch.py

@@ -296,6 +296,7 @@ def launch_setup(context, *args, **kwargs):
                 ("user_data_async", LaunchConfiguration('user_data_async_topic')),
                 ("gps/fix", LaunchConfiguration('gps_topic')),
                 ("tag_detections", LaunchConfiguration('tag_topic')),
+                ("fiducial_transforms", LaunchConfiguration('fiducial_topic')),
                 ("odom", LaunchConfiguration('odom_topic')),
                 ("imu", LaunchConfiguration('imu_topic'))],
             arguments=[LaunchConfiguration("args")],
@@ -465,6 +466,7 @@ def generate_launch_description():
         DeclareLaunchArgument('tag_topic',            default_value='/tag_detections', description='AprilTag topic async subscription. This is used for SLAM graph optimization and loop closure detection. Landmark poses are also published accordingly to current optimized map.'),
         DeclareLaunchArgument('tag_linear_variance',  default_value='0.0001',          description=''),
         DeclareLaunchArgument('tag_angular_variance', default_value='9999.0',            description='>=9999 means rotation is ignored in optimization, when rotation estimation of the tag is not reliable or not computed.'),
+        DeclareLaunchArgument('fiducial_topic',       default_value='/fiducial_transforms', description='aruco_detect async subscription, use tag_linear_variance and tag_angular_variance to set covariance.'),
         OpaqueFunction(function=launch_setup)
     ])
 

launch/rtabmap.launch

@@ -142,6 +142,7 @@
   <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"/>
@@ -382,6 +383,7 @@
       <remap from="user_data_async"        to="$(arg user_data_async_topic)"/>
       <remap from="gps/fix"                to="$(arg gps_topic)"/>
       <remap from="tag_detections"         to="$(arg tag_topic)"/>
+      <remap from="fiducial_transforms"    to="$(arg fiducial_topic)"/>
       <remap from="odom"                   to="$(arg odom_topic)"/>
       <remap from="imu"                    to="$(arg imu_topic)"/>