created ros2 branch

2026-10-06 17:57:45 +08:00 · 2020-01-30 17:18:48 -05:00
parent d80a66a724
commit e6ea46f5f0
86 changed files with 9079 additions and 9079 deletions
@@ -29,8 +29,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <QApplication>
 #include <QDir>

-#include <std_srvs/Empty.h>
-#include <std_msgs/Empty.h>
+#include <std_srvs/srv/empty.hpp>
+#include <std_msgs/msg/empty.hpp>

 #include <rtabmap/utilite/UEventsManager.h>
 #include <rtabmap/utilite/UConversion.h>
@@ -49,9 +49,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/utilite/UTimer.h>

 #include "rtabmap_ros/MsgConversion.h"
-#include "rtabmap_ros/GetMap.h"
-#include "rtabmap_ros/SetGoal.h"
-#include "rtabmap_ros/SetLabel.h"
+#include "rtabmap_ros/srv/set_goal.hpp"
+#include "rtabmap_ros/srv/set_label.hpp"
 #include "rtabmap_ros/PreferencesDialogROS.h"

 float max3( const float& a, const float& b, const float& c)
@@ -62,30 +61,34 @@ float max3( const float& a, const float& b, const float& c)

 namespace rtabmap_ros {

-GuiWrapper::GuiWrapper(int & argc, char** argv) :
-		CommonDataSubscriber(true),
+GuiWrapper::GuiWrapper(const rclcpp::NodeOptions & options) :
+		Node("rtabmapviz", options),
+		CommonDataSubscriber(*this, true),
 		mainWindow_(0),
+		cameraNodeName_(""),
+		lastOdomInfoUpdateTime_(0),
 		frameId_("base_link"),
 		odomFrameId_(""),
-		waitForTransform_(true),
-		waitForTransformDuration_(0.2), // 200 ms
+		waitForTransform_(0.2), // 200 ms
 		odomSensorSync_(false),
-		maxOdomUpdateRate_(10),
-		cameraNodeName_(""),
-		lastOdomInfoUpdateTime_(0)
+		maxOdomUpdateRate_(10)
 {
-	ros::NodeHandle nh;
-	ros::NodeHandle pnh("~");
+	tfBuffer_ = std::make_shared<tf2_ros::Buffer>(this->get_clock());
+	//auto timer_interface = std::make_shared<tf2_ros::CreateTimerROS>(
+	//	this->get_node_base_interface(),
+	//	this->get_node_timers_interface());
+	//tfBuffer_->setCreateTimerInterface(timer_interface);
+	tfListener_ = std::make_shared<tf2_ros::TransformListener>(*tfBuffer_);

 	QString configFile = QDir::homePath()+"/.ros/rtabmapGUI.ini";
-	for(int i=1; i<argc; ++i)
+	for(size_t i=0; i<options.arguments().size(); ++i)
 	{
-		if(strcmp(argv[i], "-d") == 0)
+		if(options.arguments()[i].compare("-d") == 0)
 		{
 			++i;
-			if(i < argc)
+			if(i < options.arguments().size())
 			{
-				configFile = argv[i];
+				configFile = options.arguments()[i].c_str();
 			}
 			break;
 		}
@@ -93,27 +96,24 @@ GuiWrapper::GuiWrapper(int & argc, char** argv) :

 	configFile.replace('~', QDir::homePath());

-	ROS_INFO("rtabmapviz: Using configuration from \"%s\"", configFile.toStdString().c_str());
+	RCLCPP_INFO(this->get_logger(), "rtabmapviz: Using configuration from \"%s\"", configFile.toStdString().c_str());
 	uSleep(500);
-	mainWindow_ = new MainWindow(new PreferencesDialogROS(configFile));
+	mainWindow_ = new MainWindow(new PreferencesDialogROS(this, configFile));
 	mainWindow_->setWindowTitle(mainWindow_->windowTitle()+" [ROS]");
 	mainWindow_->show();
 	bool paused = false;
-	nh.param("is_rtabmap_paused", paused, paused);
+	paused = this->declare_parameter("is_rtabmap_paused", paused);
 	mainWindow_->setMonitoringState(paused);

 	// To receive odometry events
-	std::string tfPrefix;
 	std::string initCachePath;
-	pnh.param("frame_id", frameId_, frameId_);
-	pnh.param("odom_frame_id", odomFrameId_, odomFrameId_); // set to use odom from TF
-	pnh.param("tf_prefix", tfPrefix, tfPrefix);
-	pnh.param("wait_for_transform", waitForTransform_, waitForTransform_);
-	pnh.param("wait_for_transform_duration",  waitForTransformDuration_, waitForTransformDuration_);
-	pnh.param("odom_sensor_sync", odomSensorSync_, odomSensorSync_);
-	pnh.param("max_odom_update_rate", maxOdomUpdateRate_, maxOdomUpdateRate_);
-	pnh.param("camera_node_name", cameraNodeName_, cameraNodeName_); // used to pause the rtabmap_ros/camera when pausing the process
-	pnh.param("init_cache_path", initCachePath, initCachePath);
+	frameId_ = this->declare_parameter("frame_id", frameId_);
+	this->get_parameter_or("odom_frame_id", odomFrameId_, odomFrameId_); // already declared in CommonDataSubscriber
+	waitForTransform_ = this->declare_parameter("wait_for_transform", waitForTransform_);
+	odomSensorSync_ = this->declare_parameter("odom_sensor_sync", odomSensorSync_);
+	maxOdomUpdateRate_ = this->declare_parameter("max_odom_update_rate", maxOdomUpdateRate_);
+	cameraNodeName_ = this->declare_parameter("camera_node_name", cameraNodeName_);
+	initCachePath = this->declare_parameter("init_cache_path", initCachePath);
 	if(initCachePath.size())
 	{
 		initCachePath = uReplaceChar(initCachePath, '~', UDirectory::homeDir());
@@ -121,59 +121,38 @@ GuiWrapper::GuiWrapper(int & argc, char** argv) :
 		{
 			initCachePath = UDirectory::currentDir(true) + initCachePath;
 		}
-		ROS_INFO("rtabmapviz: Initializing cache with local database \"%s\"", initCachePath.c_str());
-		uSleep(2000); // make sure rtabmap node is created if launched at the same time
-		rtabmap_ros::GetMap getMapSrv;
-		getMapSrv.request.global = false;
-		getMapSrv.request.optimized = true;
-		getMapSrv.request.graphOnly = true;
-		if(!ros::service::call("get_map", getMapSrv))
+		RCLCPP_INFO(this->get_logger(), "rtabmapviz: Initializing cache with local database \"%s\"", initCachePath.c_str());
+		if(!callMapDataService("get_map_data", false, true, true))
 		{
-			ROS_WARN("Can't call \"get_map\" service. The cache will still be loaded "
+			RCLCPP_ERROR(this->get_logger(),
+					"The cache will still be loaded "
 					"but the clouds won't be created until next time rtabmapviz "
 					"receives the optimized graph.");
 		}
-		else
-		{
-			// this will update the poses and constraints of the MainWindow
-			processRequestedMap(getMapSrv.response.data);
-		}
 		QMetaObject::invokeMethod(mainWindow_, "updateCacheFromDatabase", Q_ARG(QString, QString(initCachePath.c_str())));
 	}

-	if(!tfPrefix.empty())
-	{
-		if(!frameId_.empty())
-		{
-			frameId_ = tfPrefix + "/" + frameId_;
-		}
-		if(!odomFrameId_.empty())
-		{
-			odomFrameId_ = tfPrefix + "/" + odomFrameId_;
-		}
-	}
-
 	UEventsManager::addHandler(this);
 	UEventsManager::addHandler(mainWindow_);

-	infoTopic_.subscribe(nh, "info", 1);
-	mapDataTopic_.subscribe(nh, "mapData", 1);
+	infoTopic_.subscribe(this, "info", rmw_qos_profile_sensor_data);
+	mapDataTopic_.subscribe(this, "mapData", rmw_qos_profile_sensor_data);
 	infoMapSync_ = new message_filters::Synchronizer<MyInfoMapSyncPolicy>(
 			MyInfoMapSyncPolicy(this->getQueueSize()),
 			infoTopic_,
 			mapDataTopic_);
-	infoMapSync_->registerCallback(boost::bind(&GuiWrapper::infoMapCallback, this, _1, _2));
+	infoMapSync_->registerCallback(std::bind(&GuiWrapper::infoMapCallback, this, std::placeholders::_1, std::placeholders::_2));

-	goalTopic_.subscribe(nh, "goal_node", 1);
-	pathTopic_.subscribe(nh, "global_path", 1);
+	goalTopic_.subscribe(this, "goal_node", rmw_qos_profile_sensor_data);
+	pathTopic_.subscribe(this, "global_path", rmw_qos_profile_sensor_data);
 	goalPathSync_ = new message_filters::Synchronizer<MyGoalPathSyncPolicy>(
 			MyGoalPathSyncPolicy(this->getQueueSize()),
 			goalTopic_,
 			pathTopic_);
-	goalPathSync_->registerCallback(boost::bind(&GuiWrapper::goalPathCallback, this, _1, _2));
-	goalReachedTopic_ = nh.subscribe("goal_reached", 1, &GuiWrapper::goalReachedCallback, this);
+	goalPathSync_->registerCallback(std::bind(&GuiWrapper::goalPathCallback, this, std::placeholders::_1, std::placeholders::_2));
+	goalReachedTopic_ = this->create_subscription<std_msgs::msg::Bool>("goal_reached", rclcpp::SensorDataQoS(), std::bind(&GuiWrapper::goalReachedCallback, this, std::placeholders::_1));

-	setupCallbacks(nh, pnh, ros::this_node::getName()); // do it at the end
+	setupCallbacks(*this); // do it at the end
 }

 GuiWrapper::~GuiWrapper()
@@ -185,10 +164,10 @@ GuiWrapper::~GuiWrapper()
 }

 void GuiWrapper::infoMapCallback(
-		const rtabmap_ros::InfoConstPtr & infoMsg,
-		const rtabmap_ros::MapDataConstPtr & mapMsg)
+		const rtabmap_ros::msg::Info::ConstSharedPtr infoMsg,
+		const rtabmap_ros::msg::MapData::ConstSharedPtr mapMsg)
 {
-	//ROS_INFO("rtabmapviz: RTAB-Map info ex received!");
+	//RCLCPP_INFO(this->get_logger(), "rtabmapviz: RTAB-Map info ex received!");

 	// Map from ROS struct to rtabmap struct
 	rtabmap::Statistics stat;
@@ -216,8 +195,8 @@ void GuiWrapper::infoMapCallback(
 }

 void GuiWrapper::goalPathCallback(
-		const rtabmap_ros::GoalConstPtr & goalMsg,
-		const nav_msgs::PathConstPtr & pathMsg)
+		const rtabmap_ros::msg::Goal::ConstSharedPtr goalMsg,
+		const nav_msgs::msg::Path::ConstSharedPtr pathMsg)
 {
 	// we don't have the node ids, just generate fake ones.
 	std::vector<std::pair<int, Transform> > poses(pathMsg->poses.size());
@@ -230,12 +209,12 @@ void GuiWrapper::goalPathCallback(
 }

 void GuiWrapper::goalReachedCallback(
-		const std_msgs::BoolConstPtr & value)
+		const std_msgs::msg::Bool::ConstSharedPtr value)
 {
 	this->post(new RtabmapGoalStatusEvent(value->data?1:-1));
 }

-void GuiWrapper::processRequestedMap(const rtabmap_ros::MapData & map)
+void GuiWrapper::processRequestedMap(const rtabmap_ros::msg::MapData & map)
 {
 	std::map<int, Signature> signatures;
 	std::map<int, Transform> poses;
@@ -250,47 +229,112 @@ void GuiWrapper::processRequestedMap(const rtabmap_ros::MapData & map)
 	QMetaObject::invokeMethod(mainWindow_, "processRtabmapEvent3DMap", Q_ARG(rtabmap::RtabmapEvent3DMap, e));
 }

+bool GuiWrapper::callEmptyService(const std::string & name)
+{
+	auto client = this->create_client<std_srvs::srv::Empty>(name);
+	if(client->wait_for_service(std::chrono::seconds(1)))
+	{
+		auto request = std::make_shared<std_srvs::srv::Empty::Request>();
+		auto result_future = client->async_send_request(request);
+		auto node = rclcpp::Node::make_shared("rtabmapviz");
+		if (rclcpp::spin_until_future_complete(node, result_future) !=
+				rclcpp::executor::FutureReturnCode::SUCCESS)
+		{
+			RCLCPP_ERROR(this->get_logger(),
+					"Can't call \"%s\" service.", name.c_str());
+			return false;
+		}
+		else
+		{
+			return true;
+		}
+	}
+	else
+	{
+		RCLCPP_WARN(this->get_logger(), "Service \"%s\" not available.", name.c_str());
+	}
+	return false;
+}
+
+bool GuiWrapper::callMapDataService(const std::string & name, bool global, bool optimized, bool graphOnly)
+{
+	auto client = this->create_client<rtabmap_ros::srv::GetMap>(name);
+	if(client->wait_for_service(std::chrono::seconds(2)))
+	{
+		auto request = std::make_shared<rtabmap_ros::srv::GetMap::Request>();
+		request->global = global;
+		request->optimized = optimized;
+		request->graph_only = graphOnly;
+		auto result_future = client->async_send_request(request);
+		auto node = rclcpp::Node::make_shared("rtabmapviz");
+		if (rclcpp::spin_until_future_complete(node, result_future) != rclcpp::executor::FutureReturnCode::SUCCESS)
+		{
+			RCLCPP_ERROR(this->get_logger(), "Service \"%s\" failed to get the data.", name.c_str());
+		}
+		else
+		{
+			auto result = result_future.get();
+			processRequestedMap(result->data);
+			return true;
+		}
+	}
+	else
+	{
+		RCLCPP_WARN(this->get_logger(), "Service \"%s\" not available.", name.c_str());
+	}
+	return false;
+}
+
 bool GuiWrapper::handleEvent(UEvent * anEvent)
 {
 	if(anEvent->getClassName().compare("ParamEvent") == 0)
 	{
 		const rtabmap::ParametersMap & defaultParameters = rtabmap::Parameters::getDefaultParameters();
 		rtabmap::ParametersMap parameters = ((rtabmap::ParamEvent *)anEvent)->getParameters();
-		bool modified = false;
-		ros::NodeHandle nh;
+		std::vector<rclcpp::Parameter> rosParameters;
+		auto node = rclcpp::Node::make_shared("rtabmapviz");
 		for(rtabmap::ParametersMap::iterator i=parameters.begin(); i!=parameters.end(); ++i)
 		{
 			//save only parameters with valid names
 			if(defaultParameters.find((*i).first) != defaultParameters.end())
 			{
-				nh.setParam((*i).first, (*i).second);
-				modified = true;
+				rosParameters.push_back(rclcpp::Parameter((*i).first, (*i).second));
 			}
 			else if((*i).first.find('/') != (*i).first.npos)
 			{
-				ROS_WARN("Parameter %s is not used by the rtabmap node.", (*i).first.c_str());
+				RCLCPP_WARN(this->get_logger(), "Parameter %s is not used by the rtabmap node.", (*i).first.c_str());
 			}
 		}
-		if(modified)
+		if(rosParameters.size())
 		{
-			ROS_INFO("Parameters updated");
-			std_srvs::Empty srv;
-			if(!ros::service::call("update_parameters", srv))
+			RCLCPP_INFO(this->get_logger(), "Parameters updated");
+			auto client = std::make_shared<rclcpp::AsyncParametersClient>(this, "rtabmap");
+			if (!client->wait_for_service(std::chrono::seconds(5))) {
+				RCLCPP_ERROR(this->get_logger(), "Can't call rtabmap parameters service, is the node running?");
+			}
+			else
 			{
-				ROS_ERROR("Can't call \"update_parameters\" service");
+				auto results = client->set_parameters(rosParameters);
+				// Wait for the results.
+				if (rclcpp::spin_until_future_complete(node, results, std::chrono::seconds(5)) !=
+						rclcpp::executor::FutureReturnCode::SUCCESS)
+				{
+					RCLCPP_ERROR(this->get_logger(), "Failed to set rtabmap parameters!");
+				}
 			}
 		}
+
+
 	}
 	else if(anEvent->getClassName().compare("RtabmapEventCmd") == 0)
 	{
-		std_srvs::Empty emptySrv;
 		rtabmap::RtabmapEventCmd * cmdEvent = (rtabmap::RtabmapEventCmd *)anEvent;
 		rtabmap::RtabmapEventCmd::Cmd cmd = cmdEvent->getCmd();
 		if(cmd == rtabmap::RtabmapEventCmd::kCmdResetMemory)
 		{
-			if(!ros::service::call("reset", emptySrv))
+			if(!callEmptyService("reset"))
 			{
-				ROS_ERROR("Can't call \"reset\" service");
+				RCLCPP_ERROR(this->get_logger(), "Can't call \"reset\" service");
 			}
 		}
 		else if(cmd == rtabmap::RtabmapEventCmd::kCmdPause)
@@ -301,29 +345,29 @@ bool GuiWrapper::handleEvent(UEvent * anEvent)
 				std::string str = uFormat("rosrun dynamic_reconfigure dynparam set %s pause true", cameraNodeName_.c_str());
 				if(system(str.c_str()) !=0)
 				{
-					ROS_ERROR("Command \"%s\" returned non zero value.", str.c_str());
+					RCLCPP_ERROR(this->get_logger(), "Command \"%s\" returned non zero value.", str.c_str());
 				}
 			}

 			// Pause visual_odometry
-			ros::service::call("pause_odom", emptySrv);
+			callEmptyService("pause_odom");

 			// Pause rtabmap
-			if(!ros::service::call("pause", emptySrv))
+			if(!callEmptyService("pause"))
 			{
-				ROS_ERROR("Can't call \"pause\" service");
+				RCLCPP_ERROR(this->get_logger(), "Can't call \"pause\" service");
 			}
 		}
 		else if(cmd == rtabmap::RtabmapEventCmd::kCmdResume)
 		{
 			// Resume rtabmap
-			if(!ros::service::call("resume", emptySrv))
+			if(!callEmptyService("resume"))
 			{
-				ROS_ERROR("Can't call \"resume\" service");
+				RCLCPP_ERROR(this->get_logger(), "Can't call \"resume\" service");
 			}

 			// Pause visual_odometry
-			ros::service::call("resume_odom", emptySrv);
+			callEmptyService("resume_odom");

 			// Resume the camera if the rtabmap/camera node is used
 			if(!cameraNodeName_.empty())
@@ -331,15 +375,15 @@ bool GuiWrapper::handleEvent(UEvent * anEvent)
 				std::string str = uFormat("rosrun dynamic_reconfigure dynparam set %s pause false", cameraNodeName_.c_str());
 				if(system(str.c_str()) !=0)
 				{
-					ROS_ERROR("Command \"%s\" returned non zero value.", str.c_str());
+					RCLCPP_ERROR(this->get_logger(), "Command \"%s\" returned non zero value.", str.c_str());
 				}
 			}
 		}
 		else if(cmd == rtabmap::RtabmapEventCmd::kCmdTriggerNewMap)
 		{
-			if(!ros::service::call("trigger_new_map", emptySrv))
+			if(!callEmptyService("trigger_new_map"))
 			{
-				ROS_ERROR("Can't call \"trigger_new_map\" service");
+				RCLCPP_ERROR(this->get_logger(), "Can't call \"trigger_new_map\" service");
 			}
 		}
 		else if(cmd == rtabmap::RtabmapEventCmd::kCmdPublish3DMap)
