Create and configure rtabmap using a yaml file

.vscode/tasks.json

@@ -18,6 +18,7 @@
 			},
 			"label": "colcon: Colcon Build Release"
 		},
+
 		{
 			"type": "colcon",
 			"args": [
@@ -73,7 +74,7 @@
 		{
 			"label": "IGVC: Open RTAB",
 			"type": "shell",
-			"command": "source install/setup.bash; ros2 launch igvc_slam sim_rtabmap.launch.py",
+			"command": "source install/setup.bash; ros2 launch igvc_slam igvc_slam.launch",
 			"problemMatcher": []
 		},
 		{

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_description/urdf/sensor/camera.urdf.xacro

@@ -72,4 +72,66 @@
             <topic>oak/color</topic>
         </sensor>
     </gazebo>
+
+    <gazebo reference="oak_infra1_frame">
+        <sensor name="camera_right" type="camera">
+            <camera>
+                <horizontal_fov>1.51843645</horizontal_fov>
+                <image>
+                    <width>1280</width>
+                    <height>720</height>
+                    <format>R8G8B8</format>
+                </image>
+                <clip>
+                    <near>0.05</near>
+                    <far>20</far>
+                </clip>
+                <noise>
+                    <type>gaussian</type>
+                    <!-- Noise is sampled independently per pixel on each frame.
+                        That pixel's noise value is added to each of its color
+                        channels, which at that point lie in the range [0,1]. -->
+                    <mean>0.0</mean>
+                    <stddev>0.007</stddev>
+                </noise>
+                <optical_frame_id>oak_infra1_optical_frame</optical_frame_id>
+                <camera_info_topic>oak/camera_info</camera_info_topic>
+            </camera>
+            <always_on>1</always_on>
+            <update_rate>20</update_rate>
+            <visualize>true</visualize>
+            <topic>oak/right</topic>
+        </sensor>
+    </gazebo>
+
+    <gazebo reference="oak_infra2_frame">
+        <sensor name="camera_left" type="camera">
+            <camera>
+                <horizontal_fov>1.51843645</horizontal_fov>
+                <image>
+                    <width>1280</width>
+                    <height>720</height>
+                    <format>R8G8B8</format>
+                </image>
+                <clip>
+                    <near>0.05</near>
+                    <far>20</far>
+                </clip>
+                <noise>
+                    <type>gaussian</type>
+                    <!-- Noise is sampled independently per pixel on each frame.
+                        That pixel's noise value is added to each of its color
+                        channels, which at that point lie in the range [0,1]. -->
+                    <mean>0.0</mean>
+                    <stddev>0.007</stddev>
+                </noise>
+                <optical_frame_id>oak_infra2_optical_frame</optical_frame_id>
+                <camera_info_topic>oak/camera_info</camera_info_topic>
+            </camera>
+            <always_on>1</always_on>
+            <update_rate>20</update_rate>
+            <visualize>true</visualize>
+            <topic>oak/left</topic>
+        </sensor>
+    </gazebo>
 </robot>

src/igvc_gazebo/config/gz_bridge.yaml

@@ -15,6 +15,32 @@
 
 # Depth Camera 
 
+- ros_topic_name: "zed/zed_node/left/image_rect_color"
+  gz_topic_name: "oak/left"
+  ros_type_name: "sensor_msgs/msg/Image"
+  gz_type_name: "gz.msgs.Image"
+  direction: "GZ_TO_ROS"
+  qos_profile: SENSOR_DATA
+
+- ros_topic_name: "zed/zed_node/right/image_rect_color"
+  gz_topic_name: "oak/right"
+  ros_type_name: "sensor_msgs/msg/Image"
+  gz_type_name: "gz.msgs.Image"
+  direction: "GZ_TO_ROS"
+  qos_profile: SENSOR_DATA
+
+- ros_topic_name: "zed/zed_node/left/camera_info" #TODO: verify this matches real ros topic
+  gz_topic_name: "oak/camera_info"
+  ros_type_name: "sensor_msgs/msg/CameraInfo"
+  gz_type_name: "gz.msgs.CameraInfo"
+  direction: GZ_TO_ROS
+
+- ros_topic_name: "zed/zed_node/right/camera_info" #TODO: verify this matches real ros topic
+  gz_topic_name: "oak/camera_info"
+  ros_type_name: "sensor_msgs/msg/CameraInfo"
+  gz_type_name: "gz.msgs.CameraInfo"
+  direction: GZ_TO_ROS
+
 - ros_topic_name: "camera/camera/color/image_raw"
   gz_topic_name: "oak/color"
   ros_type_name: "sensor_msgs/msg/Image"

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,77 @@
+# RTAB-Map configuration
+rtabmap:
+    ros__parameters:
+
+        use_sim_time:           true
+
+        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
+
+        Mem:
+            IncrementalMemory: true
+            InitWithAllNodes: false
+
+        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/config/stereo_odometry.yaml

@@ -0,0 +1,26 @@
+stereo_odometry:
+  ros__parameters:
+
+    subscribe_rgbd: false
+
+    publish_tf: true
+    wait_imu_to_init: false
+    always_check_imu_tf: false
+
+    frame_id: "base_link"
+    odom_frame_id: "odom"
+    ground_truth_frame_id: ""
+    ground_truth_base_frame_id: ""
+    guess_frame_id: ""
+    config_path: "~/.ros/rtabmap.cfg" #unverified, copied from rtabmap.yaml
+
+    wait_for_transform: 0.2
+    approx_sync: false
+    approx_sync_max_interval: 0.0
+    topic_queue_size: 10
+    sync_queue_size: 10
+    qos: 1
+    qos_camera_info: 1
+    qos_imu: 1
+    guess_min_translation: 0.0
+    guess_min_rotation: 0.0

src/igvc_slam/launch/igvc_slam.launch

@@ -0,0 +1,58 @@
+<?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> 
+        <push_ros_namespace namespace="$(var camera_name)" />
+        <!-- ZED Wrapper Node-->
+        <!-- <include file="$(find-pkg-share 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-pkg-share igvc_slam)/launch/include/rtabmap.launch"/>
+        <include file="$(var custom_rtabmap_launch_file)">
+            <arg name="odom_topic"              value="$(var zed_node_name)/odom" />
+            <arg name="rgb_topic"               value="$(var zed_node_name)/rgb/image_rect_color" />
+            <arg name="depth_topic"             value="$(var zed_node_name)/depth/depth_registered" />
+            <arg name="camera_info_topic"       value="$(var zed_node_name)/rgb/camera_info" />
+            <arg name="depth_camera_info_topic" value="$(var zed_node_name)/depth/camera_info" />
+        </include>   
+
+        <!-- Rviz -->
+        <!-- <node name="rviz" pkg="rviz" type="rviz" args="-d $(find-pkg-share zed_rtabmap_example)/rviz/zed-rtabmap.rviz" output="screen" />  -->
+    </group>
+</launch>

src/igvc_slam/launch/include/rtabmap.launch

@@ -0,0 +1,44 @@
+<!-- RTAB-Map launcher -->
+
+<launch>
+    <!-- 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_odom" exec="stereo_odometry" output="screen" args="" launch-prefix="">    
+        <param from="$(find-pkg-share igvc_slam)/config/stereo_odometry.yaml" />
+
+        <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" exec="rtabmap_viz" output="screen" args="" launch-prefix="">    
+        <remap from="rgb/image"         to="$(var rgb_topic)"/>
+        <remap from="depth/image"       to="$(var depth_topic)"/>
+        <remap from="rgb/camera_info"   to="$(var camera_info_topic)"/>
+
+        <remap from="grid_map"          to="map" />
+        <remap from="odom"              to="$(var odom_topic)"/>
+    </node> 
+
+    <!-- RTABmap -->
+    <node name="rtabmap" pkg="rtabmap_slam" exec="rtabmap" output="screen" args="--delete_db_on_start" launch-prefix="">
+        <param from="$(find-pkg-share igvc_slam)/config/rtabmap.yaml" />
+
+        <remap from="rgb/image"         to="$(var rgb_topic)"/>
+        <remap from="depth/image"       to="$(var depth_topic)"/>
+        <remap from="rgb/camera_info"   to="$(var camera_info_topic)"/>
+
+        <remap from="grid_map"          to="map" />
+        <remap from="odom"              to="$(var odom_topic)"/>
+
+    </node>
+</launch>