rtabmap.launch.py: added options to connect to nav2 (through goal_pos…

…e topic or action server). RVIZ/MapCloud: fixed downloading option. rtabmap node: all services are now in node namespace. Added dev containers for Humble and Jazzy.

.devcontainer/jazzy/devcontainer.json

@@ -0,0 +1,11 @@
+{
+    "image": "introlab3it/rtabmap_ros:jazzy-latest",
+    "customizations": {
+        "vscode": {
+          "extensions": ["ms-vscode.cpptools-themes", "ms-vscode.cmake-tools", "vscjava.vscode-java-pack"]
+        }
+    },
+    "workspaceMount": "source=${localWorkspaceFolder},target=/ros2_ws/src/rtabmap_ros,type=bind",
+    "workspaceFolder": "/ros2_ws",
+    "postAttachCommand": "echo 'Initialize colcon: source /opt/ros/jazzy/setup.bash && cd /ros2_ws && colcon build --cmake-args -DRTABMAP_SYNC_MULTI_RGBD=ON -DCMAKE_BUILD_TYPE=Release'"
+}

rtabmap_launch/launch/rtabmap.launch.py

@@ -274,6 +274,7 @@ def launch_setup(context, *args, **kwargs):
                 "odom_frame_id": LaunchConfiguration('odom_frame_id').perform(context),
                 "publish_tf": LaunchConfiguration('publish_tf_map'),
                 "initial_pose": LaunchConfiguration('initial_pose'),
+                "use_action_for_goal": LaunchConfiguration('use_action_for_goal'),
                 "ground_truth_frame_id": LaunchConfiguration('ground_truth_frame_id').perform(context),
                 "ground_truth_base_frame_id": LaunchConfiguration('ground_truth_base_frame_id').perform(context),
                 "odom_tf_angular_variance": LaunchConfiguration('odom_tf_angular_variance'),
@@ -316,7 +317,8 @@ def launch_setup(context, *args, **kwargs):
                 ("tag_detections", LaunchConfiguration('tag_topic')),
                 ("fiducial_transforms", LaunchConfiguration('fiducial_topic')),
                 ("odom", LaunchConfiguration('odom_topic')),
-                ("imu", LaunchConfiguration('imu_topic'))],
+                ("imu", LaunchConfiguration('imu_topic')),
+                ("goal_out", LaunchConfiguration('output_goal_topic'))],
             arguments=[LaunchConfiguration("args"), "--ros-args", "--log-level", [LaunchConfiguration('namespace'), '.rtabmap:=', LaunchConfiguration('log_level')], "--log-level", ['rtabmap:=', LaunchConfiguration('log_level')]],
             prefix=LaunchConfiguration('launch_prefix'),
             namespace=LaunchConfiguration('namespace')),
@@ -425,6 +427,9 @@ def generate_launch_description():
         DeclareLaunchArgument('output',         default_value='screen',             description='Control node output (screen or log).'),
         DeclareLaunchArgument('initial_pose',   default_value='',                   description='Set an initial pose (only in localization mode). Format: "x y z roll pitch yaw" or "x y z qx qy qz qw". Default: see "RGBD/StartAtOrigin" doc'),
 
+        DeclareLaunchArgument('output_goal_topic', default_value='/goal_pose',      description='Output goal topic (can be connected to nav2).'),
+        DeclareLaunchArgument('use_action_for_goal', default_value='false',         description='Connect to nav2\'s navigate_to_pose action server instead of publishing the output goal topic.'),
+
         DeclareLaunchArgument('ground_truth_frame_id',      default_value='', description='e.g., "world"'),
         DeclareLaunchArgument('ground_truth_base_frame_id', default_value='', description='e.g., "tracker", a fake frame matching the frame "frame_id" (but on different TF tree)'),
 

rtabmap_rviz_plugins/include/rtabmap_rviz_plugins/MapCloudDisplay.h

