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

CMakeLists.txt

@@ -29,6 +29,7 @@ find_package(sensor_msgs REQUIRED)
 find_package(std_msgs REQUIRED)
 find_package(std_srvs REQUIRED)
 find_package(nav_msgs REQUIRED)
+find_package(nav2_msgs REQUIRED)
 find_package(stereo_msgs REQUIRED)
 find_package(geometry_msgs REQUIRED)
 find_package(visualization_msgs REQUIRED)
@@ -52,7 +53,6 @@ find_package(image_geometry REQUIRED)
 find_package(octomap_msgs)
 #find_package(apriltag_msgs)
 #find_package(find_object_2d)
-find_package(move_base_msgs)
 #find_package(fiducial_msgs)
 
 ## System dependencies are found with CMake's conventions
@@ -216,6 +216,7 @@ SET(Libraries
    sensor_msgs
    std_msgs
    nav_msgs
+   nav2_msgs
    geometry_msgs
    image_transport
    tf2
@@ -309,19 +310,6 @@ ENDIF(octomap_msgs_FOUND)
 #ADD_DEFINITIONS("-DWITH_APRILTAG_MSGS")
 #ENDIF(apriltag_msgs_FOUND)
 
-# If move_base_msgs is found, add definition
-IF(move_base_msgs_FOUND)
-MESSAGE(STATUS "WITH move_base_msgs")
-include_directories(
-  ${move_base_msgs_INCLUDE_DIRS}
-)
-SET(Libraries
-  move_base_msgs
-  ${Libraries}
-)
-ADD_DEFINITIONS("-DWITH_MOVE_BASE_MSGS")
-ENDIF(move_base_msgs_FOUND)
-
 # If fiducial_msgs is found, add definition
 IF(fiducial_msgs_FOUND)
 MESSAGE(STATUS "WITH fiducial_msgs")

include/rtabmap_ros/CoreWrapper.h

@@ -83,10 +83,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <apriltag_msgs/msg/april_tag_detection_array.hpp>
 #endif
 
-#ifdef WITH_MOVE_BASE_MSGS
-#include <move_base_msgs/action/move_base.hpp>
+#include <nav2_msgs/action/navigate_to_pose.hpp>
 #include <rclcpp_action/rclcpp_action.hpp>
-#endif
 
 //#define WITH_FIDUCIAL_MSGS
 #ifdef WITH_FIDUCIAL_MSGS
@@ -106,6 +104,9 @@ public:
 	explicit CoreWrapper(const rclcpp::NodeOptions & options);
 	virtual ~CoreWrapper();
 
+	using NavigateToPose = nav2_msgs::action::NavigateToPose;
+	using GoalHandleNav2 = rclcpp_action::ClientGoalHandle<NavigateToPose>;
+
 private:
 	bool odomUpdate(const nav_msgs::msg::Odometry & odomMsg, rclcpp::Time stamp);
 	bool odomTFUpdate(const rclcpp::Time & stamp); // TF odom
@@ -242,13 +243,10 @@ private:
 
 	void publishStats(const rclcpp::Time & stamp);
 	void publishCurrentGoal(const rclcpp::Time & stamp);
-#ifdef WITH_MOVE_BASE_MSGS
-	using MoveBase = move_base_msgs::action::MoveBase;
-	using GoalHandleMoveBase = rclcpp_action::ClientGoalHandle<MoveBase>;
-	void goalResponseCallback(std::shared_future<GoalHandleMoveBase::SharedPtr> future);
-	void feedbackCallback(GoalHandleMoveBase::SharedPtr, const std::shared_ptr<const MoveBase::Feedback> feedback);
-	void resultCallback(const GoalHandleMoveBase::WrappedResult & result);
-#endif
+
+	void goalResponseCallback(std::shared_future<GoalHandleNav2::SharedPtr> future);
+	void resultCallback(const GoalHandleNav2::WrappedResult & result);
+
 	void publishLocalPath(const rclcpp::Time & stamp);
 	void publishGlobalPath(const rclcpp::Time & stamp);
 	void republishMaps();
@@ -364,9 +362,7 @@ private:
 	rclcpp::Service<octomap_msgs::srv::GetOctomap>::SharedPtr octomapBinarySrv_;
 	rclcpp::Service<octomap_msgs::srv::GetOctomap>::SharedPtr octomapFullSrv_;
 #endif
-#ifdef WITH_MOVE_BASE_MSGS
-	rclcpp_action::Client<MoveBase>::SharedPtr moveBaseClient_;
-#endif
+	rclcpp_action::Client<NavigateToPose>::SharedPtr nav2Client_;
 
 	std::thread* transformThread_;
 	bool tfThreadRunning_;

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'),
@@ -45,15 +56,31 @@ def generate_launch_description():
             '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'),
