Merged master->ros2, added vlp16 launch example, added d435i stereo launch example

CMakeLists.txt

@@ -296,6 +296,7 @@ SET(rtabmap_plugins_lib_src
     src/nodelets/stereo_sync.cpp 
     src/nodelets/rgb_sync.cpp 
     src/nodelets/rgbd_relay.cpp
+    src/nodelets/rgbd_split.cpp
     src/nodelets/lidar_deskewing.cpp
 )
 
@@ -434,6 +435,11 @@ ament_target_dependencies(rtabmap_rgbd_relay ${Libraries})
 target_link_libraries(rtabmap_rgbd_relay rtabmap_plugins ${RTABMap_LIBRARIES})
 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_plugins ${RTABMap_LIBRARIES})
+set_target_properties(rtabmap_rgbd_split PROPERTIES OUTPUT_NAME "rgbd_split")
+
 add_executable(rtabmap_point_cloud_xyz src/PointCloudXYZNode.cpp)
 ament_target_dependencies(rtabmap_point_cloud_xyz ${Libraries})
 target_link_libraries(rtabmap_point_cloud_xyz rtabmap_plugins ${RTABMap_LIBRARIES})
@@ -551,6 +557,9 @@ foreach(typesupport_impl ${typesupport_impls})
   rosidl_get_typesupport_target(rtabmap_rgbd_relay
     ${PROJECT_NAME} ${typesupport_impl}
   )
+  rosidl_get_typesupport_target(rtabmap_rgbd_split
+    ${PROJECT_NAME} ${typesupport_impl}
+  )
   rosidl_get_typesupport_target(rtabmap_rgbd_sync
     ${PROJECT_NAME} ${typesupport_impl}
   )
@@ -732,6 +741,7 @@ install(TARGETS
    rtabmap_stereo_sync
    rtabmap_rgb_sync
    rtabmap_rgbd_relay
+   rtabmap_rgbd_split
 #   rtabmap_wifi_signal_sub
    DESTINATION lib/${PROJECT_NAME}
 )

include/rtabmap_ros/CoreWrapper.h

@@ -288,6 +288,7 @@ private:
 	int genDepthFillIterations_;
 	double genDepthFillHolesError_;
 	int scanCloudMaxPoints_;
+	bool scanCloudIs2d_;
 
 	rtabmap::Transform mapToOdom_;
 	std::mutex mapToOdomMutex_;

include/rtabmap_ros/MsgConversion.h