@@ -175,6 +175,7 @@ private Q_SLOTS:
 	void fillTransformerOptions( rviz_common::properties::EnumProperty* prop, uint32_t mask );
 
 private:
+	std::shared_ptr<rclcpp::Node> clientNode_;
 	rclcpp::Publisher<std_msgs::msg::Int32MultiArray>::SharedPtr republishNodeDataPub_;
 
 	std::map<int, CloudInfoPtr> cloud_infos_;

rtabmap_rviz_plugins/src/MapCloudDisplay.cpp

@@ -57,7 +57,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap_conversions/MsgConversion.h>
 #include <rtabmap_msgs/srv/get_map.hpp>
 
-
 namespace rtabmap_rviz_plugins
 {
 
@@ -97,6 +96,10 @@ MapCloudDisplay::MapCloudDisplay()
 	//QIcon icon;
 	//this->setIcon(icon);
 
+	auto options = rclcpp::NodeOptions().arguments(
+		{"--ros-args", "--remap", "__node:=rviz_map_cloud_action_client", "--"});
+	clientNode_ = std::make_shared<rclcpp::Node>("_", options);
+
 	style_property_ = new rviz_common::properties::EnumProperty( "Style", "Flat Squares",
 										"Rendering mode to use, in order of computational complexity.",
 										this, SLOT( updateStyle() ), this );
@@ -500,64 +503,64 @@ void MapCloudDisplay::updateCloudParameters()
 	fromScan_ = cloud_from_scan_->getBool();
 }
 
