Merge branch 'ros2' of github.com:introlab/rtabmap_ros into jazzy-devel

rtabmap_util/CMakeLists.txt

@@ -73,6 +73,7 @@ SET(rtabmap_util_plugins_lib_src
    src/nodelets/lidar_deskewing.cpp
    src/nodelets/rgbd_relay.cpp
    src/nodelets/rgbd_split.cpp
+   src/nodelets/map_assembler.cpp
 )
 
 
@@ -134,6 +135,7 @@ rclcpp_components_register_nodes(rtabmap_util_plugins "rtabmap_util::PointCloudT
 rclcpp_components_register_nodes(rtabmap_util_plugins "rtabmap_util::ObstaclesDetection")
 rclcpp_components_register_nodes(rtabmap_util_plugins "rtabmap_util::PointCloudAggregator")
 rclcpp_components_register_nodes(rtabmap_util_plugins "rtabmap_util::PointCloudAssembler")
+rclcpp_components_register_nodes(rtabmap_util_plugins "rtabmap_util::MapAssembler")
 
 add_executable(rtabmap_rgbd_relay src/RGBDRelayNode.cpp)
 ament_target_dependencies(rtabmap_rgbd_relay ${Libraries})
@@ -150,10 +152,10 @@ set_target_properties(rtabmap_rgbd_split PROPERTIES OUTPUT_NAME "rgbd_split")
 #target_link_libraries(rtabmap_map_optimizer rtabmap_util_plugins)
 #set_target_properties(rtabmap_map_optimizer PROPERTIES OUTPUT_NAME "map_optimizer")
 
-#add_executable(rtabmap_map_assembler src/MapAssemblerNode.cpp)
-#ament_target_dependencies(rtabmap_map_assembler ${Libraries})
-#target_link_libraries(rtabmap_map_assembler rtabmap_util_plugins rtabmap_util_plugins)
-#set_target_properties(rtabmap_map_assembler PROPERTIES OUTPUT_NAME "map_assembler")
+add_executable(rtabmap_map_assembler src/MapAssemblerNode.cpp)
+ament_target_dependencies(rtabmap_map_assembler ${Libraries})
+target_link_libraries(rtabmap_map_assembler rtabmap_util_plugins)
+set_target_properties(rtabmap_map_assembler PROPERTIES OUTPUT_NAME "map_assembler")
 
 add_executable(rtabmap_imu_to_tf src/ImuToTFNode.cpp)
 ament_target_dependencies(rtabmap_imu_to_tf ${Libraries})
@@ -241,7 +243,6 @@ install(TARGETS
    RUNTIME DESTINATION bin
 )
 install(TARGETS
-#   rtabmap_map_assembler
 #   rtabmap_map_optimizer
 #   rtabmap_data_player
 #   rtabmap_odom_msg_to_tf
@@ -256,6 +257,7 @@ install(TARGETS
    rtabmap_obstacles_detection
    rtabmap_point_cloud_aggregator
    rtabmap_point_cloud_assembler
+   rtabmap_map_assembler
    DESTINATION lib/${PROJECT_NAME}
 )
 

rtabmap_util/include/rtabmap_util/map_assembler.hpp

@@ -0,0 +1,104 @@
+/*
+Copyright (c) 2010-2024, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include <rtabmap_util/visibility.h>
+#include "rclcpp/rclcpp.hpp"
+
+#include <rtabmap_msgs/srv/get_map.hpp>
+#include "rtabmap_msgs/msg/map_data.hpp"
+#include "rtabmap_util/MapsManager.h"
+
+#include <std_srvs/srv/empty.hpp>
+
+#ifdef WITH_OCTOMAP_MSGS
+#ifdef RTABMAP_OCTOMAP
+#include <octomap_msgs/srv/get_octomap.hpp>
+#endif
+#endif
+
+namespace rtabmap_util
+{
+
+class MapAssembler: public rclcpp::Node
+{
+
+public:
+	RTABMAP_UTIL_PUBLIC
+	explicit MapAssembler(const rclcpp::NodeOptions & options);
+    virtual ~MapAssembler();
+
+private:
+	void mapDataReceivedCallback(const rtabmap_msgs::msg::MapData::ConstSharedPtr msg);
+
+	void processMapData(const rtabmap_msgs::msg::MapData & msg);
+
+	void reset(const std::shared_ptr<rmw_request_id_t>,
+		const std::shared_ptr<std_srvs::srv::Empty::Request>,
+		std::shared_ptr<std_srvs::srv::Empty::Response>);
+
+	void timerCallback();
+
+#ifdef WITH_OCTOMAP_MSGS
+#ifdef RTABMAP_OCTOMAP
+	void octomapBinaryCallback(
+		const std::shared_ptr<rmw_request_id_t>,
+		const std::shared_ptr<octomap_msgs::srv::GetOctomap::Request>,
+		std::shared_ptr<octomap_msgs::srv::GetOctomap::Response> res);
+
+    void octomapFullCallback(
+		const std::shared_ptr<rmw_request_id_t>,
+		const std::shared_ptr<octomap_msgs::srv::GetOctomap::Request>,
+		std::shared_ptr<octomap_msgs::srv::GetOctomap::Response> res);
+#endif
+#endif
+
+private:
+	MapsManager mapsManager_;
+	std::map<int, rtabmap::Signature> nodes_;
+	std::map<int, rtabmap::Transform> optimizedPoses_;
+	std::string mapFrameId_;
+	std::string rtabmapNodeName_;
+
+    rclcpp::Subscription<rtabmap_msgs::msg::MapData>::SharedPtr mapDataSub_;
+
+    rclcpp::Service<std_srvs::srv::Empty>::SharedPtr resetService_;
+
+	rclcpp::CallbackGroup::SharedPtr serviceCbGroup_;
+    rclcpp::CallbackGroup::SharedPtr timerCbGroup_;
+    rclcpp::TimerBase::SharedPtr timer_;
+	rclcpp::Client<rtabmap_msgs::srv::GetMap>::SharedPtr client_;
+
+#ifdef WITH_OCTOMAP_MSGS
+#ifdef RTABMAP_OCTOMAP
+    rclcpp::Service<octomap_msgs::srv::GetOctomap>::SharedPtr octomapBinarySrv_;
+    rclcpp::Service<octomap_msgs::srv::GetOctomap>::SharedPtr octomapFullSrv_;
+#endif
+#endif
+	bool localGridsRegenerated_;
+};
+
+}

rtabmap_util/include/rtabmap_util/obstacles_detection.hpp

@@ -56,6 +56,8 @@ private:
 	rtabmap::LocalGridMaker localMapMaker_;
 	bool mapFrameProjection_;
 	bool warned_;
+	float rangeMin_;
+	float rangeMax_;
 
 	std::shared_ptr<tf2_ros::Buffer> tfBuffer_;
 	std::shared_ptr<tf2_ros::TransformListener> tfListener_;

rtabmap_util/include/rtabmap_util/pointcloud_to_depthimage.hpp

@@ -57,8 +57,8 @@ private:
 			const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg);
 
 private:
-	image_transport::CameraPublisher depthImage16Pub_;
-	image_transport::CameraPublisher depthImage32Pub_;
+	image_transport::Publisher depthImage16Pub_;
+	image_transport::Publisher depthImage32Pub_;
 	rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr cameraInfo16Pub_;
 	rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr cameraInfo32Pub_;
 	rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pointCloudTransformedPub_;

rtabmap_util/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>rtabmap_util</name>
-  <version>0.21.5</version>
+  <version>0.21.9</version>
   <description>RTAB-Map's various useful nodes and nodelets.</description>
   <maintainer email="[email protected]">Mathieu Labbe</maintainer>
   <author>Mathieu Labbe</author>

rtabmap_util/src/MapAssemblerNode.cpp

@@ -25,349 +25,17 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 
-#include <ros/ros.h>
-#include "rtabmap_msgs/MapData.h"
-#include "rtabmap_conversions/MsgConversion.h"
-#include "rtabmap_util/MapsManager.h"
-#include "rtabmap_msgs/GetMap.h"
-#include <rtabmap/core/util3d_transforms.h>
-#include <rtabmap/core/util3d.h>
-#include <rtabmap/core/util3d_filtering.h>
-#include <rtabmap/core/util3d_mapping.h>
-#include <rtabmap/core/Compression.h>
-#include <rtabmap/core/Graph.h>
-#include <rtabmap/utilite/ULogger.h>
+#include "rtabmap_util/map_assembler.hpp"
+
 #include <rtabmap/utilite/UStl.h>
-#include <rtabmap/utilite/UTimer.h>
-#include <rtabmap/utilite/UFile.h>
-#include <pcl_ros/transforms.h>
-#include <pcl_conversions/pcl_conversions.h>
-#include <nav_msgs/OccupancyGrid.h>
-#include <std_srvs/Empty.h>
 
-#ifdef WITH_OCTOMAP_MSGS
-#ifdef RTABMAP_OCTOMAP
-#include <octomap_msgs/GetOctomap.h>
-#include <octomap_msgs/conversions.h>
-#include <rtabmap/core/OctoMap.h>
-#endif
-#endif
-
-using namespace rtabmap;
-
-class MapAssembler
+int main(int argc, char **argv)
 {
-
-public:
-	MapAssembler(int & argc, char** argv) :
-		localGridsRegenerated_(false)
-	{
-		ros::NodeHandle pnh("~");
-		ros::NodeHandle nh;
-
-		std::string configPath;
-		pnh.param("config_path", configPath, configPath);
-		pnh.param("regenerate_local_grids", localGridsRegenerated_, localGridsRegenerated_);
-
-		//parameters
-		rtabmap::ParametersMap parameters;
-		uInsert(parameters, rtabmap::Parameters::getDefaultParameters("Grid"));
-		uInsert(parameters, rtabmap::Parameters::getDefaultParameters("GridGlobal"));
-		uInsert(parameters, rtabmap::Parameters::getDefaultParameters("StereoBM"));
-		uInsert(parameters, rtabmap::Parameters::getDefaultParameters("StereoSGBM"));
-		uInsert(parameters, rtabmap::ParametersPair(rtabmap::Parameters::kIcpPointToPlaneGroundNormalsUp(), uNumber2Str(rtabmap::Parameters::defaultIcpPointToPlaneGroundNormalsUp())));
-		if(!configPath.empty())
-		{
-			if(UFile::exists(configPath.c_str()))
-			{
-				ROS_INFO( "%s: Loading parameters from %s", ros::this_node::getName().c_str(), configPath.c_str());
-				rtabmap::ParametersMap allParameters;
-				Parameters::readINI(configPath.c_str(), allParameters);
-				// only update odometry parameters
-				for(ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
-				{
-					ParametersMap::iterator jter = allParameters.find(iter->first);
-					if(jter!=allParameters.end())
-					{
-						iter->second = jter->second;
-					}
-				}
-			}
-			else
-			{
-				ROS_ERROR( "Config file \"%s\" not found!", configPath.c_str());
-			}
-		}
-		for(rtabmap::ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
-		{
-			std::string vStr;
-			bool vBool;
-			int vInt;
-			double vDouble;
-			if(pnh.getParam(iter->first, vStr))
-			{
-				ROS_INFO( "Setting %s parameter \"%s\"=\"%s\"", ros::this_node::getName().c_str(), iter->first.c_str(), vStr.c_str());
-				iter->second = vStr;
-			}
-			else if(pnh.getParam(iter->first, vBool))
-			{
-				ROS_INFO( "Setting %s parameter \"%s\"=\"%s\"", ros::this_node::getName().c_str(), iter->first.c_str(), uBool2Str(vBool).c_str());
-				iter->second = uBool2Str(vBool);
-			}
-			else if(pnh.getParam(iter->first, vDouble))
-			{
-				ROS_INFO( "Setting %s parameter \"%s\"=\"%s\"", ros::this_node::getName().c_str(), iter->first.c_str(), uNumber2Str(vDouble).c_str());
-				iter->second = uNumber2Str(vDouble);
-			}
-			else if(pnh.getParam(iter->first, vInt))
-			{
-				ROS_INFO( "Setting %s parameter \"%s\"=\"%s\"", ros::this_node::getName().c_str(), iter->first.c_str(), uNumber2Str(vInt).c_str());
-				iter->second = uNumber2Str(vInt);
-			}
-		}
-
-		rtabmap::ParametersMap argParameters = rtabmap::Parameters::parseArguments(argc, argv);
-		for(rtabmap::ParametersMap::iterator iter=argParameters.begin(); iter!=argParameters.end(); ++iter)
-		{
-			rtabmap::ParametersMap::iterator jter = parameters.find(iter->first);
-			if(jter!=parameters.end())
-			{
-				ROS_INFO( "Update %s parameter \"%s\"=\"%s\" from arguments", ros::this_node::getName().c_str(), iter->first.c_str(), iter->second.c_str());
-				jter->second = iter->second;
-			}
-		}
-
-		// Backward compatibility
-		for(std::map<std::string, std::pair<bool, std::string> >::const_iterator iter=Parameters::getRemovedParameters().begin();
-			iter!=Parameters::getRemovedParameters().end();
-			++iter)
-		{
-			std::string vStr;
-			if(pnh.getParam(iter->first, vStr))
-			{
-				if(iter->second.first && parameters.find(iter->second.second) != parameters.end())
-				{
-					// can be migrated
-					parameters.at(iter->second.second)= vStr;
-					ROS_WARN( "%s: Parameter name changed: \"%s\" -> \"%s\". Please update your launch file accordingly. Value \"%s\" is still set to the new parameter name.",
-							ros::this_node::getName().c_str(), iter->first.c_str(), iter->second.second.c_str(), vStr.c_str());
-				}
-				else
-				{
-					if(iter->second.second.empty())
-					{
-						ROS_ERROR( "%s: Parameter \"%s\" doesn't exist anymore!",
-								ros::this_node::getName().c_str(), iter->first.c_str());
-					}
-					else
-					{
-						ROS_ERROR( "%s: Parameter \"%s\" doesn't exist anymore! You may look at this similar parameter: \"%s\"",
-								ros::this_node::getName().c_str(), iter->first.c_str(), iter->second.second.c_str());
-					}
-				}
-			}
-		}
-
-		// set private parameters
-		for(ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
-		{
-			pnh.setParam(iter->first, iter->second);
-		}
-
-		ROS_INFO("%s: regenerate_local_grids          = %s", ros::this_node::getName().c_str(), localGridsRegenerated_?"true":"false");
-		mapsManager_.init(nh, pnh, ros::this_node::getName(), false);
-		mapsManager_.backwardCompatibilityParameters(pnh, parameters);
-		mapsManager_.setParameters(parameters);
-
-		std::list<std::string> splitName = uSplit(nh.resolveName("mapData"), '/');
-		std::string rtabmapNs;
-		for(std::list<std::string>::iterator iter=splitName.begin(); iter!=splitName.end() && iter!=--splitName.end(); ++iter)
-		{
-			if(!rtabmapNs.empty())
-			{
-				rtabmapNs += "/";
-			}
-			rtabmapNs += *iter;
-		}
-		ROS_INFO("Rtabmap namespace is \"%s\", deduced from topic \"%s\"", rtabmapNs.c_str(), nh.resolveName("mapData").c_str());
-		if(rtabmapNs.empty())
-		{
-			rtabmapNs = "get_map_data";
-		}
-		else
-		{
-			rtabmapNs += "/get_map_data";
-		}
-
-		rtabmap_msgs::GetMap getMapSrv;
-		getMapSrv.request.global = false;
-		getMapSrv.request.optimized = true;
-		getMapSrv.request.graphOnly = false;
-		if(ros::service::waitForService(rtabmapNs, 5000))
-		{
-			if(!ros::service::call(rtabmapNs, getMapSrv))
-			{
-				ROS_WARN("Cannot call \"%s\" service", rtabmapNs.c_str());
-			}
-			else
-			{
-				ROS_INFO("Called \"%s\" service, initializing cache...", rtabmapNs.c_str());
-				processMapData(getMapSrv.response.data);
-				ROS_INFO("Called \"%s\" service, initializing cache... done! The map"
-						" will be assembled on next subscriber connection.", rtabmapNs.c_str());
-			}
-		}
-		else
-		{
-			ROS_WARN("Service \"%s\" not available after waiting for 5 seconds, "
-					"may not be a problem if rtabmap is started afterwards. If rtabmap "
-					"is started after in localization mode, call /rtabmap/publish_maps "
-					"service with graph_only=false to make sure map_assembler has all the data.", rtabmapNs.c_str());
-		}
-
-
-		mapDataTopic_ = nh.subscribe("mapData", 1, &MapAssembler::mapDataReceivedCallback, this);
-
-		// private services
-		resetService_ = pnh.advertiseService("reset", &MapAssembler::reset, this);
-
-#ifdef WITH_OCTOMAP_MSGS
-#ifdef RTABMAP_OCTOMAP
-		octomapBinarySrv_ = pnh.advertiseService("octomap_binary", &MapAssembler::octomapBinaryCallback, this);
-		octomapFullSrv_ = pnh.advertiseService("octomap_full", &MapAssembler::octomapFullCallback, this);
-#endif
-#endif
-	}
-
-	~MapAssembler()
-	{
-	}
-
-	void mapDataReceivedCallback(const rtabmap_msgs::MapDataConstPtr & msg)
-	{
-		processMapData(*msg);
-	}
-	void processMapData(const rtabmap_msgs::MapData & msg)
-	{
-		UTimer timer;
-
-		std::map<int, Transform> poses;
-		std::multimap<int, Link> constraints;
-		Transform mapOdom;
-		rtabmap_conversions::mapGraphFromROS(msg.graph, poses, constraints, mapOdom);
-		for(unsigned int i=0; i<msg.nodes.size(); ++i)
-		{
-			if(msg.nodes[i].data.left_compressed.size() ||
-			   msg.nodes[i].data.right_compressed.size() ||
-			   msg.nodes[i].data.laser_scan_compressed.size())
-			{
-				Signature data = rtabmap_conversions::nodeFromROS(msg.nodes[i]);
-				if(localGridsRegenerated_)
-				{
-					data.sensorData().setOccupancyGrid(cv::Mat(), cv::Mat(), cv::Mat(), 0, cv::Point3f());
-				}
-				uInsert(nodes_, std::make_pair(msg.nodes[i].id, data));
-			}
-		}
-
-		// create a tmp signature with latest sensory data
-		if(poses.size() && nodes_.find(poses.rbegin()->first) != nodes_.end())
-		{
-			Signature tmpS = nodes_.at(poses.rbegin()->first);
-			SensorData tmpData = tmpS.sensorData();
-			tmpData.setId(0);
-			uInsert(nodes_, std::make_pair(0, Signature(0, -1, 0, tmpS.getStamp(), "", tmpS.getPose(), Transform(), tmpData)));
-			poses.insert(std::make_pair(0, poses.rbegin()->second));
-		}
-
-		// Update maps
-		if(!nodes_.empty())
-		{
-			poses = mapsManager_.updateMapCaches(
-					poses,
-					0,
-					false,
-					false,
-					nodes_);
-		}
-		double updateTime = timer.ticks();
-
-		mapFrameId_ = msg.header.frame_id;
-		optimizedPoses_ = poses;
-
-		mapsManager_.publishMaps(poses, msg.header.stamp, msg.header.frame_id);
-
-		ROS_INFO("map_assembler: Updating = %fs, Publishing data = %fs (subscribers=%s)", updateTime, timer.ticks(), mapsManager_.hasSubscribers()?"true":"false");
-	}
-
-	bool reset(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
-	{
-		ROS_INFO("map_assembler: reset!");
-		mapsManager_.clear();
-		return true;
-	}
-
-#ifdef WITH_OCTOMAP_MSGS
-#ifdef RTABMAP_OCTOMAP
-	bool octomapBinaryCallback(
-			octomap_msgs::GetOctomap::Request  &req,
-			octomap_msgs::GetOctomap::Response &res)
-	{
-		ROS_INFO("Sending binary map data on service request");
-		res.map.header.frame_id = mapFrameId_;
-		res.map.header.stamp = ros::Time::now();
-
-		mapsManager_.updateMapCaches(optimizedPoses_, 0, false, true, nodes_);
-
-		const rtabmap::OctoMap * octomap = mapsManager_.getOctomap();
-		bool success = octomap->octree()->size() && octomap_msgs::binaryMapToMsg(*octomap->octree(), res.map);
-		return success;
-	}
-
-	bool octomapFullCallback(
-			octomap_msgs::GetOctomap::Request  &req,
-			octomap_msgs::GetOctomap::Response &res)
-	{
-		ROS_INFO("Sending full map data on service request");
-		res.map.header.frame_id = mapFrameId_;
-		res.map.header.stamp = ros::Time::now();
-
-		mapsManager_.updateMapCaches(optimizedPoses_, 0, false, true, nodes_);
-
-		const rtabmap::OctoMap * octomap = mapsManager_.getOctomap();
-		bool success = octomap->octree()->size() && octomap_msgs::fullMapToMsg(*octomap->octree(), res.map);
-		return success;
-	}
-#endif
-#endif
-
-private:
-	rtabmap_util::MapsManager mapsManager_;
-	std::map<int, Signature> nodes_;
-	std::map<int, Transform> optimizedPoses_;
-	std::string mapFrameId_;
-
-	ros::Subscriber mapDataTopic_;
-
-	ros::ServiceServer resetService_;
-#ifdef WITH_OCTOMAP_MSGS
-#ifdef RTABMAP_OCTOMAP
-	ros::ServiceServer octomapBinarySrv_;
-	ros::ServiceServer octomapFullSrv_;
-#endif
-#endif
-	bool localGridsRegenerated_;
-};
-
-
-int main(int argc, char** argv)
-{
-	ULogger::setLevel(ULogger::kError);
 	ULogger::setType(ULogger::kTypeConsole);
-
-	ros::init(argc, argv, "map_assembler");
+	ULogger::setLevel(ULogger::kError);
 
 	// process "--params" argument
+	std::vector<std::string> arguments;
 	for(int i=1;i<argc;++i)
 	{
 		if(strcmp(argv[i], "--params") == 0)
@@ -389,7 +57,7 @@ int main(int argc, char** argv)
 						"]" <<
 						std::endl;
 			}
-			ROS_WARN("Node will now exit after showing default parameters because "
+			UWARN("Node will now exit after showing default parameters because "
 					 "argument \"--params\" is detected!");
 			exit(0);
 		}
@@ -401,9 +69,20 @@ int main(int argc, char** argv)
 		{
 			ULogger::setLevel(ULogger::kInfo);
 		}
+		else if(strcmp(argv[i], "--uwarn") == 0)
+		{
+			ULogger::setLevel(ULogger::kWarning);
+		}
+		arguments.push_back(argv[i]);
 	}
 
-	MapAssembler assembler(argc, argv);
-	ros::spin();
+	rclcpp::NodeOptions options;
+	options.arguments(arguments);
+	rclcpp::init(argc, argv);
+	auto node = std::make_shared<rtabmap_util::MapAssembler>(options);
+	rclcpp::executors::MultiThreadedExecutor executor;
+    executor.add_node(node);
+	executor.spin();
+	rclcpp::shutdown();
 	return 0;
-}
+}

rtabmap_util/src/MapsManager.cpp

@@ -1362,7 +1362,11 @@ void MapsManager::publishMaps(
 		(elevationMapPub_->get_subscription_count() && !latched_.at(&elevationMapPub_)))
 	{
 		grid_map_msgs::msg::GridMap::UniquePtr msg;
+#if RTABMAP_VERSION_MAJOR>0 || (RTABMAP_VERSION_MAJOR==0 && RTABMAP_VERSION_MINOR>21) || (RTABMAP_VERSION_MAJOR==0 && RTABMAP_VERSION_MINOR==21 && RTABMAP_VERSION_PATCH>=8)
+		msg = grid_map::GridMapRosConverter::toMessage(*elevationMap_->gridMap());
+#else
 		msg = grid_map::GridMapRosConverter::toMessage(elevationMap_->gridMap());
+#endif
 		msg->header.frame_id = mapFrameId;
 		msg->header.stamp = stamp;
 		elevationMapPub_->publish(std::move(msg));

rtabmap_util/src/PointCloudAssemblerNode.cpp

@@ -29,6 +29,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 int main(int argc, char **argv)
 {
+  ULogger::setType(ULogger::kTypeConsole);
+  ULogger::setLevel(ULogger::kWarning);
 	rclcpp::init(argc, argv);
 	rclcpp::spin(std::make_shared<rtabmap_util::PointCloudAssembler>(rclcpp::NodeOptions()));
 	rclcpp::shutdown();

rtabmap_util/src/PointCloudToDepthImageNode.cpp

@@ -25,10 +25,13 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 
+#include <rtabmap/utilite/ULogger.h>
 #include "rtabmap_util/pointcloud_to_depthimage.hpp"
 
 int main(int argc, char **argv)
 {
+  ULogger::setType(ULogger::kTypeConsole);
+  ULogger::setLevel(ULogger::kWarning);
 	rclcpp::init(argc, argv);
 	rclcpp::spin(std::make_shared<rtabmap_util::PointCloudToDepthImage>(rclcpp::NodeOptions()));
 	rclcpp::shutdown();

rtabmap_util/src/nodelets/disparity_to_depth.cpp

@@ -42,13 +42,11 @@ namespace rtabmap_util
 DisparityToDepth::DisparityToDepth(const rclcpp::NodeOptions & options) :
 	rclcpp::Node("disparity_to_depth", options)
 {
-	int qos = 0;
+	int qos = RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT;
 	qos = this->declare_parameter("qos", qos);
 
-	auto node = rclcpp::Node::make_shared(this->get_name());
-	image_transport::ImageTransport it(node);
-	pub32f_ = image_transport::create_publisher(node.get(), "depth", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
-	pub16u_ = image_transport::create_publisher(node.get(), "depth_raw", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
+	pub32f_ = image_transport::create_publisher(this, "depth", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
+	pub16u_ = image_transport::create_publisher(this, "depth_raw", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
 
 	sub_ = create_subscription<stereo_msgs::msg::DisparityImage>("disparity", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos), std::bind(&DisparityToDepth::callback, this, std::placeholders::_1));
 }

rtabmap_util/src/nodelets/imu_to_tf.cpp

@@ -42,7 +42,7 @@ ImuToTF::ImuToTF(const rclcpp::NodeOptions & options) :
 	tfListener_ = std::make_shared<tf2_ros::TransformListener>(*tfBuffer_);
 	tfBroadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(this);
 
-	int qos = 0;
+	int qos = RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT;
 	fixedFrameId_ = this->declare_parameter("fixed_frame_id", fixedFrameId_);
 	baseFrameId_ = this->declare_parameter("base_frame_id", baseFrameId_);
 	qos = this->declare_parameter("qos", qos);

rtabmap_util/src/nodelets/lidar_deskewing.cpp

@@ -14,14 +14,10 @@ LidarDeskewing::LidarDeskewing(const rclcpp::NodeOptions & options) :
 	slerp_(false)
 {
 	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_);
 
 	int queueSize = 5;
-	int qos = 0;
+	int qos = RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT;
 	queueSize = this->declare_parameter("queue_size", queueSize);
 	qos = this->declare_parameter("qos", qos);
 	fixedFrameId_ = this->declare_parameter("fixed_frame_id", fixedFrameId_);
@@ -77,6 +73,7 @@ void LidarDeskewing::callbackScan(const sensor_msgs::msg::LaserScan::ConstShared
 
 	sensor_msgs::msg::PointCloud2 scanOutDeskewed;
 	rtabmap_conversions::transformPointCloud(t.toEigen4f(), scanOut, scanOutDeskewed);
+	scanOutDeskewed.header.frame_id = msg->header.frame_id;
 	pubScan_->publish(scanOutDeskewed);
 }
 

rtabmap_util/src/nodelets/map_assembler.cpp

@@ -0,0 +1,355 @@
+/*
+Copyright (c) 2010-2024, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include <rtabmap_util/map_assembler.hpp>
+
+#include <rtabmap_conversions/MsgConversion.h>
+
+#include <rtabmap/utilite/UFile.h>
+#include <rtabmap/utilite/UStl.h>
+#include <rtabmap/utilite/UTimer.h>
+
+#ifdef WITH_OCTOMAP_MSGS
+#ifdef RTABMAP_OCTOMAP
+#include <octomap_msgs/conversions.h>
+#include <rtabmap/core/OctoMap.h>
+#endif
+#endif
+
+using namespace std::chrono_literals;
+
+namespace rtabmap_util
+{
+MapAssembler::MapAssembler(const rclcpp::NodeOptions & options) :
+		Node("map_assembler", options),
+		rtabmapNodeName_("rtabmap"),
+		localGridsRegenerated_(false)
+{
+	std::string configPath;
+	configPath = this->declare_parameter("config_path", configPath);
+	localGridsRegenerated_ = this->declare_parameter("regenerate_local_grids", localGridsRegenerated_);
+	rtabmapNodeName_ = this->declare_parameter("rtabmap", rtabmapNodeName_);
+
+	//parameters
+	rtabmap::ParametersMap parameters;
+	uInsert(parameters, rtabmap::Parameters::getDefaultParameters("Grid"));
+	uInsert(parameters, rtabmap::Parameters::getDefaultParameters("GridGlobal"));
+	uInsert(parameters, rtabmap::Parameters::getDefaultParameters("StereoBM"));
+	uInsert(parameters, rtabmap::Parameters::getDefaultParameters("StereoSGBM"));
+	uInsert(parameters, rtabmap::ParametersPair(rtabmap::Parameters::kIcpPointToPlaneGroundNormalsUp(), uNumber2Str(rtabmap::Parameters::defaultIcpPointToPlaneGroundNormalsUp())));
+	if(!configPath.empty())
+	{
+		if(UFile::exists(configPath.c_str()))
+		{
+			RCLCPP_INFO(this->get_logger(), "MapAssembler: Loading parameters from %s", configPath.c_str());
+			rtabmap::ParametersMap allParameters;
+			rtabmap::Parameters::readINI(configPath.c_str(), allParameters);
+			// only update odometry parameters
+			for(rtabmap::ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
+			{
+				rtabmap::ParametersMap::iterator jter = allParameters.find(iter->first);
+				if(jter!=allParameters.end())
+				{
+					iter->second = jter->second;
+				}
+			}
+		}
+		else
+		{
+			RCLCPP_ERROR(this->get_logger(), "Config file \"%s\" not found!", configPath.c_str());
+		}
+	}
+
+	for(rtabmap::ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
+	{
+		rclcpp::Parameter parameter;
+		std::string vStr = this->declare_parameter(iter->first, iter->second); 
+	 	if(vStr.compare(iter->second)!=0)
+		{
+			RCLCPP_INFO(this->get_logger(), "MapAssembler: Setting parameter \"%s\"=\"%s\"", iter->first.c_str(), vStr.c_str());
+			iter->second = vStr;
+		}
+	}
+
+	std::vector<std::string> tmpList = this->get_node_options().arguments();
+	std::vector<std::string> argList;
+	for(unsigned int i=0; i<tmpList.size(); ++i)
+	{
+	    // Issue with ros2 launch files in which we cannot pass a 
+	    // list of strings as argument (they will appear in same string)
+	    std::list<std::string> v = uSplit(tmpList[i]);
+	    for(std::list<std::string>::iterator iter=v.begin(); iter!=v.end(); ++iter)
+	    {
+	        argList.push_back(*iter);
+	    }
+	}
+	
+	char ** argv = new char*[argList.size()];
+	for(unsigned int i=0; i<argList.size(); ++i)
+	{
+		argv[i] = &argList[i].at(0);
+	}
+
+	rtabmap::ParametersMap argParameters = rtabmap::Parameters::parseArguments(argList.size(), argv);
+	delete [] argv;
+	for(rtabmap::ParametersMap::iterator iter=argParameters.begin(); iter!=argParameters.end(); ++iter)
+	{
+		rtabmap::ParametersMap::iterator jter = parameters.find(iter->first);
+		if(jter!=parameters.end())
+		{
+			RCLCPP_INFO(this->get_logger(), "MapAssembler: Update parameter \"%s\"=\"%s\" from arguments", iter->first.c_str(), iter->second.c_str());
+			jter->second = iter->second;
+		}
+		else
+		{
+			RCLCPP_INFO(this->get_logger(), "MapAssembler: Ignored parameter \"%s\"=\"%s\" from arguments", iter->first.c_str(), iter->second.c_str());
+		}
+	}
+
+	// Backward compatibility
+	for(std::map<std::string, std::pair<bool, std::string> >::const_iterator iter=rtabmap::Parameters::getRemovedParameters().begin();
+		iter!=rtabmap::Parameters::getRemovedParameters().end();
+		++iter)
+	{
+		rclcpp::Parameter parameter;
+		if(get_parameter(iter->first, parameter))
+		{
+			std::string vStr = parameter.as_string();
+			if(!iter->second.second.empty() && parameters.find(iter->second.second)!=parameters.end())
+			{
+				RCLCPP_WARN(this->get_logger(), "MapAssembler: Parameter name changed: \"%s\" -> \"%s\". The new parameter is already used with value \"%s\", ignoring the old one with value \"%s\".",
+						iter->first.c_str(), iter->second.second.c_str(), parameters.find(iter->second.second)->second.c_str(), vStr.c_str());
+			}
+			else if(iter->second.first && parameters.find(iter->second.second) != parameters.end())
+			{
+				// can be migrated
+				parameters.at(iter->second.second)= vStr;
+				RCLCPP_WARN(this->get_logger(), "MapAssembler: Parameter name changed: \"%s\" -> \"%s\". Please update your launch file accordingly. Value \"%s\" is still set to the new parameter name.",
+						iter->first.c_str(), iter->second.second.c_str(), vStr.c_str());
+			}
+			else
+			{
+				if(iter->second.second.empty())
+				{
+					RCLCPP_ERROR(this->get_logger(), "MapAssembler: Parameter \"%s\" doesn't exist anymore!",
+							iter->first.c_str());
+				}
+				else
+				{
+					RCLCPP_ERROR(this->get_logger(), "MapAssembler: Parameter \"%s\" doesn't exist anymore! You may look at this similar parameter: \"%s\"",
+							iter->first.c_str(), iter->second.second.c_str());
+				}
+			}
+		}
+	}
+
+	RCLCPP_INFO(this->get_logger(), "%s: regenerate_local_grids          = %s", this->get_name(), localGridsRegenerated_?"true":"false");
+	mapsManager_.init(*this, this->get_name(), true);
+	mapsManager_.backwardCompatibilityParameters(*this, parameters);
+	mapsManager_.setParameters(parameters);
+
+	const std::string servicePrefix = get_name() + std::string("/");
+	resetService_ = this->create_service<std_srvs::srv::Empty>(servicePrefix + "reset", std::bind(&MapAssembler::reset, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
+
+#ifdef WITH_OCTOMAP_MSGS
+#ifdef RTABMAP_OCTOMAP
+	octomapBinarySrv_ = this->create_service<octomap_msgs::srv::GetOctomap>(servicePrefix + "octomap_binary", std::bind(&MapAssembler::octomapBinaryCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
+	octomapFullSrv_ = this->create_service<octomap_msgs::srv::GetOctomap>(servicePrefix + "octomap_full", std::bind(&MapAssembler::octomapFullCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
+#endif
+#endif
+
+	std::string getMapSrv = rtabmapNodeName_+"/get_map_data";
+
+	// We cannot call the service and wait in the constructor, lets call it later and subscribe afterwards
+	serviceCbGroup_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
+	timerCbGroup_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
+	client_ = this->create_client<rtabmap_msgs::srv::GetMap>(getMapSrv, rmw_qos_profile_services_default, serviceCbGroup_); // Put it in a different group than the timer
+	timer_ = this->create_wall_timer(1s, std::bind(&MapAssembler::timerCallback, this), timerCbGroup_);
+}
+
+MapAssembler::~MapAssembler() {}
+
+void MapAssembler::timerCallback()
+{
+	// Just do this callback one time
+	timer_->cancel();
+	if(mapDataSub_.get())
+	{
+		// double call? ignore
+		return;
+	}
+
+	std::string getMapSrv = rtabmapNodeName_+"/get_map_data";
+	RCLCPP_INFO(this->get_logger(), "Calling service \"%s\"...", getMapSrv.c_str());
+	
+	if(client_->wait_for_service(5s))
+	{
+		auto request = std::make_shared<rtabmap_msgs::srv::GetMap::Request>();
+		request->global_map = false;
+		request->optimized = true;
+		request->graph_only = false;
+
+		auto future = client_->async_send_request(request);
+		std::future_status status = future.wait_for(10s);
+		
+		if (status == std::future_status::ready) {
+			RCLCPP_INFO(this->get_logger(), "Initializing cache...");
+			processMapData(future.get()->data);
+			RCLCPP_INFO(this->get_logger(), "Initializing cache... done! The map"
+					" will be assembled on next subscriber connection.");
+		}
+		else
+		{
+			RCLCPP_WARN(this->get_logger(), "Service \"%s\" not responding after waiting for 10 seconds.",
+				getMapSrv.c_str());
+		}
+	}
+	else
+	{
+		RCLCPP_WARN(this->get_logger(), "Service \"%s\" not available after waiting for 5 seconds, "
+				"may not be a problem if rtabmap is started afterwards. If rtabmap "
+				"is started after in localization mode, call %s/publish_maps "
+				"service with graph_only=false to make sure map_assembler has all the data.",
+					getMapSrv.c_str(),
+					rtabmapNodeName_.c_str());
+	}
+
+	rclcpp::SubscriptionOptions options;
+	options.callback_group =  timerCbGroup_;
+	mapDataSub_ = create_subscription<rtabmap_msgs::msg::MapData>("mapData", rclcpp::QoS(1), 
+		std::bind(&MapAssembler::mapDataReceivedCallback, this, std::placeholders::_1), options);
+}
+
+void MapAssembler::mapDataReceivedCallback(const rtabmap_msgs::msg::MapData::ConstSharedPtr msg)
+{
+	processMapData(*msg);
+}
+void MapAssembler::processMapData(const rtabmap_msgs::msg::MapData & msg)
+{
+	UTimer timer;
+
+	std::map<int, rtabmap::Transform> poses;
+	std::multimap<int, rtabmap::Link> constraints;
+	rtabmap::Transform mapOdom;
+	rtabmap_conversions::mapGraphFromROS(msg.graph, poses, constraints, mapOdom);
+	for(unsigned int i=0; i<msg.nodes.size(); ++i)
+	{
+		if(msg.nodes[i].data.left_compressed.size() ||
+			msg.nodes[i].data.right_compressed.size() ||
+			msg.nodes[i].data.laser_scan_compressed.size())
+		{
+			rtabmap::Signature data = rtabmap_conversions::nodeFromROS(msg.nodes[i]);
+			if(localGridsRegenerated_)
+			{
+				data.sensorData().setOccupancyGrid(cv::Mat(), cv::Mat(), cv::Mat(), 0, cv::Point3f());
+			}
+			uInsert(nodes_, std::make_pair(msg.nodes[i].id, data));
+		}
+	}
+
+	// create a tmp signature with latest sensory data
+	if(poses.size() && nodes_.find(poses.rbegin()->first) != nodes_.end())
+	{
+		rtabmap::Signature tmpS = nodes_.at(poses.rbegin()->first);
+		rtabmap::SensorData tmpData = tmpS.sensorData();
+		tmpData.setId(0);
+		uInsert(nodes_, std::make_pair(0, rtabmap::Signature(0, -1, 0, tmpS.getStamp(), "", tmpS.getPose(), rtabmap::Transform(), tmpData)));
+		poses.insert(std::make_pair(0, poses.rbegin()->second));
+	}
+
+	// Update maps
+	if(!nodes_.empty())
+	{
+		poses = mapsManager_.updateMapCaches(
+				poses,
+				0,
+				false,
+				false,
+				nodes_);
+	}
+	double updateTime = timer.ticks();
+
+	mapFrameId_ = msg.header.frame_id;
+	optimizedPoses_ = poses;
+
+	mapsManager_.publishMaps(poses, msg.header.stamp, msg.header.frame_id);
+
+	RCLCPP_INFO(this->get_logger(), "map_assembler: Updating = %fs, Publishing data = %fs (subscribers=%s)", updateTime, timer.ticks(), mapsManager_.hasSubscribers()?"true":"false");
+}
+
+void MapAssembler::reset(const std::shared_ptr<rmw_request_id_t>,
+		const std::shared_ptr<std_srvs::srv::Empty::Request>,
+		std::shared_ptr<std_srvs::srv::Empty::Response>)
+{
+	RCLCPP_INFO(this->get_logger(), "map_assembler: reset!");
+	mapsManager_.clear();
+}
+
+#ifdef WITH_OCTOMAP_MSGS
+#ifdef RTABMAP_OCTOMAP
+void MapAssembler::octomapBinaryCallback(
+		const std::shared_ptr<rmw_request_id_t>,
+		const std::shared_ptr<octomap_msgs::srv::GetOctomap::Request>,
+		std::shared_ptr<octomap_msgs::srv::GetOctomap::Response> res)
+{
+	RCLCPP_INFO(this->get_logger(), "Sending binary map data on service request");
+	res->map.header.frame_id = mapFrameId_;
+	res->map.header.stamp = now();
+
+	mapsManager_.updateMapCaches(optimizedPoses_, 0, false, true, nodes_);
+
+	const rtabmap::OctoMap * octomap = mapsManager_.getOctomap();
+	if(octomap->octree()->size())
+		octomap_msgs::binaryMapToMsg(*octomap->octree(), res->map);
+}
+
+void MapAssembler::octomapFullCallback(
+		const std::shared_ptr<rmw_request_id_t>,
+		const std::shared_ptr<octomap_msgs::srv::GetOctomap::Request>,
+		std::shared_ptr<octomap_msgs::srv::GetOctomap::Response> res)
+{
+	RCLCPP_INFO(this->get_logger(), "Sending full map data on service request");
+	res->map.header.frame_id = mapFrameId_;
+	res->map.header.stamp = now();
+
+	mapsManager_.updateMapCaches(optimizedPoses_, 0, false, true, nodes_);
+
+	const rtabmap::OctoMap * octomap = mapsManager_.getOctomap();
+	if(octomap->octree()->size())
+		octomap_msgs::fullMapToMsg(*octomap->octree(), res->map);
+}
+#endif
+#endif
+
+}
+
+#include "rclcpp_components/register_node_macro.hpp"
+
+// Register the component with class_loader.
+// This acts as a sort of entry point, allowing the component to be discoverable when its library
+// is being loaded into a running process.
+RCLCPP_COMPONENTS_REGISTER_NODE(rtabmap_util::MapAssembler)

rtabmap_util/src/nodelets/obstacles_detection.cpp

@@ -45,7 +45,9 @@ ObstaclesDetection::ObstaclesDetection(const rclcpp::NodeOptions & options) :
 	frameId_("base_link"),
 	waitForTransform_(0.2),
 	mapFrameProjection_(rtabmap::Parameters::defaultGridMapFrameProjection()),
-	warned_(false)
+	warned_(false),
+	rangeMin_(0),
+	rangeMax_(0)
 {
 	ULogger::setType(ULogger::kTypeConsole);
 	ULogger::setLevel(ULogger::kWarning);
@@ -53,7 +55,7 @@ ObstaclesDetection::ObstaclesDetection(const rclcpp::NodeOptions & options) :
 	frameId_ = this->declare_parameter("frame_id", frameId_);
 	mapFrameId_ = this->declare_parameter("map_frame_id", mapFrameId_);
 	waitForTransform_ = this->declare_parameter("wait_for_transform", waitForTransform_);
-	int qos = 0;
+	int qos = RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT;
 	qos = this->declare_parameter("qos", qos);
 
 	rtabmap::ParametersMap gridParameters = rtabmap::Parameters::getDefaultParameters("Grid");
@@ -75,6 +77,8 @@ ObstaclesDetection::ObstaclesDetection(const rclcpp::NodeOptions & options) :
 	}
 
 	localMapMaker_.parseParameters(gridParameters);
+	rtabmap::Parameters::parse(gridParameters, rtabmap::Parameters::kGridRangeMin(), rangeMin_);
+	rtabmap::Parameters::parse(gridParameters, rtabmap::Parameters::kGridRangeMax(), rangeMax_);
 
 	tfBuffer_ = std::make_shared< tf2_ros::Buffer >(this->get_clock());
 	tfListener_ = std::make_shared< tf2_ros::TransformListener >(*tfBuffer_);
@@ -86,6 +90,42 @@ ObstaclesDetection::ObstaclesDetection(const rclcpp::NodeOptions & options) :
 	cloudSub_ = create_subscription<sensor_msgs::msg::PointCloud2>("cloud", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos), std::bind(&ObstaclesDetection::callback, this, std::placeholders::_1));
 }
 
+pcl::PointCloud<pcl::PointXYZ> rangeFiltering(
+		const pcl::PointCloud<pcl::PointXYZ> & cloud,
+		float rangeMin,
+		float rangeMax)
+{
+	if(!cloud.empty() && (rangeMin > 0.0f || rangeMax > 0.0f))
+	{
+		pcl::PointCloud<pcl::PointXYZ> output;
+		output.reserve(cloud.size());
+		int oi = 0;
+		float rangeMinSqrd = rangeMin * rangeMin;
+		float rangeMaxSqrd = rangeMax * rangeMax;
+		for(size_t i=0; i<cloud.size(); ++i)
+		{
+			const pcl::PointXYZ & pt = cloud.at(i);
+			float r = pt.x*pt.x + pt.y*pt.y + pt.z*pt.z;
+
+			if(rangeMin > 0.0f && r < rangeMinSqrd)
+			{
+				continue;
+			}
+			if(rangeMax > 0.0f && r > rangeMaxSqrd)
+			{
+				continue;
+			}
+			
+			output.push_back(pt);
+			++oi;
+		}
+		output.resize(oi);
+		return output;
+	}
+
+	return cloud;
+}
+
 void ObstaclesDetection::callback(const sensor_msgs::msg::PointCloud2::ConstSharedPtr cloudMsg)
 {
 	rclcpp::Time time = now();
@@ -137,6 +177,11 @@ void ObstaclesDetection::callback(const sensor_msgs::msg::PointCloud2::ConstShar
 		inputCloud->is_dense = true;
 	}
 
+	if(rangeMin_ > 0.0f || rangeMax_ > 0.0f)
+	{
+		*inputCloud = rangeFiltering(*inputCloud, rangeMin_, rangeMax_);
+	}
+	
 	//Common variables for all strategies
 	pcl::IndicesPtr ground, obstacles;
 	pcl::PointCloud<pcl::PointXYZ>::Ptr obstaclesCloud(new pcl::PointCloud<pcl::PointXYZ>);

rtabmap_util/src/nodelets/point_cloud_aggregator.cpp

@@ -64,7 +64,7 @@ PointCloudAggregator::PointCloudAggregator(const rclcpp::NodeOptions & options)
 	int count = 2;
 	bool approx=true;
 	double approxSyncMaxInterval = 0.0;
-	int qos=0;
+	int qos=RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT;
 	topicQueueSize = this->declare_parameter("topic_queue_size", topicQueueSize);
 	int queueSize = this->declare_parameter("queue_size", -1);
 	if(queueSize != -1)

rtabmap_util/src/nodelets/point_cloud_assembler.cpp

@@ -73,9 +73,9 @@ PointCloudAssembler::PointCloudAssembler(const rclcpp::NodeOptions & options) :
 	//tfBuffer_->setCreateTimerInterface(timer_interface);
 	tfListener_ = std::make_shared<tf2_ros::TransformListener>(*tfBuffer_);
 
-	int topicQueueSize = 1;
-	int syncQueueSize = 5;
-	int qos = 0;
+	int topicQueueSize = 10;
+	int syncQueueSize = 10;
+	int qos = RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT;
 	bool subscribeOdomInfo = false;
 
 	topicQueueSize = this->declare_parameter("topic_queue_size", topicQueueSize);
@@ -286,13 +286,14 @@ void PointCloudAssembler::callbackCloudOdomInfo(
 	}
 	else
 	{
-		RCLCPP_WARN(this->get_logger(), "Reseting point cloud assembler as null odometry has been received.");
+		RCLCPP_WARN(this->get_logger(), "Resetting point cloud assembler as null odometry has been received.");
 		clouds_.clear();
 	}
 }
 
 void PointCloudAssembler::callbackCloud(const sensor_msgs::msg::PointCloud2::ConstSharedPtr cloudMsg)
 {
+	callbackCalled_ = true;
 	if(cloudPub_->get_subscription_count())
 	{
 		UASSERT_MSG(cloudMsg->data.size() == cloudMsg->row_step*cloudMsg->height,

rtabmap_util/src/nodelets/point_cloud_xyz.cpp

@@ -68,7 +68,7 @@ PointCloudXYZ::PointCloudXYZ(const rclcpp::NodeOptions & options) :
 {
 	int topicQueueSize = 1;
 	int syncQueueSize = 10;
-	int qos = 0;
+	int qos = RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT;
 	bool approxSync = true;
 	std::string roiStr;
 	double approxSyncMaxInterval = 0.0;

rtabmap_util/src/nodelets/point_cloud_xyzrgb.cpp

@@ -75,7 +75,7 @@ PointCloudXYZRGB::PointCloudXYZRGB(const rclcpp::NodeOptions & options) :
 	std::string roiStr;
 	int topicQueueSize = 1;
 	int syncQueueSize = 10;
-	int qos = 0;
+	int qos = RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT;
 	double approxSyncMaxInterval = 0.0;
 	approxSync = this->declare_parameter("approx_sync", approxSync);
 	approxSyncMaxInterval = this->declare_parameter("approx_sync_max_interval", approxSyncMaxInterval);

rtabmap_util/src/nodelets/pointcloud_to_depthimage.cpp

@@ -60,9 +60,9 @@ PointCloudToDepthImage::PointCloudToDepthImage(const rclcpp::NodeOptions & optio
 	//tfBuffer_->setCreateTimerInterface(timer_interface);
 	tfListener_ = std::make_shared<tf2_ros::TransformListener>(*tfBuffer_);
 
-	int topicQueueSize = 1;
+	int topicQueueSize = 10;
 	int syncQueueSize = 10;
-	int qos = 0;
+	int qos = RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT;
 	bool approx = true;
 	topicQueueSize = this->declare_parameter("topic_queue_size", topicQueueSize);
 	int queueSize = this->declare_parameter("queue_size", -1);
@@ -107,10 +107,8 @@ PointCloudToDepthImage::PointCloudToDepthImage(const rclcpp::NodeOptions & optio
 	RCLCPP_INFO(this->get_logger(), "  decimation=%d", decimation_);
 	RCLCPP_INFO(this->get_logger(), "  upscale=%s (upscale_depth_error_ratio=%f)", upscale_?"true":"false", upscaleDepthErrorRatio_);
 
-	auto node = rclcpp::Node::make_shared(this->get_name());
-	image_transport::ImageTransport it(node);
-	depthImage16Pub_ = image_transport::create_camera_publisher(node.get(), "image_raw", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile()); // 16 bits unsigned in mm
-	depthImage32Pub_ = image_transport::create_camera_publisher(node.get(), "image", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());// 32 bits float in meters
+	depthImage16Pub_ = image_transport::create_publisher(this, "image_raw", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile()); // 16 bits unsigned in mm
+	depthImage32Pub_ = image_transport::create_publisher(this, "image", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());// 32 bits float in meters
 	pointCloudTransformedPub_ = create_publisher<sensor_msgs::msg::PointCloud2>("cloud_transformed", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos));
 	cameraInfo16Pub_ = create_publisher<sensor_msgs::msg::CameraInfo>(depthImage16Pub_.getTopic()+"/camera_info", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosCamInfo));
 	cameraInfo32Pub_ = create_publisher<sensor_msgs::msg::CameraInfo>(depthImage32Pub_.getTopic()+"/camera_info", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosCamInfo));
@@ -161,6 +159,10 @@ void PointCloudToDepthImage::callback(
 
 		if(cloudDisplacement.isNull())
 		{
+			RCLCPP_ERROR(this->get_logger(), "Could not find transform between %s and %s, accordingly to %s, aborting!",
+				pointCloud2Msg->header.frame_id.c_str(), 
+				cameraInfoMsg->header.frame_id.c_str(),
+				fixedFrameId_.c_str());
 			return;
 		}
 
@@ -173,6 +175,9 @@ void PointCloudToDepthImage::callback(
 
 		if(cloudToCamera.isNull())
 		{
+			RCLCPP_ERROR(this->get_logger(), "Could not find transform between %s and %s, aborting!",
+				pointCloud2Msg->header.frame_id.c_str(), 
+				cameraInfoMsg->header.frame_id.c_str());
 			return;
 		}
 		rtabmap::Transform localTransform = cloudDisplacement*cloudToCamera;
@@ -241,7 +246,7 @@ void PointCloudToDepthImage::callback(
 		if(depthImage32Pub_.getNumSubscribers())
 		{
 			depthImage.encoding = sensor_msgs::image_encodings::TYPE_32FC1;
-			depthImage32Pub_.publish(depthImage.toImageMsg(), cameraInfoMsg);
+			depthImage32Pub_.publish(depthImage.toImageMsg());
 			if(cameraInfo32Pub_->get_subscription_count())
 			{
 				cameraInfo32Pub_->publish(cameraInfoMsgOut);
@@ -252,7 +257,7 @@ void PointCloudToDepthImage::callback(
 		{
 			depthImage.encoding = sensor_msgs::image_encodings::TYPE_16UC1;
 			depthImage.image = rtabmap::util2d::cvtDepthFromFloat(depthImage.image);
-			depthImage16Pub_.publish(depthImage.toImageMsg(), cameraInfoMsg);
+			depthImage16Pub_.publish(depthImage.toImageMsg());
 			if(cameraInfo16Pub_->get_subscription_count())
 			{
 				cameraInfo16Pub_->publish(cameraInfoMsgOut);

rtabmap_util/src/nodelets/rgbd_relay.cpp

@@ -52,7 +52,7 @@ RGBDRelay::RGBDRelay(const rclcpp::NodeOptions & options) :
 	compress_(false),
 	uncompress_(false)
 {
-	int qos = 0;
+	int qos = RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT;
 	qos = this->declare_parameter("qos", qos);
 	compress_ = this->declare_parameter("compress", compress_);
 	uncompress_ = this->declare_parameter("uncompress", uncompress_);

rtabmap_util/src/nodelets/rgbd_split.cpp

@@ -39,17 +39,15 @@ namespace rtabmap_util
 RGBDSplit::RGBDSplit(const rclcpp::NodeOptions & options) :
 	Node("rgbd_split", options)
 {
-	int qos = 0;
+	int qos = RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT;
 	qos = this->declare_parameter("qos", qos);
 
 	RCLCPP_INFO(this->get_logger(), "%s: qos         = %d", get_name(), qos);
 
 	rgbdImageSub_ = create_subscription<rtabmap_msgs::msg::RGBDImage>("rgbd_image", rclcpp::QoS(5).reliability((rmw_qos_reliability_policy_t)qos), std::bind(&RGBDSplit::callback, this, std::placeholders::_1));
 
-	auto node = rclcpp::Node::make_shared(this->get_name());
-	image_transport::ImageTransport it(node);
-	rgbPub_ = image_transport::create_camera_publisher(node.get(), std::string(rgbdImageSub_->get_topic_name()) + "/rgb", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
-	depthPub_ = image_transport::create_camera_publisher(node.get(), std::string(rgbdImageSub_->get_topic_name()) + "/depth", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
+	rgbPub_ = image_transport::create_camera_publisher(this, std::string(rgbdImageSub_->get_topic_name()) + "/rgb", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
+	depthPub_ = image_transport::create_camera_publisher(this, std::string(rgbdImageSub_->get_topic_name()) + "/depth", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
 }