ros2: Ported imu_to_tf and disparity_to_depth nodelets

.github/workflows/ros2.yml

@@ -36,6 +36,7 @@ jobs:
     - name: Install dependencies
       run: |
         sudo apt-get update
+        sudo apt-get upgrade -y
         sudo apt-get -y install ros-${{ matrix.ros_distro }}-rtabmap-ros
         sudo apt-get -y remove ros-${{ matrix.ros_distro }}-rtabmap*
                 

docker/humble/latest/Dockerfile

@@ -9,6 +9,7 @@ COPY . ros2_ws/src/rtabmap_ros
 RUN source /ros_entrypoint.sh && \
     cd ros2_ws && \
     export MAKEFLAGS="-j1" && \
+    rosdep init && \
     rosdep update && \
     rosdep install --from-paths src --ignore-src -r -y && \
     colcon build --event-handlers console_direct+ --install-base /opt/ros/humble --merge-install --cmake-args -DRTABMAP_SYNC_MULTI_RGBD=ON -DCMAKE_BUILD_TYPE=Release && \

docker/iron/latest/Dockerfile

@@ -9,6 +9,7 @@ COPY . ros2_ws/src/rtabmap_ros
 RUN source /ros_entrypoint.sh && \
     cd ros2_ws && \
     export MAKEFLAGS="-j1" && \
+    rosdep init && \
     rosdep update && \
     rosdep install --from-paths src --ignore-src -r -y && \
     colcon build --event-handlers console_direct+ --install-base /opt/ros/iron --merge-install --cmake-args -DRTABMAP_SYNC_MULTI_RGBD=ON -DCMAKE_BUILD_TYPE=Release && \

rtabmap_util/CMakeLists.txt

@@ -18,6 +18,7 @@ find_package(stereo_msgs REQUIRED)
 find_package(nav_msgs REQUIRED)
 find_package(std_msgs REQUIRED)
 find_package(tf2 REQUIRED)
+find_package(tf2_geometry_msgs REQUIRED)
 find_package(tf2_ros REQUIRED)
 find_package(laser_geometry REQUIRED)
 find_package(pcl_conversions REQUIRED)
@@ -26,8 +27,6 @@ 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)
 