@@ -263,7 +263,8 @@ bool convertScan3dMsg(
 		tf2_ros::Buffer & tfBuffer,
 		double waitForTransform,
 		int maxPoints = 0,
-		float maxRange = 0.0f);
+		float maxRange = 0.0f,
+		bool is2D = false);
 
 // Missing function in ros2 (from old pcl_ros)
 void transformPointCloud (

include/rtabmap_ros/icp_odometry.hpp

@@ -68,6 +68,7 @@ private:
 	rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr cloud_sub_;
 	rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr filtered_scan_pub_;
 	int scanCloudMaxPoints_;
+	bool scanCloudIs2d_;
 	int scanDownsamplingStep_;
 	double scanRangeMin_;
 	double scanRangeMax_;

include/rtabmap_ros/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_ros/visibility.h>
+#include "rclcpp/rclcpp.hpp"
+
+#include <sensor_msgs/image_encodings.hpp>
+
+#include <image_transport/image_transport.hpp>
+
+#include "rtabmap_ros/msg/rgbd_image.hpp"
+
+namespace rtabmap_ros
+{
+
+class RGBDSplit : public rclcpp::Node
+{
+public:
+	RTABMAP_ROS_PUBLIC
+	explicit RGBDSplit(const rclcpp::NodeOptions & options);
+
+	virtual ~RGBDSplit() {}
+
+	void callback(const rtabmap_ros::msg::RGBDImage::SharedPtr input) const;
+
+private:
+	rclcpp::Subscription<rtabmap_ros::msg::RGBDImage>::SharedPtr rgbdImageSub_;
+
+	image_transport::CameraPublisher rgbPub_;
+	image_transport::CameraPublisher depthPub_;
+};
+
+}
+

launch/ros2/realsense_d400.launch.py

@@ -17,6 +17,7 @@ def generate_launch_description():
     parameters=[{
           'frame_id':'camera_link',
           'subscribe_depth':True,
+          'subscribe_odom_info':True,
           'approx_sync':False}]
 
     remappings=[

launch/ros2/realsense_d435i_color.launch.py

@@ -15,6 +15,7 @@ def generate_launch_description():
     parameters=[{
           'frame_id':'camera_link',
           'subscribe_depth':True,
+          'subscribe_odom_info':True,
           'approx_sync':False,
           'wait_imu_to_init':True}]
 

launch/ros2/realsense_d435i_infra.launch.py

@@ -16,6 +16,7 @@ def generate_launch_description():
     parameters=[{
           'frame_id':'camera_link',
           'subscribe_depth':True,
+          'subscribe_odom_info':True,
           'approx_sync':False,
           'wait_imu_to_init':True}]
 

launch/ros2/realsense_d435i_stereo.launch.py

@@ -0,0 +1,60 @@
+# Requirements:
+#   A realsense D435i
+#   Install realsense2 ros2 package (ros-$ROS_DISTRO-realsense2-camera)
+# Example:
+#   $ ros2 launch realsense2_camera rs_launch.py enable_gyro:=true enable_accel:=true unite_imu_method:=1 enable_infra1:=true enable_infra2:=true enable_sync:=true
+#   $ ros2 param set /camera/camera depth_module.emitter_enabled 0
+#
+#   $ ros2 launch rtabmap_ros realsense_d435i_stereo.launch.py
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+
+def generate_launch_description():
+    parameters=[{
+          'frame_id':'camera_link',
+          'subscribe_stereo':True,
+          'subscribe_odom_info':True,
+          'wait_imu_to_init':True}]
+
+    remappings=[
+          ('imu', '/imu/data'),
+          ('left/image_rect', '/camera/infra1/image_rect_raw'),
+          ('left/camera_info', '/camera/infra1/camera_info'),
+          ('right/image_rect', '/camera/infra2/image_rect_raw'),
+          ('right/camera_info', '/camera/infra2/camera_info')]
+
+    return LaunchDescription([
+
+        # Nodes to launch       
+        Node(
+            package='rtabmap_ros', executable='stereo_odometry', output='screen',
+            parameters=parameters,
+            remappings=remappings),
+
+        Node(
+            package='rtabmap_ros', executable='rtabmap', output='screen',
+            parameters=parameters,
+            remappings=remappings,
+            arguments=['-d']),
+
+        Node(
+            package='rtabmap_ros', executable='rtabmapviz', output='screen',
+            parameters=parameters,
+            remappings=remappings),
+                
+        # Compute quaternion of the IMU
+        Node(
+            package='imu_filter_madgwick', executable='imu_filter_madgwick_node', output='screen',
+            parameters=[{'use_mag': False, 
+                         'world_frame':'enu', 
+                         'publish_tf':False}],
+            remappings=[('imu/data_raw', '/camera/imu')]),
+        
+        # The IMU frame is mising in TF tree, add it:
+        Node(
+            package='tf2_ros', executable='static_transform_publisher', output='screen',
+            arguments=['0', '0', '0', '0', '0', '0', 'camera_gyro_optical_frame', 'camera_imu_optical_frame']),
+    ])

launch/ros2/turtlebot3_rgbd.launch.py

@@ -24,7 +24,7 @@
 #   SLAM:
 #   $ ros2 launch rtabmap_ros turtlebot3_rgbd.launch.py
 #   OR
-#   $ ros2 launch rtabmap_ros rtabmap.launch.py visual_odometry:=false frame_id:=base_footprint odom_topic:=/odom args:="-d" use_sim_time:=true rgb_topic:=/camera/image_raw depth_topic:=/camera/depth/image_raw camera_info_topic:=/camera/camera_info approx_sync:=true
+#   $ ros2 launch rtabmap_ros rtabmap.launch.py visual_odometry:=false frame_id:=base_footprint odom_topic:=/odom args:="-d" use_sim_time:=true rgb_topic:=/camera/image_raw depth_topic:=/camera/depth/image_raw camera_info_topic:=/camera/camera_info approx_sync:=true qos:=2
 #
 #   Navigation (install nav2_bringup package):
 #     $ ros2 launch nav2_bringup navigation_launch.py use_sim_time:=True

launch/ros2/turtlebot3_rgbd_sync.launch.py

@@ -24,7 +24,7 @@
 #   SLAM:
 #   $ ros2 launch rtabmap_ros turtlebot3_rgbd_sync.launch.py
 #   OR
-#   $ ros2 launch rtabmap_ros rtabmap.launch.py visual_odometry:=false frame_id:=base_footprint subscribe_scan:=true  approx_sync:=true approx_rgbd_sync:=false odom_topic:=/odom args:="-d --RGBD/NeighborLinkRefining true --Reg/Strategy 1 --Reg/Force3DoF true --Grid/RangeMin 0.2" use_sim_time:=true rgbd_sync:=true rgb_topic:=/camera/image_raw depth_topic:=/camera/depth/image_raw camera_info_topic:=/camera/camera_info
+#   $ ros2 launch rtabmap_ros rtabmap.launch.py visual_odometry:=false frame_id:=base_footprint subscribe_scan:=true  approx_sync:=true approx_rgbd_sync:=false odom_topic:=/odom args:="-d --RGBD/NeighborLinkRefining true --Reg/Strategy 1 --Reg/Force3DoF true --Grid/RangeMin 0.2" use_sim_time:=true rgbd_sync:=true rgb_topic:=/camera/image_raw depth_topic:=/camera/depth/image_raw camera_info_topic:=/camera/camera_info qos:=2
 #
 #   Navigation (install nav2_bringup package):
 #     $ ros2 launch nav2_bringup navigation_launch.py use_sim_time:=True

launch/ros2/turtlebot3_scan.launch.py

@@ -10,7 +10,7 @@
 #   SLAM:
 #   $ ros2 launch rtabmap_ros turtlebot3_scan.launch.py
 #   OR
-#   $ ros2 launch rtabmap_ros rtabmap.launch.py visual_odometry:=false frame_id:=base_footprint subscribe_scan:=true depth:=false approx_sync:=true odom_topic:=/odom args:="-d --RGBD/NeighborLinkRefining true --Reg/Strategy 1 --Reg/Force3DoF true --Grid/RangeMin 0.2" use_sim_time:=true
+#   $ ros2 launch rtabmap_ros rtabmap.launch.py visual_odometry:=false frame_id:=base_footprint subscribe_scan:=true depth:=false approx_sync:=true odom_topic:=/odom args:="-d --RGBD/NeighborLinkRefining true --Reg/Strategy 1 --Reg/Force3DoF true --Grid/RangeMin 0.2" use_sim_time:=true qos:=2
 #
 #   Navigation (install nav2_bringup package):
 #     $ ros2 launch nav2_bringup navigation_launch.py use_sim_time:=True

launch/ros2/vlp16.launch.py

@@ -0,0 +1,126 @@
+# Example:
+#   $ ros2 launch velodyne_driver velodyne_driver_node-VLP16-launch.py
+#   $ ros2 launch velodyne_pointcloud velodyne_transform_node-VLP16-launch.py
+#
+#   SLAM:
+#   $ ros2 launch rtabmap_ros vlp16.launch.py
+
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+
+def generate_launch_description():
+
+    use_sim_time = LaunchConfiguration('use_sim_time')
+    deskewing = LaunchConfiguration('deskewing')
+    
+    return LaunchDescription([
+
+        # Launch arguments
+        DeclareLaunchArgument(
+            'use_sim_time', default_value='false',
+            description='Use simulation (Gazebo) clock if true'),
+        
+        DeclareLaunchArgument(
+            'deskewing', default_value='true',
+            description='Enable lidar deskewing'),
+          
+        # Nodes to launch
+        Node(
+            package='rtabmap_ros', executable='icp_odometry', output='screen',
+            parameters=[{
+              'frame_id':'velodyne',
+              'odom_frame_id':'odom',
+              'wait_for_transform':0.2,
+              'expected_update_rate':15.0,
+              'deskewing':deskewing,
+              'use_sim_time':use_sim_time,
+            }],
+            remappings=[
+              ('scan_cloud', '/velodyne_points')
+            ],
+            arguments=[
+              'Icp/PointToPlane', 'true',
+              'Icp/Iterations', '10',
+              'Icp/VoxelSize', '0.1',
+              'Icp/Epsilon', '0.001',
+              'Icp/PointToPlaneK', '20',
+              'Icp/PointToPlaneRadius', '0',
+              'Icp/MaxTranslation', '2',
+              'Icp/MaxCorrespondenceDistance', '1',
+              'Icp/Strategy', '1',
+              'Icp/OutlierRatio', '0.7',
+              'Icp/CorrespondenceRatio', '0.01',
+              'Odom/ScanKeyFrameThr', '0.6',
+              'OdomF2M/ScanSubtractRadius', '0.1',
+              'OdomF2M/ScanMaxSize', '15000',
+              'OdomF2M/BundleAdjustment', 'false',
+            ]),
+            
+        Node(
+            package='rtabmap_ros', executable='point_cloud_assembler', output='screen',
+            parameters=[{
+              'max_clouds':10,
+              'fixed_frame_id':'',
+              'use_sim_time':use_sim_time,
+            }],
+            remappings=[
+              ('cloud', 'odom_filtered_input_scan')
+            ]),
+            
+        Node(
+            package='rtabmap_ros', executable='rtabmap', output='screen',
+            parameters=[{
+              'frame_id':'velodyne',
+              'subscribe_depth':False,
+              'subscribe_rgb':False,
+              'subscribe_scan_cloud':True,
+              'approx_sync':False,
+              'wait_for_transform':0.2,
+              'use_sim_time':use_sim_time,
+            }],
+            remappings=[
+              ('scan_cloud', 'assembled_cloud')
+            ],
+            arguments=[
+              '-d', # This will delete the previous database (~/.ros/rtabmap.db)
+              'RGBD/ProximityMaxGraphDepth', '0',
+              'RGBD/ProximityPathMaxNeighbors', '1',
+              'RGBD/AngularUpdate', '0.05',
+              'RGBD/LinearUpdate', '0.05',
+              'RGBD/CreateOccupancyGrid', 'false',
+              'Mem/NotLinkedNodesKept', 'false',
+              'Mem/STMSize', '30',
+              'Mem/LaserScanNormalK', '20',
+              'Reg/Strategy', '1',
+              'Icp/VoxelSize', '0.1',
+              'Icp/PointToPlaneK', '20',
+              'Icp/PointToPlaneRadius', '0',
+              'Icp/PointToPlane', 'true',
+              'Icp/Iterations', '10',
+              'Icp/Epsilon', '0.001',
+              'Icp/MaxTranslation', '3',
+              'Icp/MaxCorrespondenceDistance', '1',
+              'Icp/Strategy', '1',
+              'Icp/OutlierRatio', '0.7',
+              'Icp/CorrespondenceRatio', '0.2',
+            ]), 
+     
+        Node(
+            package='rtabmap_ros', executable='rtabmapviz', output='screen',
+            parameters=[{
+              'frame_id':'velodyne',
+              'odom_frame_id':'odom',
+              'subscribe_odom_info':True,
+              'subscribe_scan_cloud':True,
+              'approx_sync':False,
+              'use_sim_time':use_sim_time,
+            }],
+            remappings=[
+               ('scan_cloud', 'odom_filtered_input_scan')
+            ]),
+    ])
+    
+    

src/CoreWrapper.cpp

@@ -114,6 +114,7 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) :
 		genDepthFillIterations_(1),
 		genDepthFillHolesError_(0.1),
 		scanCloudMaxPoints_(0),
+		scanCloudIs2d_(false),
 		mapToOdom_(rtabmap::Transform::getIdentity()),
 		transformThread_(0),
 		tfThreadRunning_(false),
@@ -201,6 +202,7 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) :
 	genDepthFillIterations_ = this->declare_parameter("gen_depth_fill_iterations", genDepthFillIterations_);
 	genDepthFillHolesError_ = this->declare_parameter("gen_depth_fill_holes_error", genDepthFillHolesError_);
 	scanCloudMaxPoints_ = this->declare_parameter("scan_cloud_max_points", scanCloudMaxPoints_);
