Create and configure rtabmap using a yaml file

.vscode/tasks.json

@@ -6,7 +6,8 @@
 			"args": [
 				"build",
 				"--symlink-install",
-				"--packages-skip-regex zed_*"
+				"--packages-skip-regex",
+				"zed_*"
 			],
 			"problemMatcher": [
 				"$colcon-gcc"
@@ -17,12 +18,14 @@
 			},
 			"label": "colcon: Colcon Build Release"
 		},
+
 		{
 			"type": "colcon",
 			"args": [
 				"build",
 				"--symlink-install",
-				"--packages-skip-regex zed_*",
+				"--packages-skip-regex",
+				"zed_*",
 				"--cmake-args",
 				"-DCMAKE_BUILD_TYPE=RelWithDebInfo"
 			],
@@ -38,7 +41,8 @@
 			"args": [
 				"build",
 				"--symlink-install",
-				"--packages-skip-regex zed_*",
+				"--packages-skip-regex",
+				"zed_*",
 				"--cmake-args",
 				"-DCMAKE_BUILD_TYPE=Debug"
 			],

src/igvc_bringup/launch/bringup.launch.py

@@ -1,15 +1,18 @@
-from launch import LaunchDescription
+import os
+
+from ament_index_python import get_package_share_directory
+from launch import LaunchDescription, LaunchDescriptionSource
 from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
 from launch.conditions import IfCondition, UnlessCondition
-from launch.substitutions import LaunchConfiguration
-from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+from launch.launch_description_sources import FrontendLaunchDescriptionSource, PythonLaunchDescriptionSource
 from launch_ros.substitutions import FindPackageShare
 
 
 def generate_launch_description():
     use_sim = LaunchConfiguration('use_sim')
     use_mock_hardware = LaunchConfiguration('use_mock_hardware')
-
+    
     return LaunchDescription([
         # Launch Arguments
         DeclareLaunchArgument(
@@ -77,16 +80,14 @@ def generate_launch_description():
             ),
             condition = UnlessCondition(use_mock_hardware)
         ),
-
+        
         # SLAM
-        IncludeLaunchDescription(
-            PythonLaunchDescriptionSource(
+        IncludeLaunchDescription(        
+            FrontendLaunchDescriptionSource(
                 [FindPackageShare('igvc_slam'),
-                 '/launch',
-                 '/sim_rtabmap.launch.py']
-            ),
+                '/launch',
+                '/igvc_slam.launch']
+            ),     
             condition = IfCondition(LaunchConfiguration('use_slam'))
-        ),
-
-        # TODO Nav
+        )
     ])

src/igvc_slam/CMakeLists.txt

@@ -13,6 +13,11 @@ install(
   DESTINATION share/${PROJECT_NAME}
 )
 
+install(
+  DIRECTORY config
+  DESTINATION share/${PROJECT_NAME}
+)
+
 if(BUILD_TESTING)
     find_package(ament_lint_auto REQUIRED)
     # the following line skips the linter which checks for copyrights

src/igvc_slam/config/rtabmap.yaml

@@ -0,0 +1,69 @@
+# RTAB-Map configuration
+
+subscribe_depth:        true
+subscribe_rgbd:         false
+subscribe_stereo:       false
+subscribe_stereo:       false
+subscribe_scan:         false
+subscribe_scan_cloud:   false
+subscribe_user_data:    false
+subscribe_odom_info:    false
+
+database_path:      "~/.ros/rtabmap.db"
+config_path:        "~/.ros/rtabmap.cfg"
+
+frame_id:       "base_link"
+map_frame_id:   "map"
+odom_frame_id:  "odom"                   # odometry from odom msg to have covariance - Remapped by launch file
+odom_tf_angular_variance:  0.001     # If TF is used to get odometry, this is the default angular variance
+odom_tf_linear_variance:   0.001     # If TF is used to get odometry, this is the default linear variance
+tf_delay:       0.02
+publish_tf:     false                # Set to false if fusing different poses (map->odom) with UKF
+
+odom_sensor_sync:               true
+wait_for_transform_duration:    0.2
+approx_sync:                    true
+
+queue_size: 10
+
+scan_normal_k:  0
+
+Grid:
+    3D:                     true
+    FlatObstacleDetected:   true
+    MapFrameProjection:     false
+    GroundIsObstacle:       false
+    PreVoxelFiltering:      true
+    RayTracing:             true
+    FromDepth:              true
+    NormalsSegmentation:    false
+    CellSize:               0.05
+    ClusterRadius:          0.1
+    MinClusterSize:         3
+    DepthDecimation:        1
+    DepthRoiRatios:         [0.0, 0.0, 0.0, 0.0]
+    FootprintHeight:        2.0
+    FootprintLength:        0.18
+    FootprintWidth:         0.18
+    MaxGroundAngle:         30.0
+    MinGroundHeight:        -0.5
+    MaxGroundHeight:        -0.4
+    MaxObstacleHeight:      0.1
+    NoiseFilteringMinNeighbors: 5
+    NoiseFilteringRadius:   0.1
+    NormalK:                20
+    RangeMin:               0.7
+    RangeMax:               3.0
+
+GridGlobal:
+    Eroded:                 false       # Erode obstacle cells
+    FootprintRadius:        0.18        # Footprint radius (m) used to clear all obstacles under the graph
+    FullUpdate:             true        # When the graph is changed, the whole map will be reconstructed instead of moving individually each cells of the map. Also, data added to cache won't be released after updating the map. This process is longer but more robust to drift that would erase some parts of the map when it should not
+    MaxNodes:               0           # Maximum nodes assembled in the map starting from the last node (0=unlimited)
+    MinSize:                1.0        # Minimum map size (m)
+    OccupancyThr:           0.55        # Occupancy threshold (value between 0 and 1)
+    ProbClampingMax:        0.971       # Probability clamping maximum (value between 0 and 1)
+    ProbClampingMin:        0.1192      # Probability clamping minimum (value between 0 and 1)
+    ProbHit:                0.7         # Probability of a hit (value between 0.5 and 1)
+    ProbMiss:               0.4         # Probability of a miss (value between 0 and 0.5)
+    UpdateError:            0.01        # Graph changed detection error (m). Update map only if poses in new optimized graph have moved more than this value