@@ -46,6 +45,7 @@ SET(Libraries
    nav_msgs
    std_msgs
    tf2
+   tf2_geometry_msgs
    tf2_ros
    laser_geometry
    pcl_conversions
@@ -63,12 +63,12 @@ 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/disparity_to_depth.cpp
    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/imu_to_tf.cpp
    src/nodelets/lidar_deskewing.cpp
    src/nodelets/rgbd_relay.cpp
    src/nodelets/rgbd_split.cpp
@@ -104,8 +104,8 @@ 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::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")
@@ -134,10 +134,15 @@ set_target_properties(rtabmap_rgbd_split PROPERTIES OUTPUT_NAME "rgbd_split")
 #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_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_disparity_to_depth src/DisparityToDepthNode.cpp)
+ament_target_dependencies(rtabmap_disparity_to_depth ${Libraries})
+target_link_libraries(rtabmap_disparity_to_depth rtabmap_util_plugins)
+set_target_properties(rtabmap_disparity_to_depth PROPERTIES OUTPUT_NAME "disparity_to_depth")
 
 add_executable(rtabmap_lidar_deskewing src/LidarDeskewingNode.cpp)
 ament_target_dependencies(rtabmap_lidar_deskewing ${Libraries})
@@ -218,6 +223,8 @@ install(TARGETS
 #   rtabmap_map_optimizer
 #   rtabmap_data_player
 #   rtabmap_odom_msg_to_tf
+   rtabmap_imu_to_tf
+   rtabmap_disparity_to_depth
    rtabmap_rgbd_relay
    rtabmap_rgbd_split
    rtabmap_lidar_deskewing

rtabmap_util/include/rtabmap_util/disparity_to_depth.hpp

@@ -0,0 +1,55 @@
+/*
+Copyright (c) 2010-2023, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include <rtabmap_util/visibility.h>
+#include "rclcpp/rclcpp.hpp"
+
+#include <sensor_msgs/msg/image.h>
+#include <stereo_msgs/msg/disparity_image.hpp>
+
+#include <image_transport/image_transport.hpp>
+
+namespace rtabmap_util
+{
+
+class DisparityToDepth : public rclcpp::Node
+{
+public:
+	RTABMAP_UTIL_PUBLIC
+	explicit DisparityToDepth(const rclcpp::NodeOptions & options);
+	virtual ~DisparityToDepth();
+
+private:
+	void callback(const stereo_msgs::msg::DisparityImage::ConstSharedPtr msg);
+
+private:
+	image_transport::Publisher pub32f_;
+	image_transport::Publisher pub16u_;
+	rclcpp::Subscription<stereo_msgs::msg::DisparityImage>::SharedPtr sub_;
+};
+
+}

rtabmap_util/include/rtabmap_util/imu_to_tf.hpp

@@ -0,0 +1,61 @@
+/*
+Copyright (c) 2010-2023, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include <rtabmap_util/visibility.h>
+#include "rclcpp/rclcpp.hpp"
+
+#include <sensor_msgs/msg/imu.hpp>
+
+#include <tf2_ros/buffer.h>
+#include <tf2_ros/transform_listener.h>
+#include <tf2_ros/transform_broadcaster.h>
+
+namespace rtabmap_util
+{
+
+class ImuToTF : public rclcpp::Node
+{
+public:
+	RTABMAP_UTIL_PUBLIC
+	explicit ImuToTF(const rclcpp::NodeOptions & options);
+	virtual ~ImuToTF();
+
+private:
+	void imuCallback(const sensor_msgs::msg::Imu::ConstSharedPtr msg);
+
+private:
+	rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr sub_;
+	std::shared_ptr<tf2_ros::TransformBroadcaster> tfBroadcaster_;
+	std::string fixedFrameId_;
+	std::string baseFrameId_;
+	std::shared_ptr<tf2_ros::Buffer> tfBuffer_;
+	std::shared_ptr<tf2_ros::TransformListener> tfListener_;
+	double waitForTransformDuration_;
+};
+
+}
+

rtabmap_util/include/rtabmap_util/lidar_deskewing.hpp

@@ -1,3 +1,29 @@
+/*
+Copyright (c) 2010-2023, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
 
 #include <rtabmap_util/visibility.h>
 #include "rclcpp/rclcpp.hpp"

rtabmap_util/package.xml

@@ -22,6 +22,7 @@
   <depend>nav_msgs</depend>
   <depend>std_msgs</depend>
   <depend>tf2</depend>
+  <depend>tf2_geometry_msgs</depend>
   <depend>tf2_ros</depend>
   <depend>laser_geometry</depend>
   <depend>pcl_conversions</depend>

rtabmap_util/src/DisparityToDepthNode.cpp

@@ -0,0 +1,39 @@
+/*
+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/disparity_to_depth.hpp"
+#include "rtabmap/utilite/ULogger.h"
+#include "rclcpp/rclcpp.hpp"
+
+int main(int argc, char **argv)
+{
+	ULogger::setType(ULogger::kTypeConsole);
+	rclcpp::init(argc, argv);
+	rclcpp::spin(std::make_shared<rtabmap_util::DisparityToDepth>(rclcpp::NodeOptions()));
+	rclcpp::shutdown();
+	return 0;
+}

rtabmap_util/src/ImuToTFNode.cpp

@@ -25,24 +25,15 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 
-#include "ros/ros.h"
-#include "nodelet/loader.h"
+#include "rtabmap_util/imu_to_tf.hpp"
+#include "rtabmap/utilite/ULogger.h"
+#include "rclcpp/rclcpp.hpp"
 
 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();
+	ULogger::setType(ULogger::kTypeConsole);
+	rclcpp::init(argc, argv);
+	rclcpp::spin(std::make_shared<rtabmap_util::ImuToTF>(rclcpp::NodeOptions()));
+	rclcpp::shutdown();
 	return 0;
 }
-