@@ -51,20 +62,35 @@ def generate_launch_description():
             '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(
             package='rtabmap_ros', executable='rgbd_sync', output='screen',
             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,35 +3,46 @@
 # 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')
+    localization = LaunchConfiguration('localization')
     
-    parameters=[{
+    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')]
@@ -47,15 +58,31 @@ def generate_launch_description():
             '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),
     ])

package.xml

@@ -49,6 +49,7 @@
   <build_depend>rviz_common</build_depend>
   <build_depend>rviz_rendering</build_depend>
   <build_depend>rviz_default_plugins</build_depend>
+  <build_depend>nav2_msgs</build_depend>
 
   <exec_depend>cv_bridge</exec_depend>
   <exec_depend>rclcpp</exec_depend>
@@ -81,6 +82,7 @@
   <exec_depend>rviz_common</exec_depend>
   <exec_depend>rviz_rendering</exec_depend>
   <exec_depend>rviz_default_plugins</exec_depend>
+  <exec_depend>nav2_msgs</exec_depend>
 
   <build_depend>libpcl-all-dev</build_depend>
 

src/CoreWrapper.cpp

@@ -183,9 +183,7 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) :
 	landmarkDefaultLinVariance_ = this->declare_parameter("landmark_linear_variance", landmarkDefaultLinVariance_);
 
 	waitForTransform_ = this->declare_parameter("wait_for_transform", waitForTransform_);
-#ifdef WITH_MOVE_BASE_MSGS
 	useActionForGoal_ = this->declare_parameter("use_action_for_goal", useActionForGoal_);
-#endif
 	useSavedMap_ = this->declare_parameter("use_saved_map", useSavedMap_);
 	maxNodesRepublished_ = this->declare_parameter("max_nodes_republished", maxNodesRepublished_);
 	genScan_ = this->declare_parameter("gen_scan", genScan_);
