Visual SLAM in rtabmap and Isaac Sim
Written by: Helena
Written by: Helena
In this tutorial, we go over the steps we took to get visual SLAM working with the Carter robot in the NVIDIA Isaac sim environment.
We used this github repository below:
Introduction to Rtabmap:
RTAB-Map (Real-Time Appearance-Based Mapping) is a RGB-D Graph SLAM approach based on using a global Bayesian loop closure detector.
Install rtabmap ros:
sudo apt install ros-$ROS_DISTRO-rtabmap-ros
When launching rtabmap_ros's nodes, if you have the error error while loading shared libraries..., try ldconfig or add the next line at the end of your ~/.bashrc to fix it:
export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/opt/ros/noetic/lib/x86_64-linux-gnu
Build from Source:
The next instructions assume that you have set up your ROS workspace using this tutorial. The workspace path is ~/catkin_ws and your ~/.bashrc contains:
$ source /opt/ros/$ROS_DISTRO/setup.bash
$ source ~/catkin_ws/devel/setup.bash
Install RTAB-Map standalone libraries. Do not clone in your Catkin workspace:
cd ~
git clone https://github.com/introlab/rtabmap.git rtabmap
cd rtabmap/build
cmake .. [<---double dots included]
make -j6
sudo make install
Install RTAB-Map ros-pkg in your src folder of your Catkin workspace:
cd ~/catkin_ws
git clone https://github.com/introlab/rtabmap_ros.git src/rtabmap_ros
catkin_make -j4
Use catkin_make -j1 if compilation requires more RAM than you have (e.g., some files require up to ~2 GB to build depending on gcc version).
Options:
Add -DRTABMAP_SYNC_MULTI_RGBD=ON to catkin_make if you plan to use multiple cameras.
Add -DRTABMAP_SYNC_USER_DATA=ON to catkin_make if you plan to use user data synchronized topics.
Make sure that carter_2dnav and carter_description folders are in the src from the catkin_urop workspace
If the folders are not in the workspace as indicated above, follow the instructions below:
In the home directory, clone the Isaac Sim ROS Workspace repo:
git clone https://github.com/isaac-sim/IsaacSim-ros_workspaces
In the terminal, navigate to this directory:
cd /home/mavric-wonse/IsaacSim-ros_workspaces/noetic_ws/src/navigation
Then copy the folders carter_2dnav and carter_description and paste it inside /catkin_urop/src.
Then cd .. and run catkin_make
Navigate to this directory to access the launch file:
/home/mavric-wonse/catkin_urop/src/rtabmap_ros/rtabmap_demos/launch/demo_isaac_carter_navigation.launch
Launch File:
<?xml version="1.0"?>
<launch>
<group ns="rtabmap">
<!-- -->
<!-- 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_demos demo_isaac_carter_navigation.launch
5) Set max depth range to 10m in rtabmap_viz 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/.
Isaac Sim 2021:
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
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
2. For all Branch nodes (green/red icon), select them, then check Compute Node -> Inputs -> Condition checkbox.
3D LIDAR: TODO: didn't find how to enable/publish it.
-->
<param name="use_sim_time" value="true" />
<arg name="localization" default="true" />
<arg name="lidar3d" default="true" /> <!-- Best results if rtabmap is built with libpointmatcher -->
<arg name="lidar3d_ray_tracing" default="true" />
<arg name="lidar3d_grid3d" default="true" />
<arg name="camera" default="true" />
<arg name="stereo" default="false" /> <!-- RGB-D mode by default -->
<!-- Load Robot Description -->
<arg name="model" default="/home/mavric-wonse/catkin_urop/src/carter_description/urdf/carter.urdf"/>
<param name="robot_description" textfile="/home/mavric-wonse/catkin_urop/src/carter_description/urdf/carter.urdf" />
<!-- 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 10 --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="/home/mavric-wonse/catkin_urop/src/rtabmap_ros/rtabmap_ros/launch/rtabmap.launch">
<arg if="$(arg localization)" name="args" value="--Reg/Strategy 1 --Reg/Force3DoF true --RGBD/NeighborLinkRefining true --Icp/MaxTranslation 0.5 $(arg args3d)"/>
<arg unless="$(arg localization)" name="args" value="-d --Reg/Strategy 1 --Reg/Force3DoF true --RGBD/NeighborLinkRefining true --Icp/MaxTranslation 0.5 $(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="stereo" value="$(eval camera and stereo)"/> -->
<arg name="depth" value="$(eval camera and not stereo)"/>
<arg name="subscribe_rgb" value="$(arg camera)"/>
<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="left_image_topic" value="/rgb_left"/>
<arg name="left_camera_info_topic" value="/camera_info_left"/>
<arg name="right_image_topic" value="/rgb_right"/>
<arg name="right_camera_info_topic" value="/camera_info_right"/>
<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="true"/>
<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="/home/mavric-wonse/catkin_urop/src/carter_2dnav/params/costmap_common_params.yaml" command="load" ns="global_costmap" />
<rosparam file="/home/mavric-wonse/catkin_urop/src/carter_2dnav/params/costmap_common_params.yaml" command="load" ns="local_costmap" />
<rosparam file="/home/mavric-wonse/catkin_urop/src/carter_2dnav/params/local_costmap_params.yaml" command="load" />
<rosparam file="/home/mavric-wonse/catkin_urop/src/carter_2dnav/params/global_costmap_params.yaml" command="load" />
<rosparam file="/home/mavric-wonse/catkin_urop/src/carter_2dnav/params/base_local_planner_params.yaml" command="load" />
</node>
<node type="rviz" name="rviz" pkg="rviz" args="-d /home/mavric-wonse/catkin_urop/src/carter_2dnav/rviz/carter_2dnav.rviz" />
</group>
</launch>
Running the Launch Code with Simulation:
In one terminal run the launch file with:
roslaunch demo_isaac_carter_navigation.launch
In another terminal, run the command below to control the robot with the keyboard:
rosrun teleop_twist_keyboard teleop_twist_keyboard.py
Save Point Cloud Data on Rtabmap:
Make sure mapping mode is turned on in rtabmap.
Drive around to create a full map of the environment.
On the rtabmap application, press the pause button on the left sidebar.
Under File, click on export to .pcd or .ply
Save in a location on the computer.
Congrats! Rtabmap should pop up and you should see something illustrated on the images below!
Create Program to View Point Cloud Data with Open3d