feat(slam): add rtabmap_ros

rtabmap_ros/rtabmap_util/include/rtabmap_util/MapsManager.h

@@ -0,0 +1,170 @@
+/*
+Copyright (c) 2010-2016, 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.
+*/
+
+#ifndef MAPSMANAGER_H_
+#define MAPSMANAGER_H_
+
+#include <rtabmap/core/Signature.h>
+#include <rtabmap/core/Parameters.h>
+#include <rtabmap/core/FlannIndex.h>
+#include <rtabmap/core/LocalGrid.h>
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+#include <rclcpp/rclcpp.hpp>
+#include <sensor_msgs/msg/point_cloud2.hpp>
+#include <nav_msgs/msg/occupancy_grid.hpp>
+
+#if defined(WITH_OCTOMAP_MSGS) and defined(RTABMAP_OCTOMAP)
+#include <octomap_msgs/msg/octomap.hpp>
+#endif
+
+#if defined(WITH_GRID_MAP_ROS) and defined(RTABMAP_GRIDMAP)
+#include <grid_map_msgs/msg/grid_map.hpp>
+#endif
+
+namespace rtabmap {
+class OctoMap;
+class Memory;
+class OccupancyGrid;
+class LocalGridMaker;
+class GridMap;
+
+}  // namespace rtabmap
+
+namespace rtabmap_util {
+
+class MapsManager {
+public:
+	MapsManager();
+	virtual ~MapsManager();
+	void init(rclcpp::Node & node, const std::string & name, bool usePublicNamespace);
+	void clear();
+	bool hasSubscribers() const;
+	bool isLatching() const {return latching_;}
+	bool isMapUpdated() const;
+	void backwardCompatibilityParameters(rclcpp::Node & node, rtabmap::ParametersMap & parameters) const;
+	void setParameters(const rtabmap::ParametersMap & parameters);
+	void set2DMap(const cv::Mat & map, float xMin, float yMin, float cellSize, const std::map<int, rtabmap::Transform> & poses, const rtabmap::Memory * memory = 0);
+
+	std::map<int, rtabmap::Transform> getFilteredPoses(
+			const std::map<int, rtabmap::Transform> & poses);
+
+	std::map<int, rtabmap::Transform> updateMapCaches(
+			const std::map<int, rtabmap::Transform> & poses,
+			const rtabmap::Memory * memory,
+			bool updateGrid,
+			bool updateOctomap,
+			const std::map<int, rtabmap::Signature> & signatures = std::map<int, rtabmap::Signature>());
+
+	void publishMaps(
+			const std::map<int, rtabmap::Transform> & poses,
+			const rclcpp::Time & stamp,
+			const std::string & mapFrameId);
+
+	cv::Mat getGridMap(
+			float & xMin,
+			float & yMin,
+			float & gridCellSize);
+
+	cv::Mat getGridProbMap(
+			float & xMin,
+			float & yMin,
+			float & gridCellSize);
+
+#ifdef RTABMAP_OCTOMAP
+	const rtabmap::OctoMap * getOctomap() const {return octomap_;}
+#endif
+	const rtabmap::OccupancyGrid * getOccupancyGrid() const {return occupancyGrid_;}
+	const rtabmap::LocalGridMaker * getLocalMapMaker() const {return localMapMaker_;}
+
+private:
+	// mapping stuff
+	bool cloudOutputVoxelized_;
+	bool cloudSubtractFiltering_;
+	int cloudSubtractFilteringMinNeighbors_;
+	double mapFilterRadius_;
+	double mapFilterAngle_;
+	bool mapCacheCleanup_;
+	bool alwaysUpdateMap_;
+	bool scanEmptyRayTracing_;
+
+	rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr cloudMapPub_;
+	rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr cloudGroundPub_;
+	rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr cloudObstaclesPub_;
+	rclcpp::Publisher<nav_msgs::msg::OccupancyGrid>::SharedPtr gridMapPub_;
+	rclcpp::Publisher<nav_msgs::msg::OccupancyGrid>::SharedPtr gridProbMapPub_;
+#ifdef RTABMAP_OCTOMAP
+#ifdef WITH_OCTOMAP_MSGS
+	rclcpp::Publisher<octomap_msgs::msg::Octomap>::SharedPtr octoMapPubBin_;
+	rclcpp::Publisher<octomap_msgs::msg::Octomap>::SharedPtr octoMapPubFull_;
+#endif
+	rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr octoMapCloud_;
+	rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr octoMapFrontierCloud_;
+	rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr octoMapGroundCloud_;
+	rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr octoMapObstacleCloud_;
+	rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr octoMapEmptySpace_;
+	rclcpp::Publisher<nav_msgs::msg::OccupancyGrid>::SharedPtr octoMapProj_;
+#endif
+#if defined(WITH_GRID_MAP_ROS) and defined(RTABMAP_GRIDMAP)
+	rclcpp::Publisher<grid_map_msgs::msg::GridMap>::SharedPtr elevationMapPub_;
+#endif
+
+	std::map<int, rtabmap::Transform> assembledGroundPoses_;
+	std::map<int, rtabmap::Transform> assembledObstaclePoses_;
+	pcl::PointCloud<pcl::PointXYZRGB>::Ptr assembledObstacles_;
+	pcl::PointCloud<pcl::PointXYZRGB>::Ptr assembledGround_;
+	rtabmap::FlannIndex assembledGroundIndex_;
+	rtabmap::FlannIndex assembledObstacleIndex_;
+	std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr > groundClouds_;
+	std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr > obstacleClouds_;
+
+	rtabmap::LocalGridCache localMaps_;
+
+	rtabmap::OccupancyGrid * occupancyGrid_;
+	rtabmap::LocalGridMaker * localMapMaker_;
+	bool gridUpdated_;
+
+#ifdef RTABMAP_OCTOMAP
+	rtabmap::OctoMap * octomap_;
+#endif
+	int octomapTreeDepth_;
+	bool octomapUpdated_;
+
+#ifdef RTABMAP_GRIDMAP
+	rtabmap::GridMap * elevationMap_;
+#endif
+	bool elevationMapUpdated_;
+
+	rtabmap::ParametersMap parameters_;
+
+	bool latching_;
+	std::map<void*, bool> latched_;
+};
+
+} // namespace rtabmap_util
+
+#endif /* MAPSMANAGER_H_ */