+	scanCloudIs2d_ = this->declare_parameter("scan_cloud_is_2d", scanCloudIs2d_);
 
 	stereoToDepth_ = this->declare_parameter("stereo_to_depth", stereoToDepth_);
 	odomSensorSync_ = this->declare_parameter("odom_sensor_sync", odomSensorSync_);
@@ -246,6 +248,7 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) :
 	if(this->isSubscribedToScan3d())
 	{
 		RCLCPP_INFO(get_logger(), "rtabmap: scan_cloud_max_points = %d", scanCloudMaxPoints_);
+		RCLCPP_INFO(get_logger(), "rtabmap: scan_cloud_is_2d = %s", scanCloudIs2d_?"true":"false");
 	}
 
 	infoPub_ = this->create_publisher<rtabmap_ros::msg::Info>("info", 1);
@@ -437,7 +440,7 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) :
 				Parameters::kGridSensor().c_str());
 		parameters_.insert(ParametersPair(Parameters::kGridRangeMax(), "0"));
 	}
-	if(this->isSubscribedToScan3d() && parameters_.find(Parameters::kIcpPointToPlaneRadius()) == parameters_.end())
+	if(this->isSubscribedToScan3d() && !scanCloudIs2d_ && parameters_.find(Parameters::kIcpPointToPlaneRadius()) == parameters_.end())
 	{
 		RCLCPP_INFO(this->get_logger(), "Setting \"%s\" parameter to 0 (default %f) as \"subscribe_scan_cloud\" is true.",
 				Parameters::kIcpPointToPlaneRadius().c_str(),
@@ -449,13 +452,14 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) :
 	if(parameters_.find(Parameters::kRGBDProximityPathMaxNeighbors()) == parameters_.end() &&
 		(regStrategy == Registration::kTypeIcp || regStrategy == Registration::kTypeVisIcp))
 	{
-		if(this->isSubscribedToScan2d())
+		if(this->isSubscribedToScan2d() || (this->isSubscribedToScan3d() && scanCloudIs2d_))
 		{
-			RCLCPP_WARN(this->get_logger(), "Setting \"%s\" parameter to 10 (default 0) as \"subscribe_scan\" is "
+			RCLCPP_WARN(this->get_logger(), "Setting \"%s\" parameter to 10 (default 0) as \"%s\" is "
 					"true and \"%s\" uses ICP. Proximity detection by space will be also done by merging close "
 					"scans. To disable, set \"%s\" to 0. To suppress this warning, "
 					"add <param name=\"%s\" type=\"string\" value=\"10\"/>",
 					Parameters::kRGBDProximityPathMaxNeighbors().c_str(),
+					this->isSubscribedToScan2d()?"subscribe_scan":"scan_cloud_is_2d",
 					Parameters::kRegStrategy().c_str(),
 					Parameters::kRGBDProximityPathMaxNeighbors().c_str(),
 					Parameters::kRGBDProximityPathMaxNeighbors().c_str());
@@ -1385,7 +1389,9 @@ void CoreWrapper::commonMultiCameraCallbackImpl(
 				scan,
 				*tfBuffer_,
 				waitForTransform_,
-				scanCloudMaxPoints_))
+				scanCloudMaxPoints_,
+				0,
+				scanCloudIs2d_))
 		{
 			RCLCPP_ERROR(this->get_logger(), "Could not convert 3d laser scan msg! Aborting rtabmap update...");
 			return;
@@ -1590,7 +1596,9 @@ void CoreWrapper::commonLaserScanCallback(
 				scan,
 				*tfBuffer_,
 				waitForTransform_,
-				scanCloudMaxPoints_))
+				scanCloudMaxPoints_,
+				0,
+				scanCloudIs2d_))
 		{
 			RCLCPP_ERROR(this->get_logger(), "Could not convert 3d laser scan msg! Aborting rtabmap update...");
 			return;

src/MsgConversion.cpp

@@ -2347,10 +2347,11 @@ bool convertScanMsg(
 		double waitForTransform,
 		bool outputInFrameId)
 {
-	// make sure the frame of the laser is updated too
+	// make sure the frame of the laser is updated during the whole scan time
 	rtabmap::Transform tmpT = getTransform(
-			odomFrameId.empty()?frameId:odomFrameId,
 			scan2dMsg.header.frame_id,
+			odomFrameId.empty()?frameId:odomFrameId,
+			rclcpp::Time(scan2dMsg.header.stamp.sec, scan2dMsg.header.stamp.nanosec),
 			rclcpp::Time(scan2dMsg.header.stamp.sec, scan2dMsg.header.stamp.nanosec) + rclcpp::Duration::from_seconds(scan2dMsg.ranges.size()*scan2dMsg.time_increment),
 			tfBuffer,
 			waitForTransform);
@@ -2486,7 +2487,8 @@ bool convertScan3dMsg(
 		tf2_ros::Buffer & listener,
 		double waitForTransform,
 		int maxPoints,
-		float maxRange)
+		float maxRange,
+		bool is2D)
 {
 	UASSERT_MSG(scan3dMsg.data.size() == scan3dMsg.row_step*scan3dMsg.height,
 			uFormat("data=%d row_step=%d height=%d", scan3dMsg.data.size(), scan3dMsg.row_step, scan3dMsg.height).c_str());
@@ -2518,8 +2520,7 @@ bool convertScan3dMsg(
 			scanLocalTransform = sensorT * scanLocalTransform;
 		}
 	}
-
-	scan = rtabmap::util3d::laserScanFromPointCloud(scan3dMsg);
+	scan = rtabmap::util3d::laserScanFromPointCloud(scan3dMsg, true, is2D);
 	scan = rtabmap::LaserScan(scan, maxPoints, maxRange, scanLocalTransform);
 	return true;
 }