@@ -348,90 +392,105 @@ bool GuiWrapper::handleEvent(UEvent * anEvent)
 			UASSERT(cmdEvent->value2().isBool());
 			UASSERT(cmdEvent->value3().isBool());

-			rtabmap_ros::GetMap getMapSrv;
-			getMapSrv.request.global = cmdEvent->value1().toBool();
-			getMapSrv.request.optimized = cmdEvent->value2().toBool();
-			getMapSrv.request.graphOnly = cmdEvent->value3().toBool();
-			if(!ros::service::call("get_map_data", getMapSrv))
+			if(!callMapDataService("get_map_data", cmdEvent->value1().toBool(), cmdEvent->value2().toBool(), cmdEvent->value3().toBool()))
 			{
-				ROS_WARN("Can't call \"get_map_data\" service");
 				this->post(new RtabmapEvent3DMap(1)); // service error
 			}
-			else
-			{
-				processRequestedMap(getMapSrv.response.data);
-			}
 		}
 		else if(cmd == rtabmap::RtabmapEventCmd::kCmdGoal)
 		{
 			UASSERT(cmdEvent->value1().isStr() || cmdEvent->value1().isInt() || cmdEvent->value1().isUInt());
-			rtabmap_ros::SetGoal setGoalSrv;
-			setGoalSrv.request.node_id = !cmdEvent->value1().isStr()?cmdEvent->value1().toInt():0;
-			setGoalSrv.request.node_label = cmdEvent->value1().isStr()?cmdEvent->value1().toStr():"";
-			if(!ros::service::call("set_goal", setGoalSrv))
+
+			auto client = this->create_client<rtabmap_ros::srv::SetGoal>("set_goal");
+			if(client->wait_for_service(std::chrono::seconds(1)))
 			{
-				ROS_ERROR("Can't call \"set_goal\" service");
+				auto request = std::make_shared<rtabmap_ros::srv::SetGoal::Request>();
+				request->node_id = !cmdEvent->value1().isStr()?cmdEvent->value1().toInt():0;
+				request->node_label = cmdEvent->value1().isStr()?cmdEvent->value1().toStr():"";
+				auto result_future = client->async_send_request(request);
+				auto node = rclcpp::Node::make_shared("rtabmapviz");
+				if (rclcpp::spin_until_future_complete(node, result_future) !=
+						rclcpp::executor::FutureReturnCode::SUCCESS)
+				{
+					RCLCPP_ERROR(this->get_logger(), "Can't call \"set_goal\" service.");
+				}
+				else
+				{
+					auto result = result_future.get();
+					UASSERT(result->path_ids.size() == result->path_poses.size());
+					std::vector<std::pair<int, Transform> > poses(result->path_poses.size());
+					for(unsigned int i=0; i<result->path_poses.size(); ++i)
+					{
+						poses[i].first = result->path_ids[i];
+						poses[i].second = rtabmap_ros::transformFromPoseMsg(result->path_poses[i]);
+					}
+					this->post(new RtabmapGlobalPathEvent(request->node_id, request->node_label, poses, result->planning_time));
+				}
 			}
 			else
 			{
-				UASSERT(setGoalSrv.response.path_ids.size() == setGoalSrv.response.path_poses.size());
-				std::vector<std::pair<int, Transform> > poses(setGoalSrv.response.path_poses.size());
-				for(unsigned int i=0; i<setGoalSrv.response.path_poses.size(); ++i)
-				{
-					poses[i].first = setGoalSrv.response.path_ids[i];
-					poses[i].second = rtabmap_ros::transformFromPoseMsg(setGoalSrv.response.path_poses[i]);
-				}
-				this->post(new RtabmapGlobalPathEvent(setGoalSrv.request.node_id, setGoalSrv.request.node_label, poses, setGoalSrv.response.planning_time));
+				RCLCPP_WARN(this->get_logger(), "Service \"set_goal\" not available.");
 			}
 		}
 		else if(cmd == rtabmap::RtabmapEventCmd::kCmdCancelGoal)
 		{
-			if(!ros::service::call("cancel_goal", emptySrv))
+			if(!callEmptyService("cancel_goal"))
 			{
-				ROS_ERROR("Can't call \"cancel_goal\" service");
+				RCLCPP_ERROR(this->get_logger(), "Can't call \"cancel_goal\" service");
 			}
 		}
 		else if(cmd == rtabmap::RtabmapEventCmd::kCmdLabel)
 		{
 			UASSERT(cmdEvent->value1().isStr());
 			UASSERT(cmdEvent->value2().isUndef() || cmdEvent->value2().isInt() || cmdEvent->value2().isUInt());
-			rtabmap_ros::SetLabel setLabelSrv;
-			setLabelSrv.request.node_label = cmdEvent->value1().toStr();
-			setLabelSrv.request.node_id = cmdEvent->value2().isUndef()?0:cmdEvent->value2().toInt();
-			if(!ros::service::call("set_label", setLabelSrv))
+
+			auto client = this->create_client<rtabmap_ros::srv::SetLabel>("set_label");
+			if(client->wait_for_service(std::chrono::seconds(1)))
 			{
-				ROS_ERROR("Can't call \"set_label\" service");
+				auto request = std::make_shared<rtabmap_ros::srv::SetLabel::Request>();
+				request->node_id = cmdEvent->value2().isUndef()?0:cmdEvent->value2().toInt();
+				request->node_label = cmdEvent->value1().toStr();
+				auto result_future = client->async_send_request(request);
+				auto node = rclcpp::Node::make_shared("rtabmapviz");
+				if (rclcpp::spin_until_future_complete(node, result_future) !=
+						rclcpp::executor::FutureReturnCode::SUCCESS)
+				{
+					RCLCPP_ERROR(this->get_logger(), "Can't call \"set_label\" service.");
+				}
+			}
+			else
+			{
+				RCLCPP_WARN(this->get_logger(), "Service \"set_label\" not available.");
 			}
 		}
 		else
 		{
-			ROS_WARN("Not handled command (%d)...", cmd);
+			RCLCPP_WARN(this->get_logger(), "Not handled command (%d)...", cmd);
 		}
 	}
 	else if(anEvent->getClassName().compare("OdometryResetEvent") == 0)
 	{
-		std_srvs::Empty srv;
-		if(!ros::service::call("reset_odom", srv))
+		if(!callEmptyService("reset_odom"))
 		{
-			ROS_ERROR("Can't call \"reset_odom\" service, (will only work with rtabmap/visual_odometry node.)");
+			RCLCPP_ERROR(this->get_logger(), "Can't call \"reset_odom\" service, (will only work with rtabmap/visual_odometry node.)");
 		}
 	}
 	return false;
 }

 void GuiWrapper::commonDepthCallback(
-		const nav_msgs::OdometryConstPtr & odomMsg,
-		const rtabmap_ros::UserDataConstPtr & userDataMsg,
+		const nav_msgs::msg::Odometry::ConstSharedPtr & odomMsg,
+		const rtabmap_ros::msg::UserData::ConstSharedPtr &,
 		const std::vector<cv_bridge::CvImageConstPtr> & imageMsgs,
 		const std::vector<cv_bridge::CvImageConstPtr> & depthMsgs,
-		const std::vector<sensor_msgs::CameraInfo> & cameraInfoMsgs,
-		const sensor_msgs::LaserScanConstPtr& scan2dMsg,
-		const sensor_msgs::PointCloud2ConstPtr& scan3dMsg,
-		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
+		const std::vector<sensor_msgs::msg::CameraInfo> & cameraInfoMsgs,
+		const sensor_msgs::msg::LaserScan::ConstSharedPtr& scan2dMsg,
+		const sensor_msgs::msg::PointCloud2::ConstSharedPtr& scan3dMsg,
+		const rtabmap_ros::msg::OdomInfo::ConstSharedPtr& odomInfoMsg)
 {
 	UASSERT(imageMsgs.size() == 0 || (imageMsgs.size() == cameraInfoMsgs.size()));

-	std_msgs::Header odomHeader;
+	std_msgs::msg::Header odomHeader;
 	if(odomMsg.get())
 	{
 		odomHeader = odomMsg->header;
@@ -461,7 +520,7 @@ void GuiWrapper::commonDepthCallback(
 		odomHeader.frame_id = odomFrameId_;
 	}

-	Transform odomT = rtabmap_ros::getTransform(odomHeader.frame_id, frameId_, odomHeader.stamp, tfListener_, waitForTransform_?waitForTransformDuration_:0);
+	Transform odomT = rtabmap_ros::getTransform(odomHeader.frame_id, frameId_, odomHeader.stamp, *tfBuffer_, waitForTransform_);
 	cv::Mat covariance = cv::Mat::eye(6,6,CV_64FC1);
 	if(odomMsg.get())
 	{
@@ -478,7 +537,7 @@ void GuiWrapper::commonDepthCallback(
 	}
 	if(odomHeader.frame_id.empty())
 	{
-		ROS_ERROR("Odometry frame not set!?");
+		RCLCPP_ERROR(this->get_logger(), "Odometry frame not set!?");
 		return;
 	}

@@ -509,10 +568,10 @@ void GuiWrapper::commonDepthCallback(
 					rgb,
 					depth,
 					cameraModels,
-					tfListener_,
-					waitForTransform_?waitForTransformDuration_:0.0))
+					*tfBuffer_,
+					waitForTransform_))
 			{
-				ROS_ERROR("Could not convert rgb/depth msgs! Aborting rtabmapviz update...");
+				RCLCPP_ERROR(this->get_logger(), "Could not convert rgb/depth msgs! Aborting rtabmapviz update...");
 				return;
 			}
 		}