src/igvc_slam/launch/igvc_slam.launch

@@ -0,0 +1,56 @@
+<?xml version="1.0"?>
+<!--
+Copyright (c) 2018, STEREOLABS.
+
+All rights reserved.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+-->
+<launch>
+    <arg name="svo_file"             default="" /> <!-- <arg name="svo_file" default="path/to/svo/file.svo"> -->
+    <arg name="stream"               default="" /> <!-- <arg name="stream" default="<ip_address>:<port>"> -->
+
+    <arg name="zed_node_name"        default="zed_node" />
+    <arg name="camera_model"         default="zed2" />
+    <arg name="publish_urdf"         default="true" />
+
+    <arg name="camera_name"          default="zed" />
+
+    <arg name="base_frame"           default="base_link" />
+
+    <group ns="$(arg camera_name)">
+        <!-- ZED Wrapper Node-->
+        <include file="$(find zed_wrapper)/launch/include/zed_camera.launch.xml">
+            <arg name="camera_name"         value="$(arg camera_name)" />
+            <arg name="svo_file"            value="$(arg svo_file)" />
+            <arg name="stream"              value="$(arg stream)" />
+            <arg name="node_name"           value="$(arg zed_node_name)" />
+            <arg name="camera_model"        value="$(arg camera_model)" />
+            <arg name="base_frame"          value="$(arg base_frame)" />
+            <arg name="publish_urdf"        value="$(arg publish_urdf)" />
+        </include>    
+
+        <!-- RTAB-Map -->
+        <arg name="custom_rtabmap_launch_file" default="$(find zed_rtabmap_example)/launch/include/rtabmap.launch.xml"/>
+        <include file="$(arg custom_rtabmap_launch_file)">
+            <arg name="odom_topic"              default="$(arg zed_node_name)/odom" />
+            <arg name="rgb_topic"               default="$(arg zed_node_name)/rgb/image_rect_color" />
+            <arg name="depth_topic"             default="$(arg zed_node_name)/depth/depth_registered" />
+            <arg name="camera_info_topic"       default="$(arg zed_node_name)/rgb/camera_info" />
+            <arg name="depth_camera_info_topic" default="$(arg zed_node_name)/depth/camera_info" />
+        </include>   
+
+        <!-- Rviz -->
+        <node name="rviz" pkg="rviz" type="rviz" args="-d $(find zed_rtabmap_example)/rviz/zed-rtabmap.rviz" output="screen" /> 
+    </group>
+</launch>

src/igvc_slam/launch/include/rtabmap.launch

@@ -0,0 +1,52 @@
+<!-- RTAB-Map launcher -->
+
+<launch>
+    <!-- Localization-only mode -->
+    <arg name="localization"            default="false"/>
+
+    <arg     if="$(arg localization)" name="args" default=""/>
+    <arg unless="$(arg localization)" name="args" default="--delete_db_on_start"/>
+
+    <!-- RGB-D related topics -->
+    <arg name="rgb_topic"               default="/zed/zed_node/rgb/image_rect_color" />
+    <arg name="depth_topic"             default="/zed/zed_node/depth/depth_registered" />
+    <arg name="camera_info_topic"       default="/zed/zed_node/rgb/camera_info" />
+    <arg name="depth_camera_info_topic" default="/zed/zed_node/depth/camera_info" />
+
+    <arg name="odom_topic"              default="/zed/zed_node/odom" />    <!-- Odometry topic name -->
+
+    <!-- Visual Odometry -->
+    <node name="rtabmap_stereo_odometry" pkg="rtabmap_ros" type="stereo_odometry" output="screen" args="" launch-prefix="">    
+        <remap from="/left/image_rect"      to="zed/zed_node/left/image_rect_color"/>
+        <remap from="/right/image_rect"     to="zed/zed_node/right/image_rect_color"/>
+        <remap from="/left/camera_info"     to="zed/zed_node/left/camera_info"/>
+        <remap from="/right/camera_info"    to="zed/zed_node/right/camera_info"/>
+    </node>
+
+    <!-- RTABmapviz -->
+    <node name="rtabmap_viz" pkg="rtabmap_viz" type="rtabmap_viz" output="screen" args="" launch-prefix="">    
+        <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="grid_map"          to="map" />
+        <remap from="odom"              to="$(arg odom_topic)"/>
+    </node> 
+
+    <!-- RTABmap -->
+    <node name="rtabmap" pkg="rtabmap_slam" type="rtabmap" output="screen" args="$(arg args)" launch-prefix="">
+        <rosparam command="load" file="$(find zed_rtabmap_example)/params/rtabmap.yaml" />
+
+        <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="grid_map"          to="map" />
+        <remap from="odom"              to="$(arg odom_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>
+</launch>