src/RGBDSplitNode.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_ros/rgbd_split.hpp"
+#include "rclcpp/rclcpp.hpp"
+
+int main(int argc, char **argv)
+{
+	rclcpp::init(argc, argv);
+	rclcpp::spin(std::make_shared<rtabmap_ros::RGBDSplit>(rclcpp::NodeOptions()));
+	rclcpp::shutdown();
+	return 0;
+}

src/costmap_2d/static_layer.cpp

@@ -42,7 +42,7 @@
 
 #include "static_layer.h"
 #include <costmap_2d/costmap_math.h>
-#include <pluginlib/class_list_macros.h>
+#include <pluginlib/class_list_macros.hpp>
 
 PLUGINLIB_EXPORT_CLASS(rtabmap_ros::StaticLayer, costmap_2d::Layer)
 

src/costmap_2d/voxel_layer.cpp

@@ -36,7 +36,7 @@
  *         David V. Lu!!
  *********************************************************************/
 #include "voxel_layer.h"
-#include <pluginlib/class_list_macros.h>
+#include <pluginlib/class_list_macros.hpp>
 #include <sensor_msgs/point_cloud2_iterator.h>
 #include <boost/thread.hpp>
 #include <pcl_conversions/pcl_conversions.h>

src/nodelets/data_odom_sync.cpp

@@ -26,7 +26,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 
 #include "ros/ros.h"
-#include "pluginlib/class_list_macros.h"
+#include "pluginlib/class_list_macros.hpp"
 #include "nodelet/nodelet.h"
 
 #include <message_filters/subscriber.h>

src/nodelets/data_throttle.cpp

@@ -26,7 +26,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 
 #include "ros/ros.h"
-#include "pluginlib/class_list_macros.h"
+#include "pluginlib/class_list_macros.hpp"
 #include "nodelet/nodelet.h"
 
 #include <message_filters/subscriber.h>

src/nodelets/disparity_to_depth.cpp

@@ -26,7 +26,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 
 #include <ros/ros.h>
-#include <pluginlib/class_list_macros.h>
+#include <pluginlib/class_list_macros.hpp>
 #include <nodelet/nodelet.h>
 
 #include <sensor_msgs/Image.h>

src/nodelets/icp_odometry.cpp

@@ -50,6 +50,7 @@ namespace rtabmap_ros
 ICPOdometry::ICPOdometry(const rclcpp::NodeOptions & options) :
 	OdometryROS("icp_odometry", options),
 	scanCloudMaxPoints_(0),
+	scanCloudIs2d_(false),
 	scanDownsamplingStep_(1),
 	scanRangeMin_(0),
 	scanRangeMax_(0),
