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

launch/ros2/nav2_bringup_launch.py

@@ -0,0 +1,120 @@
+# Copyright (c) 2018 Intel Corporation
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+import os
+
+from ament_index_python.packages import get_package_share_directory
+
+from launch import LaunchDescription
+from launch.actions import (DeclareLaunchArgument, GroupAction,
+                            IncludeLaunchDescription, SetEnvironmentVariable)
+from launch.conditions import IfCondition
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration, PythonExpression
+from launch_ros.actions import PushRosNamespace
+
+def generate_launch_description():
+    # Get the launch directory
+    bringup_dir = get_package_share_directory('nav2_bringup')
+    launch_dir = os.path.join(bringup_dir, 'launch')
+
+    # Create the launch configuration variables
+    namespace = LaunchConfiguration('namespace')
+    use_namespace = LaunchConfiguration('use_namespace')
+    use_sim_time = LaunchConfiguration('use_sim_time')
+    params_file = LaunchConfiguration('params_file')
+    default_bt_xml_filename = LaunchConfiguration('default_bt_xml_filename')
+    autostart = LaunchConfiguration('autostart')
+
+    stdout_linebuf_envvar = SetEnvironmentVariable(
+        'RCUTILS_LOGGING_BUFFERED_STREAM', '1')
+
+    declare_namespace_cmd = DeclareLaunchArgument(
+        'namespace',
+        default_value='',
+        description='Top-level namespace')
+
+    declare_use_namespace_cmd = DeclareLaunchArgument(
+        'use_namespace',
+        default_value='false',
+        description='Whether to apply a namespace to the navigation stack')
+
+    declare_slam_cmd = DeclareLaunchArgument(
+        'slam',
+        default_value='False',
+        description='Whether run in SLAM mode or in localization-only mode')
+
+    declare_map_db_cmd = DeclareLaunchArgument(
+        'map',
+        default_value='~/.ros/rtabmap.db',
+        description='Full path to map db file to load')
+
+    declare_use_sim_time_cmd = DeclareLaunchArgument(
+        'use_sim_time',
+        default_value='false',
+        description='Use simulation (Gazebo) clock if true')
+
+    declare_params_file_cmd = DeclareLaunchArgument(
+        'params_file',
+        default_value=os.path.join(bringup_dir, 'params', 'nav2_params.yaml'),
+        description='Full path to the ROS2 parameters file for navigation nodes')
+
+    declare_bt_xml_cmd = DeclareLaunchArgument(
+        'default_bt_xml_filename',
+        default_value=os.path.join(
+            get_package_share_directory('nav2_bt_navigator'),
+            'behavior_trees', 'navigate_w_replanning_and_recovery.xml'),
+        description='Full path to the behavior tree xml file to use')
+
+    declare_autostart_cmd = DeclareLaunchArgument(
+        'autostart', default_value='true',
+        description='Automatically startup the nav2 stack')
+
+    # Specify the actions
+    bringup_cmd_group = GroupAction([
+        PushRosNamespace(
+            condition=IfCondition(use_namespace),
+            namespace=namespace),
+            
+        IncludeLaunchDescription(
+            PythonLaunchDescriptionSource(os.path.join(launch_dir, 'navigation_launch.py')),
+            launch_arguments={'namespace': namespace,
+                              'use_sim_time': use_sim_time,
+                              'autostart': autostart,
+                              'params_file': params_file,
+                              'default_bt_xml_filename': default_bt_xml_filename,
+                              'use_lifecycle_mgr': 'false',
+                              'map_subscribe_transient_local': 'true'}.items()),
+    ])
+
+    # Create the launch description and populate
+    ld = LaunchDescription()
+
+    # Set environment variables
+    ld.add_action(stdout_linebuf_envvar)
+
+    # Declare the launch options
+    ld.add_action(declare_namespace_cmd)
+    ld.add_action(declare_use_namespace_cmd)
+    ld.add_action(declare_slam_cmd)
+    ld.add_action(declare_map_db_cmd)
+    ld.add_action(declare_use_sim_time_cmd)
+    ld.add_action(declare_params_file_cmd)
+    ld.add_action(declare_autostart_cmd)
+    ld.add_action(declare_bt_xml_cmd)
+
+    # Add the actions to launch all of the navigation nodes
+    ld.add_action(bringup_cmd_group)
+
+    return ld

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),
     ])

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),
     ])

launch/ros2/turtlebot3_scan.launch.py

@@ -3,36 +3,47 @@
 # 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_scan.launch.py
 #   OR
 #   $ ros2 launch rtabmap_ros rtabmap.launch.py visual_odometry:=false frame_id:=base_footprint subscribe_scan:=true depth:=false approx_sync:=true odom_topic:=/odom args:="-d --RGBD/NeighborLinkRefining true --Reg/Strategy 1" use_sim_time:=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')
-
-    parameters=[{
+    localization = LaunchConfiguration('localization')
+    
+    parameters={
           'frame_id':'base_footprint',
           'use_sim_time':use_sim_time,
           'subscribe_depth':False,
           'subscribe_rgb':False,
           'subscribe_scan':True,
           'approx_sync':True,
+          'use_action_for_goal':True,
           'qos_scan':qos,
           'qos_imu':qos,
           'Reg/Strategy':'1',
+          'Reg/Force3DoF':'true',
           'RGBD/NeighborLinkRefining':'True',
           'Optimizer/GravitySigma':'0' # Disable imu constraints (we are already in 2D)
-    }]
-
+    }
+    
     remappings=[
           ('scan', '/scan')]
 
@@ -46,16 +57,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),
     ])