@@ -969,7 +967,7 @@ bool CoreWrapper::odomUpdate(const nav_msgs::msg::Odometry & odomMsg, rclcpp::Ti
 		if(!odom.isNull())
 		{
 			Transform odomTF;
-			if(!stamp.seconds() == 0.0) {
+			if(stamp.seconds() != 0.0) {
 				odomTF = rtabmap_ros::getTransform(odomMsg.header.frame_id, frameId_, stamp, *tfBuffer_, waitForTransform_);
 			}
 			if(odomTF.isNull())
@@ -2200,10 +2198,9 @@ void CoreWrapper::process(
 				{
 					if(rtabmap_.getPath().size() == 0)
 					{
-#ifdef WITH_MOVE_BASE_MSGS
-						// Don't send status yet if move_base actionlib is used unless it failed,
-						// let move_base finish reaching the goal
-						if(moveBaseClient_ == 0 || rtabmap_.getPathStatus() <= 0)
+						// Don't send status yet if nav2 actionlib is used unless it failed,
+						// let nav2 finish reaching the goal
+						if(nav2Client_ == 0 || rtabmap_.getPathStatus() <= 0)
 						{
 							if(rtabmap_.getPathStatus() > 0)
 							{
@@ -2213,9 +2210,9 @@ void CoreWrapper::process(
 							else if(rtabmap_.getPathStatus() <= 0)
 							{
 								RCLCPP_WARN(this->get_logger(), "Planning: Plan failed!");
-								if(moveBaseClient_.get()!=NULL && moveBaseClient_->action_server_is_ready())
+								if(nav2Client_.get()!=NULL && nav2Client_->action_server_is_ready())
 								{
-									moveBaseClient_->async_cancel_all_goals();
+									nav2Client_->async_cancel_all_goals();
 								}
 							}
 
@@ -2230,7 +2227,6 @@ void CoreWrapper::process(
 							goalFrameId_.clear();
 							latestNodeWasReached_ = false;
 						}
-#endif
 					}
 					else
 					{
@@ -3960,12 +3956,11 @@ void CoreWrapper::cancelGoalCallback(
 			goalReachedPub_->publish(result);
 		}
 	}
-#ifdef WITH_MOVE_BASE_MSGS
-	if(moveBaseClient_.get() != NULL && moveBaseClient_->action_server_is_ready())
+
+	if(nav2Client_.get() != NULL && nav2Client_->action_server_is_ready())
 	{
-		moveBaseClient_->async_cancel_all_goals();
+		nav2Client_->async_cancel_all_goals();
 	}
-#endif
 }
 
 void CoreWrapper::setLabelCallback(
@@ -4377,43 +4372,37 @@ void CoreWrapper::publishCurrentGoal(const rclcpp::Time & stamp)
 		poseMsg.header.frame_id = mapFrameId_;
 		poseMsg.header.stamp = stamp;
 		rtabmap_ros::transformToPoseMsg(currentMetricGoal_, poseMsg.pose);
-#ifdef WITH_MOVE_BASE_MSGS
 		if(useActionForGoal_)
 		{
-			if(moveBaseClient_.get() == NULL || !moveBaseClient_->action_server_is_ready())
+			if(nav2Client_.get() == NULL || !nav2Client_->action_server_is_ready())
 			{
-				RCLCPP_INFO(this->get_logger(), "Connecting to move_base action server...");
-				if(moveBaseClient_.get() == NULL)
+				RCLCPP_INFO(this->get_logger(), "Connecting to navigate_to_pose action server...");
+				if(nav2Client_.get() == NULL)
 				{
-					moveBaseClient_ = rclcpp_action::create_client<MoveBase>(
+					nav2Client_ = rclcpp_action::create_client<NavigateToPose>(
 					      this,
-					      "move_base");
+					      "navigate_to_pose");
 				}
-				if (!moveBaseClient_->wait_for_action_server(std::chrono::duration<double>(5.0))) {
-				  RCLCPP_ERROR(this->get_logger(), " move_base action server not available after waiting 5 seconds");
+				if (!nav2Client_->wait_for_action_server(std::chrono::duration<double>(5.0))) {
+				  RCLCPP_ERROR(this->get_logger(), " navigate_to_pose action server not available after waiting 5 seconds");
 				}
 			}
-			if(moveBaseClient_.get() != NULL && moveBaseClient_->action_server_is_ready())
+			if(nav2Client_.get() != NULL && nav2Client_->action_server_is_ready())
 			{
-				MoveBase::Goal goal_msg;
-				goal_msg.target_pose = poseMsg;
+				NavigateToPose::Goal goal_msg;
+				goal_msg.pose = poseMsg;
 
-				auto send_goal_options = rclcpp_action::Client<MoveBase>::SendGoalOptions();
-				send_goal_options.goal_response_callback =
-				  std::bind(&CoreWrapper::goalResponseCallback, this, std::placeholders::_1);
-				send_goal_options.feedback_callback =
-				  std::bind(&CoreWrapper::feedbackCallback, this, std::placeholders::_1, std::placeholders::_2);
-				send_goal_options.result_callback =
-				  std::bind(&CoreWrapper::resultCallback, this, std::placeholders::_1);
-				moveBaseClient_->async_send_goal(goal_msg, send_goal_options);
+				auto send_goal_options = rclcpp_action::Client<NavigateToPose>::SendGoalOptions();
+				send_goal_options.goal_response_callback = std::bind(&CoreWrapper::goalResponseCallback, this, std::placeholders::_1);
+				send_goal_options.result_callback = std::bind(&CoreWrapper::resultCallback, this, std::placeholders::_1);
+				nav2Client_->async_send_goal(goal_msg, send_goal_options);
 				lastPublishedMetricGoal_ = currentMetricGoal_;
 			}
 			else
 			{
-				RCLCPP_ERROR(this->get_logger(), "Cannot connect to move_base action server!");
+				RCLCPP_ERROR(this->get_logger(), "Cannot connect to navigate_to_pose action server!");
 			}
 		}
-#endif
 		if(nextMetricGoalPub_->get_subscription_count())
 		{
 			nextMetricGoalPub_->publish(poseMsg);
@@ -4425,9 +4414,8 @@ void CoreWrapper::publishCurrentGoal(const rclcpp::Time & stamp)
 	}
 }
 
-#ifdef WITH_MOVE_BASE_MSGS
 void CoreWrapper::goalResponseCallback(
-		std::shared_future<GoalHandleMoveBase::SharedPtr> future)
+		std::shared_future<GoalHandleNav2::SharedPtr> future)
 {
 	auto goal_handle = future.get();
 	if (!goal_handle) {
@@ -4442,15 +4430,8 @@ void CoreWrapper::goalResponseCallback(
 	}
 }
 
-void CoreWrapper::feedbackCallback(
-		GoalHandleMoveBase::SharedPtr,
-		const std::shared_ptr<const MoveBase::Feedback>)
-{
-	// do nothing special
-}
-
 void CoreWrapper::resultCallback(
-		const GoalHandleMoveBase::WrappedResult & result)
+		const GoalHandleNav2::WrappedResult & result)
 {
 	bool ignore = false;
 	if(!currentMetricGoal_.isNull())
@@ -4461,19 +4442,21 @@ void CoreWrapper::resultCallback(
 				rtabmap_.getPathCurrentGoalId() != rtabmap_.getPath().back().first &&
 				(!uContains(rtabmap_.getLocalOptimizedPoses(), rtabmap_.getPath().back().first) || !latestNodeWasReached_))
 			{
-				RCLCPP_WARN(this->get_logger(), "Planning: move_base reached current goal but it is not "
+				RCLCPP_WARN(this->get_logger(), "Planning: nav2 reached current goal but it is not "
 						 "the last one planned by rtabmap. A new goal should be sent when "
 						 "rtabmap will be able to retrieve next locations on the path.");
 				ignore = true;
 			}
 			else
 			{
-				RCLCPP_INFO(this->get_logger(), "Planning: move_base success!");
+				RCLCPP_INFO(this->get_logger(), "Planning: nav2 success!");
 			}
 		}
 		else
 		{
-			RCLCPP_ERROR(this->get_logger(), "Planning: move_base failed for some reason. Aborting the plan...");
+			RCLCPP_ERROR(this->get_logger(), "Planning: nav2 failed for some reason: %s. Aborting the plan...",
+					result.code==rclcpp_action::ResultCode::ABORTED?"Aborted":
+					result.code==rclcpp_action::ResultCode::CANCELED?"Canceled":"Unkown");
 		}
 
 		if(!ignore && goalReachedPub_->get_subscription_count())
@@ -4493,7 +4476,6 @@ void CoreWrapper::resultCallback(
 		latestNodeWasReached_ = false;
 	}
 }
-#endif
 
 void CoreWrapper::publishLocalPath(const rclcpp::Time & stamp)
 {

src/MapsManager.cpp

@@ -71,7 +71,7 @@ MapsManager::MapsManager() :
 #endif
 		octomapTreeDepth_(16),
 		octomapUpdated_(true),
-		latching_(false)
+		latching_(true)
 {
 }
 
@@ -123,35 +123,35 @@ void MapsManager::init(rclcpp::Node & node, const std::string & name, bool)
 
 	// mapping topics
 	latched_.clear();
-	gridMapPub_ = node.create_publisher<nav_msgs::msg::OccupancyGrid>("map", 1); // FIXME latching option in ROS2?
+	gridMapPub_ = node.create_publisher<nav_msgs::msg::OccupancyGrid>("map", rclcpp::QoS(1).reliable().durability(latching_?RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL:RMW_QOS_POLICY_DURABILITY_VOLATILE));
 	latched_.insert(std::make_pair((void*)&gridMapPub_, false));
-	gridProbMapPub_ = node.create_publisher<nav_msgs::msg::OccupancyGrid>("grid_prob_map", 1); // FIXME latching option in ROS2?
+	gridProbMapPub_ = node.create_publisher<nav_msgs::msg::OccupancyGrid>("grid_prob_map", rclcpp::QoS(1).reliable().durability(latching_?RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL:RMW_QOS_POLICY_DURABILITY_VOLATILE));
 	latched_.insert(std::make_pair((void*)&gridProbMapPub_, false));
-	cloudMapPub_ = node.create_publisher<sensor_msgs::msg::PointCloud2>("cloud_map", 1); // FIXME latching option in ROS2?
+	cloudMapPub_ = node.create_publisher<sensor_msgs::msg::PointCloud2>("cloud_map", rclcpp::QoS(1).reliable().durability(latching_?RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL:RMW_QOS_POLICY_DURABILITY_VOLATILE));
 	latched_.insert(std::make_pair((void*)&cloudMapPub_, false));
-	cloudObstaclesPub_ = node.create_publisher<sensor_msgs::msg::PointCloud2>("cloud_obstacles", 1); // FIXME latching option in ROS2?
+	cloudObstaclesPub_ = node.create_publisher<sensor_msgs::msg::PointCloud2>("cloud_obstacles", rclcpp::QoS(1).reliable().durability(latching_?RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL:RMW_QOS_POLICY_DURABILITY_VOLATILE));
 	latched_.insert(std::make_pair((void*)&cloudObstaclesPub_, false));
-	cloudGroundPub_ = node.create_publisher<sensor_msgs::msg::PointCloud2>("cloud_ground", 1); // FIXME latching option in ROS2?
+	cloudGroundPub_ = node.create_publisher<sensor_msgs::msg::PointCloud2>("cloud_ground", rclcpp::QoS(1).reliable().durability(latching_?RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL:RMW_QOS_POLICY_DURABILITY_VOLATILE));
 	latched_.insert(std::make_pair((void*)&cloudGroundPub_, false));
 
 #ifdef RTABMAP_OCTOMAP
 #ifdef WITH_OCTOMAP_MSGS
-	octoMapPubBin_ = node.create_publisher<octomap_msgs::msg::Octomap>("octomap_binary", 1);
+	octoMapPubBin_ = node.create_publisher<octomap_msgs::msg::Octomap>("octomap_binary", rclcpp::QoS(1).reliable().durability(latching_?RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL:RMW_QOS_POLICY_DURABILITY_VOLATILE));
 	latched_.insert(std::make_pair((void*)&octoMapPubBin_, false));
-	octoMapPubFull_ = node.create_publisher<octomap_msgs::msg::Octomap>("octomap_full", 1);
+	octoMapPubFull_ = node.create_publisher<octomap_msgs::msg::Octomap>("octomap_full", rclcpp::QoS(1).reliable().durability(latching_?RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL:RMW_QOS_POLICY_DURABILITY_VOLATILE));
 	latched_.insert(std::make_pair((void*)&octoMapPubFull_, false));
 #endif
-	octoMapCloud_ = node.create_publisher<sensor_msgs::msg::PointCloud2>("octomap_occupied_space", 1); // FIXME latching option in ROS2?
+	octoMapCloud_ = node.create_publisher<sensor_msgs::msg::PointCloud2>("octomap_occupied_space", rclcpp::QoS(1).reliable().durability(latching_?RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL:RMW_QOS_POLICY_DURABILITY_VOLATILE)); // FIXME latching option in ROS2?
 	latched_.insert(std::make_pair((void*)&octoMapCloud_, false));
-	octoMapFrontierCloud_ = node.create_publisher<sensor_msgs::msg::PointCloud2>("octomap_global_frontier_space", 1); // FIXME latching option in ROS2?
+	octoMapFrontierCloud_ = node.create_publisher<sensor_msgs::msg::PointCloud2>("octomap_global_frontier_space", rclcpp::QoS(1).reliable().durability(latching_?RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL:RMW_QOS_POLICY_DURABILITY_VOLATILE));
 	latched_.insert(std::make_pair((void*)&octoMapFrontierCloud_, false));
-	octoMapObstacleCloud_ = node.create_publisher<sensor_msgs::msg::PointCloud2>("octomap_obstacles", 1); // FIXME latching option in ROS2?
+	octoMapObstacleCloud_ = node.create_publisher<sensor_msgs::msg::PointCloud2>("octomap_obstacles", rclcpp::QoS(1).reliable().durability(latching_?RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL:RMW_QOS_POLICY_DURABILITY_VOLATILE));
 	latched_.insert(std::make_pair((void*)&octoMapObstacleCloud_, false));
-	octoMapGroundCloud_ = node.create_publisher<sensor_msgs::msg::PointCloud2>("octomap_ground", 1); // FIXME latching option in ROS2?
+	octoMapGroundCloud_ = node.create_publisher<sensor_msgs::msg::PointCloud2>("octomap_ground", rclcpp::QoS(1).reliable().durability(latching_?RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL:RMW_QOS_POLICY_DURABILITY_VOLATILE));
 	latched_.insert(std::make_pair((void*)&octoMapGroundCloud_, false));
-	octoMapEmptySpace_ = node.create_publisher<sensor_msgs::msg::PointCloud2>("octomap_empty_space", 1); // FIXME latching option in ROS2?
+	octoMapEmptySpace_ = node.create_publisher<sensor_msgs::msg::PointCloud2>("octomap_empty_space", rclcpp::QoS(1).reliable().durability(latching_?RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL:RMW_QOS_POLICY_DURABILITY_VOLATILE));
 	latched_.insert(std::make_pair((void*)&octoMapEmptySpace_, false));
-	octoMapProj_ = node.create_publisher<nav_msgs::msg::OccupancyGrid>("octomap_grid", 1); // FIXME latching option in ROS2?
+	octoMapProj_ = node.create_publisher<nav_msgs::msg::OccupancyGrid>("octomap_grid", rclcpp::QoS(1).reliable().durability(latching_?RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL:RMW_QOS_POLICY_DURABILITY_VOLATILE));
 	latched_.insert(std::make_pair((void*)&octoMapProj_, false));
 #endif
 }
@@ -184,7 +184,7 @@ void parameterMoved(
 		{
 			RCLCPP_WARN(node.get_logger(), "Parameter \"%s\" has moved from "
 					 "rtabmap_ros to rtabmap library. Use "
-					 "parameter \"%s\" instead. The value \"\" is still "
+					 "parameter \"%s\" instead. The value \"%s\" is still "
 					 "copied to new parameter name.",
 					 rosName.c_str(),
 					 parameterName.c_str(),