@@ -74,6 +75,7 @@ ICPOdometry::~ICPOdometry()
 void ICPOdometry::onOdomInit()
 {
 	scanCloudMaxPoints_ = this->declare_parameter("scan_cloud_max_points", scanCloudMaxPoints_);
+	scanCloudIs2d_ = this->declare_parameter("scan_cloud_is_2d", scanCloudIs2d_);
 	scanDownsamplingStep_ = this->declare_parameter("scan_downsampling_step", scanDownsamplingStep_);
 	scanRangeMin_ = this->declare_parameter("scan_range_min", scanRangeMin_);
 	scanRangeMax_ = this->declare_parameter("scan_range_max", scanRangeMax_);
@@ -81,6 +83,8 @@ void ICPOdometry::onOdomInit()
 	scanNormalK_ = this->declare_parameter("scan_normal_k", scanNormalK_);
 	scanNormalRadius_ = this->declare_parameter("scan_normal_radius", scanNormalRadius_);
 	scanNormalGroundUp_ = this->declare_parameter("scan_normal_ground_up", scanNormalGroundUp_);
+	deskewing_ = this->declare_parameter("deskewing", deskewing_);
+	deskewingSlerp_ = this->declare_parameter("deskewing_slerp", deskewingSlerp_);
 
 	/*if (pnh.hasParam("plugins"))
 	{
@@ -113,6 +117,7 @@ void ICPOdometry::onOdomInit()
 
 	RCLCPP_INFO(this->get_logger(), "IcpOdometry: qos                    = %d", (int)qos());
 	RCLCPP_INFO(this->get_logger(), "IcpOdometry: scan_cloud_max_points  = %d", scanCloudMaxPoints_);
+	RCLCPP_INFO(this->get_logger(), "IcpOdometry: scan_cloud_is_2d       = %s", scanCloudIs2d_?"true":"false");
 	RCLCPP_INFO(this->get_logger(), "IcpOdometry: scan_downsampling_step = %d", scanDownsamplingStep_);
 	RCLCPP_INFO(this->get_logger(), "IcpOdometry: scan_range_min         = %f m", scanRangeMin_);
 	RCLCPP_INFO(this->get_logger(), "IcpOdometry: scan_range_max         = %f m", scanRangeMax_);
@@ -318,11 +323,41 @@ void ICPOdometry::callbackScan(const sensor_msgs::msg::LaserScan::SharedPtr scan
 	//transform in frameId_ frame
 	sensor_msgs::msg::PointCloud2 scanOut;
 	laser_geometry::LaserProjection projection;
-	if(deskewing_ && !guessFrameId().empty())
+	if(deskewing_ && (!guessFrameId().empty() || (frameId().compare(scanMsg->header.frame_id) != 0)))
 	{
-		projection.transformLaserScanToPointCloud(deskewing_&&!guessFrameId().empty()?guessFrameId():scanMsg->header.frame_id, *scanMsg, scanOut, this->tfBuffer());
+		// make sure the frame of the laser is updated during the whole scan time
+		rtabmap::Transform tmpT = getTransform(
+				scanMsg->header.frame_id,
+				guessFrameId().empty()?frameId():guessFrameId(),
+				scanMsg->header.stamp,
+				rclcpp::Time(scanMsg->header.stamp.sec, scanMsg->header.stamp.nanosec) + rclcpp::Duration::from_seconds(scanMsg->ranges.size()*scanMsg->time_increment),
+				this->tfBuffer(),
+				this->waitForTransform());
+		if(tmpT.isNull())
+		{
+			return;
+		}
 
-		rtabmap::Transform t = rtabmap_ros::getTransform(scanOut.header.frame_id, scanMsg->header.frame_id, scanMsg->header.stamp, tfBuffer(), waitForTransform());
+		projection.transformLaserScanToPointCloud(
+			guessFrameId().empty()?frameId():guessFrameId(),
+			*scanMsg,
+			scanOut,
+			this->tfBuffer(),
+			laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Timestamp);
+
+		if(guessFrameId().empty() && previousStamp() > 0 && !velocityGuess().isNull())
+		{
+			// deskew with constant velocity model (we are in frameId)
+			sensor_msgs::msg::PointCloud2 scanOutDeskewed;
+			if(!deskew(scanOut, scanOutDeskewed, previousStamp(), velocityGuess()))
+			{
+				RCLCPP_ERROR(this->get_logger(), "Failed to deskew input cloud, aborting odometry update!");
+				return;
+			}
+			scanOut = scanOutDeskewed;
+		}
+
+		rtabmap::Transform t = rtabmap_ros::getTransform(scanMsg->header.frame_id, scanOut.header.frame_id, scanMsg->header.stamp, tfBuffer(), waitForTransform());
 		if(t.isNull())
 		{
 			RCLCPP_ERROR(this->get_logger(), "Cannot transform back projected scan from \"%s\" frame to \"%s\" frame at time %fs.",
@@ -338,7 +373,7 @@ void ICPOdometry::callbackScan(const sensor_msgs::msg::LaserScan::SharedPtr scan
 	{
 		projection.projectLaser(*scanMsg, scanOut, -1.0, laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Timestamp);
 
-		if(previousStamp() > 0 && !velocityGuess().isNull())
+		if(deskewing_ && previousStamp() > 0 && !velocityGuess().isNull())
 		{
 			// deskew with constant velocity model
 			sensor_msgs::msg::PointCloud2 scanOutDeskewed;
@@ -347,6 +382,7 @@ void ICPOdometry::callbackScan(const sensor_msgs::msg::LaserScan::SharedPtr scan
 				RCLCPP_ERROR(this->get_logger(), "Failed to deskew input cloud, aborting odometry update!");
 				return;
 			}
+			scanOut = scanOutDeskewed;
 		}
 	}
 
@@ -515,7 +551,7 @@ void ICPOdometry::callbackCloud(const sensor_msgs::msg::PointCloud2::SharedPtr p
 		return;
 	}
 
-	sensor_msgs::msg::PointCloud2 cloudMsg;
+	std::shared_ptr<sensor_msgs::msg::PointCloud2> cloudMsg(new sensor_msgs::msg::PointCloud2);
 	/*if (!plugins_.empty())
 	{
 		if (plugins_[0]->isEnabled())
@@ -538,7 +574,14 @@ void ICPOdometry::callbackCloud(const sensor_msgs::msg::PointCloud2::SharedPtr p
 	}
 	else */
 	{
-		cloudMsg = *pointCloudMsg;
+		*cloudMsg = *pointCloudMsg;
+	}
+
+	rtabmap::Transform localScanTransform = getTransform(this->frameId(), cloudMsg->header.frame_id, cloudMsg->header.stamp, this->tfBuffer(), this->waitForTransform());
+	if(localScanTransform.isNull())
+	{
+		RCLCPP_ERROR(this->get_logger(), "TF of received scan cloud at time %fs is not set, aborting rtabmap update.", timestampFromROS(cloudMsg->header.stamp));
+		return;
 	}
 
 	if(deskewing_)
@@ -546,7 +589,7 @@ void ICPOdometry::callbackCloud(const sensor_msgs::msg::PointCloud2::SharedPtr p
 		if(!guessFrameId().empty())
 		{
 			// deskew with TF
-			if(!deskew(*pointCloudMsg, cloudMsg, guessFrameId(), tfBuffer(), waitForTransform(), deskewingSlerp_))
+			if(!deskew(*pointCloudMsg, *cloudMsg, guessFrameId(), tfBuffer(), waitForTransform(), deskewingSlerp_))
 			{
 				RCLCPP_ERROR(this->get_logger(), "Failed to deskew input cloud, aborting odometry update!");
 				return;
@@ -555,26 +598,68 @@ void ICPOdometry::callbackCloud(const sensor_msgs::msg::PointCloud2::SharedPtr p
 		else if(previousStamp() > 0 && !velocityGuess().isNull())
 		{
 			// deskew with constant velocity model
-			if(!deskew(*pointCloudMsg, cloudMsg, previousStamp(), velocityGuess()))
+			bool alreadyInBaseFrame = frameId().compare(pointCloudMsg->header.frame_id) == 0;
+			std::shared_ptr<sensor_msgs::msg::PointCloud2> cloudInBaseFrame;
+			std::shared_ptr<sensor_msgs::msg::PointCloud2> cloudPtr = cloudMsg;
+			if(!alreadyInBaseFrame)
+			{
+				// transform in base frame
+				rtabmap::Transform t = rtabmap_ros::getTransform(frameId(), pointCloudMsg->header.frame_id, pointCloudMsg->header.stamp, tfBuffer(), waitForTransform());
+				if(t.isNull())
+				{
+					RCLCPP_ERROR(this->get_logger(), "Cannot transform cloud from \"%s\" frame to \"%s\" frame at time %fs.",
+							pointCloudMsg->header.frame_id.c_str(), frameId().c_str(), timestampFromROS(pointCloudMsg->header.stamp));
+					return;
+				}
+
+				cloudInBaseFrame.reset(new sensor_msgs::msg::PointCloud2);
+				rtabmap_ros::transformPointCloud(t.toEigen4f(), *pointCloudMsg, *cloudInBaseFrame);
+				cloudPtr = cloudInBaseFrame;
+			}
+
+			std::shared_ptr<sensor_msgs::msg::PointCloud2> cloudDeskewed(new sensor_msgs::msg::PointCloud2);
+			if(!deskew(*cloudPtr, *cloudDeskewed, previousStamp(), velocityGuess()))
 			{
 				RCLCPP_ERROR(this->get_logger(), "Failed to deskew input cloud, aborting odometry update!");
 				return;
 			}
+
+			if(!alreadyInBaseFrame)
+			{
+				// put back in scan frame
+				rtabmap::Transform t = rtabmap_ros::getTransform(pointCloudMsg->header.frame_id, frameId(), pointCloudMsg->header.stamp, tfBuffer(), waitForTransform());
+				if(t.isNull())
+				{
+					RCLCPP_ERROR(this->get_logger(), "Cannot transform cloud from \"%s\" frame to \"%s\" frame at time %fs.",
+							frameId().c_str(), pointCloudMsg->header.frame_id.c_str(), timestampFromROS(pointCloudMsg->header.stamp));
+					return;
+				}
+				rtabmap_ros::transformPointCloud(t.toEigen4f(), *cloudDeskewed, *cloudMsg);
+			}
+			else
+			{
+				cloudMsg = cloudDeskewed;
+			}
 		}
 	}
 
 	LaserScan scan;
 	bool hasNormals = false;
 	bool hasIntensity = false;
-	for(unsigned int i=0; i<cloudMsg.fields.size(); ++i)
+	bool is3D = false;
+	for(unsigned int i=0; i<cloudMsg->fields.size(); ++i)
 	{
-		if(scanVoxelSize_ == 0.0f && cloudMsg.fields[i].name.compare("normal_x") == 0)
+		if(scanVoxelSize_ == 0.0f && cloudMsg->fields[i].name.compare("normal_x") == 0)
 		{
 			hasNormals = true;
 		}
-		if(cloudMsg.fields[i].name.compare("intensity") == 0)
+		if(cloudMsg->fields[i].name.compare("z") == 0 && !scanCloudIs2d_)
 		{
-			if(cloudMsg.fields[i].datatype == sensor_msgs::msg::PointField::FLOAT32)
+			is3D = true;
+		}
+		if(cloudMsg->fields[i].name.compare("intensity") == 0)
+		{
+			if(cloudMsg->fields[i].datatype == sensor_msgs::msg::PointField::FLOAT32)
 			{
 				hasIntensity = true;
 			}
@@ -585,39 +670,33 @@ void ICPOdometry::callbackCloud(const sensor_msgs::msg::PointCloud2::SharedPtr p
 				{
 					RCLCPP_WARN(this->get_logger(), "The input scan cloud has an \"intensity\" field "
 							"but the datatype (%d) is not supported. Intensity will be ignored. "
-							"This message is only shown once.", cloudMsg.fields[i].datatype);
+							"This message is only shown once.", cloudMsg->fields[i].datatype);
 					warningShown = true;
 				}
 			}
 		}
 	}
 
-	Transform localScanTransform = getTransform(this->frameId(), cloudMsg.header.frame_id, cloudMsg.header.stamp, tfBuffer(), waitForTransform());
-	if(localScanTransform.isNull())
+	if(scanCloudMaxPoints_ == 0 && cloudMsg->height > 1)
 	{
-		RCLCPP_ERROR(this->get_logger(), "TF of received scan cloud at time %fs is not set, aborting rtabmap update.", timestampFromROS(cloudMsg.header.stamp));
-		return;
-	}
-	if(scanCloudMaxPoints_ == 0 && cloudMsg.height > 1)
-	{
-		scanCloudMaxPoints_ = cloudMsg.height * cloudMsg.width;
+		scanCloudMaxPoints_ = cloudMsg->height * cloudMsg->width;
 		RCLCPP_WARN(this->get_logger(), "IcpOdometry: \"scan_cloud_max_points\" is not set but input "
 				"cloud is not dense, for convenience it will be set to %d (%dx%d)",
-				scanCloudMaxPoints_, cloudMsg.width, cloudMsg.height);
+				scanCloudMaxPoints_, cloudMsg->width, cloudMsg->height);
 	}
-	else if(cloudMsg.height > 1 && scanCloudMaxPoints_ < int(cloudMsg.height * cloudMsg.width))
+	else if(cloudMsg->height > 1 && scanCloudMaxPoints_ < int(cloudMsg->height * cloudMsg->width))
 	{
 		RCLCPP_WARN(this->get_logger(), "IcpOdometry: \"scan_cloud_max_points\" is set to %d but input "
 				"cloud is not dense and has a size of %d (%dx%d), setting to this later size.",
-				scanCloudMaxPoints_, cloudMsg.width *cloudMsg.height, cloudMsg.width, cloudMsg.height);
-		scanCloudMaxPoints_ = cloudMsg.width *cloudMsg.height;
+				scanCloudMaxPoints_, cloudMsg->width *cloudMsg->height, cloudMsg->width, cloudMsg->height);
+		scanCloudMaxPoints_ = cloudMsg->width *cloudMsg->height;
 	}
 	int maxLaserScans = scanCloudMaxPoints_;
 
 	if(hasNormals && hasIntensity)
 	{
 		pcl::PointCloud<pcl::PointXYZINormal>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZINormal>);
-		pcl::fromROSMsg(cloudMsg, *pclScan);
+		pcl::fromROSMsg(*cloudMsg, *pclScan);
 		if(pclScan->size() && scanDownsamplingStep_ > 1)
 		{
 			pclScan = util3d::downsample(pclScan, scanDownsamplingStep_);
@@ -630,12 +709,12 @@ void ICPOdometry::callbackCloud(const sensor_msgs::msg::PointCloud2::SharedPtr p
 				maxLaserScans /= scanDownsamplingStep_;
 			}
 		}
-		scan = util3d::laserScanFromPointCloud(*pclScan);
+		scan = is3D?util3d::laserScanFromPointCloud(*pclScan):util3d::laserScan2dFromPointCloud(*pclScan);
 	}
 	else if(hasNormals)
 	{
 		pcl::PointCloud<pcl::PointNormal>::Ptr pclScan(new pcl::PointCloud<pcl::PointNormal>);
-		pcl::fromROSMsg(cloudMsg, *pclScan);
+		pcl::fromROSMsg(*cloudMsg, *pclScan);
 		if(pclScan->size() && scanDownsamplingStep_ > 1)
 		{
 			pclScan = util3d::downsample(pclScan, scanDownsamplingStep_);
@@ -648,12 +727,12 @@ void ICPOdometry::callbackCloud(const sensor_msgs::msg::PointCloud2::SharedPtr p
 				maxLaserScans /= scanDownsamplingStep_;
 			}
 		}
-		scan = util3d::laserScanFromPointCloud(*pclScan);
+		scan = is3D?util3d::laserScanFromPointCloud(*pclScan):util3d::laserScan2dFromPointCloud(*pclScan);
 	}
 	else if(hasIntensity)
 	{
 		pcl::PointCloud<pcl::PointXYZI>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZI>);