@@ -520,30 +579,30 @@ void GuiWrapper::commonDepthCallback(
 		if(scan2dMsg.get() != 0)
 		{
 			if(!rtabmap_ros::convertScanMsg(
-					scan2dMsg,
+					*scan2dMsg,
 					frameId_,
 					odomSensorSync_?odomHeader.frame_id:"",
 					odomHeader.stamp,
 					scan,
-					tfListener_,
-					waitForTransform_?waitForTransformDuration_:0))
+					*tfBuffer_,
+					waitForTransform_))
 			{
-				ROS_ERROR("Could not convert laser scan msg! Aborting rtabmapviz update...");
+				RCLCPP_ERROR(this->get_logger(), "Could not convert laser scan msg! Aborting rtabmapviz update...");
 				return;
 			}
 		}
 		else if(scan3dMsg.get() != 0)
 		{
 			if(!rtabmap_ros::convertScan3dMsg(
-					scan3dMsg,
+					*scan3dMsg,
 					frameId_,
 					odomSensorSync_?odomHeader.frame_id:"",
 					odomHeader.stamp,
 					scan,
-					tfListener_,
-					waitForTransform_?waitForTransformDuration_:0))
+					*tfBuffer_,
+					waitForTransform_))
 			{
-				ROS_ERROR("Could not convert 3d laser scan msg! Aborting rtabmapviz update...");
+				RCLCPP_ERROR(this->get_logger(), "Could not convert 3d laser scan msg! Aborting rtabmapviz update...");
 				return;
 			}
 		}
