ported rtabmap_ros_pkg_split to ros2

rtabmap_util/CMakeLists.txt

@@ -0,0 +1,218 @@
+cmake_minimum_required(VERSION 3.5)
+project(rtabmap_util)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+find_package(RTABMap REQUIRED)
+MESSAGE(STATUS "RTABMap=${RTABMap_TARGETS}")
+
+find_package(ament_cmake REQUIRED)
+find_package(cv_bridge REQUIRED)
+find_package(image_transport REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rclcpp_components REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(stereo_msgs REQUIRED)
+find_package(nav_msgs REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(tf2 REQUIRED)
+find_package(tf2_ros REQUIRED)
+find_package(laser_geometry REQUIRED)
+find_package(pcl_conversions REQUIRED)
+find_package(pcl_ros REQUIRED)
+find_package(message_filters REQUIRED)
+find_package(rtabmap_msgs REQUIRED)
+find_package(rtabmap_conversions REQUIRED)
+
+MESSAGE(STATUS "rtabmap_conversions_TARGETS = ${rtabmap_conversions_TARGETS}")
+
+# Optional components
+find_package(octomap_msgs)
+
+include_directories(
+  ${CMAKE_CURRENT_SOURCE_DIR}/include
+)
+
+# libraries
+SET(Libraries
+   cv_bridge
+   image_transport
+   rclcpp
+   rclcpp_components
+   sensor_msgs
+   stereo_msgs
+   nav_msgs
+   std_msgs
+   tf2
+   tf2_ros
+   laser_geometry
+   pcl_conversions
+   pcl_ros
+   message_filters
+   rtabmap_msgs
+   rtabmap_conversions
+)
+
+###########
+## Build ##
+###########
+
+SET(rtabmap_util_plugins_lib_src
+   src/MapsManager.cpp
+   src/nodelets/point_cloud_xyzrgb.cpp
+   src/nodelets/point_cloud_xyz.cpp
+#   src/nodelets/disparity_to_depth.cpp # not ported to ROS2
+   src/nodelets/pointcloud_to_depthimage.cpp 
+   src/nodelets/obstacles_detection.cpp
+   src/nodelets/point_cloud_aggregator.cpp
+   src/nodelets/point_cloud_assembler.cpp
+#   src/nodelets/imu_to_tf.cpp # not ported to ROS2
+   src/nodelets/lidar_deskewing.cpp
+   src/nodelets/rgbd_relay.cpp
+   src/nodelets/rgbd_split.cpp
+)
+
+
+# If octomap is found, add definition
+IF(octomap_msgs_FOUND)
+MESSAGE(STATUS "WITH octomap_msgs")
+include_directories(
+  ${octomap_msgs_INCLUDE_DIRS}
+)
+SET(Libraries
+  octomap_msgs
+  ${Libraries}
+)
+ADD_DEFINITIONS("-DWITH_OCTOMAP_MSGS")
+ENDIF(octomap_msgs_FOUND)
+
+############################
+## Declare a cpp library
+############################
+add_library(rtabmap_util_plugins SHARED
+   ${rtabmap_util_plugins_lib_src}
+)
+target_include_directories(rtabmap_util_plugins
+  PUBLIC
+    $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+    $<INSTALL_INTERFACE:include>
+)
+
+ament_target_dependencies(rtabmap_util_plugins ${Libraries})
+
+rclcpp_components_register_nodes(rtabmap_util_plugins "rtabmap_util::RGBDRelay")
+rclcpp_components_register_nodes(rtabmap_util_plugins "rtabmap_util::RGBDSplit")
+#rclcpp_components_register_nodes(rtabmap_util_plugins "rtabmap_util::DisparityToDepth")
+#rclcpp_components_register_nodes(rtabmap_util_plugins "rtabmap_util::ImuToTF")
+rclcpp_components_register_nodes(rtabmap_util_plugins "rtabmap_util::LidarDeskewing")
+rclcpp_components_register_nodes(rtabmap_util_plugins "rtabmap_util::PointCloudXYZ")
+rclcpp_components_register_nodes(rtabmap_util_plugins "rtabmap_util::PointCloudXYZRGB")
+rclcpp_components_register_nodes(rtabmap_util_plugins "rtabmap_util::PointCloudToDepthImage")
+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")
+
+add_executable(rtabmap_rgbd_relay src/RGBDRelayNode.cpp)
+ament_target_dependencies(rtabmap_rgbd_relay ${Libraries})
+target_link_libraries(rtabmap_rgbd_relay rtabmap_util_plugins)
+set_target_properties(rtabmap_rgbd_relay PROPERTIES OUTPUT_NAME "rgbd_relay")
+
+add_executable(rtabmap_rgbd_split src/RGBDSplitNode.cpp)
+ament_target_dependencies(rtabmap_rgbd_split ${Libraries})
+target_link_libraries(rtabmap_rgbd_split rtabmap_util_plugins)
+set_target_properties(rtabmap_rgbd_split PROPERTIES OUTPUT_NAME "rgbd_split")
+
+#add_executable(rtabmap_map_optimizer src/MapOptimizerNode.cpp)
+#ament_target_dependencies(rtabmap_map_optimizer ${Libraries})
+#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_imu_to_tf src/ImuToTFNode.cpp)
+#ament_target_dependencies(rtabmap_imu_to_tf ${Libraries})
+#target_link_libraries(rtabmap_imu_to_tf rtabmap_util_plugins)
+#set_target_properties(rtabmap_imu_to_tf PROPERTIES OUTPUT_NAME "imu_to_tf")
+
+add_executable(rtabmap_lidar_deskewing src/LidarDeskewingNode.cpp)
+ament_target_dependencies(rtabmap_lidar_deskewing ${Libraries})
+target_link_libraries(rtabmap_lidar_deskewing rtabmap_util_plugins)
+set_target_properties(rtabmap_lidar_deskewing PROPERTIES OUTPUT_NAME "lidar_deskewing")
+
+#add_executable(rtabmap_data_player src/DbPlayerNode.cpp)
+#ament_target_dependencies(rtabmap_data_player ${Libraries})
+#target_link_libraries(rtabmap_data_player rtabmap_util_plugins)
+#set_target_properties(rtabmap_data_player PROPERTIES OUTPUT_NAME "data_player")
+
+#add_executable(rtabmap_odom_msg_to_tf src/OdomMsgToTFNode.cpp)
+#ament_target_dependencies(rtabmap_odom_msg_to_tf ${Libraries})
+#target_link_libraries(rtabmap_odom_msg_to_tf rtabmap_util_plugins)
+#set_target_properties(rtabmap_odom_msg_to_tf PROPERTIES OUTPUT_NAME "odom_msg_to_tf")
+
+add_executable(rtabmap_pointcloud_to_depthimage src/PointCloudToDepthImageNode.cpp)
+ament_target_dependencies(rtabmap_pointcloud_to_depthimage ${Libraries})
+target_link_libraries(rtabmap_pointcloud_to_depthimage rtabmap_util_plugins)
+set_target_properties(rtabmap_pointcloud_to_depthimage PROPERTIES OUTPUT_NAME "pointcloud_to_depthimage")
+
+add_executable(rtabmap_point_cloud_aggregator src/PointCloudAggregatorNode.cpp)
+ament_target_dependencies(rtabmap_point_cloud_aggregator ${Libraries})
+target_link_libraries(rtabmap_point_cloud_aggregator rtabmap_util_plugins)
+set_target_properties(rtabmap_point_cloud_aggregator PROPERTIES OUTPUT_NAME "rtabmap_point_cloud_aggregator")
+
+add_executable(rtabmap_point_cloud_assembler src/PointCloudAssemblerNode.cpp)
+ament_target_dependencies(rtabmap_point_cloud_assembler ${Libraries})
+target_link_libraries(rtabmap_point_cloud_assembler rtabmap_util_plugins)
+set_target_properties(rtabmap_point_cloud_assembler PROPERTIES OUTPUT_NAME "point_cloud_assembler")
+
+#############
+## Install ##
+#############
+
+ament_export_dependencies(${Libraries})
+ament_export_include_directories(include)
+ament_export_targets(${PROJECT_NAME})          # To include downstream with targets
+ament_export_libraries(rtabmap_util_plugins) # To include downstream without targets
+
+
+# Install Python executables
+install(PROGRAMS
+#  scripts/patrol.py
+#  scripts/objects_to_tags.py
+#  scripts/point_to_tf.py
+#  scripts/netvlad_tf_ros.py
+#  scripts/gazebo_ground_truth.py
+  scripts/transform_to_tf.py
+  scripts/yaml_to_camera_info.py
+  DESTINATION lib/${PROJECT_NAME}
+)
+
+install(TARGETS
+   rtabmap_util_plugins
+   EXPORT ${PROJECT_NAME}
+   ARCHIVE DESTINATION lib
+   LIBRARY DESTINATION lib
+   RUNTIME DESTINATION bin
+)
+install(TARGETS
+#   rtabmap_map_assembler
+#   rtabmap_map_optimizer
+#   rtabmap_data_player
+#   rtabmap_odom_msg_to_tf
+   rtabmap_pointcloud_to_depthimage
+   rtabmap_point_cloud_assembler
+   rtabmap_rgbd_relay
+   rtabmap_rgbd_split
+   DESTINATION lib/${PROJECT_NAME}
+)
+
+install(DIRECTORY include/
+   DESTINATION include
+   FILES_MATCHING PATTERN "*.h"
+)
+
+ament_package()

rtabmap_util/include/rtabmap_util/MapsManager.h

@@ -0,0 +1,158 @@
+/*
+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 <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>
+
+#ifdef RTABMAP_OCTOMAP
+#ifdef WITH_OCTOMAP_MSGS
+#include <octomap_msgs/msg/octomap.hpp>
+#endif
+#endif
+
+namespace rtabmap {
+class OctoMap;
+class Memory;
+class OccupancyGrid;
+
+}  // 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_;}
+
+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
+
+	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_;
+
+	std::map<int, rtabmap::Transform> gridPoses_;
+	cv::Mat gridMap_;
+	std::map<int, std::pair< std::pair<cv::Mat, cv::Mat>, cv::Mat> > gridMaps_; // < <ground, obstacles>, empty cells >
+	std::map<int, cv::Point3f> gridMapsViewpoints_;
+
+	rtabmap::OccupancyGrid * occupancyGrid_;
+	bool gridUpdated_;
+
+#ifdef RTABMAP_OCTOMAP
+	rtabmap::OctoMap * octomap_;
+#endif
+	int octomapTreeDepth_;
+	bool octomapUpdated_;
+
+	rtabmap::ParametersMap parameters_;
+
+	bool latching_;
+	std::map<void*, bool> latched_;
+};
+
+} // namespace rtabmap_util
+
+#endif /* MAPSMANAGER_H_ */

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_util/include/rtabmap_util/lidar_deskewing.hpp

@@ -0,0 +1,39 @@
+
+#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_util/include/rtabmap_util/obstacles_detection.hpp

@@ -0,0 +1,72 @@
+/*
+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/OccupancyGrid.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::OccupancyGrid grid_;
+	bool mapFrameProjection_;
+	bool warned_;
+
+	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_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_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_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_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_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::CameraPublisher depthImage16Pub_;
+	image_transport::CameraPublisher 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_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_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_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_
+

rtabmap_util/package.xml

@@ -0,0 +1,36 @@
+<?xml version="1.0"?>
+<?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.1.0</version>
+  <description>RTAB-Map's various useful nodes and nodelets.</description>
+  <maintainer email="[email protected]">Mathieu Labbe</maintainer>
+  <author>Mathieu Labbe</author>
+  <license>BSD</license>
+  <url type="bugtracker">https://github.com/introlab/rtabmap_ros/issues</url>
+  <url type="repository">https://github.com/introlab/rtabmap_ros</url>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>cv_bridge</depend>
+  <depend>image_transport</depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_components</depend>
+  <depend>octomap_msgs</depend>
+  <depend>sensor_msgs</depend>
+  <depend>stereo_msgs</depend>
+  <depend>nav_msgs</depend>
+  <depend>std_msgs</depend>
+  <depend>tf2</depend>
+  <depend>tf2_ros</depend>
+  <depend>laser_geometry</depend>
+  <depend>pcl_conversions</depend>
+  <depend>pcl_ros</depend>
+  <depend>message_filters</depend>
+  <depend>rtabmap_msgs</depend>
+  <depend>rtabmap_conversions</depend>
+  
+  <export>
+	<build_type>ament_cmake</build_type>
+  </export>
+</package>

rtabmap_util/scripts/assemble_local_grids.py

@@ -0,0 +1,119 @@
+#!/usr/bin/env python
+
+# Similar to map_assembler node, this minimal python example shows how
+# to reconstruct the obstacle map by subscribing only to
+# graph and latest data added to map (for constant network bandwidth usage).
+
+import rospy
+from sets import Set
+
+import message_filters
+from rtabmap_msgs.msg import MapGraph
+from sensor_msgs.msg import PointCloud2
+from geometry_msgs.msg import Pose
+from geometry_msgs.msg import TransformStamped
+from tf2_sensor_msgs.tf2_sensor_msgs import do_transform_cloud
+
+
+posesDict = {}
+cloudsDict = {}
+assembledCloud = PointCloud2()
+pub = rospy.Publisher('assembled_local_grids', PointCloud2, queue_size=10)
+
+def callback(graph, cloud):
+    global assembledCloud
+    global posesDict
+    global cloudsDict
+    global pub
+
+    begin = rospy.get_time()
+
+    nodeId = graph.posesId[-1]
+    pose = graph.poses[-1]
+    size = cloud.width
+
+    posesDict[nodeId] = pose
+    cloudsDict[nodeId] = cloud
+
+    # Update pose of our buffered clouds.
+    # Check also if the clouds have moved because of a loop closure. If so, we have to update the rendering.
+    maxDiff = 0
+    for i in range(0,len(graph.posesId)):
+        if graph.posesId[i] in posesDict:
+            currentPose = posesDict[graph.posesId[i]].position
+            newPose = graph.poses[i].position
+            diff = max([abs(currentPose.x-newPose.x), abs(currentPose.y-newPose.y), abs(currentPose.z-newPose.z)])
+            if maxDiff < diff:
+                maxDiff = diff
+        else:
+            rospy.loginfo("Old node %d not found in cache, creating an empty cloud.", graph.posesId[i])
+            posesDict[graph.posesId[i]] = graph.poses[i]
+            cloudsDict[graph.posesId[i]] = PointCloud2()
+
+    # If we don't move, some nodes would be removed from the graph, so remove them from our buffered clouds.
+    newGraph = Set(graph.posesId)
+    totalPoints = 0
+    for p in posesDict.keys():
+        if p not in newGraph:
+            posesDict.pop(p)
+            cloudsDict.pop(p)
+        else:
+            totalPoints = totalPoints + cloudsDict[p].width
+
+    if maxDiff > 0.1:
+        # if any node moved more than 10 cm, request an update of the assembled map so far
+        newAssembledCloud = PointCloud2()
+        rospy.loginfo("Map has been optimized! maxDiff=%.3fm, re-updating the whole map...", maxDiff)
+        for i in range(0,len(graph.posesId)):
+            posesDict[graph.posesId[i]] = graph.poses[i]
+            t = TransformStamped()
+            p = posesDict[graph.posesId[i]]
+            t.transform.translation = p.position
+            t.transform.rotation = p.orientation
+            transformedCloud = do_transform_cloud(cloudsDict[graph.posesId[i]], t)
+            if i==0:
+                newAssembledCloud = transformedCloud
+            else:
+                newAssembledCloud.data = newAssembledCloud.data + transformedCloud.data
+                newAssembledCloud.width = newAssembledCloud.width + transformedCloud.width
+                newAssembledCloud.row_step = newAssembledCloud.row_step + transformedCloud.row_step
+        assembledCloud = newAssembledCloud
+    else:
+        t = TransformStamped()
+        t.transform.translation = pose.position
+        t.transform.rotation = pose.orientation
+        transformedCloud = do_transform_cloud(cloud, t)
+        # just concatenate new cloud to current assembled map
+        if assembledCloud.width == 0:
+            assembledCloud = transformedCloud
+        else:
+            # Adding only the difference would be more efficient
+            assembledCloud.data = assembledCloud.data + transformedCloud.data
+            assembledCloud.width = assembledCloud.width + transformedCloud.width
+            assembledCloud.row_step = assembledCloud.row_step + transformedCloud.row_step
+
+    updateTime = rospy.get_time() - begin
+
+    rospy.loginfo("Received node %d (%d pts) at xyz=%.2f %.2f %.2f, q_xyzw=%.2f %.2f %.2f %.2f (Map: Nodes=%d Points=%d Assembled=%d Update=%.0fms)", 
+        nodeId, size,
+        pose.position.x, pose.position.y, pose.position.z,
+        pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w,
+        len(cloudsDict), totalPoints, assembledCloud.width, updateTime*1000)
+
+    assembledCloud.header = graph.header
+    pub.publish(assembledCloud)
+
+def main():
+    rospy.init_node('assemble_local_grids', anonymous=True)
+    graph_sub = message_filters.Subscriber('rtabmap/mapGraph', MapGraph)
+    cloud_sub = message_filters.Subscriber('rtabmap/local_grid_obstacle', PointCloud2)
+
+    ts = message_filters.TimeSynchronizer([graph_sub, cloud_sub], 2)
+    ts.registerCallback(callback)
+    rospy.spin()
+
+if __name__ == '__main__':
+    try:
+        main()
+    except rospy.ROSInterruptException:
+        pass

rtabmap_util/scripts/gazebo_ground_truth.py

@@ -0,0 +1,54 @@
+#!/usr/bin/env python
+import rospy
+import tf
+
+from tf2_msgs.msg import TFMessage
+from gazebo_msgs.msg import LinkStates
+from geometry_msgs.msg import TransformStamped
+
+target_frame_id = ""
+
+def callBack(linkStates):
+	global delta, first
+
+	found = False
+	for i in range(len(linkStates.name)):
+		if linkStates.name[i] == gazebo_frame_id:
+			p = linkStates.pose[i]
+			found = True
+			break
+	
+	if not found:
+ 	    roslog.warn("Gazebo link state \"" + gazebo_frame_id +"\" not found, cannot generate ground truth.")
+	    return
+   
+	t = TransformStamped()
+	t.header.frame_id = frame_id
+        t.header.stamp = rospy.Time.now()
+
+	t.child_frame_id = child_frame_id
+
+	t.transform.translation.x = p.position.x
+	t.transform.translation.y = p.position.y
+	t.transform.translation.z = p.position.z 
+
+	t.transform.rotation.x = p.orientation.x
+	t.transform.rotation.y = p.orientation.y
+	t.transform.rotation.z = p.orientation.z
+	t.transform.rotation.w = p.orientation.w
+	
+	tf_pub.publish(TFMessage([t]))
+
+if __name__ == '__main__':
+    rospy.init_node('generate_gazebo_ground_truth', disable_signals=True)
+
+    frame_id = rospy.get_param('~frame_id', 'world')
+    child_frame_id = rospy.get_param('~child_frame_id', 'base_link_gt')
+    gazebo_frame_id = rospy.get_param('~gazebo_frame_id', 'base_link')
+
+    gazebo_sub = rospy.Subscriber('/gazebo/link_states', LinkStates, callBack)
+   
+    tf_pub = rospy.Publisher('/tf', TFMessage, queue_size=10)
+    tf.TransformBroadcaster()
+
+    rospy.spin()

rtabmap_util/scripts/netvlad_tf_ros.py

@@ -0,0 +1,89 @@
+#!/usr/bin/env python
+
+# Using netvlad tensorflow-v1 implementation from https://github.com/uzh-rpg/netvlad_tf_open/
+# For ROS melodic, follow the following instructions to rebuild cv_bridge with Python3
+# https://medium.com/@beta_b0t/how-to-setup-ros-with-python-3-44a69ca36674
+# On Jetpack 4.4 (18.04 and OpenCV4), use vision_opencv's noetic branch. In cv_bridge/CMakeLists.txt,
+# apply this patch:
+#   -find_package(Boost REQUIRED python37)
+#   +find_package(Boost REQUIRED python3)
+
+from __future__ import print_function
+
+import roslib
+import sys
+import rospy
+import cv2
+import numpy as np
+import tensorflow as tf
+import time
+
+import netvlad_tf.net_from_mat as nfm
+import netvlad_tf.nets as nets
+
+from std_msgs.msg import String
+from sensor_msgs.msg import Image
+from cv_bridge import CvBridge, CvBridgeError
+from rtabmap_python import compression as cp
+from rtabmap_msgs.msg import GlobalDescriptor
+
+class netvlad_ros:
+
+  def __init__(self):
+
+    self.dim = rospy.get_param('~dim', 4096)
+    self.scale = rospy.get_param('~scale', 1.0)
+    rospy.loginfo("Parameter dim=%d", self.dim)
+    rospy.loginfo("Parameter scale=%d", self.scale)
+
+    tf.reset_default_graph()
+
+    self.image_batch = tf.placeholder(
+        dtype=tf.float32, shape=[None, None, None, 3])
+
+    self.net_out = nets.vgg16NetvladPca(self.image_batch)
+    self.saver = tf.train.Saver()
+
+    self.sess = tf.Session()
+    self.saver.restore(self.sess, nets.defaultCheckpoint())
+
+    self.pub = rospy.Publisher('netvlad_descriptor', GlobalDescriptor, queue_size=1)  
+
+    self.bridge = CvBridge()
+    self.image_sub = rospy.Subscriber("image",Image,self.callback, queue_size=1)
+
+  def callback(self,data):
+    start = time.time()
+    try:
+      cv_image = self.bridge.imgmsg_to_cv2(data, "rgb8")
+    except CvBridgeError as e:
+      print(e)
+
+    if self.scale != 1.0:
+      width = int(cv_image.shape[1] * self.scale)
+      height = int(cv_image.shape[0] * self.scale)
+      cv_image = cv2.resize(cv_image, (width, height), interpolation = cv2.INTER_AREA)
+
+    batch = np.expand_dims(cv_image, axis=0)
+    result = self.sess.run(self.net_out, feed_dict={self.image_batch: batch})
+    result = result[:,:self.dim]
+
+    descriptor = GlobalDescriptor()
+    descriptor.type = 0
+    descriptor.header = data.header
+    descriptor.data = cp.compress(result)
+    self.pub.publish(descriptor)
+    end = time.time()
+    rospy.loginfo("Extracting descriptor (img=%dx%d, dim=%d): %fs", cv_image.shape[1], cv_image.shape[0], self.dim, end-start)
+
+def main(args):
+  rospy.init_node('netvlad', anonymous=True)
+  n = netvlad_ros()
+  try:
+    rospy.spin()
+  except KeyboardInterrupt:
+    print("Shutting down")
+
+if __name__ == '__main__':
+    main(sys.argv)
+

rtabmap_util/scripts/objects_to_tags.py

@@ -0,0 +1,46 @@
+#!/usr/bin/env python
+import rospy
+from apriltag_ros.msg import AprilTagDetectionArray
+from apriltag_ros.msg import AprilTagDetection
+from find_object_2d.msg import ObjectsStamped
+import tf
+import geometry_msgs.msg
+
+objFramePrefix_ = "object"
+distanceMax_ = 0.0
+
+def callback(data):
+    global objFramePrefix_
+    global distanceMax_
+    if len(data.objects.data) > 0:
+        output = AprilTagDetectionArray()
+        output.header = data.header
+        for i in range(0,len(data.objects.data),12):
+            try:
+                objId = data.objects.data[i]
+                (trans,quat) = listener.lookupTransform(data.header.frame_id, objFramePrefix_+'_'+str(int(objId)), data.header.stamp)
+                tag = AprilTagDetection()
+                tag.id.append(objId)
+                tag.pose.pose.pose.position.x = trans[0]
+                tag.pose.pose.pose.position.y = trans[1]
+                tag.pose.pose.pose.position.z = trans[2]
+                tag.pose.pose.pose.orientation.x = quat[0]
+                tag.pose.pose.pose.orientation.y = quat[1]
+                tag.pose.pose.pose.orientation.z = quat[2]
+                tag.pose.pose.pose.orientation.w = quat[3]
+                tag.pose.header = output.header
+                if distanceMax_ <= 0.0 or trans[2] < distanceMax_:
+                    output.detections.append(tag)
+            except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
+                continue
+        if len(output.detections) > 0:
+            pub.publish(output)
+
+if __name__ == '__main__':
+    pub = rospy.Publisher('tag_detections', AprilTagDetectionArray, queue_size=10)
+    rospy.init_node('objects_to_tags', anonymous=True)
+    rospy.Subscriber("objectsStamped", ObjectsStamped, callback)
+    objFramePrefix_ = rospy.get_param('~object_prefix', objFramePrefix_)
+    distanceMax_ = rospy.get_param('~distance_max', distanceMax_)
+    listener = tf.TransformListener()
+    rospy.spin()

rtabmap_util/scripts/patrol.py

@@ -0,0 +1,87 @@
+#!/usr/bin/env python
+import rospy
+import sys
+from std_msgs.msg import Bool
+from rtabmap_ros.msg import Goal
+
+pub = rospy.Publisher('rtabmap/goal_node', Goal, queue_size=1)
+waypoints = []
+currentIndex = 0
+waitingTime = 1.0
+frameId = ""
+
+def callback(data):
+    global currentIndex
+    global waitingTime
+    global frameId
+    if data.data:
+        rospy.loginfo(rospy.get_caller_id() + ": Goal '%s' reached! Publishing next goal in %.1f sec...", waypoints[currentIndex], waitingTime)
+    else:
+        rospy.loginfo(rospy.get_caller_id() + ": Goal '%s' failed! Publishing next goal in %.1f sec...", waypoints[currentIndex], waitingTime)
+
+    currentIndex = (currentIndex+1) % len(waypoints)
+
+    # Waiting time before sending next goal
+    rospy.sleep(waitingTime)
+
+    msg = Goal()
+    msg.frame_id = frameId
+    try:
+        int(waypoints[currentIndex])
+        is_dig = True
+    except ValueError:
+        is_dig = False
+    if is_dig:
+        msg.node_id = int(waypoints[currentIndex])
+        msg.node_label = ""
+    else:
+        msg.node_id = 0
+        msg.node_label = waypoints[currentIndex]
+    
+    rospy.loginfo(rospy.get_caller_id() + ": Publishing goal '%s'! (%d/%d)", waypoints[currentIndex], currentIndex+1, len(waypoints))
+    msg.header.stamp = rospy.get_rostime()
+    pub.publish(msg)
+
+def main():
+    rospy.init_node('patrol', anonymous=False)
+    sub = rospy.Subscriber("rtabmap/goal_reached", Bool, callback)
+    global waitingTime
+    global frameId
+    waitingTime = rospy.get_param('~time', waitingTime)
+    frameId = rospy.get_param('~frame_id', frameId)
+    rospy.sleep(1.) # make sure that subscribers have seen this node before sending a goal
+
+    rospy.loginfo(rospy.get_caller_id() + ": Waypoints: [%s]", str(waypoints).strip('[]'))
+    rospy.loginfo(rospy.get_caller_id() + ": time: %f", waitingTime)
+    rospy.loginfo(rospy.get_caller_id() + ": publish goal on %s", pub.resolved_name)
+    rospy.loginfo(rospy.get_caller_id() + ": receive goal status on %s", sub.resolved_name)
+
+    # send the first goal
+    msg = Goal()
+    msg.frame_id = frameId
+    try:
+        int(waypoints[currentIndex])
+        is_dig = True
+    except ValueError:
+        is_dig = False
+    if is_dig:
+        msg.node_id = int(waypoints[currentIndex])
+        msg.node_label = ""
+    else:
+        msg.node_id = 0
+        msg.node_label = waypoints[currentIndex]
+    while rospy.Time.now().secs == 0:
+         rospy.loginfo(rospy.get_caller_id() + ": Waiting clock...")
+         rospy.sleep(.1)
+    msg.header.stamp = rospy.Time.now()
+    rospy.loginfo(rospy.get_caller_id() + ": Publishing goal '%s'! (%d/%d)", waypoints[currentIndex], currentIndex+1, len(waypoints))
+    pub.publish(msg)
+    rospy.spin()    
+
+if __name__ == '__main__':
+    if len(sys.argv) < 3:
+        print("usage: patrol.py waypointA waypointB waypointC ... [_time:=1 frame_id:=base_footprint] [topic remaps] (at least 2 waypoints, can be node id, landmark or label)")
+    else:
+        waypoints = sys.argv[1:] 
+        waypoints = [x for x in waypoints if not x.startswith('/') and not x.startswith('_')]
+        main()

rtabmap_util/scripts/point_to_tf.py

@@ -0,0 +1,28 @@
+#!/usr/bin/env python
+import rospy
+import tf
+from geometry_msgs.msg import PointStamped
+
+def callback(point):
+    global br
+    global frame_id
+    local_frame_id = point.header.frame_id
+    if not local_frame_id:
+        local_frame_id = frame_id
+    br.sendTransform(
+        (point.point.x, point.point.y, point.point.z),
+        tf.transformations.quaternion_from_euler(0,0,0),
+        point.header.stamp,
+        local_frame_id,
+        fixed_frame_id)
+
+if __name__ == "__main__":
+
+    rospy.init_node("point_to_tf", anonymous=True)
+
+    frame_id = rospy.get_param('~frame_id', 'point')
+    fixed_frame_id = rospy.get_param('~fixed_frame_id', 'world')
+
+    br = tf.TransformBroadcaster()
+    rospy.Subscriber("point", PointStamped, callback, queue_size=1)
+    rospy.spin()

rtabmap_util/scripts/transform_to_tf.py

@@ -0,0 +1,44 @@
+#!/usr/bin/env python3
+import rclpy
+from rclpy.node import Node
+from geometry_msgs.msg import TransformStamped
+from tf2_ros import TransformBroadcaster
+
+class TransformToTf(Node):
+
+    def __init__(self):
+        super().__init__('transform_to_tf')
+        
+        self.declare_parameter('frame_id', 'world')
+        self.declare_parameter('child_frame_id', 'transform')
+        self.frame_id = self.get_parameter('frame_id').get_parameter_value().string_value
+        self.child_frame_id = self.get_parameter('child_frame_id').get_parameter_value().string_value
+
+        self.tf_broadcaster = TransformBroadcaster(self)
+
+        self.subscription = self.create_subscription(
+            TransformStamped,
+            'transform',
+            self.callback,
+            1)
+        self.subscription  # prevent unused variable warning
+
+    def callback(self, t):
+        if not t.header.frame_id:
+            t.header.frame_id = self.frame_id
+        if not t.child_frame_id:
+            t.child_frame_id = self.child_frame_id
+
+        self.tf_broadcaster.sendTransform(t)
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    transform_to_tf = TransformToTf()
+    rclpy.spin(transform_to_tf)
+    transform_to_tf.destroy_node()
+    rclpy.shutdown()
+
+if __name__ == "__main__":
+    main()
+   

rtabmap_util/scripts/yaml_to_camera_info.py

@@ -0,0 +1,62 @@
+#!/usr/bin/env python3
+import rclpy
+import yaml
+from rclpy.node import Node
+from sensor_msgs.msg import CameraInfo
+from sensor_msgs.msg import Image
+
+def yaml_to_CameraInfo(yaml_fname):
+    with open(yaml_fname, "r") as file_handle:
+        calib_data = yaml.load(file_handle)
+ 
+    msg = CameraInfo()
+    msg.width = calib_data["image_width"]
+    msg.height = calib_data["image_height"]
+    msg.k = calib_data["camera_matrix"]["data"]
+    msg.d = calib_data["distortion_coefficients"]["data"]
+    msg.r = calib_data["rectification_matrix"]["data"]
+    msg.p = calib_data["projection_matrix"]["data"]
+    msg.distortion_model = calib_data["distortion_model"]
+    return msg
+
+class YamlToCameraInfo(Node):
+
+    def __init__(self):
+        super().__init__('yaml_to_camera_info')
+        
+        self.declare_parameter('yaml_path', '')
+        yaml_path = self.get_parameter('yaml_path').get_parameter_value().string_value
+
+        if not yaml_path:
+            print('yaml_path parameter should be set to path of the calibration file!')
+            sys.exit(1)
+
+        self.declare_parameter('frame_id', '')
+        self.frame_id = self.get_parameter('frame_id').get_parameter_value().string_value
+        
+        self.camera_info_msg = yaml_to_CameraInfo(yaml_path)
+
+        self.publisher_ = self.create_publisher(CameraInfo, 'camera_info', 1)
+        self.subscription = self.create_subscription(
+            Image,
+            'image',
+            self.callback,
+            1)
+        self.subscription  # prevent unused variable warning
+
+    def callback(self, image):
+        self.camera_info_msg.header = image.header
+        if self.frame_id:
+            self.camera_info_msg.header.frame_id = self.frame_id
+        self.publisher_.publish(self.camera_info_msg)
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    yaml_to_camera_info = YamlToCameraInfo()
+    rclpy.spin(yaml_to_camera_info)
+    yaml_to_camera_info.destroy_node()
+    rclpy.shutdown()
+
+if __name__ == "__main__":
+    main()

rtabmap_util/src/DbPlayerNode.cpp

@@ -0,0 +1,682 @@
+/*
+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 <ros/ros.h>
+#include <sensor_msgs/Image.h>
+#include <sensor_msgs/image_encodings.h>
+#include <sensor_msgs/PointCloud2.h>
+#include <sensor_msgs/LaserScan.h>
+#include <sensor_msgs/CameraInfo.h>
+#include <sensor_msgs/NavSatFix.h>
+#include <geometry_msgs/PoseWithCovarianceStamped.h>
+#include <rosgraph_msgs/Clock.h>
+#include <pcl_conversions/pcl_conversions.h>
+#include <nav_msgs/Odometry.h>
+#include <cv_bridge/cv_bridge.h>
+#include <image_transport/image_transport.h>
+#include <tf2_ros/transform_broadcaster.h>
+#include <std_srvs/Empty.h>
+#include <rtabmap_conversions/MsgConversion.h>
+#include <rtabmap_msgs/SetGoal.h>
+#include <rtabmap/utilite/ULogger.h>
+#include <rtabmap/utilite/UStl.h>
+#include <rtabmap/utilite/UThread.h>
+#include <rtabmap/utilite/UDirectory.h>
+#include <rtabmap/utilite/UConversion.h>
+#include <rtabmap/core/util3d.h>
+#include <rtabmap/core/DBReader.h>
+#include <rtabmap/core/OdometryEvent.h>
+#include <cmath>
+
+#ifndef _WIN32
+#include <sys/ioctl.h>
+#include <termios.h>
+bool spacehit()
+{
+	bool charAvailable = true;
+	bool hit = false;
+	while(charAvailable)
+	{
+		termios term;
+		tcgetattr(0, &term);
+
+		termios term2 = term;
+		term2.c_lflag &= ~ICANON;
+		term2.c_lflag &= ~ECHO;
+		term2.c_lflag &= ~ISIG;
+		term2.c_cc[VMIN] = 0;
+		term2.c_cc[VTIME] = 0;
+		tcsetattr(0, TCSANOW, &term2);
+
+		int c = getchar();
+		if(c != EOF)
+		{
+			if(c == ' ')
+			{
+				hit = true;
+			}
+		}
+		else
+		{
+			charAvailable = false;
+		}
+
+		tcsetattr(0, TCSANOW, &term);
+	}
+
+    return hit;
+}
+#endif
+
+bool paused = false;
+bool pauseCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
+{
+	if(paused)
+	{
+		ROS_WARN("Already paused!");
+	}
+	else
+	{
+		paused = true;
+		ROS_INFO("paused!");
+	}
+	return true;
+}
+
+bool resumeCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
+{
+	if(!paused)
+	{
+		ROS_WARN("Already running!");
+	}
+	else
+	{
+		paused = false;
+		ROS_INFO("resumed!");
+	}
+	return true;
+}
+
+int main(int argc, char** argv)
+{
+	ros::init(argc, argv, "data_player");
+
+	//ULogger::setType(ULogger::kTypeConsole);
+	//ULogger::setLevel(ULogger::kDebug);
+	//ULogger::setEventLevel(ULogger::kWarning);
+
+	bool publishClock = false;
+	for(int i=1;i<argc;++i)
+	{
+		if(strcmp(argv[i], "--clock") == 0)
+		{
+			publishClock = true;
+		}
+	}
+
+	ros::NodeHandle nh;
+	ros::NodeHandle pnh("~");
+
+	std::string frameId = "base_link";
+	std::string odomFrameId = "odom";
+	std::string cameraFrameId = "camera_optical_link";
+	std::string scanFrameId = "base_laser_link";
+	double rate = 1.0f;
+	std::string databasePath = "";
+	bool publishTf = true;
+	int startId = 0;
+	bool useDbStamps = true;
+
+	pnh.param("frame_id", frameId, frameId);
+	pnh.param("odom_frame_id", odomFrameId, odomFrameId);
+	pnh.param("camera_frame_id", cameraFrameId, cameraFrameId);
+	pnh.param("scan_frame_id", scanFrameId, scanFrameId);
+	pnh.param("rate", rate, rate); // Ratio of the database stamps
+	pnh.param("database", databasePath, databasePath);
+	pnh.param("publish_tf", publishTf, publishTf);
+	pnh.param("start_id", startId, startId);
+
+	// A general 360 lidar with 0.5 deg increment
+	double scanAngleMin, scanAngleMax, scanAngleIncrement, scanRangeMin, scanRangeMax;
+	pnh.param<double>("scan_angle_min", scanAngleMin, -M_PI);
+	pnh.param<double>("scan_angle_max", scanAngleMax, M_PI);
+	pnh.param<double>("scan_angle_increment", scanAngleIncrement, M_PI / 720.0);
+	pnh.param<double>("scan_range_min", scanRangeMin, 0.0);
+	pnh.param<double>("scan_range_max", scanRangeMax, 60);
+
+	ROS_INFO("frame_id = %s", frameId.c_str());
+	ROS_INFO("odom_frame_id = %s", odomFrameId.c_str());
+	ROS_INFO("camera_frame_id = %s", cameraFrameId.c_str());
+	ROS_INFO("scan_frame_id = %s", scanFrameId.c_str());
+	ROS_INFO("rate = %f", rate);
+	ROS_INFO("publish_tf = %s", publishTf?"true":"false");
+	ROS_INFO("start_id = %d", startId);
+	ROS_INFO("Publish clock (--clock): %s", publishClock?"true":"false");
+
+	if(databasePath.empty())
+	{
+		ROS_ERROR("Parameter \"database\" must be set (path to a RTAB-Map database).");
+		return -1;
+	}
+	databasePath = uReplaceChar(databasePath, '~', UDirectory::homeDir());
+	if(databasePath.size() && databasePath.at(0) != '/')
+	{
+		databasePath = UDirectory::currentDir(true) + databasePath;
+	}
+	ROS_INFO("database = %s", databasePath.c_str());
+
+	rtabmap::DBReader reader(databasePath, -rate, false, false, false, startId);
+	if(!reader.init())
+	{
+		ROS_ERROR("Cannot open database \"%s\".", databasePath.c_str());
+		return -1;
+	}
+
+	ros::ServiceServer pauseSrv = pnh.advertiseService("pause", pauseCallback);
+	ros::ServiceServer resumeSrv = pnh.advertiseService("resume", resumeCallback);
+
+	image_transport::ImageTransport it(nh);
+	image_transport::Publisher imagePub;
+	image_transport::Publisher rgbPub;
+	image_transport::Publisher depthPub;
+	image_transport::Publisher leftPub;
+	image_transport::Publisher rightPub;
+	ros::Publisher rgbCamInfoPub;
+	ros::Publisher depthCamInfoPub;
+	ros::Publisher leftCamInfoPub;
+	ros::Publisher rightCamInfoPub;
+	ros::Publisher odometryPub;
+	ros::Publisher scanPub;
+	ros::Publisher scanCloudPub;
+	ros::Publisher globalPosePub;
+	ros::Publisher gpsFixPub;
+	ros::Publisher clockPub;
+	tf2_ros::TransformBroadcaster tfBroadcaster;
+
+	if(publishClock)
+	{
+		clockPub = nh.advertise<rosgraph_msgs::Clock>("/clock", 1);
+	}
+
+	UTimer timer;
+	rtabmap::CameraInfo cameraInfo;
+	rtabmap::SensorData data = reader.takeImage(&cameraInfo);
+	rtabmap::OdometryInfo odomInfo;
+	odomInfo.reg.covariance = cameraInfo.odomCovariance;
+	rtabmap::OdometryEvent odom(data, cameraInfo.odomPose, odomInfo);
+	double acquisitionTime = timer.ticks();
+	while(ros::ok() && odom.data().id())
+	{
+		ROS_INFO("Reading sensor data %d...", odom.data().id());
+
+		ros::Time time(odom.data().stamp());
+
+		if(publishClock)
+		{
+			rosgraph_msgs::Clock msg;
+			msg.clock = time;
+			clockPub.publish(msg);
+		}
+
+		sensor_msgs::CameraInfo camInfoA; //rgb or left
+		sensor_msgs::CameraInfo camInfoB; //depth or right
+
+		camInfoA.K.assign(0);
+		camInfoA.K[0] = camInfoA.K[4] = camInfoA.K[8] = 1;
+		camInfoA.R.assign(0);
+		camInfoA.R[0] = camInfoA.R[4] = camInfoA.R[8] = 1;
+		camInfoA.P.assign(0);
+		camInfoA.P[10] = 1;
+
+		camInfoA.header.frame_id = cameraFrameId;
+		camInfoA.header.stamp = time;
+
+		camInfoB = camInfoA;
+
+		int type = -1;
+		if(!odom.data().depthRaw().empty() && (odom.data().depthRaw().type() == CV_32FC1 || odom.data().depthRaw().type() == CV_16UC1))
+		{
+			if(odom.data().cameraModels().size() > 1)
+			{
+				ROS_WARN("Multi-cameras detected in database but this node cannot send multi-images yet...");
+			}
+			else
+			{
+				//depth
+				if(odom.data().cameraModels().size())
+				{
+					camInfoA.D.resize(5,0);
+
+					camInfoA.P[0] = odom.data().cameraModels()[0].fx();
+					camInfoA.K[0] = odom.data().cameraModels()[0].fx();
+					camInfoA.P[5] = odom.data().cameraModels()[0].fy();
+					camInfoA.K[4] = odom.data().cameraModels()[0].fy();
+					camInfoA.P[2] = odom.data().cameraModels()[0].cx();
+					camInfoA.K[2] = odom.data().cameraModels()[0].cx();
+					camInfoA.P[6] = odom.data().cameraModels()[0].cy();
+					camInfoA.K[5] = odom.data().cameraModels()[0].cy();
+
+					camInfoB = camInfoA;
+				}
+
+				type=0;
+
+				if(rgbPub.getTopic().empty()) rgbPub = it.advertise("rgb/image", 1);
+				if(depthPub.getTopic().empty()) depthPub = it.advertise("depth_registered/image", 1);
+				if(rgbCamInfoPub.getTopic().empty()) rgbCamInfoPub = nh.advertise<sensor_msgs::CameraInfo>("rgb/camera_info", 1);
+				if(depthCamInfoPub.getTopic().empty()) depthCamInfoPub = nh.advertise<sensor_msgs::CameraInfo>("depth_registered/camera_info", 1);
+			}
+		}
+		else if(!odom.data().rightRaw().empty() && odom.data().rightRaw().type() == CV_8U)
+		{
+			if(odom.data().stereoCameraModels().size() > 1)
+			{
+				ROS_WARN("Multi-cameras detected in database but this node cannot send multi-images yet...");
+			}
+			else
+			{
+				//stereo
+				if(odom.data().stereoCameraModels()[0].isValidForProjection())
+				{
+					camInfoA.D.resize(8,0);
+
+					camInfoA.P[0] = odom.data().stereoCameraModels()[0].left().fx();
+					camInfoA.K[0] = odom.data().stereoCameraModels()[0].left().fx();
+					camInfoA.P[5] = odom.data().stereoCameraModels()[0].left().fy();
+					camInfoA.K[4] = odom.data().stereoCameraModels()[0].left().fy();
+					camInfoA.P[2] = odom.data().stereoCameraModels()[0].left().cx();
+					camInfoA.K[2] = odom.data().stereoCameraModels()[0].left().cx();
+					camInfoA.P[6] = odom.data().stereoCameraModels()[0].left().cy();
+					camInfoA.K[5] = odom.data().stereoCameraModels()[0].left().cy();
+
+					camInfoB = camInfoA;
+					camInfoB.P[3] = odom.data().stereoCameraModels()[0].right().Tx(); // Right_Tx = -baseline*fx
+				}
+
+				type=1;
+
+				if(leftPub.getTopic().empty()) leftPub = it.advertise("left/image", 1);
+				if(rightPub.getTopic().empty()) rightPub = it.advertise("right/image", 1);
+				if(leftCamInfoPub.getTopic().empty()) leftCamInfoPub = nh.advertise<sensor_msgs::CameraInfo>("left/camera_info", 1);
+				if(rightCamInfoPub.getTopic().empty()) rightCamInfoPub = nh.advertise<sensor_msgs::CameraInfo>("right/camera_info", 1);
+			}
+
+		}
+		else
+		{
+			if(imagePub.getTopic().empty()) imagePub = it.advertise("image", 1);
+		}
+
+		camInfoA.height = odom.data().imageRaw().rows;
+		camInfoA.width = odom.data().imageRaw().cols;
+		camInfoB.height = odom.data().depthOrRightRaw().rows;
+		camInfoB.width = odom.data().depthOrRightRaw().cols;
+
+		if(!odom.data().laserScanRaw().isEmpty())
+		{
+			if(scanPub.getTopic().empty() && odom.data().laserScanRaw().is2d())
+			{
+				scanPub = nh.advertise<sensor_msgs::LaserScan>("scan", 1);
+				if(odom.data().laserScanRaw().angleIncrement() > 0.0f)
+				{
+					ROS_INFO("Scan will be published.");
+				}
+				else
+				{
+					ROS_INFO("Scan will be published with those parameters:");
+					ROS_INFO("  scan_angle_min=%f", scanAngleMin);
+					ROS_INFO("  scan_angle_max=%f", scanAngleMax);
+					ROS_INFO("  scan_angle_increment=%f", scanAngleIncrement);
+					ROS_INFO("  scan_range_min=%f", scanRangeMin);
+					ROS_INFO("  scan_range_max=%f", scanRangeMax);
+				}
+			}
+			else if(scanCloudPub.getTopic().empty())
+			{
+				scanCloudPub = nh.advertise<sensor_msgs::PointCloud2>("scan_cloud", 1);
+				ROS_INFO("Scan cloud will be published.");
+			}
+		}
+
+		if(!odom.data().globalPose().isNull() &&
+			odom.data().globalPoseCovariance().cols==6 &&
+			odom.data().globalPoseCovariance().rows==6)
+		{
+			if(globalPosePub.getTopic().empty())
+			{
+				globalPosePub = nh.advertise<geometry_msgs::PoseWithCovarianceStamped>("global_pose", 1);
+				ROS_INFO("Global pose will be published.");
+			}
+		}
+
+		if(odom.data().gps().stamp() > 0.0)
+		{
+			if(gpsFixPub.getTopic().empty())
+			{
+				gpsFixPub = nh.advertise<sensor_msgs::NavSatFix>("gps/fix", 1);
+				ROS_INFO("GPS will be published.");
+			}
+		}
+
+		// publish transforms first
+		if(publishTf)
+		{
+			rtabmap::Transform localTransform;
+			if(odom.data().cameraModels().size() == 1)
+			{
+				localTransform = odom.data().cameraModels()[0].localTransform();
+			}
+			else if(odom.data().stereoCameraModels().size() == 1)
+			{
+				localTransform = odom.data().stereoCameraModels()[0].left().localTransform();
+			}
+			if(!localTransform.isNull())
+			{
+				geometry_msgs::TransformStamped baseToCamera;
+				baseToCamera.child_frame_id = cameraFrameId;
+				baseToCamera.header.frame_id = frameId;
+				baseToCamera.header.stamp = time;
+				rtabmap_conversions::transformToGeometryMsg(localTransform, baseToCamera.transform);
+				tfBroadcaster.sendTransform(baseToCamera);
+			}
+
+			if(!odom.pose().isNull())
+			{
+				geometry_msgs::TransformStamped odomToBase;
+				odomToBase.child_frame_id = frameId;
+				odomToBase.header.frame_id = odomFrameId;
+				odomToBase.header.stamp = time;
+				rtabmap_conversions::transformToGeometryMsg(odom.pose(), odomToBase.transform);
+				tfBroadcaster.sendTransform(odomToBase);
+			}
+
+			if(!scanPub.getTopic().empty() || !scanCloudPub.getTopic().empty())
+			{
+				geometry_msgs::TransformStamped baseToLaserScan;
+				baseToLaserScan.child_frame_id = scanFrameId;
+				baseToLaserScan.header.frame_id = frameId;
+				baseToLaserScan.header.stamp = time;
+				rtabmap_conversions::transformToGeometryMsg(odom.data().laserScanCompressed().localTransform(), baseToLaserScan.transform);
+				tfBroadcaster.sendTransform(baseToLaserScan);
+			}
+		}
+		if(!odom.pose().isNull())
+		{
+			if(odometryPub.getTopic().empty()) odometryPub = nh.advertise<nav_msgs::Odometry>("odom", 1);
+
+			if(odometryPub.getNumSubscribers())
+			{
+				nav_msgs::Odometry odomMsg;
+				odomMsg.child_frame_id = frameId;
+				odomMsg.header.frame_id = odomFrameId;
+				odomMsg.header.stamp = time;
+				rtabmap_conversions::transformToPoseMsg(odom.pose(), odomMsg.pose.pose);
+				UASSERT(odomMsg.pose.covariance.size() == 36 &&
+						odom.covariance().total() == 36 &&
+						odom.covariance().type() == CV_64FC1);
+				memcpy(odomMsg.pose.covariance.begin(), odom.covariance().data, 36*sizeof(double));
+				odometryPub.publish(odomMsg);
+			}
+		}
+
+		// Publish async topics first (so that they can catched by rtabmap before the image topics)
+		if(globalPosePub.getNumSubscribers() > 0 &&
+			!odom.data().globalPose().isNull() &&
+			odom.data().globalPoseCovariance().cols==6 &&
+			odom.data().globalPoseCovariance().rows==6)
+		{
+			geometry_msgs::PoseWithCovarianceStamped msg;
+			rtabmap_conversions::transformToPoseMsg(odom.data().globalPose(), msg.pose.pose);
+			memcpy(msg.pose.covariance.data(), odom.data().globalPoseCovariance().data, 36*sizeof(double));
+			msg.header.frame_id = frameId;
+			msg.header.stamp = time;
+			globalPosePub.publish(msg);
+		}
+
+		if(odom.data().gps().stamp() > 0.0)
+		{
+			sensor_msgs::NavSatFix msg;
+			msg.longitude = odom.data().gps().longitude();
+			msg.latitude = odom.data().gps().latitude();
+			msg.altitude = odom.data().gps().altitude();
+			msg.position_covariance_type = sensor_msgs::NavSatFix::COVARIANCE_TYPE_DIAGONAL_KNOWN;
+			msg.position_covariance.at(0) = msg.position_covariance.at(4) = msg.position_covariance.at(8)= odom.data().gps().error()* odom.data().gps().error();
+			msg.header.frame_id = frameId;
+			msg.header.stamp.fromSec(odom.data().gps().stamp());
+			gpsFixPub.publish(msg);
+		}
+
+		if(type >= 0)
+		{
+			if(rgbCamInfoPub.getNumSubscribers() && type == 0)
+			{
+				rgbCamInfoPub.publish(camInfoA);
+			}
+			if(leftCamInfoPub.getNumSubscribers() && type == 1)
+			{
+				leftCamInfoPub.publish(camInfoA);
+			}
+			if(depthCamInfoPub.getNumSubscribers() && type == 0)
+			{
+				depthCamInfoPub.publish(camInfoB);
+			}
+			if(rightCamInfoPub.getNumSubscribers() && type == 1)
+			{
+				rightCamInfoPub.publish(camInfoB);
+			}
+		}
+
+		if(imagePub.getNumSubscribers() || rgbPub.getNumSubscribers() || leftPub.getNumSubscribers())
+		{
+			cv_bridge::CvImage img;
+			if(odom.data().imageRaw().channels() == 1)
+			{
+				img.encoding = sensor_msgs::image_encodings::MONO8;
+			}
+			else
+			{
+				img.encoding = sensor_msgs::image_encodings::BGR8;
+			}
+			img.image = odom.data().imageRaw();
+			sensor_msgs::ImagePtr imageRosMsg = img.toImageMsg();
+			imageRosMsg->header.frame_id = cameraFrameId;
+			imageRosMsg->header.stamp = time;
+
+			if(imagePub.getNumSubscribers())
+			{
+				imagePub.publish(imageRosMsg);
+			}
+			if(rgbPub.getNumSubscribers() && type == 0)
+			{
+				rgbPub.publish(imageRosMsg);
+			}
+			if(leftPub.getNumSubscribers() && type == 1)
+			{
+				leftPub.publish(imageRosMsg);
+				leftCamInfoPub.publish(camInfoA);
+			}
+		}
+
+		if(depthPub.getNumSubscribers() && !odom.data().depthRaw().empty() && type==0)
+		{
+			cv_bridge::CvImage img;
+			if(odom.data().depthRaw().type() == CV_32FC1)
+			{
+				img.encoding = sensor_msgs::image_encodings::TYPE_32FC1;
+			}
+			else
+			{
+				img.encoding = sensor_msgs::image_encodings::TYPE_16UC1;
+			}
+			img.image = odom.data().depthRaw();
+			sensor_msgs::ImagePtr imageRosMsg = img.toImageMsg();
+			imageRosMsg->header.frame_id = cameraFrameId;
+			imageRosMsg->header.stamp = time;
+
+			depthPub.publish(imageRosMsg);
+			depthCamInfoPub.publish(camInfoB);
+		}
+
+		if(rightPub.getNumSubscribers() && !odom.data().rightRaw().empty() && type==1)
+		{
+			cv_bridge::CvImage img;
+			img.encoding = sensor_msgs::image_encodings::MONO8;
+			img.image = odom.data().rightRaw();
+			sensor_msgs::ImagePtr imageRosMsg = img.toImageMsg();
+			imageRosMsg->header.frame_id = cameraFrameId;
+			imageRosMsg->header.stamp = time;
+
+			rightPub.publish(imageRosMsg);
+			rightCamInfoPub.publish(camInfoB);
+		}
+
+		if(!odom.data().laserScanRaw().isEmpty())
+		{
+			if(scanPub.getNumSubscribers() && odom.data().laserScanRaw().is2d())
+			{
+				//inspired from pointcloud_to_laserscan package
+				sensor_msgs::LaserScan msg;
+				msg.header.frame_id = scanFrameId;
+				msg.header.stamp = time;
+
+				msg.angle_min = scanAngleMin;
+				msg.angle_max = scanAngleMax;
+				msg.angle_increment = scanAngleIncrement;
+				msg.time_increment = 0.0;
+				msg.scan_time = 0;
+				msg.range_min = scanRangeMin;
+				msg.range_max = scanRangeMax;
+				if(odom.data().laserScanRaw().angleIncrement() > 0.0f)
+				{
+					msg.angle_min = odom.data().laserScanRaw().angleMin();
+					msg.angle_max = odom.data().laserScanRaw().angleMax();
+					msg.angle_increment = odom.data().laserScanRaw().angleIncrement();
+					msg.range_min = odom.data().laserScanRaw().rangeMin();
+					msg.range_max = odom.data().laserScanRaw().rangeMax();
+				}
+
+				uint32_t rangesSize = std::ceil((msg.angle_max - msg.angle_min) / msg.angle_increment);
+				msg.ranges.assign(rangesSize, 0.0);
+
+				const cv::Mat & scan = odom.data().laserScanRaw().data();
+				for (int i=0; i<scan.cols; ++i)
+				{
+					const float * ptr = scan.ptr<float>(0,i);
+					double range = hypot(ptr[0], ptr[1]);
+					if (range >= msg.range_min && range <=msg.range_max)
+					{
+						double angle = atan2(ptr[1], ptr[0]);
+						if (angle >= msg.angle_min && angle <= msg.angle_max)
+						{
+							int index = (angle - msg.angle_min) / msg.angle_increment;
+							if (index>=0 && index<rangesSize && (range < msg.ranges[index] || msg.ranges[index]==0))
+							{
+								msg.ranges[index] = range;
+							}
+						}
+					}
+				}
+
+				scanPub.publish(msg);
+			}
+			else if(scanCloudPub.getNumSubscribers())
+			{
+				sensor_msgs::PointCloud2 msg;
+				pcl_conversions::moveFromPCL(*rtabmap::util3d::laserScanToPointCloud2(odom.data().laserScanRaw()), msg);
+				msg.header.frame_id = scanFrameId;
+				msg.header.stamp = time;
+				scanCloudPub.publish(msg);
+			}
+		}
+
+		if(odom.data().userDataRaw().type() == CV_8SC1 &&
+		   odom.data().userDataRaw().cols >= 7 && // including null str ending
+		   odom.data().userDataRaw().rows == 1 &&
+		   memcmp(odom.data().userDataRaw().data, "GOAL:", 5) == 0)
+		{
+			//GOAL format detected, remove it from the user data and send it as goal event
+			std::string goalStr = (const char *)odom.data().userDataRaw().data;
+			if(!goalStr.empty())
+			{
+				std::list<std::string> strs = uSplit(goalStr, ':');
+				if(strs.size() == 2)
+				{
+					int goalId = atoi(strs.rbegin()->c_str());
+
+					if(goalId > 0)
+					{
+						ROS_WARN("Goal %d detected, calling rtabmap's set_goal service!", goalId);
+						rtabmap_msgs::SetGoal setGoalSrv;
+						setGoalSrv.request.node_id = goalId;
+						setGoalSrv.request.node_label = "";
+						if(!ros::service::call("set_goal", setGoalSrv))
+						{
+							ROS_ERROR("Can't call \"set_goal\" service");
+						}
+					}
+				}
+			}
+		}
+
+		ros::spinOnce();
+
+		while(ros::ok())
+		{
+#ifndef _WIN32
+			if (spacehit()) {
+				paused = !paused;
+				if(paused)
+				{
+					ROS_INFO("paused!");
+				}
+				else
+				{
+					ROS_INFO("resumed!");
+				}
+			}
+#endif
+
+			if(!paused)
+			{
+				break;
+			}
+
+			uSleep(100);
+			ros::spinOnce();
+		}
+
+		timer.restart();
+		cameraInfo = rtabmap::CameraInfo();
+		data = reader.takeImage(&cameraInfo);
+		odomInfo.reg.covariance = cameraInfo.odomCovariance;
+		odom = rtabmap::OdometryEvent(data, cameraInfo.odomPose, odomInfo);
+		acquisitionTime = timer.ticks();
+	}
+
+
+	return 0;
+}

rtabmap_util/src/ImuToTFNode.cpp

@@ -0,0 +1,48 @@
+/*
+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 "ros/ros.h"
+#include "nodelet/loader.h"
+
+int main(int argc, char **argv)
+{
+	ros::init(argc, argv, "imu_to_tf");
+
+	nodelet::V_string nargv;
+	for(int i=1;i<argc;++i)
+	{
+		nargv.push_back(argv[i]);
+	}
+
+	nodelet::Loader nodelet;
+	nodelet::M_string remap(ros::names::getRemappings());
+	std::string nodelet_name = ros::this_node::getName();
+	nodelet.load(nodelet_name, "rtabmap_util/imu_to_tf", remap, nargv);
+	ros::spin();
+	return 0;
+}
+

rtabmap_util/src/LidarDeskewingNode.cpp

@@ -0,0 +1,37 @@
+/*
+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/lidar_deskewing.hpp"
+#include "rclcpp/rclcpp.hpp"
+
+int main(int argc, char **argv)
+{
+	rclcpp::init(argc, argv);
+	rclcpp::spin(std::make_shared<rtabmap_util::LidarDeskewing>(rclcpp::NodeOptions()));
+	rclcpp::shutdown();
+	return 0;
+}

rtabmap_util/src/MapAssemblerNode.cpp

@@ -0,0 +1,409 @@
+/*
+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 <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/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
+{
+
+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].image.size() ||
+			   msg.nodes[i].depth.size() ||
+			   msg.nodes[i].laserScan.size())
+			{
+				Signature data = rtabmap_conversions::nodeDataFromROS(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");
+
+	// process "--params" argument
+	for(int i=1;i<argc;++i)
+	{
+		if(strcmp(argv[i], "--params") == 0)
+		{
+			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())));
+			for(rtabmap::ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
+			{
+				std::string str = "Param: " + iter->first + " = \"" + iter->second + "\"";
+				std::cout <<
+						str <<
+						std::setw(60 - str.size()) <<
+						" [" <<
+						rtabmap::Parameters::getDescription(iter->first).c_str() <<
+						"]" <<
+						std::endl;
+			}
+			ROS_WARN("Node will now exit after showing default parameters because "
+					 "argument \"--params\" is detected!");
+			exit(0);
+		}
+		else if(strcmp(argv[i], "--udebug") == 0)
+		{
+			ULogger::setLevel(ULogger::kDebug);
+		}
+		else if(strcmp(argv[i], "--uinfo") == 0)
+		{
+			ULogger::setLevel(ULogger::kInfo);
+		}
+	}
+
+	MapAssembler assembler(argc, argv);
+	ros::spin();
+	return 0;
+}

rtabmap_util/src/MapOptimizerNode.cpp

@@ -0,0 +1,345 @@
+/*
+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 <ros/ros.h>
+#include "rtabmap_msgs/MapData.h"
+#include "rtabmap_msgs/MapGraph.h"
+#include "rtabmap_conversions/MsgConversion.h"
+#include <rtabmap/core/util3d.h>
+#include <rtabmap/core/Graph.h>
+#include <rtabmap/core/Optimizer.h>
+#include <rtabmap/core/Parameters.h>
+#include <rtabmap/utilite/ULogger.h>
+#include <rtabmap/utilite/UTimer.h>
+#include <rtabmap/utilite/UConversion.h>
+#include <ros/subscriber.h>
+#include <ros/publisher.h>
+#include <tf2_ros/transform_broadcaster.h>
+#include <boost/thread.hpp>
+
+using namespace rtabmap;
+
+class MapOptimizer
+{
+
+public:
+	MapOptimizer() :
+		mapFrameId_("map"),
+		odomFrameId_("odom"),
+		globalOptimization_(true),
+		optimizeFromLastNode_(false),
+		mapToOdom_(rtabmap::Transform::getIdentity()),
+		transformThread_(0)
+	{
+		ros::NodeHandle nh;
+		ros::NodeHandle pnh("~");
+
+		double epsilon = 0.0;
+		bool robust = true;
+		bool slam2d =false;
+		int strategy = 0; // 0=TORO, 1=g2o, 2=GTSAM
+		int iterations = 100;
+		bool ignoreVariance = false;
+
+		pnh.param("map_frame_id", mapFrameId_, mapFrameId_);
+		pnh.param("odom_frame_id", odomFrameId_, odomFrameId_);
+		pnh.param("iterations", iterations, iterations);
+		pnh.param("ignore_variance", ignoreVariance, ignoreVariance);
+		pnh.param("global_optimization", globalOptimization_, globalOptimization_);
+		pnh.param("optimize_from_last_node", optimizeFromLastNode_, optimizeFromLastNode_);
+		pnh.param("epsilon", epsilon, epsilon);
+		pnh.param("robust", robust, robust);
+		pnh.param("slam_2d", slam2d, slam2d);
+		pnh.param("strategy", strategy, strategy);
+
+
+		UASSERT(iterations > 0);
+
+		ParametersMap parameters;
+		parameters.insert(ParametersPair(Parameters::kOptimizerStrategy(), uNumber2Str(strategy)));
+		parameters.insert(ParametersPair(Parameters::kOptimizerEpsilon(), uNumber2Str(epsilon)));
+		parameters.insert(ParametersPair(Parameters::kOptimizerIterations(), uNumber2Str(iterations)));
+		parameters.insert(ParametersPair(Parameters::kOptimizerRobust(), uBool2Str(robust)));
+		parameters.insert(ParametersPair(Parameters::kRegForce3DoF(), uBool2Str(slam2d)));
+		parameters.insert(ParametersPair(Parameters::kOptimizerVarianceIgnored(), uBool2Str(ignoreVariance)));
+		optimizer_ = Optimizer::create(parameters);
+
+		double tfDelay = 0.05; // 20 Hz
+		bool publishTf = true;
+		pnh.param("publish_tf", publishTf, publishTf);
+		pnh.param("tf_delay", tfDelay, tfDelay);
+
+		mapDataTopic_ = nh.subscribe("mapData", 1, &MapOptimizer::mapDataReceivedCallback, this);
+		mapDataPub_ = nh.advertise<rtabmap_msgs::MapData>(nh.resolveName("mapData")+"_optimized", 1);
+		mapGraphPub_ = nh.advertise<rtabmap_msgs::MapGraph>(nh.resolveName("mapData")+"Graph_optimized", 1);
+
+		if(publishTf)
+		{
+			ROS_INFO("map_optimizer will publish tf between frames \"%s\" and \"%s\"", mapFrameId_.c_str(), odomFrameId_.c_str());
+			ROS_INFO("map_optimizer: map_frame_id = %s", mapFrameId_.c_str());
+			ROS_INFO("map_optimizer: odom_frame_id = %s", odomFrameId_.c_str());
+			ROS_INFO("map_optimizer: tf_delay = %f", tfDelay);
+			transformThread_ = new boost::thread(std::bind(&MapOptimizer::publishLoop, this, tfDelay));
+		}
+	}
+
+	~MapOptimizer()
+	{
+		if(transformThread_)
+		{
+			transformThread_->join();
+			delete transformThread_;
+		}
+	}
+
+	void publishLoop(double tfDelay)
+	{
+		if(tfDelay == 0)
+			return;
+		ros::Rate r(1.0 / tfDelay);
+		while(ros::ok())
+		{
+			mapToOdomMutex_.lock();
+			ros::Time tfExpiration = ros::Time::now() + ros::Duration(tfDelay);
+			geometry_msgs::TransformStamped msg;
+			msg.child_frame_id = odomFrameId_;
+			msg.header.frame_id = mapFrameId_;
+			msg.header.stamp = tfExpiration;
+			rtabmap_conversions::transformToGeometryMsg(mapToOdom_, msg.transform);
+			tfBroadcaster_.sendTransform(msg);
+			mapToOdomMutex_.unlock();
+			r.sleep();
+		}
+	}
+
+	void mapDataReceivedCallback(const rtabmap_msgs::MapDataConstPtr & msg)
+	{
+		// save new poses and constraints
+		// Assuming that nodes/constraints are all linked together
+		UASSERT(msg->graph.posesId.size() == msg->graph.poses.size());
+
+		bool dataChanged = false;
+
+		std::multimap<int, Link> newConstraints;
+		for(unsigned int i=0; i<msg->graph.links.size(); ++i)
+		{
+			Link link = rtabmap_conversions::linkFromROS(msg->graph.links[i]);
+			newConstraints.insert(std::make_pair(link.from(), link));
+
+			bool edgeAlreadyAdded = false;
+			for(std::multimap<int, Link>::iterator iter = cachedConstraints_.lower_bound(link.from());
+					iter != cachedConstraints_.end() && iter->first == link.from();
+					++iter)
+			{
+				if(iter->second.to() == link.to())
+				{
+					edgeAlreadyAdded = true;
+					if(iter->second.transform().getDistanceSquared(link.transform()) > 0.0001)
+					{
+						ROS_WARN("%d ->%d (%s vs %s)",iter->second.from(), iter->second.to(), iter->second.transform().prettyPrint().c_str(),
+								link.transform().prettyPrint().c_str());
+						dataChanged = true;
+					}
+				}
+			}
+			if(!edgeAlreadyAdded)
+			{
+				cachedConstraints_.insert(std::make_pair(link.from(), link));
+			}
+		}
+
+		std::map<int, Signature> newNodeInfos;
+		// add new odometry poses
+		for(unsigned int i=0; i<msg->nodes.size(); ++i)
+		{
+			int id = msg->nodes[i].id;
+			Transform pose = rtabmap_conversions::transformFromPoseMsg(msg->nodes[i].pose);
+			Signature s = rtabmap_conversions::nodeInfoFromROS(msg->nodes[i]);
+			newNodeInfos.insert(std::make_pair(id, s));
+
+			std::pair<std::map<int, Signature>::iterator, bool> p = cachedNodeInfos_.insert(std::make_pair(id, s));
+			if(!p.second && pose.getDistanceSquared(cachedNodeInfos_.at(id).getPose()) > 0.0001)
+			{
+				dataChanged = true;
+			}
+		}
+
+		if(dataChanged)
+		{
+			ROS_WARN("Graph data has changed! Reset cache...");
+			cachedConstraints_ = newConstraints;
+			cachedNodeInfos_ = newNodeInfos;
+		}
+
+		//match poses in the graph
+		std::multimap<int, Link> constraints;
+		std::map<int, Signature> nodeInfos;
+		if(globalOptimization_)
+		{
+			constraints = cachedConstraints_;
+			nodeInfos = cachedNodeInfos_;
+		}
+		else
+		{
+			constraints = newConstraints;
+			for(unsigned int i=0; i<msg->graph.posesId.size(); ++i)
+			{
+				std::map<int, Signature>::iterator iter = cachedNodeInfos_.find(msg->graph.posesId[i]);
+				if(iter != cachedNodeInfos_.end())
+				{
+					nodeInfos.insert(*iter);
+				}
+				else
+				{
+					ROS_ERROR("Odometry pose of node %d not found in cache!", msg->graph.posesId[i]);
+					return;
+				}
+			}
+		}
+
+		std::map<int, Transform> poses;
+		for(std::map<int, Signature>::iterator iter=nodeInfos.begin(); iter!=nodeInfos.end(); ++iter)
+		{
+			poses.insert(std::make_pair(iter->first, iter->second.getPose()));
+		}
+
+		// Optimize only if there is a subscriber
+		if(mapDataPub_.getNumSubscribers() || mapGraphPub_.getNumSubscribers())
+		{
+			UTimer timer;
+			std::map<int, Transform> optimizedPoses;
+			Transform mapCorrection = Transform::getIdentity();
+			std::map<int, rtabmap::Transform> posesOut;
+			std::multimap<int, rtabmap::Link> linksOut;
+			if(poses.size() > 1 && constraints.size() > 0)
+			{
+				int fromId = optimizeFromLastNode_?poses.rbegin()->first:poses.begin()->first;
+				optimizer_->getConnectedGraph(
+						fromId,
+						poses,
+						constraints,
+						posesOut,
+						linksOut);
+				optimizedPoses = optimizer_->optimize(fromId, posesOut, linksOut);
+				mapToOdomMutex_.lock();
+				mapCorrection = optimizedPoses.at(posesOut.rbegin()->first) * posesOut.rbegin()->second.inverse();
+				mapToOdom_ = mapCorrection;
+				mapToOdomMutex_.unlock();
+			}
+			else if(poses.size() == 1 && constraints.size() == 0)
+			{
+				optimizedPoses = poses;
+			}
+			else if(poses.size() == 0 && constraints.size())
+			{
+				ROS_ERROR("map_optimizer: Poses=%d and edges=%d: poses must "
+					   "not be null if there are edges.",
+					  (int)poses.size(), (int)constraints.size());
+			}
+
+			rtabmap_msgs::MapData outputDataMsg;
+			rtabmap_msgs::MapGraph outputGraphMsg;
+			rtabmap_conversions::mapGraphToROS(optimizedPoses,
+					linksOut,
+					mapCorrection,
+					outputGraphMsg);
+
+			if(mapGraphPub_.getNumSubscribers())
+			{
+				outputGraphMsg.header = msg->header;
+				mapGraphPub_.publish(outputGraphMsg);
+			}
+
+			if(mapDataPub_.getNumSubscribers())
+			{
+				outputDataMsg.header = msg->header;
+				outputDataMsg.graph = outputGraphMsg;
+				outputDataMsg.nodes = msg->nodes;
+				if(posesOut.size() > msg->nodes.size())
+				{
+					std::set<int> addedNodes;
+					for(unsigned int i=0; i<msg->nodes.size(); ++i)
+					{
+						addedNodes.insert(msg->nodes[i].id);
+					}
+					std::list<int> toAdd;
+					for(std::map<int, Transform>::iterator iter=posesOut.begin(); iter!=posesOut.end(); ++iter)
+					{
+						if(addedNodes.find(iter->first) == addedNodes.end())
+						{
+							toAdd.push_back(iter->first);
+						}
+					}
+					if(toAdd.size())
+					{
+						int oi = outputDataMsg.nodes.size();
+						outputDataMsg.nodes.resize(outputDataMsg.nodes.size()+toAdd.size());
+						for(std::list<int>::iterator iter=toAdd.begin(); iter!=toAdd.end(); ++iter)
+						{
+							UASSERT(cachedNodeInfos_.find(*iter) != cachedNodeInfos_.end());
+							rtabmap_conversions::nodeDataToROS(cachedNodeInfos_.at(*iter), outputDataMsg.nodes[oi]);
+							++oi;
+						}
+					}
+				}
+				mapDataPub_.publish(outputDataMsg);
+			}
+
+			ROS_INFO("Time graph optimization = %f s", timer.ticks());
+		}
+	}
+
+private:
+	std::string mapFrameId_;
+	std::string odomFrameId_;
+	bool globalOptimization_;
+	bool optimizeFromLastNode_;
+	Optimizer * optimizer_;
+
+	rtabmap::Transform mapToOdom_;
+	boost::mutex mapToOdomMutex_;
+
+	ros::Subscriber mapDataTopic_;
+
+	ros::Publisher mapDataPub_;
+	ros::Publisher mapGraphPub_;
+
+	std::multimap<int, Link> cachedConstraints_;
+	std::map<int, Signature> cachedNodeInfos_;
+
+	tf2_ros::TransformBroadcaster tfBroadcaster_;
+	boost::thread* transformThread_;
+};
+
+
+int main(int argc, char** argv)
+{
+	ros::init(argc, argv, "map_optimizer");
+	MapOptimizer optimizer;
+	ros::spin();
+	return 0;
+}

rtabmap_util/src/ObstaclesDetectionNode.cpp

@@ -0,0 +1,38 @@
+/*
+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/obstacles_detection.hpp"
+
+int main(int argc, char **argv)
+{
+	rclcpp::init(argc, argv);
+	rclcpp::spin(std::make_shared<rtabmap_util::ObstaclesDetection>(rclcpp::NodeOptions()));
+	rclcpp::shutdown();
+	return 0;
+}
+
+

rtabmap_util/src/OdomMsgToTFNode.cpp

@@ -0,0 +1,92 @@
+/*
+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 <ros/ros.h>
+#include <nav_msgs/Odometry.h>
+#include <tf2_ros/transform_broadcaster.h>
+#include <rtabmap_conversions/MsgConversion.h>
+
+class OdomMsgToTF
+{
+
+public:
+	OdomMsgToTF() :
+		frameId_(""),
+		odomFrameId_("")
+	{
+		ros::NodeHandle pnh("~");
+		pnh.param("frame_id", frameId_, frameId_);
+		pnh.param("odom_frame_id", odomFrameId_, odomFrameId_);
+
+		ros::NodeHandle nh;
+		odomTopic_ = nh.subscribe("odom", 1, &OdomMsgToTF::odomReceivedCallback, this);
+	}
+
+	virtual ~OdomMsgToTF(){}
+
+	void odomReceivedCallback(const nav_msgs::OdometryConstPtr & msg)
+	{
+		if(frameId_.empty())
+		{
+			frameId_ = msg->child_frame_id;
+		}
+		if(odomFrameId_.empty())
+		{
+			odomFrameId_ = msg->header.frame_id;
+		}
+		geometry_msgs::TransformStamped t;
+		rtabmap::Transform pose = rtabmap_conversions::transformFromPoseMsg(msg->pose.pose);
+		if(pose.isNull())
+		{
+			ROS_WARN("Odometry received is null! Cannot send tf...");
+		}
+		else
+		{
+			t.child_frame_id = frameId_;
+			t.header.frame_id = odomFrameId_;
+			t.header.stamp = msg->header.stamp;
+			rtabmap_conversions::transformToGeometryMsg(pose, t.transform);
+			tfBroadcaster_.sendTransform(t);
+		}
+	}
+
+private:
+	std::string frameId_;
+	std::string odomFrameId_;
+
+	ros::Subscriber odomTopic_;
+	tf2_ros::TransformBroadcaster tfBroadcaster_;
+};
+
+
+int main(int argc, char** argv)
+{
+	ros::init(argc, argv, "odom_msg_to_tf");
+	OdomMsgToTF odomToTf;
+	ros::spin();
+	return 0;
+}

rtabmap_util/src/PointCloudAggregatorNode.cpp

@@ -0,0 +1,37 @@
+/*
+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/point_cloud_aggregator.hpp"
+
+int main(int argc, char **argv)
+{
+	rclcpp::init(argc, argv);
+	rclcpp::spin(std::make_shared<rtabmap_util::PointCloudAggregator>(rclcpp::NodeOptions()));
+	rclcpp::shutdown();
+	return 0;
+}
+

rtabmap_util/src/PointCloudAssemblerNode.cpp

@@ -0,0 +1,38 @@
+/*
+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/point_cloud_assembler.hpp"
+
+int main(int argc, char **argv)
+{
+	rclcpp::init(argc, argv);
+	rclcpp::spin(std::make_shared<rtabmap_util::PointCloudAssembler>(rclcpp::NodeOptions()));
+	rclcpp::shutdown();
+	return 0;
+}
+
+

rtabmap_util/src/PointCloudToDepthImageNode.cpp

@@ -0,0 +1,38 @@
+/*
+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/pointcloud_to_depthimage.hpp"
+
+int main(int argc, char **argv)
+{
+	rclcpp::init(argc, argv);
+	rclcpp::spin(std::make_shared<rtabmap_util::PointCloudToDepthImage>(rclcpp::NodeOptions()));
+	rclcpp::shutdown();
+	return 0;
+}
+
+

rtabmap_util/src/PointCloudXYZNode.cpp

@@ -0,0 +1,38 @@
+/*
+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/point_cloud_xyz.hpp"
+
+int main(int argc, char **argv)
+{
+	rclcpp::init(argc, argv);
+	rclcpp::spin(std::make_shared<rtabmap_util::PointCloudXYZ>(rclcpp::NodeOptions()));
+	rclcpp::shutdown();
+	return 0;
+}
+
+

rtabmap_util/src/PointCloudXYZRGBNode.cpp

@@ -0,0 +1,38 @@
+/*
+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/point_cloud_xyzrgb.hpp"
+
+int main(int argc, char **argv)
+{
+	rclcpp::init(argc, argv);
+	rclcpp::spin(std::make_shared<rtabmap_util::PointCloudXYZRGB>(rclcpp::NodeOptions()));
+	rclcpp::shutdown();
+	return 0;
+}
+
+

rtabmap_util/src/RGBDRelayNode.cpp

@@ -0,0 +1,38 @@
+/*
+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 <memory>
+#include "rtabmap_util/rgbd_relay.hpp"
+#include "rclcpp/rclcpp.hpp"
+
+int main(int argc, char **argv)
+{
+	rclcpp::init(argc, argv);
+	rclcpp::spin(std::make_shared<rtabmap_util::RGBDRelay>(rclcpp::NodeOptions()));
+	rclcpp::shutdown();
+	return 0;
+}

rtabmap_util/src/RGBDSplitNode.cpp

@@ -0,0 +1,37 @@
+/*
+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 <memory>
+#include "rtabmap_util/rgbd_split.hpp"
+#include "rclcpp/rclcpp.hpp"
+
+int main(int argc, char **argv)
+{
+	rclcpp::init(argc, argv);
+	rclcpp::spin(std::make_shared<rtabmap_util::RGBDSplit>(rclcpp::NodeOptions()));
+	rclcpp::shutdown();
+}

rtabmap_util/src/nodelets/disparity_to_depth.cpp

@@ -0,0 +1,140 @@
+/*
+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 <ros/ros.h>
+#include <pluginlib/class_list_macros.hpp>
+#include <nodelet/nodelet.h>
+
+#include <sensor_msgs/Image.h>
+#include <sensor_msgs/image_encodings.h>
+#include <stereo_msgs/DisparityImage.h>
+
+#include <image_transport/image_transport.h>
+
+#include <cv_bridge/cv_bridge.h>
+
+namespace rtabmap_util
+{
+
+class DisparityToDepth : public nodelet::Nodelet
+{
+public:
+	DisparityToDepth() {}
+
+	virtual ~DisparityToDepth(){}
+
+private:
+	virtual void onInit()
+	{
+		ros::NodeHandle & nh = getNodeHandle();
+		ros::NodeHandle & pnh = getPrivateNodeHandle();
+
+		image_transport::ImageTransport it(nh);
+		pub32f_ = it.advertise("depth", 1);
+		pub16u_ = it.advertise("depth_raw", 1);
+		sub_ = nh.subscribe("disparity", 1, &DisparityToDepth::callback, this);
+	}
+
+	void callback(const stereo_msgs::DisparityImageConstPtr& disparityMsg)
+	{
+		if(disparityMsg->image.encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1) !=0)
+		{
+			NODELET_ERROR("Input type must be disparity=32FC1");
+			return;
+		}
+
+		bool publish32f = pub32f_.getNumSubscribers();
+		bool publish16u = pub16u_.getNumSubscribers();
+
+		if(publish32f || publish16u)
+		{
+			// sensor_msgs::image_encodings::TYPE_32FC1
+			cv::Mat disparity(disparityMsg->image.height, disparityMsg->image.width, CV_32FC1, const_cast<uchar*>(disparityMsg->image.data.data()));
+
+			cv::Mat depth32f;
+			cv::Mat depth16u;
+			if(publish32f)
+			{
+				depth32f = cv::Mat::zeros(disparity.rows, disparity.cols, CV_32F);
+			}
+			if(publish16u)
+			{
+				depth16u = cv::Mat::zeros(disparity.rows, disparity.cols, CV_16U);
+			}
+			for (int i = 0; i < disparity.rows; i++)
+			{
+				for (int j = 0; j < disparity.cols; j++)
+				{
+					float disparity_value = disparity.at<float>(i,j);
+					if (disparity_value > disparityMsg->min_disparity && disparity_value < disparityMsg->max_disparity)
+					{
+						// baseline * focal / disparity
+						float depth = disparityMsg->T * disparityMsg->f / disparity_value;
+						if(publish32f)
+						{
+							depth32f.at<float>(i,j) = depth;
+						}
+						if(publish16u)
+						{
+							depth16u.at<unsigned short>(i,j) = (unsigned short)(depth*1000.0f);
+						}
+					}
+				}
+			}
+
+			if(publish32f)
+			{
+				// convert to ROS sensor_msg::Image
+				cv_bridge::CvImage cvDepth(disparityMsg->header, sensor_msgs::image_encodings::TYPE_32FC1, depth32f);
+				sensor_msgs::Image depthMsg;
+				cvDepth.toImageMsg(depthMsg);
+
+				//publish the message
+				pub32f_.publish(depthMsg);
+			}
+
+			if(publish16u)
+			{
+				// convert to ROS sensor_msg::Image
+				cv_bridge::CvImage cvDepth(disparityMsg->header, sensor_msgs::image_encodings::TYPE_16UC1, depth16u);
+				sensor_msgs::Image depthMsg;
+				cvDepth.toImageMsg(depthMsg);
+
+				//publish the message
+				pub16u_.publish(depthMsg);
+			}
+		}
+}
+
+private:
+	image_transport::Publisher pub32f_;
+	image_transport::Publisher pub16u_;
+	ros::Subscriber sub_;
+};
+
+PLUGINLIB_EXPORT_CLASS(rtabmap_util::DisparityToDepth, nodelet::Nodelet);
+}

rtabmap_util/src/nodelets/imu_to_tf.cpp

@@ -0,0 +1,122 @@
+/*
+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 <ros/ros.h>
+#include <pluginlib/class_list_macros.hpp>
+#include <nodelet/nodelet.h>
+#include <sensor_msgs/Imu.h>
+#include <tf/transform_broadcaster.h>
+#include <tf/LinearMath/Matrix3x3.h>
+#include <tf/transform_listener.h>
+
+namespace rtabmap_util
+{
+
+class ImuToTF : public nodelet::Nodelet
+{
+public:
+	ImuToTF() :
+		fixedFrameId_("odom"),
+		waitForTransformDuration_(0.1)
+	{}
+
+	virtual ~ImuToTF()
+	{
+	}
+
+private:
+	virtual void onInit()
+	{
+		ros::NodeHandle & nh = getNodeHandle();
+		ros::NodeHandle & pnh = getPrivateNodeHandle();
+
+		pnh.param("fixed_frame_id", fixedFrameId_, fixedFrameId_);
+		pnh.param("base_frame_id", baseFrameId_, baseFrameId_);
+		pnh.param("wait_for_transform_duration", waitForTransformDuration_, waitForTransformDuration_);
+		NODELET_INFO("fixed_frame_id: %s", fixedFrameId_.c_str());
+		NODELET_INFO("base_frame_id: %s", baseFrameId_.c_str());
+
+		sub_ = nh.subscribe<sensor_msgs::Imu>("imu/data", 1, &ImuToTF::imuCallback, this);
+	}
+
+	void imuCallback(const sensor_msgs::ImuConstPtr & msg)
+	{
+		tf::Quaternion q;
+		tf::quaternionMsgToTF(msg->orientation, q);
+		tf::StampedTransform st;
+		st.setRotation(q);
+
+		st.frame_id_ = fixedFrameId_;
+		st.stamp_ = msg->header.stamp;
+
+		if(!baseFrameId_.empty() &&
+			baseFrameId_.compare(msg->header.frame_id) != 0)
+		{
+			try
+			{
+				std::string errorMsg;
+				if(!tfListener_.waitForTransform(baseFrameId_, msg->header.frame_id, msg->header.stamp, ros::Duration(waitForTransformDuration_), ros::Duration(0.01), &errorMsg))
+				{
+					NODELET_ERROR("Could not get transform from %s to %s after %f seconds (for stamp=%f)! Error=\"%s\".",
+							baseFrameId_.c_str(), msg->header.frame_id.c_str(), 0.1, msg->header.stamp.toSec(), errorMsg.c_str());
+					return;
+				}
+
+				tf::StampedTransform tmp;
+				tfListener_.lookupTransform(msg->header.frame_id, baseFrameId_, msg->header.stamp, tmp);
+				tf::Transform t = tmp.inverse()*st*tmp;
+				st.setRotation(t.getRotation());
+				st.child_frame_id_ = baseFrameId_;
+			}
+			catch(tf::TransformException & ex)
+			{
+				NODELET_ERROR("(getting transform %s -> %s) %s", baseFrameId_.c_str(), msg->header.frame_id.c_str(), ex.what());
+				return;
+			}
+		}
+		else
+		{
+			st.child_frame_id_ = msg->header.frame_id;
+		}
+		st.setOrigin(tf::Vector3(0,0,0));
+
+		pub_.sendTransform(st);
+	}
+
+private:
+	ros::Subscriber sub_;
+	tf::TransformBroadcaster pub_;
+	std::string fixedFrameId_;
+	std::string baseFrameId_;
+	tf::TransformListener tfListener_;
+	double waitForTransformDuration_;
+};
+
+
+PLUGINLIB_EXPORT_CLASS(rtabmap_util::ImuToTF, nodelet::Nodelet);
+}
+

rtabmap_util/src/nodelets/lidar_deskewing.cpp

@@ -0,0 +1,106 @@
+
+#include <rtabmap_util/lidar_deskewing.hpp>
+
+#include <laser_geometry/laser_geometry.hpp>
+
+#include <rtabmap_conversions/MsgConversion.h>
+
+namespace rtabmap_util
+{
+
+LidarDeskewing::LidarDeskewing(const rclcpp::NodeOptions & options) :
+	Node("lidar_deskewing", options),
+	waitForTransformDuration_(0.01),
+	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;
+	queueSize = this->declare_parameter("queue_size", queueSize);
+	qos = this->declare_parameter("qos", qos);
+	fixedFrameId_ = this->declare_parameter("fixed_frame_id", fixedFrameId_);
+	waitForTransformDuration_ = this->declare_parameter("wait_for_transform", waitForTransformDuration_);
+	slerp_ = this->declare_parameter("slerp", slerp_);
+
+	RCLCPP_INFO(this->get_logger(), "  fixed_frame_id:  %s", fixedFrameId_.c_str());
+	RCLCPP_INFO(this->get_logger(), "  wait_for_transform:  %fs", waitForTransformDuration_);
+	RCLCPP_INFO(this->get_logger(), "  slerp:  %s", slerp_?"true":"false");
+
+	if(fixedFrameId_.empty())
+	{
+		RCLCPP_FATAL(this->get_logger(), "fixed_frame_id parameter cannot be empty!");
+	}
+
+	subScan_ = create_subscription<sensor_msgs::msg::LaserScan>("input_scan", rclcpp::QoS(queueSize).reliability((rmw_qos_reliability_policy_t)qos), std::bind(&LidarDeskewing::callbackScan, this, std::placeholders::_1));
+	subCloud_ = create_subscription<sensor_msgs::msg::PointCloud2>("input_cloud", rclcpp::QoS(queueSize).reliability((rmw_qos_reliability_policy_t)qos), std::bind(&LidarDeskewing::callbackCloud, this, std::placeholders::_1));
+
+	pubScan_ = create_publisher<sensor_msgs::msg::PointCloud2>(std::string(subScan_->get_topic_name()) + "/deskewed", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos));
+	pubCloud_ = create_publisher<sensor_msgs::msg::PointCloud2>(std::string(subCloud_->get_topic_name()) + "/deskewed", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos));
+}
+
+LidarDeskewing::~LidarDeskewing()
+{
+}
+
+void LidarDeskewing::callbackScan(const sensor_msgs::msg::LaserScan::ConstSharedPtr msg)
+{
+	// make sure the frame of the laser is updated during the whole scan time
+	rtabmap::Transform tmpT = rtabmap_conversions::getTransform(
+			msg->header.frame_id,
+			fixedFrameId_,
+			msg->header.stamp,
+			rclcpp::Time(msg->header.stamp.sec, msg->header.stamp.nanosec) + rclcpp::Duration::from_seconds(msg->ranges.size()*msg->time_increment),
+			*tfBuffer_,
+			waitForTransformDuration_);
+	if(tmpT.isNull())
+	{
+		return;
+	}
+
+	sensor_msgs::msg::PointCloud2 scanOut;
+	laser_geometry::LaserProjection projection;
+	projection.transformLaserScanToPointCloud(fixedFrameId_, *msg, scanOut, *tfBuffer_);
+
+	rtabmap::Transform t = rtabmap_conversions::getTransform(msg->header.frame_id, scanOut.header.frame_id, msg->header.stamp, *tfBuffer_, waitForTransformDuration_);
+	if(t.isNull())
+	{
+		RCLCPP_ERROR(this->get_logger(), "Cannot transform back projected scan from \"%s\" frame to \"%s\" frame at time %fs.",
+				scanOut.header.frame_id.c_str(), msg->header.frame_id.c_str(), rtabmap_conversions::timestampFromROS(msg->header.stamp));
+		return;
+	}
+
+	sensor_msgs::msg::PointCloud2 scanOutDeskewed;
+	rtabmap_conversions::transformPointCloud(t.toEigen4f(), scanOut, scanOutDeskewed);
+	pubScan_->publish(scanOutDeskewed);
+}
+
+void LidarDeskewing::callbackCloud(const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg)
+{
+	sensor_msgs::msg::PointCloud2 msgDeskewed;
+	if(rtabmap_conversions::deskew(*msg, msgDeskewed, fixedFrameId_, *tfBuffer_, waitForTransformDuration_, slerp_))
+	{
+		pubCloud_->publish(msgDeskewed);
+	}
+	else
+	{
+		// Just republish the msg to not breakdown downstream
+		// A warning should be already shown (see deskew() source code)
+		RCLCPP_WARN(this->get_logger(), "deskewing failed! returning possible skewed cloud!");
+		pubCloud_->publish(*msg);
+	}
+}
+
+}
+
+#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::LidarDeskewing)

rtabmap_util/src/nodelets/obstacles_detection.cpp

@@ -0,0 +1,270 @@
+/*
+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/obstacles_detection.hpp>
+
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+#include <pcl_conversions/pcl_conversions.h>
+#include <pcl/filters/filter.h>
+
+#include <rtabmap_conversions/MsgConversion.h>
+
+#include "rtabmap/utilite/UStl.h"
+
+namespace rtabmap_util
+{
+
+ObstaclesDetection::ObstaclesDetection(const rclcpp::NodeOptions & options) :
+	Node("obstacles_detection", options),
+	frameId_("base_link"),
+	waitForTransform_(0.2),
+	mapFrameProjection_(rtabmap::Parameters::defaultGridMapFrameProjection()),
+	warned_(false)
+{
+	ULogger::setType(ULogger::kTypeConsole);
+	ULogger::setLevel(ULogger::kWarning);
+
+	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;
+	qos = this->declare_parameter("qos", qos);
+
+	rtabmap::ParametersMap gridParameters = rtabmap::Parameters::getDefaultParameters("Grid");
+	for(rtabmap::ParametersMap::iterator iter=gridParameters.begin(); iter!=gridParameters.end(); ++iter)
+	{
+		std::string vStr = declare_parameter(iter->first, iter->second);
+		if(vStr.compare(iter->second) != 0)
+		{
+			RCLCPP_INFO(this->get_logger(), "obstacles_detection: Setting parameter \"%s\"=\"%s\"", iter->first.c_str(), vStr.c_str());
+			iter->second = vStr;
+		}
+	}
+
+	UASSERT(uContains(gridParameters, rtabmap::Parameters::kGridMapFrameProjection()));
+	mapFrameProjection_ = uStr2Bool(gridParameters.at(rtabmap::Parameters::kGridMapFrameProjection()));
+	if(mapFrameProjection_ && mapFrameId_.empty())
+	{
+		RCLCPP_ERROR(this->get_logger(), "obstacles_detection: Parameter \"%s\" is true but map_frame_id is not set!", rtabmap::Parameters::kGridMapFrameProjection().c_str());
+	}
+
+	grid_.parseParameters(gridParameters);
+
+	tfBuffer_ = std::make_shared< tf2_ros::Buffer >(this->get_clock());
+	tfListener_ = std::make_shared< tf2_ros::TransformListener >(*tfBuffer_);
+
+	groundPub_ = create_publisher<sensor_msgs::msg::PointCloud2>("ground", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos));
+	obstaclesPub_ = create_publisher<sensor_msgs::msg::PointCloud2>("obstacles", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos));
+	projObstaclesPub_ = create_publisher<sensor_msgs::msg::PointCloud2>("proj_obstacles", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos));
+
+	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));
+}
+
+
+
+void ObstaclesDetection::callback(const sensor_msgs::msg::PointCloud2::ConstSharedPtr cloudMsg)
+{
+	rclcpp::Time time = now();
+
+	if (groundPub_->get_subscription_count() == 0 && obstaclesPub_->get_subscription_count() == 0 && projObstaclesPub_->get_subscription_count() == 0)
+	{
+		// no one wants the results
+		return;
+	}
+
+	rtabmap::Transform localTransform = rtabmap::Transform::getIdentity();
+	localTransform = rtabmap_conversions::getTransform(frameId_, cloudMsg->header.frame_id, cloudMsg->header.stamp, *tfBuffer_, waitForTransform_);
+	if(localTransform.isNull())
+	{
+		RCLCPP_ERROR(this->get_logger(), "Failed to get transform between %s and %s frames", frameId_.c_str(), cloudMsg->header.frame_id.c_str());
+		return;
+	}
+
+	rtabmap::Transform pose = rtabmap::Transform::getIdentity();
+	if(!mapFrameId_.empty())
+	{
+		pose = rtabmap_conversions::getTransform(mapFrameId_, frameId_, cloudMsg->header.stamp, *tfBuffer_, waitForTransform_);
+		if(pose.isNull())
+		{
+			RCLCPP_ERROR(this->get_logger(), "Failed to get transform between %s and %s frames", mapFrameId_.c_str(), frameId_.c_str());
+			return;
+		}
+	}
+
+	UASSERT_MSG(cloudMsg->data.size() == cloudMsg->row_step*cloudMsg->height,
+			uFormat("data=%d row_step=%d height=%d", cloudMsg->data.size(), cloudMsg->row_step, cloudMsg->height).c_str());
+
+	pcl::PointCloud<pcl::PointXYZ>::Ptr inputCloud(new pcl::PointCloud<pcl::PointXYZ>);
+	pcl::fromROSMsg(*cloudMsg, *inputCloud);
+	if(inputCloud->isOrganized())
+	{
+		std::vector<int> indices;
+		pcl::removeNaNFromPointCloud(*inputCloud, *inputCloud, indices);
+	}
+	else if(!inputCloud->is_dense && inputCloud->height == 1)
+	{
+		if(!warned_)
+		{
+			RCLCPP_WARN(this->get_logger(), "Detected possible wrong format of point cloud \"%s\", it is "
+					"indicated that it is not dense, but there is only one row. "
+					"Assuming it is dense... This message will only appear once.", cloudSub_->get_topic_name());
+			warned_ = true;
+		}
+		inputCloud->is_dense = true;
+	}
+
+	//Common variables for all strategies
+	pcl::IndicesPtr ground, obstacles;
+	pcl::PointCloud<pcl::PointXYZ>::Ptr obstaclesCloud(new pcl::PointCloud<pcl::PointXYZ>);
+	pcl::PointCloud<pcl::PointXYZ>::Ptr groundCloud(new pcl::PointCloud<pcl::PointXYZ>);
+	pcl::PointCloud<pcl::PointXYZ>::Ptr obstaclesCloudWithoutFlatSurfaces(new pcl::PointCloud<pcl::PointXYZ>);
+
+	if(inputCloud->size())
+	{
+		inputCloud = rtabmap::util3d::transformPointCloud(inputCloud, localTransform);
+
+		pcl::IndicesPtr flatObstacles(new std::vector<int>);
+		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = grid_.segmentCloud<pcl::PointXYZ>(
+				inputCloud,
+				pcl::IndicesPtr(new std::vector<int>),
+				pose,
+				cv::Point3f(localTransform.x(), localTransform.y(), localTransform.z()),
+				ground,
+				obstacles,
+				&flatObstacles);
+
+		if(cloud->size() && ((ground.get() && ground->size()) || (obstacles.get() && obstacles->size())))
+		{
+			if(groundPub_->get_subscription_count() &&
+					ground.get() && ground->size())
+			{
+				pcl::copyPointCloud(*cloud, *ground, *groundCloud);
+			}
+
+			if((obstaclesPub_->get_subscription_count() || projObstaclesPub_->get_subscription_count()) &&
+					obstacles.get() && obstacles->size())
+			{
+				// remove flat obstacles from obstacles
+				std::set<int> flatObstaclesSet;
+				if(projObstaclesPub_->get_subscription_count())
+				{
+					flatObstaclesSet.insert(flatObstacles->begin(), flatObstacles->end());
+				}
+
+				obstaclesCloud->resize(obstacles->size());
+				obstaclesCloudWithoutFlatSurfaces->resize(obstacles->size());
+
+				int oi=0;
+				for(unsigned int i=0; i<obstacles->size(); ++i)
+				{
+					obstaclesCloud->points[i] = cloud->at(obstacles->at(i));
+					if(flatObstaclesSet.size() == 0 ||
+					   flatObstaclesSet.find(obstacles->at(i))==flatObstaclesSet.end())
+					{
+						obstaclesCloudWithoutFlatSurfaces->points[oi] = obstaclesCloud->points[i];
+						obstaclesCloudWithoutFlatSurfaces->points[oi].z = 0;
+						++oi;
+					}
+
+				}
+
+				obstaclesCloudWithoutFlatSurfaces->resize(oi);
+			}
+
+			if(!localTransform.isIdentity() || !pose.isIdentity())
+			{
+				//transform back in topic frame for 3d clouds and base frame for 2d clouds
+
+				float roll, pitch, yaw;
+				pose.getEulerAngles(roll, pitch, yaw);
+				rtabmap::Transform t = rtabmap::Transform(0,0, mapFrameProjection_?pose.z():0, roll, pitch, 0);
+
+				if(obstaclesCloudWithoutFlatSurfaces->size() && !pose.isIdentity())
+				{
+					obstaclesCloudWithoutFlatSurfaces = rtabmap::util3d::transformPointCloud(obstaclesCloudWithoutFlatSurfaces, t.inverse());
+				}
+
+				t = (t*localTransform).inverse();
+				if(groundCloud->size())
+				{
+					groundCloud = rtabmap::util3d::transformPointCloud(groundCloud, t);
+				}
+				if(obstaclesCloud->size())
+				{
+					obstaclesCloud = rtabmap::util3d::transformPointCloud(obstaclesCloud, t);
+				}
+			}
+		}
+	}
+	else
+	{
+		RCLCPP_WARN(this->get_logger(), "obstacles_detection: Input cloud is empty! (%d x %d, is_dense=%d)", cloudMsg->width, cloudMsg->height, cloudMsg->is_dense?1:0);
+	}
+
+	if(groundPub_->get_subscription_count())
+	{
+		sensor_msgs::msg::PointCloud2::UniquePtr rosCloud(new sensor_msgs::msg::PointCloud2);
+		pcl::toROSMsg(*groundCloud, *rosCloud);
+		rosCloud->header = cloudMsg->header;
+
+		//publish the message
+		groundPub_->publish(std::move(rosCloud));
+	}
+
+	if(obstaclesPub_->get_subscription_count())
+	{
+		sensor_msgs::msg::PointCloud2::UniquePtr rosCloud(new sensor_msgs::msg::PointCloud2);
+		pcl::toROSMsg(*obstaclesCloud, *rosCloud);
+		rosCloud->header = cloudMsg->header;
+
+		//publish the message
+		obstaclesPub_->publish(std::move(rosCloud));
+	}
+
+	if(projObstaclesPub_->get_subscription_count())
+	{
+		sensor_msgs::msg::PointCloud2::UniquePtr rosCloud(new sensor_msgs::msg::PointCloud2);
+		pcl::toROSMsg(*obstaclesCloudWithoutFlatSurfaces, *rosCloud);
+		rosCloud->header.stamp = cloudMsg->header.stamp;
+		rosCloud->header.frame_id = frameId_;
+
+		//publish the message
+		projObstaclesPub_->publish(std::move(rosCloud));
+	}
+
+	RCLCPP_DEBUG(this->get_logger(), "Obstacles segmentation time = %f s", (now() - time).seconds());
+}
+
+}
+
+#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::ObstaclesDetection)

rtabmap_util/src/nodelets/point_cloud_aggregator.cpp

@@ -0,0 +1,419 @@
+/*
+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/point_cloud_aggregator.hpp>
+
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+#include <pcl_conversions/pcl_conversions.h>
+
+#include <rtabmap_conversions/MsgConversion.h>
+#include <rtabmap/utilite/UConversion.h>
+#include <rtabmap/utilite/ULogger.h>
+#include <rtabmap/core/util3d_filtering.h>
+
+namespace rtabmap_util
+{
+
+PointCloudAggregator::PointCloudAggregator(const rclcpp::NodeOptions & options) :
+	Node("point_cloud_aggregator", options),
+	warningThread_(0),
+	callbackCalled_(false),
+	exactSync4_(0),
+	approxSync4_(0),
+	exactSync3_(0),
+	approxSync3_(0),
+	exactSync2_(0),
+	approxSync2_(0),
+	waitForTransform_(0.1),
+	xyzOutput_(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 count = 2;
+	bool approx=true;
+	double approxSyncMaxInterval = 0.0;
+	int qos=0;
+	queueSize = this->declare_parameter("queue_size", queueSize);
+	qos = this->declare_parameter("qos", qos);
+	frameId_ = this->declare_parameter("frame_id", frameId_);
+	fixedFrameId_ = this->declare_parameter("fixed_frame_id", fixedFrameId_);
+	approx = this->declare_parameter("approx_sync", approx);
+	approxSyncMaxInterval = this->declare_parameter("approx_sync_max_interval", approxSyncMaxInterval);
+	count = this->declare_parameter("count", count);
+	waitForTransform_ = this->declare_parameter("wait_for_transform", waitForTransform_);
+	xyzOutput_ = this->declare_parameter("xyz_output", xyzOutput_);
+
+	cloudPub_ = create_publisher<sensor_msgs::msg::PointCloud2>("combined_cloud", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos));
+
+	cloudSub_1_.subscribe(this, "cloud1", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
+	cloudSub_2_.subscribe(this, "cloud2", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
+
+	std::string subscribedTopicsMsg;
+	if(count == 4)
+	{
+		cloudSub_3_.subscribe(this, "cloud3", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
+		cloudSub_4_.subscribe(this, "cloud4", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
+		if(approx)
+		{
+			approxSync4_ = new message_filters::Synchronizer<ApproxSync4Policy>(ApproxSync4Policy(queueSize), cloudSub_1_, cloudSub_2_, cloudSub_3_, cloudSub_4_);
+			if(approxSyncMaxInterval > 0.0)
+				approxSync4_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval));
+			approxSync4_->registerCallback(std::bind(&rtabmap_util::PointCloudAggregator::clouds4_callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4));
+		}
+		else
+		{
+			exactSync4_ = new message_filters::Synchronizer<ExactSync4Policy>(ExactSync4Policy(queueSize), cloudSub_1_, cloudSub_2_, cloudSub_3_, cloudSub_4_);
+			exactSync4_->registerCallback(std::bind(&rtabmap_util::PointCloudAggregator::clouds4_callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4));
+		}
+		subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n   %s,\n   %s,\n   %s,\n   %s",
+				get_name(),
+				approx?"approx":"exact",
+				approx&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
+				cloudSub_1_.getTopic().c_str(),
+				cloudSub_2_.getTopic().c_str(),
+				cloudSub_3_.getTopic().c_str(),
+				cloudSub_4_.getTopic().c_str());
+	}
+	else if(count == 3)
+	{
+		cloudSub_3_.subscribe(this, "cloud3", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
+		if(approx)
+		{
+			approxSync3_ = new message_filters::Synchronizer<ApproxSync3Policy>(ApproxSync3Policy(queueSize), cloudSub_1_, cloudSub_2_, cloudSub_3_);
+			if(approxSyncMaxInterval > 0.0)
+				approxSync3_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval));
+			approxSync3_->registerCallback(std::bind(&rtabmap_util::PointCloudAggregator::clouds3_callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
+		}
+		else
+		{
+			exactSync3_ = new message_filters::Synchronizer<ExactSync3Policy>(ExactSync3Policy(queueSize), cloudSub_1_, cloudSub_2_, cloudSub_3_);
+			exactSync3_->registerCallback(std::bind(&rtabmap_util::PointCloudAggregator::clouds3_callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
+		}
+		subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n   %s,\n   %s,\n   %s",
+				this->get_name(),
+				approx?"approx":"exact",
+				approx&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
+				cloudSub_1_.getTopic().c_str(),
+				cloudSub_2_.getTopic().c_str(),
+				cloudSub_3_.getTopic().c_str());
+	}
+	else
+	{
+		if(approx)
+		{
+			approxSync2_ = new message_filters::Synchronizer<ApproxSync2Policy>(ApproxSync2Policy(queueSize), cloudSub_1_, cloudSub_2_);
+			if(approxSyncMaxInterval > 0.0)
+				approxSync2_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval));
+			approxSync2_->registerCallback(std::bind(&rtabmap_util::PointCloudAggregator::clouds2_callback, this, std::placeholders::_1, std::placeholders::_2));
+		}
+		else
+		{
+			exactSync2_ = new message_filters::Synchronizer<ExactSync2Policy>(ExactSync2Policy(queueSize), cloudSub_1_, cloudSub_2_);
+			exactSync2_->registerCallback(std::bind(&rtabmap_util::PointCloudAggregator::clouds2_callback, this, std::placeholders::_1, std::placeholders::_2));
+		}
+		subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n   %s,\n   %s",
+				this->get_name(),
+				approx?"approx":"exact",
+				approx&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
+				cloudSub_1_.getTopic().c_str(),
+				cloudSub_2_.getTopic().c_str());
+	}
+
+
+	warningThread_ = new std::thread([&](){
+		rclcpp::Rate r(1.0/5.0);
+		while(!callbackCalled_)
+		{
+			r.sleep();
+			if(!callbackCalled_)
+			{
+				RCLCPP_WARN(this->get_logger(), "%s: Did not receive data since 5 seconds! Make sure the input topics are "
+						"published (\"$ rostopic hz my_topic\") and the timestamps in their "
+						"header are set. %s%s",
+						this->get_name(),
+						approx?"":"Parameter \"approx_sync\" is false, which means that input "
+							"topics should have all the exact timestamp for the callback to be called.",
+							subscribedTopicsMsg.c_str());
+			}
+		}
+	});
+}
+
+PointCloudAggregator::~PointCloudAggregator()
+{
+	delete exactSync4_;
+	delete approxSync4_;
+	delete exactSync3_;
+	delete approxSync3_;
+	delete exactSync2_;
+	delete approxSync2_;
+
+	if(warningThread_)
+	{
+		callbackCalled_=true;
+		warningThread_->join();
+		delete warningThread_;
+	}
+}
+
+void PointCloudAggregator::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)
+{
+	std::vector<sensor_msgs::msg::PointCloud2::ConstSharedPtr> clouds;
+	clouds.push_back(cloudMsg_1);
+	clouds.push_back(cloudMsg_2);
+	clouds.push_back(cloudMsg_3);
+	clouds.push_back(cloudMsg_4);
+
+	combineClouds(clouds);
+}
+void PointCloudAggregator::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)
+{
+	std::vector<sensor_msgs::msg::PointCloud2::ConstSharedPtr> clouds;
+	clouds.push_back(cloudMsg_1);
+	clouds.push_back(cloudMsg_2);
+	clouds.push_back(cloudMsg_3);
+
+	combineClouds(clouds);
+}
+void PointCloudAggregator::clouds2_callback(const sensor_msgs::msg::PointCloud2::ConstSharedPtr cloudMsg_1,
+					 const sensor_msgs::msg::PointCloud2::ConstSharedPtr cloudMsg_2)
+{
+	std::vector<sensor_msgs::msg::PointCloud2::ConstSharedPtr> clouds;
+	clouds.push_back(cloudMsg_1);
+	clouds.push_back(cloudMsg_2);
+
+	combineClouds(clouds);
+}
+void PointCloudAggregator::combineClouds(const std::vector<sensor_msgs::msg::PointCloud2::ConstSharedPtr> & cloudMsgs)
+{
+	callbackCalled_ = true;
+	UASSERT(cloudMsgs.size() > 1);
+	if(cloudPub_->get_subscription_count())
+	{
+		pcl::PCLPointCloud2::Ptr output(new pcl::PCLPointCloud2);
+
+		std::string frameId = frameId_;
+		if(!frameId.empty() && frameId.compare(cloudMsgs[0]->header.frame_id) != 0)
+		{
+			sensor_msgs::msg::PointCloud2 tmp;
+			rtabmap::Transform t = rtabmap_conversions::getTransform(frameId, cloudMsgs[0]->header.frame_id, cloudMsgs[0]->header.stamp, *tfBuffer_, waitForTransform_);
+			if(t.isNull())
+			{
+				return;
+			}
+			rtabmap_conversions::transformPointCloud(t.toEigen4f(), *cloudMsgs[0], tmp);
+			pcl_conversions::toPCL(tmp, *output);
+		}
+		else
+		{
+			pcl_conversions::toPCL(*cloudMsgs[0], *output);
+			frameId = cloudMsgs[0]->header.frame_id;
+		}
+
+		if(xyzOutput_ && !output->data.empty())
+		{
+			// convert only if not already XYZ cloud
+			bool hasField[4] = {false};
+			for(size_t i=0; i<output->fields.size(); ++i)
+			{
+				if(output->fields[i].name.compare("x") == 0)
+				{
+					hasField[0] = true;
+				}
+				else if(output->fields[i].name.compare("y") == 0)
+				{
+					hasField[1] = true;
+				}
+				else if(output->fields[i].name.compare("z") == 0)
+				{
+					hasField[2] = true;
+				}
+				else
+				{
+					hasField[3] = true; // other
+					break;
+				}
+			}
+			if(hasField[0] && hasField[1] && hasField[2] && !hasField[3])
+			{
+				// do nothing, already XYZ
+			}
+			else
+			{
+				pcl::PointCloud<pcl::PointXYZ> cloudxyz;
+				pcl::fromPCLPointCloud2(*output, cloudxyz);
+				pcl::toPCLPointCloud2(cloudxyz, *output);
+			}
+		}
+
+		for(unsigned int i=1; i<cloudMsgs.size(); ++i)
+		{
+			rtabmap::Transform cloudDisplacement;
+			if(!fixedFrameId_.empty() &&
+			   cloudMsgs[0]->header.stamp != cloudMsgs[i]->header.stamp)
+			{
+				// approx sync
+				cloudDisplacement = rtabmap_conversions::getTransform(
+						frameId, //sourceTargetFrame
+						fixedFrameId_, //fixedFrame
+						cloudMsgs[i]->header.stamp, //stampSource
+						cloudMsgs[0]->header.stamp, //stampTarget
+						*tfBuffer_,
+						waitForTransform_);
+			}
+
+			pcl::PCLPointCloud2::Ptr cloud2(new pcl::PCLPointCloud2);
+			if(frameId.compare(cloudMsgs[i]->header.frame_id) != 0)
+			{
+				sensor_msgs::msg::PointCloud2 tmp;
+				rtabmap::Transform t = rtabmap_conversions::getTransform(frameId, cloudMsgs[i]->header.frame_id, cloudMsgs[i]->header.stamp, *tfBuffer_, waitForTransform_);
+				rtabmap_conversions::transformPointCloud(t.toEigen4f(), *cloudMsgs[i], tmp);
+				if(!cloudDisplacement.isNull())
+				{
+					sensor_msgs::msg::PointCloud2 tmp2;
+					rtabmap_conversions::transformPointCloud(cloudDisplacement.toEigen4f(), tmp, tmp2);
+					pcl_conversions::toPCL(tmp2, *cloud2);
+				}
+				else
+				{
+					pcl_conversions::toPCL(tmp, *cloud2);
+				}
+			}
+			else
+			{
+				if(!cloudDisplacement.isNull())
+				{
+					sensor_msgs::msg::PointCloud2 tmp;
+					rtabmap_conversions::transformPointCloud(cloudDisplacement.toEigen4f(), *cloudMsgs[i], tmp);
+					pcl_conversions::toPCL(tmp, *cloud2);
+				}
+				else
+				{
+					pcl_conversions::toPCL(*cloudMsgs[i], *cloud2);
+				}
+			}
+
+			if(!cloud2->is_dense)
+			{
+				// remove nans
+				cloud2 = rtabmap::util3d::removeNaNFromPointCloud(cloud2);
+			}
+
+			if(xyzOutput_ && !cloud2->data.empty())
+			{
+				// convert only if not already XYZ cloud
+				bool hasField[4] = {false};
+				for(size_t i=0; i<cloud2->fields.size(); ++i)
+				{
+					if(cloud2->fields[i].name.compare("x") == 0)
+					{
+						hasField[0] = true;
+					}
+					else if(cloud2->fields[i].name.compare("y") == 0)
+					{
+						hasField[1] = true;
+					}
+					else if(cloud2->fields[i].name.compare("z") == 0)
+					{
+						hasField[2] = true;
+					}
+					else
+					{
+						hasField[3] = true; // other
+						break;
+					}
+				}
+				if(hasField[0] && hasField[1] && hasField[2] && !hasField[3])
+				{
+					// do nothing, already XYZ
+				}
+				else
+				{
+					pcl::PointCloud<pcl::PointXYZ> cloudxyz;
+					pcl::fromPCLPointCloud2(*cloud2, cloudxyz);
+					pcl::toPCLPointCloud2(cloudxyz, *cloud2);
+				}
+			}
+
+			if(output->data.empty())
+			{
+				output = cloud2;
+			}
+			else if(!cloud2->data.empty())
+			{
+
+				if(output->fields.size() != cloud2->fields.size())
+				{
+					RCLCPP_WARN(this->get_logger(), "%s: Input topics don't have all the "
+							"same number of fields (cloud1=%d, cloud%d=%d), concatenation "
+							"may fails. You can enable \"xyz_output\" option "
+							"to convert all inputs to XYZ.",
+							get_name(),
+							(int)output->fields.size(),
+							i+1,
+							(int)output->fields.size());
+				}
+
+				pcl::PCLPointCloud2::Ptr tmp_output(new pcl::PCLPointCloud2);
+#if PCL_VERSION_COMPARE(>=, 1, 10, 0)
+				pcl::concatenate(*output, *cloud2, *tmp_output);
+#else
+				pcl::concatenatePointCloud(*output, *cloud2, *tmp_output);
+#endif
+				//Make sure row_step is the sum of both
+				tmp_output->row_step = tmp_output->width * tmp_output->point_step;
+				output = tmp_output;
+			}
+		}
+
+		sensor_msgs::msg::PointCloud2::UniquePtr rosCloud(new sensor_msgs::msg::PointCloud2);
+		pcl_conversions::moveFromPCL(*output, *rosCloud);
+		rosCloud->header.stamp = cloudMsgs[0]->header.stamp;
+		rosCloud->header.frame_id = frameId;
+		cloudPub_->publish(std::move(rosCloud));
+	}
+}
+}
+
+#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::PointCloudAggregator)

rtabmap_util/src/nodelets/point_cloud_assembler.cpp

@@ -0,0 +1,521 @@
+/*
+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/point_cloud_assembler.hpp>
+
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+#include <pcl_conversions/pcl_conversions.h>
+#include <pcl/io/pcd_io.h>
+#include <pcl/filters/voxel_grid.h>
+#include <pcl/filters/radius_outlier_removal.h>
+
+#include <rtabmap_conversions/MsgConversion.h>
+#include <rtabmap_msgs/msg/odom_info.hpp>
+#include <rtabmap/core/util3d.h>
+#include <rtabmap/core/util3d_filtering.h>
+#include <rtabmap/core/Version.h>
+
+namespace rtabmap_util
+{
+
+PointCloudAssembler::PointCloudAssembler(const rclcpp::NodeOptions & options) :
+	Node("point_cloud_assembler", options),
+	warningThread_(0),
+	callbackCalled_(false),
+	exactSync_(0),
+	exactInfoSync_(0),
+	maxClouds_(0),
+	skipClouds_(0),
+	cloudsSkipped_(0),
+	circularBuffer_(false),
+	linearUpdate_(0),
+	angularUpdate_(0),
+	assemblingTime_(0),
+	waitForTransform_(0.1),
+	rangeMin_(0),
+	rangeMax_(0),
+	voxelSize_(0),
+	noiseRadius_(0),
+	noiseMinNeighbors_(5),
+	removeZ_(false),
+	fixedFrameId_("odom"),
+	frameId_("")
+{
+	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;
+	bool subscribeOdomInfo = false;
+
+	queueSize = this->declare_parameter("queue_size", queueSize);
+	qos = this->declare_parameter("qos", qos);
+	int qosOdom = this->declare_parameter("qos_odom", qos);
+	fixedFrameId_ = this->declare_parameter("fixed_frame_id", fixedFrameId_);
+	frameId_ = this->declare_parameter("frame_id", frameId_);
+	maxClouds_ = this->declare_parameter("max_clouds", maxClouds_);
+	assemblingTime_ = this->declare_parameter("assembling_time", assemblingTime_);
+	skipClouds_ = this->declare_parameter("skip_clouds", skipClouds_);
+	circularBuffer_ = this->declare_parameter("circular_buffer", circularBuffer_);
+	linearUpdate_ = this->declare_parameter("linear_update", linearUpdate_);
+	angularUpdate_ = this->declare_parameter("angular_update", angularUpdate_);
+	waitForTransform_ = this->declare_parameter("wait_for_transform", waitForTransform_);
+	rangeMin_ = this->declare_parameter("range_min", rangeMin_);
+	rangeMax_ = this->declare_parameter("range_max", rangeMax_);
+	voxelSize_ = this->declare_parameter("voxel_size", voxelSize_);
+	noiseRadius_ = this->declare_parameter("noise_radius", noiseRadius_);
+	noiseMinNeighbors_ = this->declare_parameter("noise_min_neighbors", noiseMinNeighbors_);
+	removeZ_ = this->declare_parameter("remove_z", removeZ_);
+	subscribeOdomInfo = this->declare_parameter("subscribe_odom_info", subscribeOdomInfo);
+
+	RCLCPP_INFO(this->get_logger(), "%s: queue_size=%d", get_name(), queueSize);
+	RCLCPP_INFO(this->get_logger(), "%s: qos=%d", get_name(), qos);
+	RCLCPP_INFO(this->get_logger(), "%s: qos_odom=%d", get_name(), qosOdom);
+	RCLCPP_INFO(this->get_logger(), "%s: fixed_frame_id=%s", get_name(), fixedFrameId_.c_str());
+	RCLCPP_INFO(this->get_logger(), "%s: frame_id=%s", get_name(), frameId_.c_str());
+	RCLCPP_INFO(this->get_logger(), "%s: max_clouds=%d", get_name(), maxClouds_);
+	RCLCPP_INFO(this->get_logger(), "%s: assembling_time=%fs", get_name(), assemblingTime_);
+	RCLCPP_INFO(this->get_logger(), "%s: skip_clouds=%d", get_name(), skipClouds_);
+	RCLCPP_INFO(this->get_logger(), "%s: circular_buffer=%s", get_name(), circularBuffer_?"true":"false");
+	RCLCPP_INFO(this->get_logger(), "%s: linear_update=%f m", get_name(), linearUpdate_);
+	RCLCPP_INFO(this->get_logger(), "%s: angular_update=%f rad", get_name(), angularUpdate_);
+	RCLCPP_INFO(this->get_logger(), "%s: wait_for_transform=%f", get_name(), waitForTransform_);
+	RCLCPP_INFO(this->get_logger(), "%s: range_min=%f", get_name(), rangeMin_);
+	RCLCPP_INFO(this->get_logger(), "%s: range_max=%f", get_name(), rangeMax_);
+	RCLCPP_INFO(this->get_logger(), "%s: voxel_size=%fm", get_name(), voxelSize_);
+	RCLCPP_INFO(this->get_logger(), "%s: noise_radius=%fm", get_name(), noiseRadius_);
+	RCLCPP_INFO(this->get_logger(), "%s: noise_min_neighbors=%d", get_name(), noiseMinNeighbors_);
+	RCLCPP_INFO(this->get_logger(), "%s: remove_z=%s", get_name(), removeZ_?"true":"false");
+
+	if(maxClouds_==0 && assemblingTime_ ==0.0)
+	{
+		RCLCPP_ERROR(get_logger(), "point_cloud_assembler: max_cloud or assembling_time parameters should be set!");
+		exit(-1);
+	}
+
+	cloudsSkipped_ = skipClouds_;
+
+	cloudPub_ = create_publisher<sensor_msgs::msg::PointCloud2>("assembled_cloud", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos));
+
+	if(!fixedFrameId_.empty())
+	{
+		cloudSub_ = create_subscription<sensor_msgs::msg::PointCloud2>("cloud", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos), std::bind(&PointCloudAssembler::callbackCloud, this, std::placeholders::_1));
+		subscribedTopicsMsg_ = uFormat("\n%s subscribed to %s",
+							get_name(),
+							cloudSub_->get_topic_name());
+	}
+	else if(subscribeOdomInfo)
+	{
+		syncCloudSub_.subscribe(this, "cloud", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
+		syncOdomSub_.subscribe(this, "odom", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosOdom).get_rmw_qos_profile());
+		syncOdomInfoSub_.subscribe(this, "odom_info", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosOdom).get_rmw_qos_profile());
+		exactInfoSync_ = new message_filters::Synchronizer<syncInfoPolicy>(syncInfoPolicy(queueSize), syncCloudSub_, syncOdomSub_, syncOdomInfoSub_);
+		exactInfoSync_->registerCallback(std::bind(&rtabmap_util::PointCloudAssembler::callbackCloudOdomInfo, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
+		subscribedTopicsMsg_ = uFormat("\n%s subscribed to (exact sync):\n   %s,\n   %s",
+							get_name(),
+							syncCloudSub_.getTopic().c_str(),
+							syncOdomSub_.getTopic().c_str(),
+							syncOdomInfoSub_.getTopic().c_str());
+	}
+	else
+	{
+		syncCloudSub_.subscribe(this, "cloud", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
+		syncOdomSub_.subscribe(this, "odom", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosOdom).get_rmw_qos_profile());
+		exactSync_ = new message_filters::Synchronizer<syncPolicy>(syncPolicy(queueSize), syncCloudSub_, syncOdomSub_);
+		exactSync_->registerCallback(std::bind(&rtabmap_util::PointCloudAssembler::callbackCloudOdom, this, std::placeholders::_1, std::placeholders::_2));
+		subscribedTopicsMsg_ = uFormat("\n%s subscribed to (exact sync):\n   %s,\n   %s",
+							get_name(),
+							syncCloudSub_.getTopic().c_str(),
+							syncOdomSub_.getTopic().c_str());
+	}
+
+	warningThread_ = new std::thread([&](){
+		rclcpp::Rate r(1.0/5.0);
+		while(!callbackCalled_)
+		{
+			r.sleep();
+			if(!callbackCalled_)
+			{
+				RCLCPP_WARN(this->get_logger(),
+						"%s: Did not receive data since 5 seconds! Make sure the input topics are "
+						"published (\"$ rostopic hz my_topic\") and the timestamps in their "
+						"header are set. %s",
+						get_name(),
+						subscribedTopicsMsg_.c_str());
+			}
+		}
+	});
+
+	RCLCPP_INFO(this->get_logger(), "%s", subscribedTopicsMsg_.c_str());
+}
+
+PointCloudAssembler::~PointCloudAssembler()
+{
+	delete exactSync_;
+	delete exactInfoSync_;
+
+	if(warningThread_)
+	{
+		callbackCalled_=true;
+		warningThread_->join();
+		delete warningThread_;
+	}
+}
+
+void PointCloudAssembler::callbackCloudOdom(
+		const sensor_msgs::msg::PointCloud2::ConstSharedPtr cloudMsg,
+		const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg)
+{
+	callbackCalled_ = true;
+	rtabmap::Transform odom = rtabmap_conversions::transformFromPoseMsg(odomMsg->pose.pose);
+	if(!odom.isNull())
+	{
+		fixedFrameId_ = odomMsg->header.frame_id;
+		callbackCloud(cloudMsg);
+	}
+	else
+	{
+		RCLCPP_WARN(this->get_logger(), "Reseting point cloud assembler as null odometry has been received.");
+		clouds_.clear();
+	}
+}
+
+sensor_msgs::msg::PointCloud2 removeField(const sensor_msgs::msg::PointCloud2 & input, const std::string & field)
+{
+	sensor_msgs::msg::PointCloud2 output;
+	int offset = 0;
+	std::vector<int> inputFieldIndex;
+	for(size_t i=0; i<input.fields.size(); ++i)
+	{
+		if(input.fields[i].name.compare(field) == 0)
+		{
+			continue;
+		}
+		else
+		{
+			sensor_msgs::msg::PointField outputField = input.fields[i];
+			outputField.offset = offset;
+			offset += outputField.count * rtabmap_conversions::sizeOfPointField(outputField.datatype);
+			output.fields.push_back(outputField);
+			inputFieldIndex.push_back(i);
+		}
+	}
+	output.header = input.header;
+	output.height = input.height;
+	output.width = input.width;
+	output.is_bigendian = input.is_bigendian;
+	output.is_dense = input.is_dense;
+	output.point_step = offset;
+	output.row_step = output.width * output.point_step;
+	output.data.resize(output.height*output.row_step);
+	int total = output.height*output.width;
+	for(int i=0; i<total; ++i)
+	{
+		// for each point, copy fields
+		int oi = i*output.point_step;
+		int pi = i*input.point_step;
+		for(size_t j=0;j<output.fields.size(); ++j)
+		{
+			memcpy(&output.data[oi + output.fields[j].offset],
+				   &input.data[pi + input.fields[inputFieldIndex[j]].offset],
+				   output.fields[j].count * rtabmap_conversions::sizeOfPointField(output.fields[j].datatype));
+		}
+	}
+	return output;
+}
+
+void PointCloudAssembler::callbackCloudOdomInfo(
+			const sensor_msgs::msg::PointCloud2::ConstSharedPtr cloudMsg,
+			const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
+			const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
+{
+	callbackCalled_ = true;
+	rtabmap::Transform odom = rtabmap_conversions::transformFromPoseMsg(odomMsg->pose.pose);
+	if(!odom.isNull())
+	{
+		if(odomInfoMsg->key_frame_added)
+		{
+			fixedFrameId_ = odomMsg->header.frame_id;
+			callbackCloud(cloudMsg);
+		}
+		else
+		{
+			RCLCPP_INFO(this->get_logger(), "Skipping non keyframe...");
+		}
+	}
+	else
+	{
+		RCLCPP_WARN(this->get_logger(), "Reseting point cloud assembler as null odometry has been received.");
+		clouds_.clear();
+	}
+}
+
+void PointCloudAssembler::callbackCloud(const sensor_msgs::msg::PointCloud2::ConstSharedPtr cloudMsg)
+{
+	if(cloudPub_->get_subscription_count())
+	{
+		UASSERT_MSG(cloudMsg->data.size() == cloudMsg->row_step*cloudMsg->height,
+						uFormat("data=%d row_step=%d height=%d", cloudMsg->data.size(), cloudMsg->row_step, cloudMsg->height).c_str());
+
+		if(skipClouds_<=0 || cloudsSkipped_ >= skipClouds_)
+		{
+			cloudsSkipped_ = 0;
+
+			rtabmap::Transform pose = rtabmap_conversions::getTransform(
+					fixedFrameId_, //fromFrame
+					cloudMsg->header.frame_id, //toFrame
+					cloudMsg->header.stamp,
+					*tfBuffer_,
+					waitForTransform_);
+
+			if(pose.isNull())
+			{
+				RCLCPP_ERROR(get_logger(), "Cloud not transform all clouds! Resetting...");
+				clouds_.clear();
+				return;
+			}
+
+			bool isMoving = true;
+			if(!previousPose_.isNull() && (linearUpdate_>0 || angularUpdate_>0))
+			{
+				rtabmap::Transform delta = previousPose_.inverse()*pose;
+				float roll, pitch, yaw;
+				delta.getEulerAngles(roll, pitch, yaw);
+				isMoving = fabs(delta.x()) > linearUpdate_ ||
+								fabs(delta.y()) > linearUpdate_ ||
+								fabs(delta.z()) > linearUpdate_ ||
+								(angularUpdate_>0.0f && (
+									fabs(roll) > angularUpdate_ ||
+									fabs(pitch) > angularUpdate_ ||
+									fabs(yaw) > angularUpdate_));
+			}
+
+			pcl::PCLPointCloud2::Ptr newCloud(new pcl::PCLPointCloud2);
+			if(rangeMin_ > 0.0 || rangeMax_ > 0.0 || voxelSize_ > 0.0f)
+			{
+				pcl_conversions::toPCL(*cloudMsg, *newCloud);
+				rtabmap::LaserScan scan = rtabmap::util3d::laserScanFromPointCloud(*newCloud);
+				scan = rtabmap::util3d::commonFiltering(scan, 1, rangeMin_, rangeMax_, voxelSize_);
+#if PCL_VERSION_COMPARE(>=, 1, 10, 0)
+				std::uint64_t stamp = newCloud->header.stamp;
+#else
+				pcl::uint64_t stamp = newCloud->header.stamp;
+#endif
+				newCloud = rtabmap::util3d::laserScanToPointCloud2(scan, pose);
+				newCloud->header.stamp = stamp;
+			}
+			else
+			{
+				sensor_msgs::msg::PointCloud2 output;
+				rtabmap_conversions::transformPointCloud(pose.toEigen4f(), *cloudMsg, output);
+				pcl_conversions::toPCL(output, *newCloud);
+			}
+
+			if(!newCloud->is_dense)
+			{
+				// remove nans
+				newCloud = rtabmap::util3d::removeNaNFromPointCloud(newCloud);
+			}
+
+			clouds_.push_back(newCloud);
+
+#if PCL_VERSION_COMPARE(>=, 1, 10, 0)
+			bool reachedMaxSize =
+					((int)clouds_.size() >= maxClouds_ && maxClouds_ > 0)
+					||
+					((*newCloud).header.stamp >= clouds_.front()->header.stamp + static_cast<std::uint64_t>(assemblingTime_*1000000.0) && assemblingTime_ > 0.0);
+#else
+			bool reachedMaxSize =
+					((int)clouds_.size() >= maxClouds_ && maxClouds_ > 0)
+					||
+					((*newCloud).header.stamp >= clouds_.front()->header.stamp + static_cast<pcl::uint64_t>(assemblingTime_*1000000.0) && assemblingTime_ > 0.0);
+#endif
+
+			if( circularBuffer_ || reachedMaxSize )
+			{
+				pcl::PCLPointCloud2Ptr assembled(new pcl::PCLPointCloud2);
+				for(std::list<pcl::PCLPointCloud2::Ptr>::iterator iter=clouds_.begin(); iter!=clouds_.end(); ++iter)
+				{
+					if(assembled->data.empty())
+					{
+						*assembled = *(*iter);
+					}
+					else
+					{
+						pcl::PCLPointCloud2Ptr assembledTmp(new pcl::PCLPointCloud2);
+#if PCL_VERSION_COMPARE(>=, 1, 10, 0)
+						pcl::concatenate(*assembled, *(*iter), *assembledTmp);
+#else
+						pcl::concatenatePointCloud(*assembled, *(*iter), *assembledTmp);
+#endif
+						//Make sure row_step is the sum of both
+						assembledTmp->row_step = assembled->row_step + (*iter)->row_step;
+						assembled = assembledTmp;
+					}
+				}
+
+				sensor_msgs::msg::PointCloud2 rosCloud;
+				if(voxelSize_>0.0)
+				{
+					// estimate if there would be an overflow
+					int x_idx=-1, y_idx=-1, z_idx=-1;
+					for (std::size_t d = 0; d < assembled->fields.size (); ++d)
+					{
+						if (assembled->fields[d].name.compare("x")==0)
+							x_idx = d;
+						if (assembled->fields[d].name.compare("y")==0)
+							y_idx = d;
+						if (assembled->fields[d].name.compare("z")==0)
+							z_idx = d;
+					}
+					bool overflow = false;
+					if(x_idx>=0 && y_idx>=0 && z_idx>=0) {
+						Eigen::Vector4f min_p, max_p;
+						pcl::getMinMax3D(assembled, x_idx, y_idx, z_idx, min_p, max_p);
+						float inverseVoxelSize = 1.0f/voxelSize_;
+						std::int64_t dx = static_cast<std::int64_t>((max_p[0] - min_p[0]) * inverseVoxelSize)+1;
+						std::int64_t dy = static_cast<std::int64_t>((max_p[1] - min_p[1]) * inverseVoxelSize)+1;
+						std::int64_t dz = static_cast<std::int64_t>((max_p[2] - min_p[2]) * inverseVoxelSize)+1;
+
+						if ((dx*dy*dz) > static_cast<std::int64_t>(std::numeric_limits<std::int32_t>::max()))
+						{
+							overflow = true;
+						}
+					}
+					if(overflow)
+					{
+						rtabmap::LaserScan scan = rtabmap::util3d::laserScanFromPointCloud(*assembled);
+						scan = rtabmap::util3d::commonFiltering(scan, 1, 0, 0, voxelSize_);
+#if PCL_VERSION_COMPARE(>=, 1, 10, 0)
+						std::uint64_t stamp = assembled->header.stamp;
+#else
+						pcl::uint64_t stamp = assembled->header.stamp;
+#endif
+						assembled = rtabmap::util3d::laserScanToPointCloud2(scan);
+						assembled->header.stamp = stamp;
+					}
+					else
+					{
+						pcl::VoxelGrid<pcl::PCLPointCloud2> filter;
+						filter.setLeafSize(voxelSize_, voxelSize_, voxelSize_);
+						filter.setInputCloud(assembled);
+						pcl::PCLPointCloud2Ptr output(new pcl::PCLPointCloud2);
+						filter.filter(*output);
+						assembled = output;
+					}
+				}
+				if(noiseRadius_>0.0 && noiseMinNeighbors_>0)
+				{
+					pcl::RadiusOutlierRemoval<pcl::PCLPointCloud2> filter;
+					filter.setRadiusSearch(noiseRadius_);
+					filter.setMinNeighborsInRadius(noiseMinNeighbors_);
+					filter.setInputCloud(assembled);
+					pcl::PCLPointCloud2Ptr output(new pcl::PCLPointCloud2);
+					filter.filter(*output);
+					assembled = output;
+				}
+				pcl_conversions::moveFromPCL(*assembled, rosCloud);
+				rtabmap::Transform t = pose;
+				if(!frameId_.empty())
+				{
+					// transform in target frame_id instead of sensor frame
+					t = rtabmap_conversions::getTransform(
+							fixedFrameId_, //fromFrame
+							frameId_, //toFrame
+							cloudMsg->header.stamp,
+							*tfBuffer_,
+							waitForTransform_);
+					if(t.isNull())
+					{
+						RCLCPP_ERROR(this->get_logger(), "Cloud not transform back assembled clouds in target frame \"%s\"! Resetting...", frameId_.c_str());
+						clouds_.clear();
+						return;
+					}
+				}
+				rtabmap_conversions::transformPointCloud(t.toEigen4f().inverse(), rosCloud, rosCloud);
+
+				if(removeZ_)
+				{
+					rosCloud = removeField(rosCloud, "z");
+				}
+
+				rosCloud.header = cloudMsg->header;
+				if(!frameId_.empty())
+				{
+					rosCloud.header.frame_id = frameId_;
+				}
+				cloudPub_->publish(rosCloud);
+				if(circularBuffer_)
+				{
+					if(!isMoving)
+					{
+						clouds_.pop_back();
+					}
+					else
+					{
+						previousPose_ = pose;
+						if(reachedMaxSize)
+						{
+							clouds_.pop_front();
+						}
+					}
+				}
+				else
+				{
+					clouds_.clear();
+					previousPose_.setNull();
+				}
+			}
+			else if(!isMoving)
+			{
+				clouds_.pop_back();
+			}
+			else
+			{
+				previousPose_ = pose;
+			}
+		}
+		else
+		{
+			++cloudsSkipped_;
+		}
+	}
+}
+
+}
+
+#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::PointCloudAssembler)

rtabmap_util/src/nodelets/point_cloud_xyz.cpp

@@ -0,0 +1,345 @@
+/*
+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/point_cloud_xyz.hpp>
+
+#include <rtabmap_conversions/MsgConversion.h>
+
+#include <pcl_conversions/pcl_conversions.h>
+
+#include <image_geometry/pinhole_camera_model.h>
+
+#include <cv_bridge/cv_bridge.h>
+#include <opencv2/highgui/highgui.hpp>
+
+#include "rtabmap/core/util2d.h"
+#include "rtabmap/core/util3d.h"
+#include "rtabmap/core/util3d_filtering.h"
+#include "rtabmap/core/util3d_surface.h"
+#include "rtabmap/utilite/UConversion.h"
+#include "rtabmap/utilite/UStl.h"
+
+namespace rtabmap_util
+{
+
+PointCloudXYZ::PointCloudXYZ(const rclcpp::NodeOptions & options) :
+		Node("point_cloud_xyz", options),
+		maxDepth_(0.0),
+		minDepth_(0.0),
+		voxelSize_(0.0),
+		decimation_(1),
+		noiseFilterRadius_(0.0),
+		noiseFilterMinNeighbors_(5),
+		normalK_(0),
+		normalRadius_(0.0),
+		filterNaNs_(false),
+		approxSyncDepth_(0),
+		approxSyncDisparity_(0),
+		exactSyncDepth_(0),
+		exactSyncDisparity_(0)
+{
+	int queueSize = 10;
+	int qos = 0;
+	bool approxSync = true;
+	std::string roiStr;
+	double approxSyncMaxInterval = 0.0;
+	approxSync = this->declare_parameter("approx_sync", approxSync);
+	approxSyncMaxInterval = this->declare_parameter("approx_sync_max_interval", approxSyncMaxInterval);
+	queueSize = this->declare_parameter("queue_size", queueSize);
+	qos = this->declare_parameter("qos", qos);
+	int qosCamInfo = this->declare_parameter("qos_camera_info", qos);
+	maxDepth_ = this->declare_parameter("max_depth", maxDepth_);
+	minDepth_ = this->declare_parameter("min_depth", minDepth_);
+	voxelSize_ = this->declare_parameter("voxel_size", voxelSize_);
+	decimation_ = this->declare_parameter("decimation", decimation_);
+	noiseFilterRadius_ = this->declare_parameter("noise_filter_radius", noiseFilterRadius_);
+	noiseFilterMinNeighbors_ = this->declare_parameter("noise_filter_min_neighbors", noiseFilterMinNeighbors_);
+	normalK_ = this->declare_parameter("normal_k", normalK_);
+	normalRadius_ = this->declare_parameter("normal_radius", normalRadius_);
+	filterNaNs_ = this->declare_parameter("filter_nans", filterNaNs_);
+	roiStr = this->declare_parameter("roi_ratios", roiStr);
+
+	//parse roi (region of interest)
+	roiRatios_.resize(4, 0);
+	if(!roiStr.empty())
+	{
+		std::list<std::string> strValues = uSplit(roiStr, ' ');
+		if(strValues.size() != 4)
+		{
+			RCLCPP_ERROR(this->get_logger(), "The number of values must be 4 (\"roi_ratios\"=\"%s\")", roiStr.c_str());
+		}
+		else
+		{
+			std::vector<float> tmpValues(4);
+			unsigned int i=0;
+			for(std::list<std::string>::iterator jter = strValues.begin(); jter!=strValues.end(); ++jter)
+			{
+				tmpValues[i] = uStr2Float(*jter);
+				++i;
+			}
+
+			if(tmpValues[0] >= 0 && tmpValues[0] < 1 && tmpValues[0] < 1.0f-tmpValues[1] &&
+				tmpValues[1] >= 0 && tmpValues[1] < 1 && tmpValues[1] < 1.0f-tmpValues[0] &&
+				tmpValues[2] >= 0 && tmpValues[2] < 1 && tmpValues[2] < 1.0f-tmpValues[3] &&
+				tmpValues[3] >= 0 && tmpValues[3] < 1 && tmpValues[3] < 1.0f-tmpValues[2])
+			{
+				roiRatios_ = tmpValues;
+			}
+			else
+			{
+				RCLCPP_ERROR(this->get_logger(), "The roi ratios are not valid (\"roi_ratios\"=\"%s\")", roiStr.c_str());
+			}
+		}
+	}
+
+	RCLCPP_INFO(this->get_logger(), "Approximate time sync = %s", approxSync?"true":"false");
+
+	if(approxSync)
+	{
+		approxSyncDepth_ = new message_filters::Synchronizer<MyApproxSyncDepthPolicy>(MyApproxSyncDepthPolicy(queueSize), imageDepthSub_, cameraInfoSub_);
+		if(approxSyncMaxInterval > 0.0)
+			approxSyncDepth_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval));
+		approxSyncDepth_->registerCallback(std::bind(&PointCloudXYZ::callback, this, std::placeholders::_1, std::placeholders::_2));
+
+		approxSyncDisparity_ = new message_filters::Synchronizer<MyApproxSyncDisparityPolicy>(MyApproxSyncDisparityPolicy(queueSize), disparitySub_, disparityCameraInfoSub_);
+		if(approxSyncMaxInterval > 0.0)
+			approxSyncDisparity_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval));
+		approxSyncDisparity_->registerCallback(std::bind(&PointCloudXYZ::callbackDisparity, this, std::placeholders::_1, std::placeholders::_2));
+	}
+	else
+	{
+		exactSyncDepth_ = new message_filters::Synchronizer<MyExactSyncDepthPolicy>(MyExactSyncDepthPolicy(queueSize), imageDepthSub_, cameraInfoSub_);
+		exactSyncDepth_->registerCallback(std::bind(&PointCloudXYZ::callback, this, std::placeholders::_1, std::placeholders::_2));
+
+		exactSyncDisparity_ = new message_filters::Synchronizer<MyExactSyncDisparityPolicy>(MyExactSyncDisparityPolicy(queueSize), disparitySub_, disparityCameraInfoSub_);
+		exactSyncDisparity_->registerCallback(std::bind(&PointCloudXYZ::callbackDisparity, this, std::placeholders::_1, std::placeholders::_2));
+	}
+
+	cloudPub_ = create_publisher<sensor_msgs::msg::PointCloud2>("cloud", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos));
+
+	image_transport::TransportHints hints(this);
+	imageDepthSub_.subscribe(this, "depth/image", hints.getTransport(), rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
+	cameraInfoSub_.subscribe(this, "depth/camera_info", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosCamInfo).get_rmw_qos_profile());
+
+	disparitySub_.subscribe(this, "disparity/image", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
+	disparityCameraInfoSub_.subscribe(this, "disparity/camera_info", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosCamInfo).get_rmw_qos_profile());
+}
+
+PointCloudXYZ::~PointCloudXYZ()
+{
+	delete approxSyncDepth_;
+	delete approxSyncDisparity_;
+	delete exactSyncDepth_;
+	delete exactSyncDisparity_;
+}
+void PointCloudXYZ::callback(
+		  const sensor_msgs::msg::Image::ConstSharedPtr depthMsg,
+		  const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfo)
+{
+	if(depthMsg->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1)!=0 &&
+	   depthMsg->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1)!=0 &&
+	   depthMsg->encoding.compare(sensor_msgs::image_encodings::MONO16)!=0)
+	{
+		RCLCPP_ERROR(this->get_logger(), "Input type depth=32FC1,16UC1,MONO16");
+		return;
+	}
+
+	if(cloudPub_->get_subscription_count())
+	{
+		rclcpp::Time time = now();
+
+		cv_bridge::CvImageConstPtr imageDepthPtr = cv_bridge::toCvShare(depthMsg);
+
+		rtabmap::CameraModel model = rtabmap_conversions::cameraModelFromROS(*cameraInfo);
+
+		pcl::PointCloud<pcl::PointXYZ>::Ptr pclCloud;
+
+		cv::Mat depth = imageDepthPtr->image;
+		if( roiRatios_.size() == 4 &&
+			((roiRatios_[0] > 0.0f && roiRatios_[0] <= 1.0f) ||
+			 (roiRatios_[1] > 0.0f && roiRatios_[1] <= 1.0f) ||
+			 (roiRatios_[2] > 0.0f && roiRatios_[2] <= 1.0f) ||
+			 (roiRatios_[3] > 0.0f && roiRatios_[3] <= 1.0f)))
+		{
+			cv::Rect roiDepth = rtabmap::util2d::computeRoi(depth, roiRatios_);
+			cv::Rect roiRgb;
+			if(model.imageWidth() && model.imageHeight())
+			{
+				roiRgb = rtabmap::util2d::computeRoi(model.imageSize(), roiRatios_);
+			}
+			if(	roiDepth.width%decimation_==0 &&
+				roiDepth.height%decimation_==0 &&
+				(roiRgb.width != 0 ||
+				   (roiRgb.width%decimation_==0 &&
+					roiRgb.height%decimation_==0)))
+			{
+				depth = cv::Mat(depth, roiDepth);
+				if(model.imageWidth() != 0 && model.imageHeight() != 0)
+				{
+					model = model.roi(roiRgb);
+				}
+				else
+				{
+					model = model.roi(roiDepth);
+				}
+			}
+			else
+			{
+				RCLCPP_ERROR(this->get_logger(), "Cannot apply ROI ratios [%f,%f,%f,%f] because resulting "
+					  "dimension (depth=%dx%d rgb=%dx%d) cannot be divided exactly "
+					  "by decimation parameter (%d). Ignoring ROI ratios...",
+					  roiRatios_[0],
+					  roiRatios_[1],
+					  roiRatios_[2],
+					  roiRatios_[3],
+					  roiDepth.width,
+					  roiDepth.height,
+					  roiRgb.width,
+					  roiRgb.height,
+					  decimation_);
+			}
+		}
+
+		pcl::IndicesPtr indices(new std::vector<int>);
+		pclCloud = rtabmap::util3d::cloudFromDepth(
+				depth,
+				model,
+				decimation_,
+				maxDepth_,
+				minDepth_,
+				indices.get());
+		processAndPublish(pclCloud, indices, depthMsg->header);
+		RCLCPP_DEBUG(this->get_logger(), "point_cloud_xyz from depth time = %f s", (now() - time).seconds());
+	}
+}
+
+void PointCloudXYZ::callbackDisparity(
+		const stereo_msgs::msg::DisparityImage::ConstSharedPtr disparityMsg,
+		const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfo)
+{
+	if(disparityMsg->image.encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1) !=0 &&
+	   disparityMsg->image.encoding.compare(sensor_msgs::image_encodings::TYPE_16SC1) !=0)
+	{
+		RCLCPP_ERROR(this->get_logger(), "Input type must be disparity=32FC1 or 16SC1");
+		return;
+	}
+
+	cv::Mat disparity;
+	if(disparityMsg->image.encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1) == 0)
+	{
+		disparity = cv::Mat(disparityMsg->image.height, disparityMsg->image.width, CV_32FC1, const_cast<uchar*>(disparityMsg->image.data.data()));
+	}
+	else
+	{
+		disparity = cv::Mat(disparityMsg->image.height, disparityMsg->image.width, CV_16SC1, const_cast<uchar*>(disparityMsg->image.data.data()));
+	}
+
+	if(cloudPub_->get_subscription_count())
+	{
+		rclcpp::Time time = now();
+
+		cv::Rect roi = rtabmap::util2d::computeRoi(disparity, roiRatios_);
+
+		pcl::PointCloud<pcl::PointXYZ>::Ptr pclCloud;
+		rtabmap::CameraModel leftModel = rtabmap_conversions::cameraModelFromROS(*cameraInfo);
+		UASSERT(disparity.cols == leftModel.imageWidth() && disparity.rows == leftModel.imageHeight());
+		rtabmap::StereoCameraModel stereoModel(disparityMsg->f, disparityMsg->f, leftModel.cx()-roiRatios_[0]*double(disparity.cols), leftModel.cy()-roiRatios_[2]*double(disparity.rows), disparityMsg->t);
+		pcl::IndicesPtr indices(new std::vector<int>);
+		pclCloud = rtabmap::util3d::cloudFromDisparity(
+				cv::Mat(disparity, roi),
+				stereoModel,
+				decimation_,
+				maxDepth_,
+				minDepth_,
+				indices.get());
+
+		processAndPublish(pclCloud, indices, disparityMsg->header);
+		RCLCPP_DEBUG(this->get_logger(), "point_cloud_xyz from disparity time = %f s", (now() - time).seconds());
+	}
+}
+
+void PointCloudXYZ::processAndPublish(pcl::PointCloud<pcl::PointXYZ>::Ptr & pclCloud, pcl::IndicesPtr & indices, const std_msgs::msg::Header & header)
+{
+	if(indices->size() && voxelSize_ > 0.0)
+	{
+		pclCloud = rtabmap::util3d::voxelize(pclCloud, indices, voxelSize_);
+		pclCloud->is_dense = true;
+	}
+
+	// Do radius filtering after voxel filtering ( a lot faster)
+	if(!pclCloud->empty() && (pclCloud->is_dense || !indices->empty()) && noiseFilterRadius_ > 0.0 && noiseFilterMinNeighbors_ > 0)
+	{
+		if(pclCloud->is_dense)
+		{
+			indices = rtabmap::util3d::radiusFiltering(pclCloud, noiseFilterRadius_, noiseFilterMinNeighbors_);
+		}
+		else
+		{
+			indices = rtabmap::util3d::radiusFiltering(pclCloud, indices, noiseFilterRadius_, noiseFilterMinNeighbors_);
+		}
+		pcl::PointCloud<pcl::PointXYZ>::Ptr tmp(new pcl::PointCloud<pcl::PointXYZ>);
+		pcl::copyPointCloud(*pclCloud, *indices, *tmp);
+		pclCloud = tmp;
+	}
+
+	sensor_msgs::msg::PointCloud2::UniquePtr rosCloud(new sensor_msgs::msg::PointCloud2);
+if(!pclCloud->empty() && (pclCloud->is_dense || !indices->empty()) && (normalK_ > 0 || normalRadius_ > 0.0f))
+	{
+		//compute normals
+		pcl::PointCloud<pcl::Normal>::Ptr normals = rtabmap::util3d::computeNormals(pclCloud, normalK_, normalRadius_);
+		pcl::PointCloud<pcl::PointNormal>::Ptr pclCloudNormal(new pcl::PointCloud<pcl::PointNormal>);
+		pcl::concatenateFields(*pclCloud, *normals, *pclCloudNormal);
+		if(filterNaNs_)
+		{
+			pclCloudNormal = rtabmap::util3d::removeNaNNormalsFromPointCloud(pclCloudNormal);
+		}
+		pcl::toROSMsg(*pclCloudNormal, *rosCloud);
+	}
+	else
+	{
+		if(filterNaNs_ && !pclCloud->is_dense)
+		{
+			pclCloud = rtabmap::util3d::removeNaNFromPointCloud(pclCloud);
+		}
+		pcl::toROSMsg(*pclCloud, *rosCloud);
+	}
+	rosCloud->header.stamp = header.stamp;
+	rosCloud->header.frame_id = header.frame_id;
+
+	//publish the message
+	cloudPub_->publish(std::move(rosCloud));
+}
+
+}
+
+#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::PointCloudXYZ)
+

rtabmap_util/src/nodelets/point_cloud_xyzrgb.cpp

@@ -0,0 +1,508 @@
+/*
+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/point_cloud_xyzrgb.hpp>
+
+#include <pcl_conversions/pcl_conversions.h>
+
+#include <rtabmap_conversions/MsgConversion.h>
+
+#include <image_geometry/pinhole_camera_model.h>
+#include <image_geometry/stereo_camera_model.h>
+
+#include <cv_bridge/cv_bridge.h>
+#include <opencv2/highgui/highgui.hpp>
+
+#include "rtabmap/core/util2d.h"
+#include "rtabmap/core/util3d.h"
+#include "rtabmap/core/util3d_filtering.h"
+#include "rtabmap/core/util3d_surface.h"
+#include "rtabmap/utilite/UConversion.h"
+#include "rtabmap/utilite/UStl.h"
+
+namespace rtabmap_util
+{
+
+PointCloudXYZRGB::PointCloudXYZRGB(const rclcpp::NodeOptions & options) :
+	Node("point_cloud_xyzrgb", options),
+	maxDepth_(0.0),
+	minDepth_(0.0),
+	voxelSize_(0.0),
+	decimation_(1),
+	noiseFilterRadius_(0.0),
+	noiseFilterMinNeighbors_(5),
+	normalK_(0),
+	normalRadius_(0.0),
+	filterNaNs_(false),
+	approxSyncDepth_(0),
+	approxSyncDisparity_(0),
+	approxSyncStereo_(0),
+	exactSyncDepth_(0),
+	exactSyncDisparity_(0),
+	exactSyncStereo_(0)
+{
+	bool approxSync = true;
+	std::string roiStr;
+	int queueSize = 10;
+	int qos = 0;
+	double approxSyncMaxInterval = 0.0;
+	approxSync = this->declare_parameter("approx_sync", approxSync);
+	approxSyncMaxInterval = this->declare_parameter("approx_sync_max_interval", approxSyncMaxInterval);
+	queueSize = this->declare_parameter("queue_size", queueSize);
+	qos = this->declare_parameter("qos", qos);
+	int qosCamInfo = this->declare_parameter("qos_camera_info", qos);
+	maxDepth_ = this->declare_parameter("max_depth", maxDepth_);
+	minDepth_ = this->declare_parameter("min_depth", minDepth_);
+	voxelSize_ = this->declare_parameter("voxel_size", voxelSize_);
+	decimation_ = this->declare_parameter("decimation", decimation_);
+	noiseFilterRadius_ = this->declare_parameter("noise_filter_radius", noiseFilterRadius_);
+	noiseFilterMinNeighbors_ = this->declare_parameter("noise_filter_min_neighbors", noiseFilterMinNeighbors_);
+	normalK_ = this->declare_parameter("normal_k", normalK_);
+	normalRadius_ = this->declare_parameter("normal_radius", normalRadius_);
+	filterNaNs_ = this->declare_parameter("filter_nans", filterNaNs_);
+	roiStr = this->declare_parameter("roi_ratios", roiStr);
+
+	//parse roi (region of interest)
+	roiRatios_.resize(4, 0);
+	if(!roiStr.empty())
+	{
+		std::list<std::string> strValues = uSplit(roiStr, ' ');
+		if(strValues.size() != 4)
+		{
+			RCLCPP_ERROR(this->get_logger(), "The number of values must be 4 (\"roi_ratios\"=\"%s\")", roiStr.c_str());
+		}
+		else
+		{
+			std::vector<float> tmpValues(4);
+			unsigned int i=0;
+			for(std::list<std::string>::iterator jter = strValues.begin(); jter!=strValues.end(); ++jter)
+			{
+				tmpValues[i] = uStr2Float(*jter);
+				++i;
+			}
+
+			if(tmpValues[0] >= 0 && tmpValues[0] < 1 && tmpValues[0] < 1.0f-tmpValues[1] &&
+				tmpValues[1] >= 0 && tmpValues[1] < 1 && tmpValues[1] < 1.0f-tmpValues[0] &&
+				tmpValues[2] >= 0 && tmpValues[2] < 1 && tmpValues[2] < 1.0f-tmpValues[3] &&
+				tmpValues[3] >= 0 && tmpValues[3] < 1 && tmpValues[3] < 1.0f-tmpValues[2])
+			{
+				roiRatios_ = tmpValues;
+			}
+			else
+			{
+				RCLCPP_ERROR(this->get_logger(), "The roi ratios are not valid (\"roi_ratios\"=\"%s\")", roiStr.c_str());
+			}
+		}
+	}
+
+	// StereoBM parameters
+	stereoBMParameters_ = rtabmap::Parameters::getDefaultParameters("StereoBM");
+	for(rtabmap::ParametersMap::iterator iter=stereoBMParameters_.begin(); iter!=stereoBMParameters_.end(); ++iter)
+	{
+		std::string vStr = declare_parameter(iter->first, iter->second);
+		if(vStr.compare(iter->second)!=0)
+		{
+			RCLCPP_INFO(this->get_logger(), "point_cloud_xyzrgb: Setting parameter \"%s\"=\"%s\"", iter->first.c_str(), vStr.c_str());
+			iter->second = vStr;
+		}
+	}
+
+	RCLCPP_INFO(this->get_logger(), "Approximate time sync = %s", approxSync?"true":"false");
+
+	cloudPub_ = create_publisher<sensor_msgs::msg::PointCloud2>("cloud", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos));
+
+	rgbdImageSub_ = create_subscription<rtabmap_msgs::msg::RGBDImage>("rgbd_image", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos), std::bind(&PointCloudXYZRGB::rgbdImageCallback, this, std::placeholders::_1));
+
+	if(approxSync)
+	{
+
+		approxSyncDepth_ = new message_filters::Synchronizer<MyApproxSyncDepthPolicy>(MyApproxSyncDepthPolicy(queueSize), imageSub_, imageDepthSub_, cameraInfoSub_);
+		if(approxSyncMaxInterval > 0.0)
+			approxSyncDepth_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval));
+		approxSyncDepth_->registerCallback(std::bind(&PointCloudXYZRGB::depthCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
+
+		approxSyncDisparity_ = new message_filters::Synchronizer<MyApproxSyncDisparityPolicy>(MyApproxSyncDisparityPolicy(queueSize), imageLeft_, imageDisparitySub_, cameraInfoLeft_);
+		if(approxSyncMaxInterval > 0.0)
+			approxSyncDisparity_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval));
+		approxSyncDisparity_->registerCallback(std::bind(&PointCloudXYZRGB::disparityCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
+
+		approxSyncStereo_ = new message_filters::Synchronizer<MyApproxSyncStereoPolicy>(MyApproxSyncStereoPolicy(queueSize), imageLeft_, imageRight_, cameraInfoLeft_, cameraInfoRight_);
+		if(approxSyncMaxInterval > 0.0)
+			approxSyncStereo_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval));
+		approxSyncStereo_->registerCallback(std::bind(&PointCloudXYZRGB::stereoCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4));
+	}
+	else
+	{
+		exactSyncDepth_ = new message_filters::Synchronizer<MyExactSyncDepthPolicy>(MyExactSyncDepthPolicy(queueSize), imageSub_, imageDepthSub_, cameraInfoSub_);
+		exactSyncDepth_->registerCallback(std::bind(&PointCloudXYZRGB::depthCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
+
+		exactSyncDisparity_ = new message_filters::Synchronizer<MyExactSyncDisparityPolicy>(MyExactSyncDisparityPolicy(queueSize), imageLeft_, imageDisparitySub_, cameraInfoLeft_);
+		exactSyncDisparity_->registerCallback(std::bind(&PointCloudXYZRGB::disparityCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
+
+		exactSyncStereo_ = new message_filters::Synchronizer<MyExactSyncStereoPolicy>(MyExactSyncStereoPolicy(queueSize), imageLeft_, imageRight_, cameraInfoLeft_, cameraInfoRight_);
+		exactSyncStereo_->registerCallback(std::bind(&PointCloudXYZRGB::stereoCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4));
+	}
+
+	image_transport::TransportHints hints(this);
+	imageSub_.subscribe(this, "rgb/image", hints.getTransport(), rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
+	imageDepthSub_.subscribe(this, "depth/image", hints.getTransport(), rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
+	cameraInfoSub_.subscribe(this, "rgb/camera_info", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosCamInfo).get_rmw_qos_profile());
+
+	imageDisparitySub_.subscribe(this, "disparity", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
+
+	imageLeft_.subscribe(this, "left/image", hints.getTransport(), rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
+	imageRight_.subscribe(this, "right/image", hints.getTransport(), rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
+	cameraInfoLeft_.subscribe(this, "left/camera_info", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosCamInfo).get_rmw_qos_profile());
+	cameraInfoRight_.subscribe(this, "right/camera_info", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosCamInfo).get_rmw_qos_profile());
+}
+
+PointCloudXYZRGB::~PointCloudXYZRGB()
+{
+	delete approxSyncDepth_;
+	delete approxSyncDisparity_;
+	delete approxSyncStereo_;
+	delete exactSyncDepth_;
+	delete exactSyncDisparity_;
+	delete exactSyncStereo_;
+}
+
+void PointCloudXYZRGB::depthCallback(
+		  const sensor_msgs::msg::Image::ConstSharedPtr image,
+		  const sensor_msgs::msg::Image::ConstSharedPtr imageDepth,
+		  const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfo)
+{
+	if(!(image->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) ==0 ||
+		image->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
+		image->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0 ||
+		image->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
+		image->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0 ||
+		image->encoding.compare(sensor_msgs::image_encodings::BGRA8) == 0 ||
+		image->encoding.compare(sensor_msgs::image_encodings::RGBA8) == 0 ||
+		image->encoding.compare(sensor_msgs::image_encodings::BAYER_GRBG8) == 0) ||
+	   !(imageDepth->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1)==0 ||
+		 imageDepth->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1)==0 ||
+		 imageDepth->encoding.compare(sensor_msgs::image_encodings::MONO16)==0))
+	{
+		RCLCPP_ERROR(this->get_logger(), "Input type must be image=mono8,mono16,rgb8,bgr8 and image_depth=32FC1,16UC1,mono16");
+		return;
+	}
+
+	if(cloudPub_->get_subscription_count())
+	{
+		rclcpp::Time time = now();
+
+		cv_bridge::CvImageConstPtr imagePtr;
+		if(image->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1)==0)
+		{
+			imagePtr = cv_bridge::toCvShare(image);
+		}
+		else if(image->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
+				image->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0)
+		{
+			imagePtr = cv_bridge::toCvShare(image, "mono8");
+		}
+		else
+		{
+			imagePtr = cv_bridge::toCvShare(image, "bgr8");
+		}
+
+		cv_bridge::CvImageConstPtr imageDepthPtr = cv_bridge::toCvShare(imageDepth);
+
+		rtabmap::CameraModel model = rtabmap_conversions::cameraModelFromROS(*cameraInfo);
+
+		pcl::PointCloud<pcl::PointXYZRGB>::Ptr pclCloud;
+
+		cv::Mat rgb = imagePtr->image;
+		cv::Mat depth = imageDepthPtr->image;
+		if( roiRatios_.size() == 4 &&
+			((roiRatios_[0] > 0.0f && roiRatios_[0] <= 1.0f) ||
+			 (roiRatios_[1] > 0.0f && roiRatios_[1] <= 1.0f) ||
+			 (roiRatios_[2] > 0.0f && roiRatios_[2] <= 1.0f) ||
+			 (roiRatios_[3] > 0.0f && roiRatios_[3] <= 1.0f)))
+		{
+			cv::Rect roiDepth = rtabmap::util2d::computeRoi(depth, roiRatios_);
+			cv::Rect roiRgb = rtabmap::util2d::computeRoi(rgb, roiRatios_);
+			if(	roiDepth.width%decimation_==0 &&
+				roiDepth.height%decimation_==0 &&
+				roiRgb.width%decimation_==0 &&
+				roiRgb.height%decimation_==0)
+			{
+				depth = cv::Mat(depth, roiDepth);
+				rgb = cv::Mat(rgb, roiRgb);
+				model = model.roi(roiRgb);
+			}
+			else
+			{
+				RCLCPP_ERROR(this->get_logger(), "Cannot apply ROI ratios [%f,%f,%f,%f] because resulting "
+					  "dimension (depth=%dx%d rgb=%dx%d) cannot be divided exactly "
+					  "by decimation parameter (%d). Ignoring ROI ratios...",
+					  roiRatios_[0],
+					  roiRatios_[1],
+					  roiRatios_[2],
+					  roiRatios_[3],
+					  roiDepth.width,
+					  roiDepth.height,
+					  roiRgb.width,
+					  roiRgb.height,
+					  decimation_);
+			}
+		}
+
+		pcl::IndicesPtr indices(new std::vector<int>);
+		pclCloud = rtabmap::util3d::cloudFromDepthRGB(
+				rgb,
+				depth,
+				model,
+				decimation_,
+				maxDepth_,
+				minDepth_,
+				indices.get());
+
+		processAndPublish(pclCloud, indices, imagePtr->header);
+
+		RCLCPP_DEBUG(this->get_logger(), "point_cloud_xyzrgb from RGB-D time = %f s", (now() - time).seconds());
+	}
+}
+
+void PointCloudXYZRGB::disparityCallback(
+		const sensor_msgs::msg::Image::ConstSharedPtr image,
+		const stereo_msgs::msg::DisparityImage::ConstSharedPtr imageDisparity,
+		const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfo)
+{
+	cv_bridge::CvImageConstPtr imagePtr;
+	if(image->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1)==0)
+	{
+		imagePtr = cv_bridge::toCvShare(image);
+	}
+	else if(image->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
+			image->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0)
+	{
+		imagePtr = cv_bridge::toCvShare(image, "mono8");
+	}
+	else
+	{
+		imagePtr = cv_bridge::toCvShare(image, "bgr8");
+	}
+
+	if(imageDisparity->image.encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1) !=0 &&
+	   imageDisparity->image.encoding.compare(sensor_msgs::image_encodings::TYPE_16SC1) !=0)
+	{
+		RCLCPP_ERROR(this->get_logger(), "Input type must be disparity=32FC1 or 16SC1");
+		return;
+	}
+
+	cv::Mat disparity;
+	if(imageDisparity->image.encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1) == 0)
+	{
+		disparity = cv::Mat(imageDisparity->image.height, imageDisparity->image.width, CV_32FC1, const_cast<uchar*>(imageDisparity->image.data.data()));
+	}
+	else
+	{
+		disparity = cv::Mat(imageDisparity->image.height, imageDisparity->image.width, CV_16SC1, const_cast<uchar*>(imageDisparity->image.data.data()));
+	}
+
+	if(cloudPub_->get_subscription_count())
+	{
+		rclcpp::Time time = now();
+
+		cv::Rect roi = rtabmap::util2d::computeRoi(disparity, roiRatios_);
+
+		pcl::PointCloud<pcl::PointXYZRGB>::Ptr pclCloud;
+		rtabmap::CameraModel leftModel = rtabmap_conversions::cameraModelFromROS(*cameraInfo);
+		UASSERT(disparity.cols == leftModel.imageWidth() && disparity.rows == leftModel.imageHeight());
+		UASSERT(imagePtr->image.cols == leftModel.imageWidth() && imagePtr->image.rows == leftModel.imageHeight());
+		rtabmap::StereoCameraModel stereoModel(imageDisparity->f, imageDisparity->f, leftModel.cx()-roiRatios_[0]*double(disparity.cols), leftModel.cy()-roiRatios_[2]*double(disparity.rows), imageDisparity->t);
+		pcl::IndicesPtr indices(new std::vector<int>);
+		pclCloud = rtabmap::util3d::cloudFromDisparityRGB(
+				cv::Mat(imagePtr->image, roi),
+				cv::Mat(disparity, roi),
+				stereoModel,
+				decimation_,
+				maxDepth_,
+				minDepth_,
+				indices.get());
+
+		processAndPublish(pclCloud, indices, imageDisparity->header);
+
+		RCLCPP_DEBUG(this->get_logger(), "point_cloud_xyzrgb from disparity time = %f s", (now() - time).seconds());
+	}
+}
+
+void PointCloudXYZRGB::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)
+{
+	if(!(imageLeft->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
+			imageLeft->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0 ||
+			imageLeft->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
+			imageLeft->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0 ||
+			imageLeft->encoding.compare(sensor_msgs::image_encodings::RGBA8) == 0 ||
+			imageLeft->encoding.compare(sensor_msgs::image_encodings::BGRA8) == 0) ||
+		!(imageRight->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
+			imageRight->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0 ||
+			imageRight->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
+			imageRight->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0 ||
+			imageRight->encoding.compare(sensor_msgs::image_encodings::RGBA8) == 0 ||
+			imageRight->encoding.compare(sensor_msgs::image_encodings::BGRA8) == 0))
+	{
+		RCLCPP_ERROR(this->get_logger(), "Input type must be image=mono8,mono16,rgb8,bgr8,rgba8,bgra8 (enc=%s)", imageLeft->encoding.c_str());
+		return;
+	}
+
+	if(cloudPub_->get_subscription_count())
+	{
+		rclcpp::Time time = now();
+
+		cv_bridge::CvImageConstPtr ptrLeftImage, ptrRightImage;
+		if(imageLeft->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
+			imageLeft->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0)
+		{
+			ptrLeftImage = cv_bridge::toCvShare(imageLeft, "mono8");
+		}
+		else
+		{
+			ptrLeftImage = cv_bridge::toCvShare(imageLeft, "bgr8");
+		}
+		ptrRightImage = cv_bridge::toCvShare(imageRight, "mono8");
+
+		if(roiRatios_[0]!=0.0f || roiRatios_[1]!=0.0f || roiRatios_[2]!=0.0f || roiRatios_[3]!=0.0f)
+		{
+			RCLCPP_WARN(this->get_logger(), "\"roi_ratios\" set but ignored for stereo images.");
+		}
+
+		pcl::PointCloud<pcl::PointXYZRGB>::Ptr pclCloud;
+		pcl::IndicesPtr indices(new std::vector<int>);
+		pclCloud = rtabmap::util3d::cloudFromStereoImages(
+				ptrLeftImage->image,
+				ptrRightImage->image,
+				rtabmap_conversions::stereoCameraModelFromROS(*camInfoLeft, *camInfoRight),
+				decimation_,
+				maxDepth_,
+				minDepth_,
+				indices.get(),
+				stereoBMParameters_);
+
+		processAndPublish(pclCloud, indices, imageLeft->header);
+
+		RCLCPP_DEBUG(this->get_logger(), "point_cloud_xyzrgb from stereo time = %f s", (now() - time).seconds());
+	}
+}
+
+void PointCloudXYZRGB::rgbdImageCallback(
+		const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image)
+{
+	if(cloudPub_->get_subscription_count())
+	{
+		rclcpp::Time time = now();
+
+		rtabmap::SensorData data = rtabmap_conversions::rgbdImageFromROS(image);
+		pcl::PointCloud<pcl::PointXYZRGB>::Ptr pclCloud;
+		pcl::IndicesPtr indices(new std::vector<int>);
+		if(data.isValid())
+		{
+			pclCloud = rtabmap::util3d::cloudRGBFromSensorData(
+					data,
+					decimation_,
+					maxDepth_,
+					minDepth_,
+					indices.get(),
+					stereoBMParameters_,
+					roiRatios_);
+
+			processAndPublish(pclCloud, indices, image->header);
+		}
+
+		RCLCPP_DEBUG(this->get_logger(), "point_cloud_xyzrgb from rgbd_image time = %f s", (now() - time).seconds());
+	}
+}
+
+void PointCloudXYZRGB::processAndPublish(
+		pcl::PointCloud<pcl::PointXYZRGB>::Ptr & pclCloud,
+		pcl::IndicesPtr & indices,
+		const std_msgs::msg::Header & header)
+{
+	if(indices->size() && voxelSize_ > 0.0)
+	{
+		pclCloud = rtabmap::util3d::voxelize(pclCloud, indices, voxelSize_);
+		pclCloud->is_dense = true;
+	}
+
+	// Do radius filtering after voxel filtering ( a lot faster)
+	if(!pclCloud->empty() && (pclCloud->is_dense || !indices->empty()) && noiseFilterRadius_ > 0.0 && noiseFilterMinNeighbors_ > 0)
+	{
+		if(pclCloud->is_dense)
+		{
+			indices = rtabmap::util3d::radiusFiltering(pclCloud, noiseFilterRadius_, noiseFilterMinNeighbors_);
+		}
+		else
+		{
+			indices = rtabmap::util3d::radiusFiltering(pclCloud, indices, noiseFilterRadius_, noiseFilterMinNeighbors_);
+		}
+		pcl::PointCloud<pcl::PointXYZRGB>::Ptr tmp(new pcl::PointCloud<pcl::PointXYZRGB>);
+		pcl::copyPointCloud(*pclCloud, *indices, *tmp);
+		pclCloud = tmp;
+	}
+
+	sensor_msgs::msg::PointCloud2::UniquePtr rosCloud(new sensor_msgs::msg::PointCloud2);
+	if(!pclCloud->empty() && (pclCloud->is_dense || !indices->empty()) && (normalK_ > 0 || normalRadius_ > 0.0f))
+	{
+		//compute normals
+		pcl::PointCloud<pcl::Normal>::Ptr normals = rtabmap::util3d::computeNormals(pclCloud, normalK_, normalRadius_);
+		pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr pclCloudNormal(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
+		pcl::concatenateFields(*pclCloud, *normals, *pclCloudNormal);
+		if(filterNaNs_)
+		{
+			pclCloudNormal = rtabmap::util3d::removeNaNNormalsFromPointCloud(pclCloudNormal);
+		}
+		pcl::toROSMsg(*pclCloudNormal, *rosCloud);
+	}
+	else
+	{
+		if(filterNaNs_ && !pclCloud->is_dense)
+		{
+			pclCloud = rtabmap::util3d::removeNaNFromPointCloud(pclCloud);
+		}
+		pcl::toROSMsg(*pclCloud, *rosCloud);
+	}
+	rosCloud->header.stamp = header.stamp;
+	rosCloud->header.frame_id = header.frame_id;
+
+	//publish the message
+	cloudPub_->publish(std::move(rosCloud));
+}
+
+}
+
+#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::PointCloudXYZRGB)
+

rtabmap_util/src/nodelets/pointcloud_to_depthimage.cpp

@@ -0,0 +1,269 @@
+/*
+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/pointcloud_to_depthimage.hpp>
+
+#include <rtabmap_conversions/MsgConversion.h>
+#include <rtabmap/core/util3d.h>
+#include <rtabmap/core/util2d.h>
+#include <rtabmap/utilite/ULogger.h>
+
+#include <sensor_msgs/image_encodings.hpp>
+
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+#include <pcl_conversions/pcl_conversions.h>
+
+namespace rtabmap_util
+{
+
+PointCloudToDepthImage::PointCloudToDepthImage(const rclcpp::NodeOptions & options) :
+		Node("pointcloud_to_depthimage", options),
+		waitForTransform_(0.1),
+		fillHolesSize_ (0),
+		fillHolesError_(0.1),
+		fillIterations_(1),
+		decimation_(1),
+		upscale_(false),
+		upscaleDepthErrorRatio_(0.02),
+		approxSync_(0),
+		exactSync_(0)
+{
+	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 = 10;
+	int qos = 0;
+	bool approx = true;
+	queueSize = this->declare_parameter("queue_size", queueSize);
+	qos = this->declare_parameter("qos", qos);
+	int qosCamInfo = this->declare_parameter("qos_camera_info", qos);
+	fixedFrameId_ = this->declare_parameter("fixed_frame_id", fixedFrameId_);
+	waitForTransform_ = this->declare_parameter("wait_for_transform", waitForTransform_);
+	fillHolesSize_ = this->declare_parameter("fill_holes_size", fillHolesSize_);
+	fillHolesError_ = this->declare_parameter("fill_holes_error", fillHolesError_);
+	fillIterations_ = this->declare_parameter("fill_iterations", fillIterations_);
+	decimation_ = this->declare_parameter("decimation", decimation_);
+	approx = this->declare_parameter("approx", approx);
+	upscale_ = this->declare_parameter("upscale", upscale_);
+	upscaleDepthErrorRatio_ = this->declare_parameter("upscale_depth_error_ratio", upscaleDepthErrorRatio_);
+
+	if(fixedFrameId_.empty() && approx)
+	{
+		RCLCPP_FATAL(this->get_logger(), "fixed_frame_id should be set when using approximate "
+				"time synchronization (approx=true)! If the robot "
+				"is moving, it could be \"odom\". If not moving, it "
+				"could be \"base_link\".");
+	}
+
+	RCLCPP_INFO(this->get_logger(), "Params:");
+	RCLCPP_INFO(this->get_logger(), "  approx=%s", approx?"true":"false");
+	RCLCPP_INFO(this->get_logger(), "  queue_size=%d", queueSize);
+	RCLCPP_INFO(this->get_logger(), "  fixed_frame_id=%s", fixedFrameId_.c_str());
+	RCLCPP_INFO(this->get_logger(), "  wait_for_transform=%fs", waitForTransform_);
+	RCLCPP_INFO(this->get_logger(), "  fill_holes_size=%d pixels (0=disabled)", fillHolesSize_);
+	RCLCPP_INFO(this->get_logger(), "  fill_holes_error=%f", fillHolesError_);
+	RCLCPP_INFO(this->get_logger(), "  fill_iterations=%d", fillIterations_);
+	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
+	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));
+
+	if(approx)
+	{
+		approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize), pointCloudSub_, cameraInfoSub_);
+		approxSync_->registerCallback(std::bind(&PointCloudToDepthImage::callback, this, std::placeholders::_1, std::placeholders::_2));
+	}
+	else
+	{
+		fixedFrameId_.clear();
+		exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(queueSize), pointCloudSub_, cameraInfoSub_);
+		exactSync_->registerCallback(std::bind(&PointCloudToDepthImage::callback, this, std::placeholders::_1, std::placeholders::_2));
+	}
+
+	pointCloudSub_.subscribe(this, "cloud", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
+	cameraInfoSub_.subscribe(this, "camera_info", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosCamInfo).get_rmw_qos_profile());
+}
+
+PointCloudToDepthImage::~PointCloudToDepthImage()
+{
+	delete approxSync_;
+	delete exactSync_;
+}
+
+void PointCloudToDepthImage::callback(
+		const sensor_msgs::msg::PointCloud2::ConstSharedPtr pointCloud2Msg,
+		const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg)
+{
+	if(depthImage32Pub_.getNumSubscribers() > 0 || depthImage16Pub_.getNumSubscribers() > 0)
+	{
+		double cloudStamp = rtabmap_conversions::timestampFromROS(pointCloud2Msg->header.stamp);
+		double infoStamp = rtabmap_conversions::timestampFromROS(cameraInfoMsg->header.stamp);
+
+		rtabmap::Transform cloudDisplacement = rtabmap::Transform::getIdentity();
+		if(!fixedFrameId_.empty())
+		{
+			// approx sync
+			cloudDisplacement = rtabmap_conversions::getTransform(
+					pointCloud2Msg->header.frame_id,
+					fixedFrameId_,
+					cameraInfoMsg->header.stamp,
+					pointCloud2Msg->header.stamp,
+					*tfBuffer_,
+					waitForTransform_);
+		}
+
+		if(cloudDisplacement.isNull())
+		{
+			return;
+		}
+
+		rtabmap::Transform cloudToCamera = rtabmap_conversions::getTransform(
+				pointCloud2Msg->header.frame_id,
+				cameraInfoMsg->header.frame_id,
+				cameraInfoMsg->header.stamp,
+				*tfBuffer_,
+				waitForTransform_);
+
+		if(cloudToCamera.isNull())
+		{
+			return;
+		}
+		rtabmap::Transform localTransform = cloudDisplacement*cloudToCamera;
+
+		rtabmap::CameraModel model = rtabmap_conversions::cameraModelFromROS(*cameraInfoMsg, localTransform);
+		sensor_msgs::msg::CameraInfo cameraInfoMsgOut = *cameraInfoMsg;
+
+		if(decimation_ > 1)
+		{
+			if(model.imageWidth()%decimation_ == 0 && model.imageHeight()%decimation_ == 0)
+			{
+				float scale = 1.0f/float(decimation_);
+				model = model.scaled(scale);
+				rtabmap_conversions::cameraModelToROS(model, cameraInfoMsgOut);
+			}
+			else
+			{
+				RCLCPP_ERROR(this->get_logger(), "decimation (%d) not valid for image size %dx%d",
+						decimation_,
+						model.imageWidth(),
+						model.imageHeight());
+			}
+		}
+
+		UASSERT_MSG(pointCloud2Msg->data.size() == pointCloud2Msg->row_step*pointCloud2Msg->height,
+						uFormat("data=%d row_step=%d height=%d", pointCloud2Msg->data.size(), pointCloud2Msg->row_step, pointCloud2Msg->height).c_str());
+
+		pcl::PCLPointCloud2::Ptr cloud(new pcl::PCLPointCloud2);
+		pcl_conversions::toPCL(*pointCloud2Msg, *cloud);
+
+		cv_bridge::CvImage depthImage;
+
+		if(cloud->data.empty())
+		{
+			RCLCPP_WARN(this->get_logger(), "Received an empty cloud on topic \"%s\"! A depth image with all zeros is returned.", pointCloudSub_.getTopic().c_str());
+			depthImage.image = cv::Mat::zeros(model.imageSize(), CV_32FC1);
+		}
+		else
+		{
+			depthImage.image = rtabmap::util3d::projectCloudToCamera(model.imageSize(), model.K(), cloud, model.localTransform());
+
+			if(fillHolesSize_ > 0 && fillIterations_ > 0)
+			{
+				for(int i=0; i<fillIterations_;++i)
+				{
+					depthImage.image = rtabmap::util2d::fillDepthHoles(depthImage.image, fillHolesSize_, fillHolesError_);
+				}
+
+				if(pointCloudTransformedPub_->get_subscription_count()>0)
+				{
+					sensor_msgs::msg::PointCloud2 pointCloud2Out;
+					rtabmap_conversions::transformPointCloud(model.localTransform().inverse().toEigen4f(), *pointCloud2Msg, pointCloud2Out);
+					pointCloud2Out.header = cameraInfoMsg->header;
+					pointCloudTransformedPub_->publish(pointCloud2Out);
+				}
+			}
+		}
+
+		depthImage.header = cameraInfoMsg->header;
+
+		if(decimation_>1 && upscale_)
+		{
+			depthImage.image = rtabmap::util2d::interpolate(depthImage.image, decimation_, upscaleDepthErrorRatio_);
+		}
+
+		if(depthImage32Pub_.getNumSubscribers())
+		{
+			depthImage.encoding = sensor_msgs::image_encodings::TYPE_32FC1;
+			depthImage32Pub_.publish(depthImage.toImageMsg(), cameraInfoMsg);
+			if(cameraInfo32Pub_->get_subscription_count())
+			{
+				cameraInfo32Pub_->publish(cameraInfoMsgOut);
+			}
+		}
+
+		if(depthImage16Pub_.getNumSubscribers())
+		{
+			depthImage.encoding = sensor_msgs::image_encodings::TYPE_16UC1;
+			depthImage.image = rtabmap::util2d::cvtDepthFromFloat(depthImage.image);
+			depthImage16Pub_.publish(depthImage.toImageMsg(), cameraInfoMsg);
+			if(cameraInfo16Pub_->get_subscription_count())
+			{
+				cameraInfo16Pub_->publish(cameraInfoMsgOut);
+			}
+		}
+
+		if( cloudStamp != rtabmap_conversions::timestampFromROS(pointCloud2Msg->header.stamp) ||
+			infoStamp != rtabmap_conversions::timestampFromROS(cameraInfoMsg->header.stamp))
+		{
+			RCLCPP_ERROR(this->get_logger(), "Input stamps changed between the beginning and the end of the callback! Make "
+					"sure the node publishing the topics doesn't override the same data after publishing them. A "
+					"solution is to use this node within another nodelet manager. Stamps: "
+					"cloud=%f->%f info=%f->%f",
+					cloudStamp, rtabmap_conversions::timestampFromROS(pointCloud2Msg->header.stamp),
+					infoStamp, rtabmap_conversions::timestampFromROS(cameraInfoMsg->header.stamp));
+		}
+	}
+}
+}
+
+#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::PointCloudToDepthImage)

rtabmap_util/src/nodelets/rgbd_relay.cpp

@@ -0,0 +1,162 @@
+/*
+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/rgbd_relay.hpp"
+
+#include <sensor_msgs/msg/image.hpp>
+#include <sensor_msgs/msg/compressed_image.hpp>
+#include <sensor_msgs/msg/camera_info.hpp>
+#include <sensor_msgs/image_encodings.hpp>
+
+#include <cv_bridge/cv_bridge.h>
+#include <opencv2/highgui/highgui.hpp>
+
+#include "rtabmap_conversions/MsgConversion.h"
+
+#include "rtabmap/core/Compression.h"
+#include "rtabmap/utilite/UConversion.h"
+
+namespace rtabmap_util
+{
+
+RGBDRelay::RGBDRelay(const rclcpp::NodeOptions & options) :
+	Node("rgbd_relay", options),
+	compress_(false),
+	uncompress_(false)
+{
+	int qos = 0;
+	qos = this->declare_parameter("qos", qos);
+	compress_ = this->declare_parameter("compress", compress_);
+	uncompress_ = this->declare_parameter("uncompress", uncompress_);
+
+	rgbdImageSub_ = create_subscription<rtabmap_msgs::msg::RGBDImage>("rgbd_image", rclcpp::QoS(5).reliability((rmw_qos_reliability_policy_t)qos), std::bind(&RGBDRelay::callback, this, std::placeholders::_1));
+	rgbdImagePub_ = create_publisher<rtabmap_msgs::msg::RGBDImage>("rgbd_image_relay", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos));
+}
+
+void RGBDRelay::callback(const rtabmap_msgs::msg::RGBDImage::SharedPtr input) const
+{
+	if(rgbdImagePub_->get_subscription_count())
+	{
+		if(!compress_ && !uncompress_)
+		{
+			//just republish it
+			rgbdImagePub_->publish(*input);
+			return;
+		}
+
+		auto output = std::make_unique<rtabmap_msgs::msg::RGBDImage>();
+		output->header = input->header;
+		output->rgb_camera_info = input->rgb_camera_info;
+		output->depth_camera_info = input->depth_camera_info;
+		output->key_points = input->key_points;
+		output->points = input->points;
+		output->descriptors = input->descriptors;
+		output->global_descriptor = input->global_descriptor;
+
+		rtabmap::StereoCameraModel stereoModel = rtabmap_conversions::stereoCameraModelFromROS(input->rgb_camera_info, input->depth_camera_info, rtabmap::Transform::getIdentity());
+
+		if(compress_)
+		{
+			if(!input->rgb_compressed.data.empty())
+			{
+				// already compressed, just copy pointer
+				output->rgb_compressed = input->rgb_compressed;
+			}
+			else if(!input->rgb.data.empty())
+			{
+				cv_bridge::CvImageConstPtr rgb = cv_bridge::toCvShare(input->rgb, input);
+				rgb->toCompressedImageMsg(output->rgb_compressed, cv_bridge::JPG);
+			}
+
+			if(!input->depth_compressed.data.empty())
+			{
+				// already compressed, just copy pointer
+				output->depth_compressed = input->depth_compressed;
+			}
+			else if(!input->depth.data.empty())
+			{
+				if(stereoModel.isValidForProjection())
+				{
+					// right stereo image
+					cv_bridge::CvImageConstPtr imageRightPtr = cv_bridge::toCvShare(input->depth, input);
+					imageRightPtr->toCompressedImageMsg(output->depth_compressed, cv_bridge::JPG);
+				}
+				else
+				{
+					// depth image
+					cv_bridge::CvImageConstPtr imageDepthPtr = cv_bridge::toCvShare(input->depth, input);
+					output->depth_compressed.data = rtabmap::compressImage(imageDepthPtr->image, ".png");
+					output->depth_compressed.format = "png";
+				}
+			}
+		}
+		if(uncompress_)
+		{
+			if(!input->rgb.data.empty())
+			{
+				// already raw, just copy pointer
+				output->rgb = input->rgb;
+			}
+			if(!input->rgb_compressed.data.empty())
+			{
+				cv_bridge::toCvCopy(input->rgb_compressed)->toImageMsg(output->rgb);
+			}
+
+			if(!input->depth.data.empty())
+			{
+				// already raw, just copy pointer
+				output->depth = input->depth;
+			}
+			else if(input->depth_compressed.format.compare("jpg")==0)
+			{
+				// right stereo image
+				cv_bridge::toCvCopy(input->depth_compressed)->toImageMsg(output->depth);
+			}
+			else
+			{
+				// dpeth image
+				auto cvImg = std::make_unique<cv_bridge::CvImage>();
+				cvImg->header = input->depth_compressed.header;
+				cvImg->image = rtabmap::uncompressImage(input->depth_compressed.data);
+				UASSERT(cvImg->image.empty() || cvImg->image.type() == CV_32FC1 || cvImg->image.type() == CV_16UC1);
+				cvImg->encoding = cvImg->image.empty()?"":cvImg->image.type() == CV_32FC1?sensor_msgs::image_encodings::TYPE_32FC1:sensor_msgs::image_encodings::TYPE_16UC1;
+				cvImg->toImageMsg(output->depth);
+			}
+		}
+
+		rgbdImagePub_->publish(std::move(output));
+	}
+}
+
+}
+
+#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::RGBDRelay)

rtabmap_util/src/nodelets/rgbd_split.cpp

@@ -0,0 +1,112 @@
+/*
+Copyright (c) 2010-2022, 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/rgbd_split.hpp>
+
+#include <cv_bridge/cv_bridge.h>
+
+namespace rtabmap_util
+{
+
+RGBDSplit::RGBDSplit(const rclcpp::NodeOptions & options) :
+	Node("rgbd_split", options)
+{
+	int queueSize = 10;
+	int qos = 0;
+	queueSize = this->declare_parameter("queue_size", queueSize);
+	qos = this->declare_parameter("qos", qos);
+
+	RCLCPP_INFO(this->get_logger(), "%s: queue_size  = %d", get_name(), queueSize);
+	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());
+}
+
+
+void RGBDSplit::callback(const rtabmap_msgs::msg::RGBDImage::SharedPtr input) const
+{
+	if(rgbPub_.getNumSubscribers())
+	{
+		sensor_msgs::msg::Image outputImage;
+		sensor_msgs::msg::CameraInfo outputCameraInfo;
+		outputImage.header = outputCameraInfo.header = input->header;
+		outputCameraInfo = input->rgb_camera_info;
+
+		if(!input->rgb.data.empty())
+		{
+			// already raw, just copy pointer
+			outputImage = input->rgb;
+		}
+		else if(!input->rgb_compressed.data.empty())
+		{
+#ifdef CV_BRIDGE_HYDRO
+			ROS_ERROR("Unsupported compressed image copy, please upgrade at least to ROS Indigo to use this.");
+#else
+			cv_bridge::toCvCopy(input->rgb_compressed)->toImageMsg(outputImage);
+#endif
+		}
+		rgbPub_.publish(outputImage, outputCameraInfo);
+	}
+
+	if(depthPub_.getNumSubscribers())
+	{
+		sensor_msgs::msg::Image outputImage;
+		sensor_msgs::msg::CameraInfo outputCameraInfo;
+		outputCameraInfo = input->depth_camera_info;
+
+		if(!input->depth.data.empty())
+		{
+			// already raw, just copy pointer
+			outputImage = input->depth;
+		}
+		else if(!input->depth_compressed.data.empty())
+		{
+#ifdef CV_BRIDGE_HYDRO
+			ROS_ERROR("Unsupported compressed image copy, please upgrade at least to ROS Indigo to use this.");
+#else
+			cv_bridge::toCvCopy(input->depth_compressed)->toImageMsg(outputImage);
+#endif
+		}
+		outputImage.header = outputCameraInfo.header = input->header;
+		depthPub_.publish(outputImage, outputCameraInfo);
+	}
+}
+
+}
+
+#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::RGBDSplit)
+