-		pcl::fromROSMsg(cloudMsg, *pclScan);
+		pcl::fromROSMsg(*cloudMsg, *pclScan);
 		if(pclScan->size() && scanDownsamplingStep_ > 1)
 		{
 			pclScan = util3d::downsample(pclScan, scanDownsamplingStep_);
@@ -683,21 +762,23 @@ void ICPOdometry::callbackCloud(const sensor_msgs::msg::PointCloud2::SharedPtr p
 			if(scanNormalK_ > 0 || scanNormalRadius_>0.0f)
 			{
 				//compute normals
-				pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(pclScan, scanNormalK_, scanNormalRadius_);
+				pcl::PointCloud<pcl::Normal>::Ptr normals = is3D?
+						util3d::computeNormals(pclScan, scanNormalK_, scanNormalRadius_):
+						util3d::computeNormals2D(pclScan, scanNormalK_, scanNormalRadius_);
 				pcl::PointCloud<pcl::PointXYZINormal>::Ptr pclScanNormal(new pcl::PointCloud<pcl::PointXYZINormal>);
 				pcl::concatenateFields(*pclScan, *normals, *pclScanNormal);
-				scan = util3d::laserScanFromPointCloud(*pclScanNormal);
+				scan = is3D?util3d::laserScanFromPointCloud(*pclScanNormal):util3d::laserScan2dFromPointCloud(*pclScanNormal);
 			}
 			else
 			{
-				scan = util3d::laserScanFromPointCloud(*pclScan);
+				scan = is3D?util3d::laserScanFromPointCloud(*pclScan):util3d::laserScan2dFromPointCloud(*pclScan);
 			}
 		}
 	}
 	else
 	{
 		pcl::PointCloud<pcl::PointXYZ>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZ>);