@@ -572,7 +631,7 @@ void GuiWrapper::commonDepthCallback(
 				rgb,
 				depth,
 				cameraModels,
-				odomHeader.seq,
+				0,
 				rtabmap_ros::timestampFromROS(odomHeader.stamp)),
 		odomMsg.get()?rtabmap_ros::transformFromPoseMsg(odomMsg->pose.pose):odomT,
 		info);
@@ -581,17 +640,17 @@ void GuiWrapper::commonDepthCallback(
 }

 void GuiWrapper::commonStereoCallback(
-		const nav_msgs::OdometryConstPtr & odomMsg,
-		const rtabmap_ros::UserDataConstPtr & userDataMsg,
+		const nav_msgs::msg::Odometry::ConstSharedPtr & odomMsg,
+		const rtabmap_ros::msg::UserData::ConstSharedPtr &,
 		const cv_bridge::CvImageConstPtr& leftImageMsg,
 		const cv_bridge::CvImageConstPtr& rightImageMsg,
-		const sensor_msgs::CameraInfo& leftCamInfoMsg,
-		const sensor_msgs::CameraInfo& rightCamInfoMsg,
-		const sensor_msgs::LaserScanConstPtr& scan2dMsg,
-		const sensor_msgs::PointCloud2ConstPtr& scan3dMsg,
-		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
+		const sensor_msgs::msg::CameraInfo& leftCamInfoMsg,
+		const sensor_msgs::msg::CameraInfo& rightCamInfoMsg,
+		const sensor_msgs::msg::LaserScan::ConstSharedPtr& scan2dMsg,
+		const sensor_msgs::msg::PointCloud2::ConstSharedPtr& scan3dMsg,
+		const rtabmap_ros::msg::OdomInfo::ConstSharedPtr& odomInfoMsg)
 {
-	std_msgs::Header odomHeader;
+	std_msgs::msg::Header odomHeader;
 	if(odomMsg.get())
 	{
 		odomHeader = odomMsg->header;
@@ -613,7 +672,7 @@ void GuiWrapper::commonStereoCallback(
 		odomHeader.frame_id = odomFrameId_;
 	}

-	Transform odomT = rtabmap_ros::getTransform(odomHeader.frame_id, frameId_, odomHeader.stamp, tfListener_, waitForTransform_?waitForTransformDuration_:0);
+	Transform odomT = rtabmap_ros::getTransform(odomHeader.frame_id, frameId_, odomHeader.stamp, *tfBuffer_, waitForTransform_);
 	cv::Mat covariance = cv::Mat::eye(6,6,CV_64FC1);
 	if(odomMsg.get())
 	{
@@ -630,7 +689,7 @@ void GuiWrapper::commonStereoCallback(
 	}
 	if(odomHeader.frame_id.empty())
 	{
-		ROS_ERROR("Odometry frame not set!?");
+		RCLCPP_ERROR(this->get_logger(), "Odometry frame not set!?");
 		return;
 	}

@@ -660,40 +719,40 @@ void GuiWrapper::commonStereoCallback(
 				left,
 				right,
 				stereoModel,
-				tfListener_,
-				waitForTransform_?waitForTransformDuration_:0.0))
+				*tfBuffer_,
+				waitForTransform_))
 		{
-			ROS_ERROR("Could not convert stereo msgs! Aborting rtabmapviz update...");
+			RCLCPP_ERROR(this->get_logger(), "Could not convert stereo msgs! Aborting rtabmapviz update...");
 			return;
 		}

 		if(scan2dMsg.get() != 0)
 		{
 			if(!rtabmap_ros::convertScanMsg(
-					scan2dMsg,
+					*scan2dMsg,
 					frameId_,
 					odomSensorSync_?odomHeader.frame_id:"",
 					odomHeader.stamp,
 					scan,
-					tfListener_,
-					waitForTransform_?waitForTransformDuration_:0))
+					*tfBuffer_,
+					waitForTransform_))
 			{
-				ROS_ERROR("Could not convert laser scan msg! Aborting rtabmapviz update...");
+				RCLCPP_ERROR(this->get_logger(), "Could not convert laser scan msg! Aborting rtabmapviz update...");
 				return;
 			}
 		}
 		else if(scan3dMsg.get() != 0)
 		{
 			if(!rtabmap_ros::convertScan3dMsg(
-					scan3dMsg,
+					*scan3dMsg,
 					frameId_,
 					odomSensorSync_?odomHeader.frame_id:"",
 					odomHeader.stamp,
 					scan,
-					tfListener_,
-					waitForTransform_?waitForTransformDuration_:0))
+					*tfBuffer_,
+					waitForTransform_))
 			{
-				ROS_ERROR("Could not convert 3d laser scan msg! Aborting rtabmapviz update...");
+				RCLCPP_ERROR(this->get_logger(), "Could not convert 3d laser scan msg! Aborting rtabmapviz update...");
 				return;
 			}
 		}