rtabmap_ros/rtabmap_util/include/rtabmap_util/ULogToRosout.h

@@ -0,0 +1,79 @@
+/*
+Copyright (c) 2010-2022, Mathieu Labbe
+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 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 <rclcpp/rclcpp.hpp>
+#include <rtabmap/utilite/UEventsHandler.h>
+
+namespace rtabmap_util {
+
+class ULogToRosout : public UEventsHandler
+{
+public:
+	ULogToRosout(const rclcpp::Node * node) :
+		node_(node)
+	{
+		registerToEventsManager();
+	}
+	virtual ~ULogToRosout()
+	{
+		unregisterFromEventsManager();
+	}
+protected:
+	virtual bool handleEvent(UEvent * event)
+	{
+		if(event->getClassName().compare("ULogEvent") == 0)
+		{
+			ULogEvent * logEvent = (ULogEvent *)event;
+			if(logEvent->getCode() == ULogger::kDebug)
+			{
+				RCLCPP_DEBUG(node_->get_logger(), "%s", logEvent->getMsg().c_str());
+			}
+			else if(logEvent->getCode() == ULogger::kInfo)
+			{
+				RCLCPP_INFO(node_->get_logger(), "%s", logEvent->getMsg().c_str());
+			}
+			else if(logEvent->getCode() == ULogger::kWarning)
+			{
+				RCLCPP_WARN(node_->get_logger(), "%s", logEvent->getMsg().c_str());
+			}
+			else if(logEvent->getCode() == ULogger::kError)
+			{
+				RCLCPP_ERROR(node_->get_logger(), "%s", logEvent->getMsg().c_str());
+			}
+			else if(logEvent->getCode() == ULogger::kFatal)
+			{
+				RCLCPP_FATAL(node_->get_logger(), "%s", logEvent->getMsg().c_str());
+			}
+			return true;
+		}
+		return false;
+	}
+
+private:
+	const rclcpp::Node * node_;
+};
+
+}

rtabmap_ros/rtabmap_util/include/rtabmap_util/disparity_to_depth.hpp

@@ -0,0 +1,55 @@
+/*
+Copyright (c) 2010-2023, 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 <sensor_msgs/msg/image.h>
+#include <stereo_msgs/msg/disparity_image.hpp>
+
+#include <image_transport/image_transport.hpp>
+
+namespace rtabmap_util
+{
+
+class DisparityToDepth : public rclcpp::Node
+{
+public:
+	RTABMAP_UTIL_PUBLIC
+	explicit DisparityToDepth(const rclcpp::NodeOptions & options);
+	virtual ~DisparityToDepth();
+
+private:
+	void callback(const stereo_msgs::msg::DisparityImage::ConstSharedPtr msg);
+
+private:
+	image_transport::Publisher pub32f_;
+	image_transport::Publisher pub16u_;
+	rclcpp::Subscription<stereo_msgs::msg::DisparityImage>::SharedPtr sub_;
+};
+
+}

rtabmap_ros/rtabmap_util/include/rtabmap_util/imu_to_tf.hpp

@@ -0,0 +1,61 @@
+/*
+Copyright (c) 2010-2023, 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 <sensor_msgs/msg/imu.hpp>
+
+#include <tf2_ros/buffer.h>
+#include <tf2_ros/transform_listener.h>
+#include <tf2_ros/transform_broadcaster.h>
+
+namespace rtabmap_util
+{
+
+class ImuToTF : public rclcpp::Node
+{
+public:
+	RTABMAP_UTIL_PUBLIC
+	explicit ImuToTF(const rclcpp::NodeOptions & options);
+	virtual ~ImuToTF();
+
+private:
+	void imuCallback(const sensor_msgs::msg::Imu::ConstSharedPtr msg);
+
+private:
+	rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr sub_;
+	std::shared_ptr<tf2_ros::TransformBroadcaster> tfBroadcaster_;
+	std::string fixedFrameId_;
+	std::string baseFrameId_;
+	std::shared_ptr<tf2_ros::Buffer> tfBuffer_;
+	std::shared_ptr<tf2_ros::TransformListener> tfListener_;
+	double waitForTransformDuration_;
+};
+
+}
+

rtabmap_ros/rtabmap_util/include/rtabmap_util/lidar_deskewing.hpp

@@ -0,0 +1,65 @@
+/*
+Copyright (c) 2010-2023, 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 <tf2_ros/buffer.h>
+#include <tf2_ros/transform_listener.h>
+
+#include <sensor_msgs/msg/point_cloud2.hpp>
+#include <sensor_msgs/msg/laser_scan.hpp>
+
+namespace rtabmap_util
+{
+
+class LidarDeskewing : public rclcpp::Node
+{
+public:
+	RTABMAP_UTIL_PUBLIC
+	explicit LidarDeskewing(const rclcpp::NodeOptions & options);
+	virtual ~LidarDeskewing();
+
+private:
+	void callbackScan(const sensor_msgs::msg::LaserScan::ConstSharedPtr msg);
+	void callbackCloud(const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg);
+
+private:
+	rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pubScan_;
+	rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pubCloud_;
+	rclcpp::Subscription<sensor_msgs::msg::LaserScan>::SharedPtr subScan_;
+	rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr subCloud_;
+	std::string fixedFrameId_;
+	double waitForTransformDuration_;
+	bool slerp_;
+	std::shared_ptr<tf2_ros::Buffer> tfBuffer_;
+	std::shared_ptr<tf2_ros::TransformListener> tfListener_;
+};
+
+}
+
+

rtabmap_ros/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_ros/rtabmap_util/include/rtabmap_util/obstacles_detection.hpp

@@ -0,0 +1,74 @@
+/*
+Copyright (c) 2010-2016, 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 "rclcpp/rclcpp.hpp"
+#include <rtabmap_util/visibility.h>
+
+#include <tf2_ros/buffer.h>
+#include <tf2_ros/transform_listener.h>
+
+#include <sensor_msgs/msg/point_cloud2.hpp>
+
+#include <rtabmap/core/LocalGridMaker.h>
+
+namespace rtabmap_util
+{
+
+class ObstaclesDetection : public rclcpp::Node
+{
+public:
+	RTABMAP_UTIL_PUBLIC
+	explicit ObstaclesDetection(const rclcpp::NodeOptions & options);
+	virtual ~ObstaclesDetection() {}
+
+private:
+	void callback(const sensor_msgs::msg::PointCloud2::ConstSharedPtr cloudMsg);
+
+private:
+	std::string frameId_;
+	std::string mapFrameId_;
+	double waitForTransform_;
+
+	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_;
+
+	rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr groundPub_;
+	rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr obstaclesPub_;
+	rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr projObstaclesPub_;
+
+	rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr cloudSub_;
+};
+
+}
+
+

rtabmap_ros/rtabmap_util/include/rtabmap_util/point_cloud_aggregator.hpp

@@ -0,0 +1,100 @@
+/*
+Copyright (c) 2010-2019, 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 <sensor_msgs/msg/point_cloud2.hpp>
+
+#include <tf2_ros/buffer.h>
+#include <tf2_ros/transform_listener.h>
+
+#include <message_filters/sync_policies/approximate_time.h>
+#include <message_filters/subscriber.h>
+#include <message_filters/sync_policies/exact_time.h>
+
+namespace rtabmap_util
+{
+
+/**
+ * Nodelet used to merge point clouds from different sensors into a single
+ * assembled cloud. If fixed_frame_id is set and approx_sync is true,
+ * the clouds are adjusted to include the displacement of the robot
+ * in the output cloud.
+ */
+class PointCloudAggregator : public rclcpp::Node
+{
+public:
+	RTABMAP_UTIL_PUBLIC
+	explicit PointCloudAggregator(const rclcpp::NodeOptions & options);
+
+	virtual ~PointCloudAggregator();
+
+private:
+	void clouds4_callback(const sensor_msgs::msg::PointCloud2::ConstSharedPtr cloudMsg_1,
+						 const sensor_msgs::msg::PointCloud2::ConstSharedPtr cloudMsg_2,
+						 const sensor_msgs::msg::PointCloud2::ConstSharedPtr cloudMsg_3,
+						 const sensor_msgs::msg::PointCloud2::ConstSharedPtr cloudMsg_4);
+	void clouds3_callback(const sensor_msgs::msg::PointCloud2::ConstSharedPtr cloudMsg_1,
+	                     const sensor_msgs::msg::PointCloud2::ConstSharedPtr cloudMsg_2,
+	                     const sensor_msgs::msg::PointCloud2::ConstSharedPtr cloudMsg_3);
+	void clouds2_callback(const sensor_msgs::msg::PointCloud2::ConstSharedPtr cloudMsg_1,
+						 const sensor_msgs::msg::PointCloud2::ConstSharedPtr cloudMsg_2);
+	void combineClouds(const std::vector<sensor_msgs::msg::PointCloud2::ConstSharedPtr> & cloudMsgs);
+
+	std::thread * warningThread_;
+	bool callbackCalled_;
+
+	typedef message_filters::sync_policies::ExactTime<sensor_msgs::msg::PointCloud2, sensor_msgs::msg::PointCloud2, sensor_msgs::msg::PointCloud2, sensor_msgs::msg::PointCloud2> ExactSync4Policy;
+	typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::msg::PointCloud2, sensor_msgs::msg::PointCloud2, sensor_msgs::msg::PointCloud2, sensor_msgs::msg::PointCloud2> ApproxSync4Policy;
+	typedef message_filters::sync_policies::ExactTime<sensor_msgs::msg::PointCloud2, sensor_msgs::msg::PointCloud2, sensor_msgs::msg::PointCloud2> ExactSync3Policy;
+	typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::msg::PointCloud2, sensor_msgs::msg::PointCloud2, sensor_msgs::msg::PointCloud2> ApproxSync3Policy;
+	typedef message_filters::sync_policies::ExactTime<sensor_msgs::msg::PointCloud2, sensor_msgs::msg::PointCloud2> ExactSync2Policy;
+	typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::msg::PointCloud2, sensor_msgs::msg::PointCloud2> ApproxSync2Policy;
+	message_filters::Synchronizer<ExactSync4Policy>* exactSync4_;
+	message_filters::Synchronizer<ApproxSync4Policy>* approxSync4_;
+	message_filters::Synchronizer<ExactSync3Policy>* exactSync3_;
+	message_filters::Synchronizer<ApproxSync3Policy>* approxSync3_;
+	message_filters::Synchronizer<ExactSync2Policy>* exactSync2_;
+	message_filters::Synchronizer<ApproxSync2Policy>* approxSync2_;
+	message_filters::Subscriber<sensor_msgs::msg::PointCloud2> cloudSub_1_;
+	message_filters::Subscriber<sensor_msgs::msg::PointCloud2> cloudSub_2_;
+	message_filters::Subscriber<sensor_msgs::msg::PointCloud2> cloudSub_3_;
+	message_filters::Subscriber<sensor_msgs::msg::PointCloud2> cloudSub_4_;
+
+	rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr cloudPub_;
+
+	std::string frameId_;
+	std::string fixedFrameId_;
+	double waitForTransform_;
+	bool xyzOutput_;
+	std::shared_ptr<tf2_ros::Buffer> tfBuffer_;
+	std::shared_ptr<tf2_ros::TransformListener> tfListener_;
+};
+
+}
+