-void MapCloudDisplay::downloadMap(bool /*graphOnly*/)
+void MapCloudDisplay::downloadMap(bool graphOnly)
 {
-	RCLCPP_ERROR(rviz_ros_node_.lock()->get_raw_node()->get_logger(), "MapCloud plugin: DownloadMap still not working on ros2");
-	return;
-	// FIXME: ros2: can connect to client, rtabmap returns data but the callback here is never called?!
-	/*
 	auto request = std::make_shared<rtabmap_msgs::srv::GetMap::Request>();
 	request->global_map = false;
 	request->optimized = true;
 	request->graph_only = graphOnly;
 	std::string rtabmapNs = download_namespace->getStdString();
-	std::string srvName = uFormat("%s/get_map_data", rtabmapNs.c_str());
-//	QMessageBox * messageBox = new QMessageBox(
-//			QMessageBox::NoIcon,
-//			tr("Calling \"%1\" service...").arg(srvName.c_str()),
-//			tr("Downloading the map... please wait (rviz could become gray!)"),
-//			QMessageBox::NoButton);
-//	messageBox->setAttribute(Qt::WA_DeleteOnClose, true);
-//	messageBox->show();
-//	QApplication::processEvents();
-//	uSleep(100); // hack make sure the text in the QMessageBox is shown...
-//	QApplication::processEvents();
+	std::string srvName = rtabmapNs+"/get_map_data";
+	QMessageBox * messageBox = new QMessageBox(
+			QMessageBox::NoIcon,
+			tr("Calling \"%1\" service...").arg(srvName.c_str()),
+			tr("Downloading the map... please wait (rviz could become gray!)"),
+			QMessageBox::NoButton,
+			getAssociatedWidget());
+	messageBox->setAttribute(Qt::WA_DeleteOnClose, true);
+	messageBox->show();
+	QApplication::processEvents();
+	uSleep(100); // hack make sure the text in the QMessageBox is shown...
+	QApplication::processEvents();
 
-	RVIZ_COMMON_LOG_WARNING(uFormat("Wait for service %s", srvName.c_str()));
-	auto client = rviz_ros_node_.lock()->get_raw_node()->create_client<rtabmap_ros::srv::GetMap>(srvName);
+	RVIZ_COMMON_LOG_INFO(uFormat("Wait for service %s", srvName.c_str()));
+
+	auto client = clientNode_->create_client<rtabmap_msgs::srv::GetMap>(srvName);
 	if(client->wait_for_service(std::chrono::seconds(1)))
 	{
-		using ServiceResponseFuture = rclcpp::Client<rtabmap_ros::srv::GetMap>::SharedFuture;
-		auto response_received_callback = [this, &graphOnly](ServiceResponseFuture future) {
-			auto result = future.get();
-			RVIZ_COMMON_LOG_WARNING(uFormat("Process data"));
+		RVIZ_COMMON_LOG_INFO(uFormat("Calling service %s", srvName.c_str()));
+		auto result = client->async_send_request(request);
+		if (rclcpp::spin_until_future_complete(clientNode_, result) ==
+    		rclcpp::FutureReturnCode::SUCCESS)
+		{
+			RVIZ_COMMON_LOG_INFO(uFormat("Process data"));
+			auto future = result.get();
 			if(graphOnly)
 			{
-				//messageBox->setText(tr("Updating the map (%1 nodes downloaded)...").arg(result->data.graph.poses.size()));
-				//QApplication::processEvents();
-				processMapData(result->data);
-				//messageBox->setText(tr("Updating the map (%1 nodes downloaded)... done!").arg(result->data.graph.poses.size()));
-
-			//	QTimer::singleShot(1000, messageBox, SLOT(close()));
+				messageBox->setText(tr("Updating the map (%1 nodes downloaded)...").arg(future->data.graph.poses.size()));
+				QApplication::processEvents();
+				processMapData(future->data);
+				messageBox->setText(tr("Updating the map (%1 nodes downloaded)... done!").arg(future->data.graph.poses.size()));
+				QApplication::processEvents();
+				QTimer::singleShot(1000, messageBox, SLOT(close()));
 			}
 			else
 			{
-				//messageBox->setText(tr("Creating all clouds (%1 poses and %2 clouds downloaded)...")
-				//		.arg(result->data.graph.poses.size()).arg(result->data.nodes.size()));
-				//QApplication::processEvents();
+				messageBox->setText(tr("Creating all clouds (%1 poses and %2 clouds downloaded)...")
+						.arg(future->data.graph.poses.size()).arg(future->data.nodes.size()));
+				QApplication::processEvents();
 				this->reset();
-				processMapData(result->data);
-				//messageBox->setText(tr("Creating all clouds (%1 poses and %2 clouds downloaded)... done!")
-				//		.arg(result->data.graph.poses.size()).arg(result->data.nodes.size()));
+				processMapData(future->data);
+				messageBox->setText(tr("Creating all clouds (%1 poses and %2 clouds downloaded)... done!")
+						.arg(future->data.graph.poses.size()).arg(future->data.nodes.size()));
 
-			//	QTimer::singleShot(1000, messageBox, SLOT(close()));
+				QTimer::singleShot(1000, messageBox, SLOT(close()));
 			}
-		};
-		RVIZ_COMMON_LOG_WARNING(uFormat("Calling service %s", srvName.c_str()));
-		auto result_future = client->async_send_request(request, response_received_callback);
-		RVIZ_COMMON_LOG_WARNING(uFormat("Wait"));
-		result_future.wait();
-		RVIZ_COMMON_LOG_WARNING(uFormat("Wait end"));
+		} else {
+			std::string msg = uFormat("Failed to call service %s", srvName.c_str());
+			RVIZ_COMMON_LOG_ERROR(msg);
+			messageBox->setText(msg.c_str());
+		}
 	}
 	else
 	{
@@ -567,8 +570,8 @@ void MapCloudDisplay::downloadMap(bool /*graphOnly*/)
 				  srvName.c_str(),
 				  rtabmapNs.c_str());
 		RVIZ_COMMON_LOG_ERROR(msg);
-		//messageBox->setText(msg.c_str());
-	}*/
+		messageBox->setText(msg.c_str());
+	}
 }
 
 void MapCloudDisplay::downloadNamespaceChanged()