@@ -722,7 +781,7 @@ void GuiWrapper::commonStereoCallback(
 				left,
 				right,
 				stereoModel,
-				odomHeader.seq,
+				0,
 				rtabmap_ros::timestampFromROS(odomHeader.stamp)),
 		odomMsg.get()?rtabmap_ros::transformFromPoseMsg(odomMsg->pose.pose):odomT,
 		info);
@@ -731,15 +790,15 @@ void GuiWrapper::commonStereoCallback(
 }

 void GuiWrapper::commonLaserScanCallback(
-		const nav_msgs::OdometryConstPtr & odomMsg,
-		const rtabmap_ros::UserDataConstPtr & userDataMsg,
-		const sensor_msgs::LaserScanConstPtr& scan2dMsg,
-		const sensor_msgs::PointCloud2ConstPtr& scan3dMsg,
-		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
+		const nav_msgs::msg::Odometry::ConstSharedPtr & odomMsg,
+		const rtabmap_ros::msg::UserData::ConstSharedPtr &,
+		const sensor_msgs::msg::LaserScan::ConstSharedPtr& scan2dMsg,
+		const sensor_msgs::msg::PointCloud2::ConstSharedPtr& scan3dMsg,
+		const rtabmap_ros::msg::OdomInfo::ConstSharedPtr& odomInfoMsg)
 {
 	UASSERT(scan2dMsg.get() || scan3dMsg.get());

-	std_msgs::Header odomHeader;
+	std_msgs::msg::Header odomHeader;
 	if(odomMsg.get())
 	{
 		odomHeader = odomMsg->header;
@@ -761,7 +820,7 @@ void GuiWrapper::commonLaserScanCallback(
 		odomHeader.frame_id = odomFrameId_;
 	}

-	Transform odomT = rtabmap_ros::getTransform(odomHeader.frame_id, frameId_, odomHeader.stamp, tfListener_, waitForTransform_?waitForTransformDuration_:0);
+	Transform odomT = rtabmap_ros::getTransform(odomHeader.frame_id, frameId_, odomHeader.stamp, *tfBuffer_, waitForTransform_);
 	cv::Mat covariance = cv::Mat::eye(6,6,CV_64FC1);
 	if(odomMsg.get())
 	{
@@ -778,7 +837,7 @@ void GuiWrapper::commonLaserScanCallback(
 	}
 	if(odomHeader.frame_id.empty())
 	{
-		ROS_ERROR("Odometry frame not set!?");
+		RCLCPP_ERROR(this->get_logger(), "Odometry frame not set!?");
 		return;
 	}

@@ -798,30 +857,30 @@ void GuiWrapper::commonLaserScanCallback(
 		if(scan2dMsg.get() != 0)
 		{
 			if(!rtabmap_ros::convertScanMsg(
-					scan2dMsg,
+					*scan2dMsg,
 					frameId_,
 					odomSensorSync_?odomHeader.frame_id:"",
 					odomHeader.stamp,
 					scan,
-					tfListener_,
-					waitForTransform_?waitForTransformDuration_:0))
+					*tfBuffer_,
+					waitForTransform_))
 			{
-				ROS_ERROR("Could not convert laser scan msg! Aborting rtabmapviz update...");
+				RCLCPP_ERROR(this->get_logger(), "Could not convert laser scan msg! Aborting rtabmapviz update...");
 				return;
 			}
 		}
 		else if(scan3dMsg.get() != 0)
 		{
 			if(!rtabmap_ros::convertScan3dMsg(
-					scan3dMsg,
+					*scan3dMsg,
 					frameId_,
 					odomSensorSync_?odomHeader.frame_id:"",
 					odomHeader.stamp,
 					scan,
-					tfListener_,
-					waitForTransform_?waitForTransformDuration_:0))
+					*tfBuffer_,
+					waitForTransform_))
 			{
-				ROS_ERROR("Could not convert 3d laser scan msg! Aborting rtabmapviz update...");
+				RCLCPP_ERROR(this->get_logger(), "Could not convert 3d laser scan msg! Aborting rtabmapviz update...");
 				return;
 			}
 		}