-		pcl::fromROSMsg(cloudMsg, *pclScan);
+		pcl::fromROSMsg(*cloudMsg, *pclScan);
 		if(pclScan->size() && scanDownsamplingStep_ > 1)
 		{
 			pclScan = util3d::downsample(pclScan, scanDownsamplingStep_);
@@ -728,14 +809,16 @@ void ICPOdometry::callbackCloud(const sensor_msgs::msg::PointCloud2::SharedPtr p
 			if(scanNormalK_ > 0 || scanNormalRadius_>0.0f)
 			{
 				//compute normals
-				pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(pclScan, scanNormalK_, scanNormalRadius_);
+				pcl::PointCloud<pcl::Normal>::Ptr normals = is3D?
+						util3d::computeNormals(pclScan, scanNormalK_, scanNormalRadius_):
+						util3d::computeNormals2D(pclScan, scanNormalK_, scanNormalRadius_);
 				pcl::PointCloud<pcl::PointNormal>::Ptr pclScanNormal(new pcl::PointCloud<pcl::PointNormal>);
 				pcl::concatenateFields(*pclScan, *normals, *pclScanNormal);
-				scan = util3d::laserScanFromPointCloud(*pclScanNormal);
+				scan = is3D?util3d::laserScanFromPointCloud(*pclScanNormal):util3d::laserScan2dFromPointCloud(*pclScanNormal);
 			}
 			else
 			{
-				scan = util3d::laserScanFromPointCloud(*pclScan);
+				scan = is3D?util3d::laserScanFromPointCloud(*pclScan):util3d::laserScan2dFromPointCloud(*pclScan);
 			}
 		}
 	}
@@ -759,9 +842,9 @@ void ICPOdometry::callbackCloud(const sensor_msgs::msg::PointCloud2::SharedPtr p
 			cv::Mat(),
 			CameraModel(),
 			0,
-			rtabmap_ros::timestampFromROS(cloudMsg.header.stamp));
+			rtabmap_ros::timestampFromROS(cloudMsg->header.stamp));
 
-	this->processData(data, cloudMsg.header);
+	this->processData(data, cloudMsg->header);
 }
 
 void ICPOdometry::flushCallbacks()

src/nodelets/imu_to_tf.cpp

@@ -26,7 +26,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 
 #include <ros/ros.h>
-#include <pluginlib/class_list_macros.h>
+#include <pluginlib/class_list_macros.hpp>
 #include <nodelet/nodelet.h>
 #include <sensor_msgs/Imu.h>
 #include <tf/transform_broadcaster.h>

src/nodelets/lidar_deskewing.cpp

@@ -50,6 +50,19 @@ 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_ros::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_);
@@ -61,6 +74,7 @@ void LidarDeskewing::callbackScan(const sensor_msgs::msg::LaserScan::ConstShared
 				scanOut.header.frame_id.c_str(), msg->header.frame_id.c_str(), rtabmap_ros::timestampFromROS(msg->header.stamp));
 		return;
 	}
+
 	sensor_msgs::msg::PointCloud2 scanOutDeskewed;
 	rtabmap_ros::transformPointCloud(t.toEigen4f(), scanOut, scanOutDeskewed);
 	pubScan_->publish(scanOutDeskewed);

src/nodelets/obstacles_detection_old.cpp

@@ -26,7 +26,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 
 #include <ros/ros.h>
-#include <pluginlib/class_list_macros.h>
+#include <pluginlib/class_list_macros.hpp>
 #include <nodelet/nodelet.h>
 
 #include <pcl/point_cloud.h>

src/nodelets/rgbd_odometry.cpp

@@ -366,13 +366,13 @@ void RGBDOdometry::commonCallback(
 			!(depthImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1) == 0 ||
 			 depthImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1) == 0 ||
 			 depthImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0))
