From 83c16dcf24297da008e298a47bedda533db65dfc Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sat, 14 Oct 2023 19:41:34 -0700 Subject: [PATCH] ros2: Ported imu_to_tf and disparity_to_depth nodelets --- .github/workflows/ros2.yml | 1 + docker/humble/latest/Dockerfile | 1 + docker/iron/latest/Dockerfile | 1 + rtabmap_util/CMakeLists.txt | 27 +-- .../rtabmap_util/disparity_to_depth.hpp | 55 +++++++ .../include/rtabmap_util/imu_to_tf.hpp | 61 +++++++ .../include/rtabmap_util/lidar_deskewing.hpp | 26 +++ rtabmap_util/package.xml | 1 + rtabmap_util/src/DisparityToDepthNode.cpp | 39 +++++ rtabmap_util/src/ImuToTFNode.cpp | 23 +-- .../src/nodelets/disparity_to_depth.cpp | 155 +++++++++--------- rtabmap_util/src/nodelets/imu_to_tf.cpp | 136 ++++++++------- 12 files changed, 352 insertions(+), 174 deletions(-) create mode 100644 rtabmap_util/include/rtabmap_util/disparity_to_depth.hpp create mode 100644 rtabmap_util/include/rtabmap_util/imu_to_tf.hpp create mode 100644 rtabmap_util/src/DisparityToDepthNode.cpp diff --git a/.github/workflows/ros2.yml b/.github/workflows/ros2.yml index e77c7ec1b..bb5a853c9 100644 --- a/.github/workflows/ros2.yml +++ b/.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* diff --git a/docker/humble/latest/Dockerfile b/docker/humble/latest/Dockerfile index 7688b0ef2..059640b1b 100644 --- a/docker/humble/latest/Dockerfile +++ b/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 && \ diff --git a/docker/iron/latest/Dockerfile b/docker/iron/latest/Dockerfile index 9bbccc89a..6ac1ce305 100644 --- a/docker/iron/latest/Dockerfile +++ b/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 && \ diff --git a/rtabmap_util/CMakeLists.txt b/rtabmap_util/CMakeLists.txt index 162af0364..ab40e1719 100644 --- a/rtabmap_util/CMakeLists.txt +++ b/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 diff --git a/rtabmap_util/include/rtabmap_util/disparity_to_depth.hpp b/rtabmap_util/include/rtabmap_util/disparity_to_depth.hpp new file mode 100644 index 000000000..30daa339a --- /dev/null +++ b/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 +#include "rclcpp/rclcpp.hpp" + +#include +#include + +#include + +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::SharedPtr sub_; +}; + +} diff --git a/rtabmap_util/include/rtabmap_util/imu_to_tf.hpp b/rtabmap_util/include/rtabmap_util/imu_to_tf.hpp new file mode 100644 index 000000000..9622ba7bc --- /dev/null +++ b/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 +#include "rclcpp/rclcpp.hpp" + +#include + +#include +#include +#include + +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::SharedPtr sub_; + std::shared_ptr tfBroadcaster_; + std::string fixedFrameId_; + std::string baseFrameId_; + std::shared_ptr tfBuffer_; + std::shared_ptr tfListener_; + double waitForTransformDuration_; +}; + +} + diff --git a/rtabmap_util/include/rtabmap_util/lidar_deskewing.hpp b/rtabmap_util/include/rtabmap_util/lidar_deskewing.hpp index 4373ff840..3f19b09cb 100644 --- a/rtabmap_util/include/rtabmap_util/lidar_deskewing.hpp +++ b/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 #include "rclcpp/rclcpp.hpp" diff --git a/rtabmap_util/package.xml b/rtabmap_util/package.xml index 00d81ce04..910b7aad6 100644 --- a/rtabmap_util/package.xml +++ b/rtabmap_util/package.xml @@ -22,6 +22,7 @@ nav_msgs std_msgs tf2 + tf2_geometry_msgs tf2_ros laser_geometry pcl_conversions diff --git a/rtabmap_util/src/DisparityToDepthNode.cpp b/rtabmap_util/src/DisparityToDepthNode.cpp new file mode 100644 index 000000000..18ab4e947 --- /dev/null +++ b/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(rclcpp::NodeOptions())); + rclcpp::shutdown(); + return 0; +} diff --git a/rtabmap_util/src/ImuToTFNode.cpp b/rtabmap_util/src/ImuToTFNode.cpp index c0657d64f..45bb7f3d2 100644 --- a/rtabmap_util/src/ImuToTFNode.cpp +++ b/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(rclcpp::NodeOptions())); + rclcpp::shutdown(); return 0; } - diff --git a/rtabmap_util/src/nodelets/disparity_to_depth.cpp b/rtabmap_util/src/nodelets/disparity_to_depth.cpp index d880510ca..46f625612 100644 --- a/rtabmap_util/src/nodelets/disparity_to_depth.cpp +++ b/rtabmap_util/src/nodelets/disparity_to_depth.cpp @@ -25,116 +25,119 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ -#include -#include -#include +#include +#include -#include -#include -#include - -#include +#include #include namespace rtabmap_util { -class DisparityToDepth : public nodelet::Nodelet +DisparityToDepth::DisparityToDepth(const rclcpp::NodeOptions & options) : + rclcpp::Node("disparity_to_depth", options) { -public: - DisparityToDepth() {} + int qos = 0; + qos = this->declare_parameter("qos", qos); - virtual ~DisparityToDepth(){} + auto node = rclcpp::Node::make_shared(this->get_name()); + image_transport::ImageTransport it(node); + pub32f_ = image_transport::create_publisher(node.get(), "depth", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile()); + pub16u_ = image_transport::create_publisher(node.get(), "depth_raw", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile()); -private: - virtual void onInit() - { - ros::NodeHandle & nh = getNodeHandle(); - ros::NodeHandle & pnh = getPrivateNodeHandle(); + sub_ = create_subscription("disparity", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos), std::bind(&DisparityToDepth::callback, this, std::placeholders::_1)); +} + +DisparityToDepth::~DisparityToDepth(){} - 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 DisparityToDepth::callback(const stereo_msgs::msg::DisparityImage::ConstSharedPtr disparityMsg) +{ + if(disparityMsg->image.encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1) !=0) + { + RCLCPP_ERROR(this->get_logger(), "Input type must be disparity=32FC1"); + return; } - void callback(const stereo_msgs::DisparityImageConstPtr& disparityMsg) + bool publish32f = pub32f_.getNumSubscribers(); + bool publish16u = pub16u_.getNumSubscribers(); + + if(publish32f || publish16u) { - if(disparityMsg->image.encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1) !=0) + // sensor_msgs::image_encodings::TYPE_32FC1 + cv::Mat disparity(disparityMsg->image.height, disparityMsg->image.width, CV_32FC1, const_cast(disparityMsg->image.data.data())); + + cv::Mat depth32f; + cv::Mat depth16u; + if(publish32f) { - NODELET_ERROR("Input type must be disparity=32FC1"); - return; + depth32f = cv::Mat::zeros(disparity.rows, disparity.cols, CV_32F); } - - bool publish32f = pub32f_.getNumSubscribers(); - bool publish16u = pub16u_.getNumSubscribers(); - - if(publish32f || publish16u) + if(publish16u) { - // sensor_msgs::image_encodings::TYPE_32FC1 - cv::Mat disparity(disparityMsg->image.height, disparityMsg->image.width, CV_32FC1, const_cast(disparityMsg->image.data.data())); - - cv::Mat depth32f; - cv::Mat depth16u; + depth16u = cv::Mat::zeros(disparity.rows, disparity.cols, CV_16U); + } + float * depth32fPtr=0; + unsigned short * depth16uPtr=0; + for (int i = 0; i < disparity.rows; ++i) + { + const float * rowPtr = (const float*)disparity.ptr(i); if(publish32f) { - depth32f = cv::Mat::zeros(disparity.rows, disparity.cols, CV_32F); + depth32fPtr = (float*)depth32f.ptr(i); } if(publish16u) { - depth16u = cv::Mat::zeros(disparity.rows, disparity.cols, CV_16U); + depth16uPtr = (unsigned short*)depth16u.ptr(i); } - for (int i = 0; i < disparity.rows; i++) + for (int j = 0; j < disparity.cols; ++j) { - for (int j = 0; j < disparity.cols; j++) + const float & disparity_value = rowPtr[j]; + if (disparity_value > disparityMsg->min_disparity && disparity_value < disparityMsg->max_disparity) { - float disparity_value = disparity.at(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) + { + depth32fPtr[j] = depth; + } + if(publish16u) { - // baseline * focal / disparity - float depth = disparityMsg->T * disparityMsg->f / disparity_value; - if(publish32f) - { - depth32f.at(i,j) = depth; - } - if(publish16u) - { - depth16u.at(i,j) = (unsigned short)(depth*1000.0f); - } + depth16uPtr[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); + if(publish32f) + { + // convert to ROS sensor_msg::Image + cv_bridge::CvImage cvDepth(disparityMsg->header, sensor_msgs::image_encodings::TYPE_32FC1, depth32f); + sensor_msgs::msg::Image depthMsg; + cvDepth.toImageMsg(depthMsg); - //publish the message - pub32f_.publish(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); + if(publish16u) + { + // convert to ROS sensor_msg::Image + cv_bridge::CvImage cvDepth(disparityMsg->header, sensor_msgs::image_encodings::TYPE_16UC1, depth16u); + sensor_msgs::msg::Image depthMsg; + cvDepth.toImageMsg(depthMsg); - //publish the message - pub16u_.publish(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); } + +#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::DisparityToDepth) diff --git a/rtabmap_util/src/nodelets/imu_to_tf.cpp b/rtabmap_util/src/nodelets/imu_to_tf.cpp index 49b225926..4cb9f0565 100644 --- a/rtabmap_util/src/nodelets/imu_to_tf.cpp +++ b/rtabmap_util/src/nodelets/imu_to_tf.cpp @@ -25,98 +25,90 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ -#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include namespace rtabmap_util { -class ImuToTF : public nodelet::Nodelet +ImuToTF::ImuToTF(const rclcpp::NodeOptions & options) : + rclcpp::Node("imu_to_tf", options), + fixedFrameId_("odom"), + waitForTransformDuration_(0.1) { -public: - ImuToTF() : - fixedFrameId_("odom"), - waitForTransformDuration_(0.1) - {} + tfBuffer_ = std::make_shared(this->get_clock()); + tfListener_ = std::make_shared(*tfBuffer_); + tfBroadcaster_ = std::make_shared(this); - virtual ~ImuToTF() - { - } + int qos = 0; + fixedFrameId_ = this->declare_parameter("fixed_frame_id", fixedFrameId_); + baseFrameId_ = this->declare_parameter("base_frame_id", baseFrameId_); + qos = this->declare_parameter("qos", qos); + waitForTransformDuration_ = this->declare_parameter("wait_for_transform_duration", waitForTransformDuration_); -private: - virtual void onInit() - { - ros::NodeHandle & nh = getNodeHandle(); - ros::NodeHandle & pnh = getPrivateNodeHandle(); + RCLCPP_INFO(this->get_logger(), "fixed_frame_id: %s", fixedFrameId_.c_str()); + RCLCPP_INFO(this->get_logger(), "base_frame_id: %s", baseFrameId_.c_str()); + RCLCPP_INFO(this->get_logger(), "qos: %d", qos); - 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_ = create_subscription("imu/data", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos), std::bind(&ImuToTF::imuCallback, this, std::placeholders::_1)); +} - sub_ = nh.subscribe("imu/data", 1, &ImuToTF::imuCallback, this); - } +ImuToTF::~ImuToTF() +{ +} - void imuCallback(const sensor_msgs::ImuConstPtr & msg) - { - tf::Quaternion q; - tf::quaternionMsgToTF(msg->orientation, q); - tf::StampedTransform st; - st.setRotation(q); +void ImuToTF::imuCallback(const sensor_msgs::msg::Imu::ConstSharedPtr msg) +{ + tf2::Quaternion q; + tf2::fromMsg(msg->orientation, q); + tf2::Transform st; + st.setRotation(q); - st.frame_id_ = fixedFrameId_; - st.stamp_ = msg->header.stamp; + std::string childFrameId = msg->header.frame_id; - if(!baseFrameId_.empty() && - baseFrameId_.compare(msg->header.frame_id) != 0) + if(!baseFrameId_.empty() && + baseFrameId_.compare(msg->header.frame_id) != 0) + { + try { - 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) + std::string errorMsg; + if(!tfBuffer_->canTransform(baseFrameId_, msg->header.frame_id, msg->header.stamp, rclcpp::Duration::from_seconds(waitForTransformDuration_), &errorMsg)) { - NODELET_ERROR("(getting transform %s -> %s) %s", baseFrameId_.c_str(), msg->header.frame_id.c_str(), ex.what()); + RCLCPP_ERROR(this->get_logger(), "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, rtabmap_conversions::timestampFromROS(msg->header.stamp), errorMsg.c_str()); return; } + + geometry_msgs::msg::TransformStamped tmp = tfBuffer_->lookupTransform(msg->header.frame_id, baseFrameId_, msg->header.stamp); + tf2::Transform tmp_t; + tf2::fromMsg(tmp.transform, tmp_t); + tf2::Transform t = tmp_t.inverse()*st*tmp_t; + st.setRotation(t.getRotation()); + childFrameId = baseFrameId_; } - else + catch(tf2::TransformException & ex) { - st.child_frame_id_ = msg->header.frame_id; + RCLCPP_ERROR(this->get_logger(), "(getting transform %s -> %s) %s", baseFrameId_.c_str(), msg->header.frame_id.c_str(), ex.what()); + return; } - st.setOrigin(tf::Vector3(0,0,0)); - - pub_.sendTransform(st); } + st.setOrigin(tf2::Vector3(0,0,0)); + + geometry_msgs::msg::TransformStamped output; + output.header.frame_id = fixedFrameId_; + output.header.stamp = msg->header.stamp; + output.child_frame_id = childFrameId; + output.transform = tf2::toMsg(st); + tfBroadcaster_->sendTransform(output); +} -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); } +#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::ImuToTF)