@@ -837,11 +896,11 @@ void GuiWrapper::commonLaserScanCallback(
 		//just get scan local transform to adjust camera frame
 		if(scan2dMsg.get() != 0)
 		{
-			fakeCameraLocalTransform = getTransform(frameId_, scan2dMsg->header.frame_id, scan2dMsg->header.stamp, tfListener_, waitForTransform_?waitForTransformDuration_:0);
+			fakeCameraLocalTransform = getTransform(frameId_, scan2dMsg->header.frame_id, scan2dMsg->header.stamp, *tfBuffer_, waitForTransform_);
 		}
 		else if(scan3dMsg.get() != 0)
 		{
-			fakeCameraLocalTransform = getTransform(frameId_, scan3dMsg->header.frame_id, scan3dMsg->header.stamp, tfListener_, waitForTransform_?waitForTransformDuration_:0);
+			fakeCameraLocalTransform = getTransform(frameId_, scan3dMsg->header.frame_id, scan3dMsg->header.stamp, *tfBuffer_, waitForTransform_);
 		}

 		info = rtabmap_ros::odomInfoFromROS(*odomInfoMsg).copyWithoutData();
@@ -871,7 +930,7 @@ void GuiWrapper::commonLaserScanCallback(
 				rgb,
 				depth,
 				model,
-				odomHeader.seq,
+				0,
 				rtabmap_ros::timestampFromROS(odomHeader.stamp)),
 		odomMsg.get()?rtabmap_ros::transformFromPoseMsg(odomMsg->pose.pose):odomT,
 		info);