-			 {
-				RCLCPP_ERROR(this->get_logger(), "Input type must be image=mono8,mono16,rgb8,bgr8,bgra8,rgba8 and "
-				"image_depth=32FC1,16UC1,mono16. Current rgb=%s and depth=%s",
-					rgbImages[i]->encoding.c_str(),
-					depthImages[i]->encoding.c_str());
-				return;
-			}
+		{
+			RCLCPP_ERROR(this->get_logger(), "Input type must be image=mono8,mono16,rgb8,bgr8,bgra8,rgba8 and "
+			"image_depth=32FC1,16UC1,mono16. Current rgb=%s and depth=%s",
+				rgbImages[i]->encoding.c_str(),
+				depthImages[i]->encoding.c_str());
+			return;
+		}
 		UASSERT_MSG(rgbImages[i]->image.cols == imageWidth && rgbImages[i]->image.rows == imageHeight,
 				uFormat("imageWidth=%d vs %d imageHeight=%d vs %d",
 						imageWidth,

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_ros/rgbd_split.hpp>
+
+#include <cv_bridge/cv_bridge.h>
+
+namespace rtabmap_ros
+{
+
+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_ros::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_ros::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_ros::RGBDSplit)
+

src/nodelets/rgbdicp_odometry.cpp

@@ -27,7 +27,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include <rtabmap_ros/OdometryROS.h>
 
-#include <pluginlib/class_list_macros.h>
+#include <pluginlib/class_list_macros.hpp>
 #include <nodelet/nodelet.h>
 
 #include <message_filters/subscriber.h>
@@ -281,7 +281,7 @@ private:
 				}
 			}
 
-			Transform localTransform = getTransform(this->frameId(), image->header.frame_id, stamp);
+			Transform localTransform = getTransform(this->frameId(), image->header.frame_id, stamp, this->tfListener(), this->waitForTransformDuration());
 			if(localTransform.isNull())
 			{
 				return;
@@ -316,7 +316,9 @@ private:
 					// make sure the frame of the laser is updated too
 					localScanTransform = getTransform(this->frameId(),
 							scanMsg->header.frame_id,
-							scanMsg->header.stamp + ros::Duration().fromSec(scanMsg->ranges.size()*scanMsg->time_increment));
+							scanMsg->header.stamp + ros::Duration().fromSec(scanMsg->ranges.size()*scanMsg->time_increment),
+							this->tfListener(),
+							this->waitForTransformDuration());
 					if(localScanTransform.isNull())
 					{
 						ROS_ERROR("TF of received laser scan topic at time %fs is not set, aborting odometry update.", scanMsg->header.stamp.toSec());
@@ -381,7 +383,7 @@ private:
 							}
 						}
 					}
-					localScanTransform = getTransform(this->frameId(), cloudMsg->header.frame_id, cloudMsg->header.stamp);
+					localScanTransform = getTransform(this->frameId(), cloudMsg->header.frame_id, cloudMsg->header.stamp, this->tfListener(), this->waitForTransformDuration());
 					if(localScanTransform.isNull())
 					{
 						ROS_ERROR("TF of received scan cloud at time %fs is not set, aborting rtabmap update.", cloudMsg->header.stamp.toSec());

src/nodelets/stereo_odometry.cpp

@@ -368,6 +368,49 @@ void StereoOdometry::commonCallback(
 						return;
 					}
 					else
+					{
+						stereoTransform = getTransform(
+								rightCameraInfos[i].header.frame_id,
+								leftCameraInfos[i].header.frame_id,
+								leftCameraInfos[i].header.stamp,
+								tfBuffer(),
+								waitForTransform());
+						if(stereoTransform.isNull())
+						{
+							RCLCPP_ERROR(this->get_logger(), "Parameter %s is false but we cannot get TF between the two cameras! (between frames %s and %s)",
+									Parameters::kRtabmapImagesAlreadyRectified().c_str(),
+									rightCameraInfos[i].header.frame_id.c_str(),
+									leftCameraInfos[i].header.frame_id.c_str());
+							return;
+						}
+						else if(stereoTransform.isIdentity())
+						{
+							RCLCPP_ERROR(this->get_logger(), "Parameter %s is false but we cannot get a valid TF between the two cameras! "
+									"Identity transform returned between left and right cameras. Verify that if TF between "
+									"the cameras is valid: \"rosrun tf tf_echo %s %s\".",
+									Parameters::kRtabmapImagesAlreadyRectified().c_str(),
+									rightCameraInfos[i].header.frame_id.c_str(),
+									leftCameraInfos[i].header.frame_id.c_str());
+							return;
+						}
+					}
+				}
+
+				rtabmap::StereoCameraModel stereoModel = rtabmap_ros::stereoCameraModelFromROS(leftCameraInfos[i], rightCameraInfos[i], localTransform, stereoTransform);
+
+				if( stereoModel.baseline() == 0 &&
+					alreadyRectified &&
+					!rightCameraInfos[i].header.frame_id.empty() &&
+					!leftCameraInfos[i].header.frame_id.empty())
+				{
+					stereoTransform = getTransform(
+							leftCameraInfos[i].header.frame_id,
+							rightCameraInfos[i].header.frame_id,
+							leftCameraInfos[i].header.stamp,
+							tfBuffer(),
+							waitForTransform());
+
+					if(!stereoTransform.isNull() && stereoTransform.x()>0)
 					{
 						static bool warned = false;
 						if(!warned)

src/nodelets/stereo_throttle.cpp

@@ -26,7 +26,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 
 #include "ros/ros.h"
-#include "pluginlib/class_list_macros.h"
+#include "pluginlib/class_list_macros.hpp"
 #include "nodelet/nodelet.h"
 
 #include <message_filters/subscriber.h>

src/nodelets/undistort_depth.cpp

@@ -26,7 +26,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 
 #include <ros/ros.h>
-#include <pluginlib/class_list_macros.h>
+#include <pluginlib/class_list_macros.hpp>
 #include <nodelet/nodelet.h>
 
 #include <sensor_msgs/Image.h>

src/rviz/MapCloudDisplay.cpp

@@ -970,5 +970,5 @@ bool MapCloudDisplay::transformCloud(const CloudInfoPtr& cloud_info, bool update
 
 } // namespace rtabmap
 
-#include <pluginlib/class_list_macros.hpp>  // NOLINT
+#include <pluginlib/class_list_macros.hpp>
 PLUGINLIB_EXPORT_CLASS(rtabmap_ros::MapCloudDisplay, rviz_common::Display)

src/rviz/OrbitOrientedViewController.cpp

@@ -72,5 +72,5 @@ void OrbitOrientedViewController::updateCamera()
 
 }
 
-#include <pluginlib/class_list_macros.h>
+#include <pluginlib/class_list_macros.hpp>
 PLUGINLIB_EXPORT_CLASS( rtabmap_ros::OrbitOrientedViewController, rviz::ViewController )