Added nav2 integration (fixed latched /map, connected nav2_msgs/NavigateToGoal action to rtabmap, updated turteblebot3 example launch file to work with navigation)

launch/ros2/turtlebot3_rgbd_sync.launch.py

@@ -4,15 +4,23 @@
 # Example:
 #   $ export TURTLEBOT3_MODEL=waffle
 #   $ ros2 launch turtlebot3_gazebo turtlebot3_world.launch.py
-#   $ ros2 run turtlebot3_teleop teleop_keyboard
 #
+#   SLAM:
 #   $ ros2 launch rtabmap_ros turtlebot3_rgbd_sync.launch.py
 #   OR
 #   $ ros2 launch rtabmap_ros rtabmap.launch.py visual_odometry:=false frame_id:=base_footprint subscribe_scan:=true  approx_sync:=true odom_topic:=/odom args:="-d --RGBD/NeighborLinkRefining true --Reg/Strategy 1" use_sim_time:=true rgbd_sync:=true rgb_topic:=/intel_realsense_r200_depth/image_raw depth_topic:=/intel_realsense_r200_depth/depth/image_raw camera_info_topic:=/intel_realsense_r200_depth/camera_info
+#
+#   Navigation (install nav2_bringup package):
+#     $ ros2 launch rtabmap_ros nav2_bringup_launch.py use_sim_time:=True
+#     $ ros2 launch nav2_bringup rviz_launch.py
+#
+#   Teleop:
+#     $ ros2 run turtlebot3_teleop teleop_keyboard
 
 from launch import LaunchDescription
 from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable
 from launch.substitutions import LaunchConfiguration
+from launch.conditions import IfCondition, UnlessCondition
 from launch_ros.actions import Node
 
 
@@ -20,20 +28,23 @@ def generate_launch_description():
 
     use_sim_time = LaunchConfiguration('use_sim_time')
     qos = LaunchConfiguration('qos')
+    localization = LaunchConfiguration('localization')
 
-    parameters=[{
+    parameters={
           'frame_id':'base_footprint',
           'use_sim_time':use_sim_time,
           'subscribe_rgbd':True,
           'subscribe_scan':True,
+          'use_action_for_goal':True,
           'qos_scan':qos,
           'qos_image':qos,
           'qos_imu':qos,
           # RTAB-Map's parameters should be strings:
           'Reg/Strategy':'1',
+          'Reg/Force3DoF':'true',
           'RGBD/NeighborLinkRefining':'True',
           'Optimizer/GravitySigma':'0' # Disable imu constraints (we are already in 2D)
-    }]
+    }
 
     remappings=[
           ('rgb/image', '/intel_realsense_r200_depth/image_raw'),
@@ -50,6 +61,10 @@ def generate_launch_description():
         DeclareLaunchArgument(
             'qos', default_value='2',
             description='QoS used for input sensor topics'),
+            
+        DeclareLaunchArgument(
+            'localization', default_value='false',
+            description='Launch in localization mode.'),
 
         # Nodes to launch
         Node(
@@ -57,14 +72,25 @@ def generate_launch_description():
             parameters=[{'approx_sync':True, 'use_sim_time':use_sim_time, 'qos':qos}],
             remappings=remappings),
 
+        # SLAM Mode:
         Node(
+            condition=UnlessCondition(localization),
             package='rtabmap_ros', executable='rtabmap', output='screen',
-            parameters=parameters,
+            parameters=[parameters],
             remappings=remappings,
             arguments=['-d']),
+            
+        # Localization mode:
+        Node(
+            condition=IfCondition(localization),
+            package='rtabmap_ros', executable='rtabmap', output='screen',
+            parameters=[parameters,
+              {'Mem/IncrementalMemory':'False',
+               'Mem/InitWMWithAllNodes':'True'}],
+            remappings=remappings),
 
         Node(
             package='rtabmap_ros', executable='rtabmapviz', output='screen',
-            parameters=parameters,
+            parameters=[parameters],
             remappings=remappings),
     ])