@@ -880,15 +939,15 @@ void GuiWrapper::commonLaserScanCallback(
 }

 void GuiWrapper::commonOdomCallback(
-		const nav_msgs::OdometryConstPtr & odomMsg,
-		const rtabmap_ros::UserDataConstPtr & userDataMsg,
-		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
+		const nav_msgs::msg::Odometry::ConstSharedPtr & odomMsg,
+		const rtabmap_ros::msg::UserData::ConstSharedPtr &,
+		const rtabmap_ros::msg::OdomInfo::ConstSharedPtr& odomInfoMsg)
 {
 	UASSERT(odomMsg.get());

-	std_msgs::Header odomHeader = odomMsg->header;
+	std_msgs::msg::Header odomHeader = odomMsg->header;

-	Transform odomT = rtabmap_ros::getTransform(odomHeader.frame_id, frameId_, odomHeader.stamp, tfListener_, waitForTransform_?waitForTransformDuration_:0);
+	Transform odomT = rtabmap_ros::getTransform(odomHeader.frame_id, frameId_, odomHeader.stamp, *tfBuffer_, waitForTransform_);
 	cv::Mat covariance = cv::Mat::eye(6,6,CV_64FC1);
 	if(odomMsg.get())
 	{
@@ -905,7 +964,7 @@ void GuiWrapper::commonOdomCallback(
 	}
 	if(odomHeader.frame_id.empty())
 	{
-		ROS_ERROR("Odometry frame not set!?");
+		RCLCPP_ERROR(this->get_logger(), "Odometry frame not set!?");
 		return;
 	}

@@ -954,7 +1013,7 @@ void GuiWrapper::commonOdomCallback(
 				rgb,
 				depth,
 				model,
-				odomHeader.seq,
+				0,
 				rtabmap_ros::timestampFromROS(odomHeader.stamp)),
 		odomMsg.get()?rtabmap_ros::transformFromPoseMsg(odomMsg->pose.pose):odomT,
 		info);