From 0ad3d7bd7f7e2c55215c4cad0d2d57bea216e497 Mon Sep 17 00:00:00 2001
From: matlabbe <matlabbe@gmail.com>
Date: Tue, 7 Dec 2021 21:26:29 -0500
Subject: [PATCH] updated isaac demo with 3d lidar

---
 launch/demo/demo_husky.launch                 |  2 +-
 .../demo/demo_isaac_carter_navigation.launch  | 30 +++++++++++++++++--
 2 files changed, 28 insertions(+), 4 deletions(-)

diff --git a/launch/demo/demo_husky.launch b/launch/demo/demo_husky.launch
index 012603fe..ec0e01b9 100644
--- a/launch/demo/demo_husky.launch
+++ b/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):
diff --git a/launch/demo/demo_isaac_carter_navigation.launch b/launch/demo/demo_isaac_carter_navigation.launch
index 23c9870f..e1f86bb7 100644
--- a/launch/demo/demo_isaac_carter_navigation.launch
+++ b/launch/demo/demo_isaac_carter_navigation.launch
@@ -11,31 +11,55 @@
 
          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 unless="$(arg localization)" name="args" value="-d --Reg/Strategy 1 --Reg/Force3DoF true --RGBD/NeighborLinkRefining true"/>
+        <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 name="subscribe_scan"    value="true"/>
+        <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">