rtabmap_ros/rtabmap_util/include/rtabmap_util/point_cloud_assembler.hpp

@@ -0,0 +1,115 @@
+/*
+Copyright (c) 2010-2019, 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 <tf2_ros/buffer.h>
+#include <tf2_ros/transform_listener.h>
+
+#include <nav_msgs/msg/odometry.hpp>
+#include <sensor_msgs/msg/point_cloud2.hpp>
+
+#include <message_filters/subscriber.h>
+#include <message_filters/sync_policies/exact_time.h>
+
+#include <rtabmap_conversions/MsgConversion.h>
+#include <rtabmap/core/util3d.h>
+#include <rtabmap/core/util3d_filtering.h>
+
+namespace rtabmap_util
+{
+
+/**
+ * This nodelet can assemble a number of clouds (max_clouds) coming
+ * from the same sensor, taking into account the displacement of the robot based on
+ * fixed_frame_id, then publish the resulting cloud.
+ * If fixed_frame_id is set to "" (empty), the nodelet will subscribe to
+ * an odom topic that should have the exact same stamp than to input cloud.
+ * The output cloud has the same stamp and frame than the last assembled cloud.
+ */
+class PointCloudAssembler : public rclcpp::Node
+{
+public:
+	RTABMAP_UTIL_PUBLIC
+	explicit PointCloudAssembler(const rclcpp::NodeOptions & options);
+
+	virtual ~PointCloudAssembler();
+
+private:
+	void callbackCloudOdomInfo(
+				const sensor_msgs::msg::PointCloud2::ConstSharedPtr cloudMsg,
+				const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
+				const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg);
+
+	void callbackCloudOdom(
+			const sensor_msgs::msg::PointCloud2::ConstSharedPtr cloudMsg,
+			const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg);
+
+	void callbackCloud(const sensor_msgs::msg::PointCloud2::ConstSharedPtr cloudMsg);
+
+private:
+	std::thread * warningThread_;
+	bool callbackCalled_;
+
+	rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr cloudSub_;
+	rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr cloudPub_;
+
+	typedef message_filters::sync_policies::ExactTime<sensor_msgs::msg::PointCloud2, nav_msgs::msg::Odometry> syncPolicy;
+	typedef message_filters::sync_policies::ExactTime<sensor_msgs::msg::PointCloud2, nav_msgs::msg::Odometry, rtabmap_msgs::msg::OdomInfo> syncInfoPolicy;
+	message_filters::Synchronizer<syncPolicy>* exactSync_;
+	message_filters::Synchronizer<syncInfoPolicy>* exactInfoSync_;
+	message_filters::Subscriber<sensor_msgs::msg::PointCloud2> syncCloudSub_;
+	message_filters::Subscriber<nav_msgs::msg::Odometry> syncOdomSub_;
+	message_filters::Subscriber<rtabmap_msgs::msg::OdomInfo> syncOdomInfoSub_;
+
+	int maxClouds_;
+	int skipClouds_;
+	int cloudsSkipped_;
+	bool circularBuffer_;
+	double linearUpdate_;
+	double angularUpdate_;
+	double assemblingTime_;
+	double waitForTransform_;
+	double rangeMin_;
+	double rangeMax_;
+	double voxelSize_;
+	double noiseRadius_;
+	int noiseMinNeighbors_;
+	bool removeZ_;
+	std::string fixedFrameId_;
+	std::string frameId_;
+	std::shared_ptr<tf2_ros::Buffer> tfBuffer_;
+	std::shared_ptr<tf2_ros::TransformListener> tfListener_;
+	rtabmap::Transform previousPose_;
+
+	std::list<pcl::PCLPointCloud2::Ptr> clouds_;
+
+	std::string subscribedTopicsMsg_;
+};
+
+}

