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.launch.py

@@ -4,30 +4,41 @@
 # 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.launch.py
 #   OR
 #   $ ros2 launch rtabmap_ros rtabmap.launch.py visual_odometry:=false frame_id:=base_footprint odom_topic:=/odom args:="-d" use_sim_time:=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 approx_sync:=true
+#
+#   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
 
 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_depth':True,
+          'use_action_for_goal':True,
           'qos_image':qos,
           'qos_imu':qos,
+          'Reg/Force3DoF':'true',
           'Optimizer/GravitySigma':'0' # Disable imu constraints (we are already in 2D)
-    }]
+    }
 
     remappings=[
           ('rgb/image', '/intel_realsense_r200_depth/image_raw'),
@@ -44,16 +55,32 @@ 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
+        
+        # SLAM mode:
         Node(
+            condition=UnlessCondition(localization),
             package='rtabmap_ros', executable='rtabmap', output='screen',
-            parameters=parameters,
+            parameters=[parameters],
             remappings=remappings,
-            arguments=['-d']),
+            arguments=['-d']), # This will delete the previous database (~/.ros/rtabmap.db)
+            
+        # 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),
     ])