rtabmap_ros/rtabmap_util/include/rtabmap_util/point_cloud_xyz.hpp

@@ -0,0 +1,104 @@
+/*
+Copyright (c) 2010-2016, 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 <sensor_msgs/msg/point_cloud2.hpp>
+#include <sensor_msgs/msg/image.hpp>
+#include <sensor_msgs/msg/camera_info.hpp>
+#include <stereo_msgs/msg/disparity_image.hpp>
+
+#include <image_transport/image_transport.hpp>
+#include <image_transport/subscriber_filter.hpp>
+
+#include <message_filters/sync_policies/approximate_time.h>
+#include <message_filters/sync_policies/exact_time.h>
+#include <message_filters/subscriber.h>
+
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+#include <pcl/pcl_base.h>
+
+namespace rtabmap_util
+{
+
+class PointCloudXYZ : public rclcpp::Node
+{
+public:
+	RTABMAP_UTIL_PUBLIC
+	explicit PointCloudXYZ(const rclcpp::NodeOptions & options);
+	virtual ~PointCloudXYZ();
+
+private:
+	void callback(
+			  const sensor_msgs::msg::Image::ConstSharedPtr depth,
+			  const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfo);
+
+	void callbackDisparity(
+			const stereo_msgs::msg::DisparityImage::ConstSharedPtr disparityMsg,
+			const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfo);
+
+	void processAndPublish(pcl::PointCloud<pcl::PointXYZ>::Ptr & pclCloud, pcl::IndicesPtr & indices, const std_msgs::msg::Header & header);
+
+private:
+
+	double maxDepth_;
+	double minDepth_;
+	double voxelSize_;
+	int decimation_;
+	double noiseFilterRadius_;
+	int noiseFilterMinNeighbors_;
+	int normalK_;
+	double normalRadius_;
+	bool filterNaNs_;
+	std::vector<float> roiRatios_;
+
+	rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr cloudPub_;
+
+	image_transport::SubscriberFilter imageDepthSub_;
+	message_filters::Subscriber<sensor_msgs::msg::CameraInfo> cameraInfoSub_;
+
+	message_filters::Subscriber<stereo_msgs::msg::DisparityImage> disparitySub_;
+	message_filters::Subscriber<sensor_msgs::msg::CameraInfo> disparityCameraInfoSub_;
+
+	typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo> MyApproxSyncDepthPolicy;
+	message_filters::Synchronizer<MyApproxSyncDepthPolicy> * approxSyncDepth_;
+
+	typedef message_filters::sync_policies::ApproximateTime<stereo_msgs::msg::DisparityImage, sensor_msgs::msg::CameraInfo> MyApproxSyncDisparityPolicy;
+	message_filters::Synchronizer<MyApproxSyncDisparityPolicy> * approxSyncDisparity_;
+
+	typedef message_filters::sync_policies::ExactTime<sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo> MyExactSyncDepthPolicy;
+	message_filters::Synchronizer<MyExactSyncDepthPolicy> * exactSyncDepth_;
+
+	typedef message_filters::sync_policies::ExactTime<stereo_msgs::msg::DisparityImage, sensor_msgs::msg::CameraInfo> MyExactSyncDisparityPolicy;
+	message_filters::Synchronizer<MyExactSyncDisparityPolicy> * exactSyncDisparity_;
+
+};
+
+}
+

rtabmap_ros/rtabmap_util/include/rtabmap_util/point_cloud_xyzrgb.hpp

@@ -0,0 +1,130 @@
+/*
+Copyright (c) 2010-2016, 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 <sensor_msgs/msg/point_cloud2.hpp>
+#include <sensor_msgs/msg/image.hpp>
+#include <sensor_msgs/msg/camera_info.hpp>
+#include <stereo_msgs/msg/disparity_image.hpp>
+
+#include <rtabmap_msgs/msg/rgbd_image.hpp>
+
+#include <image_transport/image_transport.hpp>
+#include <image_transport/subscriber_filter.hpp>
+
+#include <message_filters/sync_policies/approximate_time.h>
+#include <message_filters/sync_policies/exact_time.h>
+#include <message_filters/subscriber.h>
+
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+#include <pcl/pcl_base.h>
+
+#include <rtabmap/core/Parameters.h>
+
+namespace rtabmap_util
+{
+
+class PointCloudXYZRGB : public rclcpp::Node
+{
+public:
+	RTABMAP_UTIL_PUBLIC
+	explicit PointCloudXYZRGB(const rclcpp::NodeOptions & options);
+	virtual ~PointCloudXYZRGB();
+
+private:
+	void depthCallback(
+			  const sensor_msgs::msg::Image::ConstSharedPtr image,
+			  const sensor_msgs::msg::Image::ConstSharedPtr imageDepth,
+			  const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfo);
+
+	void disparityCallback(
+			const sensor_msgs::msg::Image::ConstSharedPtr image,
+			const stereo_msgs::msg::DisparityImage::ConstSharedPtr imageDisparity,
+			const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfo);
+
+	void stereoCallback(const sensor_msgs::msg::Image::ConstSharedPtr imageLeft,
+			const sensor_msgs::msg::Image::ConstSharedPtr imageRight,
+			const sensor_msgs::msg::CameraInfo::ConstSharedPtr camInfoLeft,
+			const sensor_msgs::msg::CameraInfo::ConstSharedPtr camInfoRight);
+
+	void rgbdImageCallback(const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image);
+
+	void processAndPublish(pcl::PointCloud<pcl::PointXYZRGB>::Ptr & pclCloud, pcl::IndicesPtr & indices, const std_msgs::msg::Header & header);
+
+private:
+
+	double maxDepth_;
+	double minDepth_;
+	double voxelSize_;
+	int decimation_;
+	double noiseFilterRadius_;
+	int noiseFilterMinNeighbors_;
+	int normalK_;
+	double normalRadius_;
+	bool filterNaNs_;
+	std::vector<float> roiRatios_;
+	rtabmap::ParametersMap stereoBMParameters_;
+
+	rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr cloudPub_;
+
+	rclcpp::Subscription<rtabmap_msgs::msg::RGBDImage>::SharedPtr rgbdImageSub_;
+
+	image_transport::SubscriberFilter imageSub_;
+	image_transport::SubscriberFilter imageDepthSub_;
+	message_filters::Subscriber<sensor_msgs::msg::CameraInfo> cameraInfoSub_;
+
+	message_filters::Subscriber<stereo_msgs::msg::DisparityImage> imageDisparitySub_;
+
+	image_transport::SubscriberFilter imageLeft_;
+	image_transport::SubscriberFilter imageRight_;
+	message_filters::Subscriber<sensor_msgs::msg::CameraInfo> cameraInfoLeft_;
+	message_filters::Subscriber<sensor_msgs::msg::CameraInfo> cameraInfoRight_;
+
+	typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::msg::Image, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo> MyApproxSyncDepthPolicy;
+	message_filters::Synchronizer<MyApproxSyncDepthPolicy> * approxSyncDepth_;
+
+	typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::msg::Image, stereo_msgs::msg::DisparityImage, sensor_msgs::msg::CameraInfo> MyApproxSyncDisparityPolicy;
+	message_filters::Synchronizer<MyApproxSyncDisparityPolicy> * approxSyncDisparity_;
+
+	typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::msg::Image, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo, sensor_msgs::msg::CameraInfo> MyApproxSyncStereoPolicy;
+	message_filters::Synchronizer<MyApproxSyncStereoPolicy> * approxSyncStereo_;
+
+	typedef message_filters::sync_policies::ExactTime<sensor_msgs::msg::Image, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo> MyExactSyncDepthPolicy;
+	message_filters::Synchronizer<MyExactSyncDepthPolicy> * exactSyncDepth_;
+
+	typedef message_filters::sync_policies::ExactTime<sensor_msgs::msg::Image, stereo_msgs::msg::DisparityImage, sensor_msgs::msg::CameraInfo> MyExactSyncDisparityPolicy;
+	message_filters::Synchronizer<MyExactSyncDisparityPolicy> * exactSyncDisparity_;
+
+	typedef message_filters::sync_policies::ExactTime<sensor_msgs::msg::Image, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo, sensor_msgs::msg::CameraInfo> MyExactSyncStereoPolicy;
+	message_filters::Synchronizer<MyExactSyncStereoPolicy> * exactSyncStereo_;
+};
+
+}
+

rtabmap_ros/rtabmap_util/include/rtabmap_util/pointcloud_to_depthimage.hpp

@@ -0,0 +1,85 @@
+/*
+Copyright (c) 2010-2016, 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 <sensor_msgs/msg/point_cloud2.hpp>
+#include <sensor_msgs/msg/image.hpp>
+#include <sensor_msgs/msg/camera_info.hpp>
+
+#include <image_transport/image_transport.hpp>
+
+#include <tf2_ros/buffer.h>
+#include <tf2_ros/transform_listener.h>
+
+#include <message_filters/sync_policies/approximate_time.h>
+#include <message_filters/sync_policies/exact_time.h>
+#include <message_filters/subscriber.h>
+
+namespace rtabmap_util
+{
+
+class PointCloudToDepthImage : public rclcpp::Node
+{
+public:
+	RTABMAP_UTIL_PUBLIC
+	explicit PointCloudToDepthImage(const rclcpp::NodeOptions & options);
+	virtual ~PointCloudToDepthImage();
+
+private:
+	void callback(
+			const sensor_msgs::msg::PointCloud2::ConstSharedPtr pointCloud2Msg,
+			const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg);
+
+private:
+	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_;
+	message_filters::Subscriber<sensor_msgs::msg::PointCloud2> pointCloudSub_;
+	message_filters::Subscriber<sensor_msgs::msg::CameraInfo> cameraInfoSub_;
+	std::string fixedFrameId_;
+	std::shared_ptr<tf2_ros::Buffer> tfBuffer_;
+	std::shared_ptr<tf2_ros::TransformListener> tfListener_;
+	double waitForTransform_;
+	int fillHolesSize_;
+	double fillHolesError_;
+	int fillIterations_;
+	int decimation_;
+	bool upscale_;
+	double upscaleDepthErrorRatio_;
+
+	typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::msg::PointCloud2, sensor_msgs::msg::CameraInfo> MyApproxSyncPolicy;
+	message_filters::Synchronizer<MyApproxSyncPolicy> * approxSync_;
+	typedef message_filters::sync_policies::ExactTime<sensor_msgs::msg::PointCloud2, sensor_msgs::msg::CameraInfo> MyExactSyncPolicy;
+	message_filters::Synchronizer<MyExactSyncPolicy> * exactSync_;
+};
+
+}
+

rtabmap_ros/rtabmap_util/include/rtabmap_util/rgbd_relay.hpp

@@ -0,0 +1,53 @@
+/*
+Copyright (c) 2010-2016, 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/msg/rgbd_image.hpp"
+
+namespace rtabmap_util
+{
+
+class RGBDRelay : public rclcpp::Node
+{
+public:
+	RTABMAP_UTIL_PUBLIC
+	explicit RGBDRelay(const rclcpp::NodeOptions & options);
+	virtual ~RGBDRelay() {}
+private:
+	void callback(const rtabmap_msgs::msg::RGBDImage::SharedPtr input) const;
+
+private:
+	bool compress_;
+	bool uncompress_;
+	rclcpp::Subscription<rtabmap_msgs::msg::RGBDImage>::SharedPtr rgbdImageSub_;
+	rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr rgbdImagePub_;
+};
+
+}
+

rtabmap_ros/rtabmap_util/include/rtabmap_util/rgbd_split.hpp

@@ -0,0 +1,58 @@
+/*
+Copyright (c) 2010-2016, 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 <sensor_msgs/image_encodings.hpp>
+
+#include <image_transport/image_transport.hpp>
+
+#include "rtabmap_msgs/msg/rgbd_image.hpp"
+
+namespace rtabmap_util
+{
+
+class RGBDSplit : public rclcpp::Node
+{
+public:
+	RTABMAP_UTIL_PUBLIC
+	explicit RGBDSplit(const rclcpp::NodeOptions & options);
+
+	virtual ~RGBDSplit() {}
+
+	void callback(const rtabmap_msgs::msg::RGBDImage::SharedPtr input) const;
+
+private:
+	rclcpp::Subscription<rtabmap_msgs::msg::RGBDImage>::SharedPtr rgbdImageSub_;
+
+	image_transport::CameraPublisher rgbPub_;
+	image_transport::CameraPublisher depthPub_;
+};
+
+}
+

rtabmap_ros/rtabmap_util/include/rtabmap_util/visibility.h

@@ -0,0 +1,59 @@
+// Copyright 2016 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RTABMAP_UTIL__VISIBILITY_CONTROL_H_
+#define RTABMAP_UTIL__VISIBILITY_CONTROL_H_
+
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+
+// This logic was borrowed (then namespaced) from the examples on the gcc wiki:
+//     https://gcc.gnu.org/wiki/Visibility
+
+#if defined _WIN32 || defined __CYGWIN__
+  #ifdef __GNUC__
+    #define RTABMAP_UTIL_EXPORT __attribute__ ((dllexport))
+    #define RTABMAP_UTIL_IMPORT __attribute__ ((dllimport))
+  #else
+    #define RTABMAP_UTIL_EXPORT __declspec(dllexport)
+    #define RTABMAP_UTIL_IMPORT __declspec(dllimport)
+  #endif
+  #ifdef RTABMAP_UTIL_BUILDING_DLL
+    #define RTABMAP_UTIL_PUBLIC RTABMAP_UTIL_EXPORT
+  #else
+    #define RTABMAP_UTIL_PUBLIC RTABMAP_UTIL_IMPORT
+  #endif
+  #define RTABMAP_UTIL_PUBLIC_TYPE RTABMAP_UTIL_PUBLIC
+  #define RTABMAP_UTIL_LOCAL
+#else
+  #define RTABMAP_UTIL_EXPORT __attribute__ ((visibility("default")))
+  #define RTABMAP_UTIL_IMPORT
+  #if __GNUC__ >= 4
+    #define RTABMAP_UTIL_PUBLIC __attribute__ ((visibility("default")))
+    #define RTABMAP_UTIL_LOCAL  __attribute__ ((visibility("hidden")))
+  #else
+    #define RTABMAP_UTIL_PUBLIC
+    #define RTABMAP_UTIL_LOCAL
+  #endif
+  #define RTABMAP_UTIL_PUBLIC_TYPE
+#endif
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif  // RTABMAP_UTIL__VISIBILITY_CONTROL_H_
+