From 7d3b5244146e02bf2228f964d5a46a5285ae65e4 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sat, 12 Jul 2025 17:32:19 -0700 Subject: [PATCH 01/56] disabling velodyne dep for rolling in ros2 branch --- .github/workflows/ros2.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/ros2.yml b/.github/workflows/ros2.yml index 8f33cab2..a15f79e0 100644 --- a/.github/workflows/ros2.yml +++ b/.github/workflows/ros2.yml @@ -25,7 +25,7 @@ jobs: - ros_distro: kilted skip_keys: '' - ros_distro: rolling - skip_keys: 'nav2_bringup nav2_msgs' + skip_keys: 'nav2_bringup nav2_msgs velodyne' fail-fast: false container: image: osrf/ros:${{ matrix.ros_distro }}-desktop-full From ad785e23275e8e587fa89066f781b902b586ac19 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Tue, 5 Aug 2025 03:04:32 +0000 Subject: [PATCH 02/56] Support any number of distortion coefficients --- rtabmap_conversions/src/MsgConversion.cpp | 19 ------------------- 1 file changed, 19 deletions(-) diff --git a/rtabmap_conversions/src/MsgConversion.cpp b/rtabmap_conversions/src/MsgConversion.cpp index a61544dd..ff155b4f 100644 --- a/rtabmap_conversions/src/MsgConversion.cpp +++ b/rtabmap_conversions/src/MsgConversion.cpp @@ -818,25 +818,6 @@ rtabmap::CameraModel cameraModelFromROS( D.at(0,4) = camInfo.D[2]; D.at(0,5) = camInfo.D[3]; } - else if(camInfo.D.size()>8) - { - bool zerosAfter8 = true; - for(size_t i=8; i Date: Sat, 9 Aug 2025 15:05:08 -0700 Subject: [PATCH 03/56] Resolving image_transport hints support (#1181) (#1348) * Resolving image_transport hints support (#1181) * updated dev container to be able to install packages, updated comment --- .devcontainer/humble/Dockerfile | 4 ++- .devcontainer/jazzy/Dockerfile | 4 ++- .devcontainer/kilted/Dockerfile | 4 ++- rtabmap_odom/src/nodelets/rgbd_odometry.cpp | 28 +++++++++-------- rtabmap_odom/src/nodelets/stereo_odometry.cpp | 15 ++++++---- rtabmap_slam/src/CoreWrapper.cpp | 5 ++-- .../rtabmap_sync/CommonDataSubscriber.h | 2 ++ rtabmap_sync/src/CommonDataSubscriber.cpp | 6 ++++ .../src/impl/CommonDataSubscriberDepth.cpp | 9 ++++-- .../src/impl/CommonDataSubscriberRGB.cpp | 5 ++-- .../src/impl/CommonDataSubscriberStereo.cpp | 8 +++-- rtabmap_sync/src/nodelets/rgb_sync.cpp | 7 +++-- rtabmap_sync/src/nodelets/rgbd_sync.cpp | 30 +++++++++++++++---- rtabmap_sync/src/nodelets/stereo_sync.cpp | 10 +++++-- rtabmap_util/src/nodelets/point_cloud_xyz.cpp | 6 ++-- .../src/nodelets/point_cloud_xyzrgb.cpp | 17 +++++++---- 16 files changed, 111 insertions(+), 49 deletions(-) diff --git a/.devcontainer/humble/Dockerfile b/.devcontainer/humble/Dockerfile index 62978bb7..53447987 100644 --- a/.devcontainer/humble/Dockerfile +++ b/.devcontainer/humble/Dockerfile @@ -8,7 +8,9 @@ ARG USER_GID=1000 RUN set -ex && \ groupadd --gid ${USER_GID} ${USERNAME} && \ useradd --uid ${USER_UID} --gid ${USER_GID} -m ${USERNAME} && \ - usermod -a -G sudo ${USERNAME} + usermod -a -G sudo ${USERNAME} && \ + echo "${USERNAME} ALL=(ALL) NOPASSWD:ALL" > /etc/sudoers.d/${USERNAME} && \ + chmod 0440 /etc/sudoers.d/${USERNAME} RUN mkdir -p /home/${USERNAME}/ros2_ws/src && \ chown -R ${USERNAME}:${USERNAME} /home/${USERNAME}/ros2_ws diff --git a/.devcontainer/jazzy/Dockerfile b/.devcontainer/jazzy/Dockerfile index 13b36c5a..063aa809 100644 --- a/.devcontainer/jazzy/Dockerfile +++ b/.devcontainer/jazzy/Dockerfile @@ -11,7 +11,9 @@ ARG USER_GID=1000 RUN set -ex && \ groupadd --gid ${USER_GID} ${USERNAME} && \ useradd --uid ${USER_UID} --gid ${USER_GID} -m ${USERNAME} && \ - usermod -a -G sudo ${USERNAME} + usermod -a -G sudo ${USERNAME} && \ + echo "${USERNAME} ALL=(ALL) NOPASSWD:ALL" > /etc/sudoers.d/${USERNAME} && \ + chmod 0440 /etc/sudoers.d/${USERNAME} RUN mkdir -p /home/${USERNAME}/ros2_ws/src && \ chown -R ${USERNAME}:${USERNAME} /home/${USERNAME}/ros2_ws diff --git a/.devcontainer/kilted/Dockerfile b/.devcontainer/kilted/Dockerfile index 9aee0a83..60af4061 100644 --- a/.devcontainer/kilted/Dockerfile +++ b/.devcontainer/kilted/Dockerfile @@ -11,7 +11,9 @@ ARG USER_GID=1000 RUN set -ex && \ groupadd --gid ${USER_GID} ${USERNAME} && \ useradd --uid ${USER_UID} --gid ${USER_GID} -m ${USERNAME} && \ - usermod -a -G sudo ${USERNAME} + usermod -a -G sudo ${USERNAME} && \ + echo "${USERNAME} ALL=(ALL) NOPASSWD:ALL" > /etc/sudoers.d/${USERNAME} && \ + chmod 0440 /etc/sudoers.d/${USERNAME} RUN mkdir -p /home/${USERNAME}/ros2_ws/src && \ chown -R ${USERNAME}:${USERNAME} /home/${USERNAME}/ros2_ws diff --git a/rtabmap_odom/src/nodelets/rgbd_odometry.cpp b/rtabmap_odom/src/nodelets/rgbd_odometry.cpp index 9976065f..7a98fc13 100644 --- a/rtabmap_odom/src/nodelets/rgbd_odometry.cpp +++ b/rtabmap_odom/src/nodelets/rgbd_odometry.cpp @@ -113,8 +113,16 @@ void RGBDOdometry::onOdomInit() rgbdCameras = 0; } keepColor_ = this->declare_parameter("keep_color", keepColor_); - std::string rgbdTransport = this->declare_parameter("rgb_transport", std::string("raw")); + std::string rgbTransport = this->declare_parameter("rgb_transport", std::string("raw")); + if(rgbTransport != "raw") { + RCLCPP_WARN(this->get_logger(), "Parameter \"rgb_transport\" has been renamed " + "to \"image_transport\" and will be removed " + "in future versions! The value (%s) is copied to " + "\"image_transport\".", rgbTransport.c_str()); + } + std::string imageTransport = this->declare_parameter("image_transport", rgbTransport); std::string depthTransport = this->declare_parameter("depth_transport", std::string("raw")); + RCLCPP_INFO(this->get_logger(), "RGBDOdometry: approx_sync = %s", approxSync?"true":"false"); if(approxSync) @@ -126,7 +134,7 @@ void RGBDOdometry::onOdomInit() RCLCPP_INFO(this->get_logger(), "RGBDOdometry: subscribe_rgbd = %s", subscribeRGBD?"true":"false"); RCLCPP_INFO(this->get_logger(), "RGBDOdometry: rgbd_cameras = %d", rgbdCameras); RCLCPP_INFO(this->get_logger(), "RGBDOdometry: keep_color = %s", keepColor_?"true":"false"); - RCLCPP_INFO(this->get_logger(), "RGBDOdometry: rgb_transport = %s", rgbdTransport.c_str()); + RCLCPP_INFO(this->get_logger(), "RGBDOdometry: image_transport = %s", imageTransport.c_str()); RCLCPP_INFO(this->get_logger(), "RGBDOdometry: depth_transport = %s", depthTransport.c_str()); rclcpp::SubscriptionOptions options; @@ -354,18 +362,12 @@ void RGBDOdometry::onOdomInit() } else { - image_transport::TransportHints rgb_hints(this, "raw", "rgb_transport"); + image_transport::TransportHints rgb_hints(this); // using "image_transport" parameter image_transport::TransportHints depth_hints(this, "raw", "depth_transport"); - - std::string rgb_topic = get_node_base_interface()->resolve_topic_or_service_name( - "rgb/image", false, false - ); - std::string depth_topic = get_node_base_interface()->resolve_topic_or_service_name( - "depth/image", false, false - ); - - image_mono_sub_.subscribe(this, rgb_topic, rgb_hints.getTransport(), rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile(), options); - image_depth_sub_.subscribe(this, depth_topic, depth_hints.getTransport(), rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile(), options); + std::string rgbTopic = this->get_node_topics_interface()->resolve_topic_name("rgb/image"); // Humble/Jazzy don't resolve base topic, fixed by https://github.com/ros-perception/image_common/commit/ea7589ae8c1f7ecb83d6aab7b4c890c2d630d27a + std::string depthTopic = this->get_node_topics_interface()->resolve_topic_name("depth/image"); // Humble/Jazzy don't resolve base topic, fixed by https://github.com/ros-perception/image_common/commit/ea7589ae8c1f7ecb83d6aab7b4c890c2d630d27a + image_mono_sub_.subscribe(this, rgbTopic, rgb_hints.getTransport(), rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile(), options); + image_depth_sub_.subscribe(this, depthTopic, depth_hints.getTransport(), rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile(), options); info_sub_.subscribe(this, "rgb/camera_info", RCLCPP_QOS(topicQueueSize_, qosCamInfo), options); if(approxSync) diff --git a/rtabmap_odom/src/nodelets/stereo_odometry.cpp b/rtabmap_odom/src/nodelets/stereo_odometry.cpp index 00f1f9e2..afcd2254 100644 --- a/rtabmap_odom/src/nodelets/stereo_odometry.cpp +++ b/rtabmap_odom/src/nodelets/stereo_odometry.cpp @@ -109,6 +109,7 @@ void StereoOdometry::onOdomInit() subscribeRGBD = this->declare_parameter("subscribe_rgbd", subscribeRGBD); rgbdCameras = this->declare_parameter("rgbd_cameras", rgbdCameras); keepColor_ = this->declare_parameter("keep_color", keepColor_); + std::string imageTransport = this->declare_parameter("image_transport", std::string("raw")); RCLCPP_INFO(this->get_logger(), "StereoOdometry: approx_sync = %s", approxSync?"true":"false"); if(approxSync) @@ -119,6 +120,7 @@ void StereoOdometry::onOdomInit() RCLCPP_INFO(this->get_logger(), "StereoOdometry: qos_camera_info = %d", qosCamInfo); RCLCPP_INFO(this->get_logger(), "StereoOdometry: subscribe_rgbd = %s", subscribeRGBD?"true":"false"); RCLCPP_INFO(this->get_logger(), "StereoOdometry: keep_color = %s", keepColor_?"true":"false"); + RCLCPP_INFO(this->get_logger(), "StereoOdometry: image_transport = %s", imageTransport.c_str()); rclcpp::SubscriptionOptions options; options.callback_group = dataCallbackGroup_; @@ -347,9 +349,12 @@ void StereoOdometry::onOdomInit() } else { - image_transport::TransportHints hints(this); - imageRectLeft_.subscribe(this, "left/image_rect", hints.getTransport(), rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile(), options); - imageRectRight_.subscribe(this, "right/image_rect", hints.getTransport(), rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile(), options); + image_transport::TransportHints hints(this); // using "image_transport" parameter + + std::string leftTopic = this->get_node_topics_interface()->resolve_topic_name("left/image_rect"); // Humble/Jazzy don't resolve base topic, fixed by https://github.com/ros-perception/image_common/commit/ea7589ae8c1f7ecb83d6aab7b4c890c2d630d27a + std::string rightTopic = this->get_node_topics_interface()->resolve_topic_name("right/image_rect"); // Humble/Jazzy don't resolve base topic, fixed by https://github.com/ros-perception/image_common/commit/ea7589ae8c1f7ecb83d6aab7b4c890c2d630d27a + imageRectLeft_.subscribe(this, leftTopic, hints.getTransport(), rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile(), options); + imageRectRight_.subscribe(this, rightTopic, hints.getTransport(), rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile(), options); cameraInfoLeft_.subscribe(this, "left/camera_info", RCLCPP_QOS(topicQueueSize_, qosCamInfo), options); cameraInfoRight_.subscribe(this, "right/camera_info", RCLCPP_QOS(topicQueueSize_, qosCamInfo), options); @@ -373,8 +378,8 @@ void StereoOdometry::onOdomInit() approxSync&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"", topicQueueSize_, syncQueueSize_, - imageRectLeft_.getTopic().c_str(), - imageRectRight_.getTopic().c_str(), + imageRectLeft_.getSubscriber().getTopic().c_str(), + imageRectRight_.getSubscriber().getTopic().c_str(), cameraInfoLeft_.getSubscriber()->get_topic_name(), cameraInfoRight_.getSubscriber()->get_topic_name()); } diff --git a/rtabmap_slam/src/CoreWrapper.cpp b/rtabmap_slam/src/CoreWrapper.cpp index 29b7e0e6..c235b484 100644 --- a/rtabmap_slam/src/CoreWrapper.cpp +++ b/rtabmap_slam/src/CoreWrapper.cpp @@ -768,8 +768,9 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) : Parameters::kRGBDEnabled().c_str(), Parameters::kRGBDEnabled().c_str()); } - image_transport::TransportHints hints(this); - defaultSub_ = image_transport::create_subscription(this, "image", std::bind(&CoreWrapper::defaultCallback, this, std::placeholders::_1), hints.getTransport(), rclcpp::QoS(this->getTopicQueueSize()).reliability((rmw_qos_reliability_policy_t)qosImage_).get_rmw_qos_profile(), subOptions); + image_transport::TransportHints hints(this); // using "image_transport" parameter + std::string imageTopic = this->get_node_topics_interface()->resolve_topic_name("image"); // Humble/Jazzy don't resolve base topic, fixed by https://github.com/ros-perception/image_common/commit/ea7589ae8c1f7ecb83d6aab7b4c890c2d630d27a + defaultSub_ = image_transport::create_subscription(this, imageTopic, std::bind(&CoreWrapper::defaultCallback, this, std::placeholders::_1), hints.getTransport(), rclcpp::QoS(this->getTopicQueueSize()).reliability((rmw_qos_reliability_policy_t)qosImage_).get_rmw_qos_profile(), subOptions); RCLCPP_INFO(this->get_logger(), "\n%s subscribed to:\n %s", get_name(), defaultSub_.getTopic().c_str()); diff --git a/rtabmap_sync/include/rtabmap_sync/CommonDataSubscriber.h b/rtabmap_sync/include/rtabmap_sync/CommonDataSubscriber.h index 1f3f63b2..8fb31a6d 100644 --- a/rtabmap_sync/include/rtabmap_sync/CommonDataSubscriber.h +++ b/rtabmap_sync/include/rtabmap_sync/CommonDataSubscriber.h @@ -269,6 +269,8 @@ private: std::string odomFrameId_; int rgbdCameras_; std::string name_; + std::string imageTransport_; + std::string depthTransport_; rclcpp::CallbackGroup::SharedPtr syncCallbackGroup_; diff --git a/rtabmap_sync/src/CommonDataSubscriber.cpp b/rtabmap_sync/src/CommonDataSubscriber.cpp index d1370c8e..f671b439 100644 --- a/rtabmap_sync/src/CommonDataSubscriber.cpp +++ b/rtabmap_sync/src/CommonDataSubscriber.cpp @@ -47,6 +47,8 @@ CommonDataSubscriber::CommonDataSubscriber(rclcpp::Node & node, bool gui) : subscribedToUserData_(false), odomFrameId_(""), rgbdCameras_(1), + imageTransport_("raw"), + depthTransport_("raw"), // RGB + Depth SYNC_INIT(depth), @@ -392,6 +394,8 @@ CommonDataSubscriber::CommonDataSubscriber(rclcpp::Node & node, bool gui) : "\"sync_queue_size\".", syncQueueSize_); } syncQueueSize_ = node.declare_parameter("sync_queue_size", syncQueueSize_); + imageTransport_ = node.declare_parameter("image_transport", imageTransport_); + depthTransport_ = node.declare_parameter("depth_transport", depthTransport_); int qos = node.declare_parameter("qos", (int)RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT); int qosOdom = node.declare_parameter("qos_odom", qos); @@ -540,6 +544,8 @@ void CommonDataSubscriber::setupCallbacks( RCLCPP_INFO(node.get_logger(), "%s: qos_odom = %d", name_.c_str(), qosOdom_); RCLCPP_INFO(node.get_logger(), "%s: qos_user_data = %d", name_.c_str(), qosUserData_); RCLCPP_INFO(node.get_logger(), "%s: approx_sync = %s", name_.c_str(), approxSync_?"true":"false"); + RCLCPP_INFO(node.get_logger(), "%s: image_transport = %s", name_.c_str(), imageTransport_.c_str()); + RCLCPP_INFO(node.get_logger(), "%s: depth_transport = %s", name_.c_str(), depthTransport_.c_str()); rclcpp::SubscriptionOptions callbackOptions; callbackOptions.callback_group = syncCallbackGroup_; diff --git a/rtabmap_sync/src/impl/CommonDataSubscriberDepth.cpp b/rtabmap_sync/src/impl/CommonDataSubscriberDepth.cpp index 4370e9cc..362b7e81 100644 --- a/rtabmap_sync/src/impl/CommonDataSubscriberDepth.cpp +++ b/rtabmap_sync/src/impl/CommonDataSubscriberDepth.cpp @@ -503,9 +503,12 @@ void CommonDataSubscriber::setupDepthCallbacks( { RCLCPP_INFO(node.get_logger(), "Setup depth callback"); - image_transport::TransportHints hints(&node); - imageSub_.subscribe(&node, "rgb/image", hints.getTransport(), rclcpp::QoS(topicQueueSize_).reliability(qosImage_).get_rmw_qos_profile(), options); - imageDepthSub_.subscribe(&node, "depth/image", hints.getTransport(), rclcpp::QoS(topicQueueSize_).reliability(qosImage_).get_rmw_qos_profile(), options); + image_transport::TransportHints rgbHints(&node); // using "image_transport" parameter + image_transport::TransportHints depthHints(&node, "raw", "depth_transport"); + std::string rgbTopic = node.get_node_topics_interface()->resolve_topic_name("rgb/image"); // Humble/Jazzy don't resolve base topic, fixed by https://github.com/ros-perception/image_common/commit/ea7589ae8c1f7ecb83d6aab7b4c890c2d630d27a + std::string depthTopic = node.get_node_topics_interface()->resolve_topic_name("depth/image"); // Humble/Jazzy don't resolve base topic, fixed by https://github.com/ros-perception/image_common/commit/ea7589ae8c1f7ecb83d6aab7b4c890c2d630d27a + imageSub_.subscribe(&node, rgbTopic, rgbHints.getTransport(), rclcpp::QoS(topicQueueSize_).reliability(qosImage_).get_rmw_qos_profile(), options); + imageDepthSub_.subscribe(&node, depthTopic, depthHints.getTransport(), rclcpp::QoS(topicQueueSize_).reliability(qosImage_).get_rmw_qos_profile(), options); cameraInfoSub_.subscribe(&node, "rgb/camera_info", RCLCPP_QOS(topicQueueSize_, qosCameraInfo_), options); #ifdef RTABMAP_SYNC_USER_DATA diff --git a/rtabmap_sync/src/impl/CommonDataSubscriberRGB.cpp b/rtabmap_sync/src/impl/CommonDataSubscriberRGB.cpp index 9665e223..64a6967d 100644 --- a/rtabmap_sync/src/impl/CommonDataSubscriberRGB.cpp +++ b/rtabmap_sync/src/impl/CommonDataSubscriberRGB.cpp @@ -503,8 +503,9 @@ void CommonDataSubscriber::setupRGBCallbacks( { RCLCPP_INFO(node.get_logger(), "Setup rgb-only callback"); - image_transport::TransportHints hints(&node); - imageSub_.subscribe(&node, "rgb/image", hints.getTransport(), rclcpp::QoS(topicQueueSize_).reliability(qosImage_).get_rmw_qos_profile(), options); + image_transport::TransportHints hints(&node); // using "image_transport" parameter + std::string rgbTopic = node.get_node_topics_interface()->resolve_topic_name("rgb/image"); // Humble/Jazzy don't resolve base topic, fixed by https://github.com/ros-perception/image_common/commit/ea7589ae8c1f7ecb83d6aab7b4c890c2d630d27a + imageSub_.subscribe(&node, rgbTopic, hints.getTransport(), rclcpp::QoS(topicQueueSize_).reliability(qosImage_).get_rmw_qos_profile(), options); cameraInfoSub_.subscribe(&node, "rgb/camera_info", RCLCPP_QOS(topicQueueSize_, qosCameraInfo_), options); #ifdef RTABMAP_SYNC_USER_DATA diff --git a/rtabmap_sync/src/impl/CommonDataSubscriberStereo.cpp b/rtabmap_sync/src/impl/CommonDataSubscriberStereo.cpp index 8ddd3e65..6916e9d0 100644 --- a/rtabmap_sync/src/impl/CommonDataSubscriberStereo.cpp +++ b/rtabmap_sync/src/impl/CommonDataSubscriberStereo.cpp @@ -97,9 +97,11 @@ void CommonDataSubscriber::setupStereoCallbacks( { RCLCPP_INFO(node.get_logger(), "Setup stereo callback"); - image_transport::TransportHints hints(&node); - imageRectLeft_.subscribe(&node, "left/image_rect", hints.getTransport(), rclcpp::QoS(topicQueueSize_).reliability(qosImage_).get_rmw_qos_profile(), options); - imageRectRight_.subscribe(&node, "right/image_rect", hints.getTransport(), rclcpp::QoS(topicQueueSize_).reliability(qosImage_).get_rmw_qos_profile(), options); + image_transport::TransportHints hints(&node); // using "image_transport" parameter + std::string leftTopic = node.get_node_topics_interface()->resolve_topic_name("left/image_rect"); // Humble/Jazzy don't resolve base topic, fixed by https://github.com/ros-perception/image_common/commit/ea7589ae8c1f7ecb83d6aab7b4c890c2d630d27a + std::string rightTopic = node.get_node_topics_interface()->resolve_topic_name("right/image_rect"); // Humble/Jazzy don't resolve base topic, fixed by https://github.com/ros-perception/image_common/commit/ea7589ae8c1f7ecb83d6aab7b4c890c2d630d27a + imageRectLeft_.subscribe(&node, leftTopic, hints.getTransport(), rclcpp::QoS(topicQueueSize_).reliability(qosImage_).get_rmw_qos_profile(), options); + imageRectRight_.subscribe(&node, rightTopic, hints.getTransport(), rclcpp::QoS(topicQueueSize_).reliability(qosImage_).get_rmw_qos_profile(), options); cameraInfoLeft_.subscribe(&node, "left/camera_info", RCLCPP_QOS(topicQueueSize_, qosCameraInfo_), options); cameraInfoRight_.subscribe(&node, "right/camera_info", RCLCPP_QOS(topicQueueSize_, qosCameraInfo_), options); diff --git a/rtabmap_sync/src/nodelets/rgb_sync.cpp b/rtabmap_sync/src/nodelets/rgb_sync.cpp index 9f249390..5498135d 100644 --- a/rtabmap_sync/src/nodelets/rgb_sync.cpp +++ b/rtabmap_sync/src/nodelets/rgb_sync.cpp @@ -72,6 +72,7 @@ RGBSync::RGBSync(const rclcpp::NodeOptions & options) : qos = this->declare_parameter("qos", qos); int qosCaminfo = this->declare_parameter("qos_camera_info", qos); compressedRate_ = this->declare_parameter("compressed_rate", compressedRate_); + std::string imageTransport = this->declare_parameter("image_transport", std::string("raw")); RCLCPP_INFO(this->get_logger(), "%s: approx_sync = %s", get_name(), approxSync?"true":"false"); if(approxSync) @@ -81,6 +82,7 @@ RGBSync::RGBSync(const rclcpp::NodeOptions & options) : RCLCPP_INFO(this->get_logger(), "%s: qos = %d", get_name(), qos); RCLCPP_INFO(this->get_logger(), "%s: qos_camera_info = %d", get_name(), qosCaminfo); RCLCPP_INFO(this->get_logger(), "%s: compressed_rate = %f", get_name(), compressedRate_); + RCLCPP_INFO(this->get_logger(), "%s: image_transport = %s", get_name(), imageTransport.c_str()); rgbdImagePub_ = this->create_publisher("rgbd_image", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos)); rgbdImageCompressedPub_ = this->create_publisher("rgbd_image/compressed", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos)); @@ -98,8 +100,9 @@ RGBSync::RGBSync(const rclcpp::NodeOptions & options) : exactSync_->registerCallback(std::bind(&RGBSync::callback, this, std::placeholders::_1, std::placeholders::_2)); } - image_transport::TransportHints hints(this); - imageSub_.subscribe(this, "rgb/image", hints.getTransport(), rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile()); + image_transport::TransportHints hints(this); // using "image_transport" parameter + std::string rgbTopic = this->get_node_topics_interface()->resolve_topic_name("rgb/image"); // Humble/Jazzy don't resolve base topic, fixed by https://github.com/ros-perception/image_common/commit/ea7589ae8c1f7ecb83d6aab7b4c890c2d630d27a + imageSub_.subscribe(this, rgbTopic, hints.getTransport(), rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile()); cameraInfoSub_.subscribe(this, "rgb/camera_info", RCLCPP_QOS(topicQueueSize, qosCaminfo)); std::string subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s,\n %s", diff --git a/rtabmap_sync/src/nodelets/rgbd_sync.cpp b/rtabmap_sync/src/nodelets/rgbd_sync.cpp index 9aecf358..1bc213ec 100644 --- a/rtabmap_sync/src/nodelets/rgbd_sync.cpp +++ b/rtabmap_sync/src/nodelets/rgbd_sync.cpp @@ -76,6 +76,22 @@ RGBDSync::RGBDSync(const rclcpp::NodeOptions & options) : depthScale_ = this->declare_parameter("depth_scale", depthScale_); decimation_ = this->declare_parameter("decimation", decimation_); compressedRate_ = this->declare_parameter("compressed_rate", compressedRate_); + std::string rgbImageTransport = this->declare_parameter("rgb_image_transport", std::string("raw")); + std::string depthImageTransport = this->declare_parameter("depth_image_transport", std::string("raw")); + if(rgbImageTransport != "raw") { + RCLCPP_WARN(this->get_logger(), "Parameter \"rgb_image_transport\" has been renamed " + "to \"image_transport\" and will be removed " + "in future versions! The value (%s) is copied to " + "\"image_transport\".", rgbImageTransport.c_str()); + } + if(depthImageTransport != "raw") { + RCLCPP_WARN(this->get_logger(), "Parameter \"depth_image_transport\" has been renamed " + "to \"depth_transport\" and will be removed " + "in future versions! The value (%s) is copied to " + "\"depth_transport\".", depthImageTransport.c_str()); + } + std::string imageTransport = this->declare_parameter("image_transport", rgbImageTransport); + std::string depthTransport = this->declare_parameter("depth_transport", depthImageTransport); if(decimation_<1) { @@ -92,6 +108,8 @@ RGBDSync::RGBDSync(const rclcpp::NodeOptions & options) : RCLCPP_INFO(this->get_logger(), "%s: depth_scale = %f", get_name(), depthScale_); RCLCPP_INFO(this->get_logger(), "%s: decimation = %d", get_name(), decimation_); RCLCPP_INFO(this->get_logger(), "%s: compressed_rate = %f", get_name(), compressedRate_); + RCLCPP_INFO(this->get_logger(), "%s: image_transport = %s", get_name(), imageTransport.c_str()); + RCLCPP_INFO(this->get_logger(), "%s: depth_transport = %s", get_name(), depthTransport.c_str()); rgbdImagePub_ = this->create_publisher("rgbd_image", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos)); rgbdImageCompressedPub_ = this->create_publisher("rgbd_image/compressed", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos)); @@ -109,12 +127,12 @@ RGBDSync::RGBDSync(const rclcpp::NodeOptions & options) : exactSyncDepth_->registerCallback(std::bind(&RGBDSync::callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); } - std::string rgbImageTransport = this->declare_parameter("rgb_image_transport", "raw"); - std::string depthImageTransport = this->declare_parameter("depth_image_transport", "raw"); - std::string rgbTopic = this->get_node_topics_interface()->resolve_topic_name("rgb/image"); // Humble doesn't resolve base topic, fixed by https://github.com/ros-perception/image_common/commit/ea7589ae8c1f7ecb83d6aab7b4c890c2d630d27a - std::string depthTopic = this->get_node_topics_interface()->resolve_topic_name("depth/image"); // Humble doesn't resolve base topic, fixed by https://github.com/ros-perception/image_common/commit/ea7589ae8c1f7ecb83d6aab7b4c890c2d630d27a - imageSub_.subscribe(this, rgbTopic, rgbImageTransport, rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile()); - imageDepthSub_.subscribe(this, depthTopic, depthImageTransport, rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile()); + image_transport::TransportHints rgbHints(this); // using "image_transport" parameter + image_transport::TransportHints depthHints(this, "raw", "depth_transport"); + std::string rgbTopic = this->get_node_topics_interface()->resolve_topic_name("rgb/image"); // Humble/Jazzy don't resolve base topic, fixed by https://github.com/ros-perception/image_common/commit/ea7589ae8c1f7ecb83d6aab7b4c890c2d630d27a + std::string depthTopic = this->get_node_topics_interface()->resolve_topic_name("depth/image"); // Humble/Jazzy doesn't resolve base topic, fixed by https://github.com/ros-perception/image_common/commit/ea7589ae8c1f7ecb83d6aab7b4c890c2d630d27a + imageSub_.subscribe(this, rgbTopic, rgbHints.getTransport(), rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile()); + imageDepthSub_.subscribe(this, depthTopic, depthHints.getTransport(), rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile()); cameraInfoSub_.subscribe(this, "rgb/camera_info", RCLCPP_QOS(topicQueueSize, qosCamInfo)); std::string subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s,\n %s,\n %s", diff --git a/rtabmap_sync/src/nodelets/stereo_sync.cpp b/rtabmap_sync/src/nodelets/stereo_sync.cpp index a1b4767c..bb344b39 100644 --- a/rtabmap_sync/src/nodelets/stereo_sync.cpp +++ b/rtabmap_sync/src/nodelets/stereo_sync.cpp @@ -71,6 +71,7 @@ StereoSync::StereoSync(const rclcpp::NodeOptions & options) : qos = this->declare_parameter("qos", qos); int qosCamInfo = this->declare_parameter("qos_camera_info", qos); compressedRate_ = this->declare_parameter("compressed_rate", compressedRate_); + std::string imageTransport = this->declare_parameter("image_transport", std::string("raw")); RCLCPP_INFO(this->get_logger(), "%s: approx_sync = %s", get_name(), approxSync?"true":"false"); RCLCPP_INFO(this->get_logger(), "%s: approx_sync_max_interval = %f", get_name(), approxSyncMaxInterval); @@ -79,6 +80,7 @@ StereoSync::StereoSync(const rclcpp::NodeOptions & options) : RCLCPP_INFO(this->get_logger(), "%s: qos = %d", get_name(), qos); RCLCPP_INFO(this->get_logger(), "%s: qos_camera_info = %d", get_name(), qosCamInfo); RCLCPP_INFO(this->get_logger(), "%s: compressed_rate = %f", get_name(), compressedRate_); + RCLCPP_INFO(this->get_logger(), "%s: image_transport = %s", get_name(), imageTransport.c_str()); rgbdImagePub_ = create_publisher("rgbd_image", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos)); rgbdImageCompressedPub_ = create_publisher("rgbd_image/compressed", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos)); @@ -96,9 +98,11 @@ StereoSync::StereoSync(const rclcpp::NodeOptions & options) : exactSync_->registerCallback(std::bind(&StereoSync::callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4)); } - image_transport::TransportHints hints(this); - imageLeftSub_.subscribe(this, "left/image_rect", hints.getTransport(), rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile()); - imageRightSub_.subscribe(this, "right/image_rect", hints.getTransport(), rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile()); + image_transport::TransportHints hints(this); // using "image_transport" parameter + std::string leftTopic = this->get_node_topics_interface()->resolve_topic_name("left/image_rect"); // Humble/Jazzy don't resolve base topic, fixed by https://github.com/ros-perception/image_common/commit/ea7589ae8c1f7ecb83d6aab7b4c890c2d630d27a + std::string rightTopic = this->get_node_topics_interface()->resolve_topic_name("right/image_rect"); // Humble/Jazzy don't resolve base topic, fixed by https://github.com/ros-perception/image_common/commit/ea7589ae8c1f7ecb83d6aab7b4c890c2d630d27a + imageLeftSub_.subscribe(this, leftTopic, hints.getTransport(), rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile()); + imageRightSub_.subscribe(this, rightTopic, hints.getTransport(), rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile()); cameraInfoLeftSub_.subscribe(this, "left/camera_info", RCLCPP_QOS(topicQueueSize, qosCamInfo)); cameraInfoRightSub_.subscribe(this, "right/camera_info", RCLCPP_QOS(topicQueueSize, qosCamInfo)); diff --git a/rtabmap_util/src/nodelets/point_cloud_xyz.cpp b/rtabmap_util/src/nodelets/point_cloud_xyz.cpp index f66f4663..acc88973 100644 --- a/rtabmap_util/src/nodelets/point_cloud_xyz.cpp +++ b/rtabmap_util/src/nodelets/point_cloud_xyz.cpp @@ -97,6 +97,7 @@ PointCloudXYZ::PointCloudXYZ(const rclcpp::NodeOptions & options) : normalRadius_ = this->declare_parameter("normal_radius", normalRadius_); filterNaNs_ = this->declare_parameter("filter_nans", filterNaNs_); roiStr = this->declare_parameter("roi_ratios", roiStr); + this->declare_parameter("depth_transport", std::string("raw")); //parse roi (region of interest) roiRatios_.resize(4, 0); @@ -156,8 +157,9 @@ PointCloudXYZ::PointCloudXYZ(const rclcpp::NodeOptions & options) : cloudPub_ = create_publisher("cloud", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos)); - image_transport::TransportHints hints(this); - imageDepthSub_.subscribe(this, "depth/image", hints.getTransport(), rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile()); + image_transport::TransportHints hints(this, "raw", "depth_transport"); + std::string depthTopic = this->get_node_topics_interface()->resolve_topic_name("depth/image"); // Humble/Jazzy don't resolve base topic, fixed by https://github.com/ros-perception/image_common/commit/ea7589ae8c1f7ecb83d6aab7b4c890c2d630d27a + imageDepthSub_.subscribe(this, depthTopic, hints.getTransport(), rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile()); cameraInfoSub_.subscribe(this, "depth/camera_info", RCLCPP_QOS(topicQueueSize, qosCamInfo)); disparitySub_.subscribe(this, "disparity/image", RCLCPP_QOS(topicQueueSize, qos)); diff --git a/rtabmap_util/src/nodelets/point_cloud_xyzrgb.cpp b/rtabmap_util/src/nodelets/point_cloud_xyzrgb.cpp index 5d33ffba..eb60b24f 100644 --- a/rtabmap_util/src/nodelets/point_cloud_xyzrgb.cpp +++ b/rtabmap_util/src/nodelets/point_cloud_xyzrgb.cpp @@ -102,6 +102,8 @@ PointCloudXYZRGB::PointCloudXYZRGB(const rclcpp::NodeOptions & options) : normalRadius_ = this->declare_parameter("normal_radius", normalRadius_); filterNaNs_ = this->declare_parameter("filter_nans", filterNaNs_); roiStr = this->declare_parameter("roi_ratios", roiStr); + this->declare_parameter("image_transport", std::string("raw")); + this->declare_parameter("depth_transport", std::string("raw")); //parse roi (region of interest) roiRatios_.resize(4, 0); @@ -184,15 +186,20 @@ PointCloudXYZRGB::PointCloudXYZRGB(const rclcpp::NodeOptions & options) : exactSyncStereo_->registerCallback(std::bind(&PointCloudXYZRGB::stereoCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4)); } - image_transport::TransportHints hints(this); - imageSub_.subscribe(this, "rgb/image", hints.getTransport(), rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile()); - imageDepthSub_.subscribe(this, "depth/image", hints.getTransport(), rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile()); + image_transport::TransportHints rgbHints(this); // using "image_transport" parameter + image_transport::TransportHints depthHints(this, "raw", "depth_transport"); + std::string rgbTopic = this->get_node_topics_interface()->resolve_topic_name("rgb/image"); // Humble/Jazzy don't resolve base topic, fixed by https://github.com/ros-perception/image_common/commit/ea7589ae8c1f7ecb83d6aab7b4c890c2d630d27a + std::string depthTopic = this->get_node_topics_interface()->resolve_topic_name("depth/image"); // Humble/Jazzy don't resolve base topic, fixed by https://github.com/ros-perception/image_common/commit/ea7589ae8c1f7ecb83d6aab7b4c890c2d630d27a + imageSub_.subscribe(this, rgbTopic, rgbHints.getTransport(), rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile()); + imageDepthSub_.subscribe(this, depthTopic, depthHints.getTransport(), rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile()); cameraInfoSub_.subscribe(this, "rgb/camera_info", RCLCPP_QOS(topicQueueSize, qosCamInfo)); imageDisparitySub_.subscribe(this, "disparity", RCLCPP_QOS(topicQueueSize, qos)); - imageLeft_.subscribe(this, "left/image", hints.getTransport(), rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile()); - imageRight_.subscribe(this, "right/image", hints.getTransport(), rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile()); + std::string leftTopic = this->get_node_topics_interface()->resolve_topic_name("left/image"); // Humble/Jazzy don't resolve base topic, fixed by https://github.com/ros-perception/image_common/commit/ea7589ae8c1f7ecb83d6aab7b4c890c2d630d27a + std::string rightTopic = this->get_node_topics_interface()->resolve_topic_name("right/image"); // Humble/Jazzy don't resolve base topic, fixed by https://github.com/ros-perception/image_common/commit/ea7589ae8c1f7ecb83d6aab7b4c890c2d630d27a + imageLeft_.subscribe(this, leftTopic, rgbHints.getTransport(), rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile()); + imageRight_.subscribe(this, rightTopic, rgbHints.getTransport(), rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile()); cameraInfoLeft_.subscribe(this, "left/camera_info", RCLCPP_QOS(topicQueueSize, qosCamInfo)); cameraInfoRight_.subscribe(this, "right/camera_info", RCLCPP_QOS(topicQueueSize, qosCamInfo)); } From ca4d33151cc59bf9b8bffb7231ef29e308903b64 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Mon, 18 Aug 2025 16:59:12 -0700 Subject: [PATCH 04/56] rtabmap node: added map_cache_loaded_on_init parameter (default true, like before) to avoid (when false) loading all local grids in cache on init (in case we know we are in localization mode and not using memory management) --- rtabmap_util/include/rtabmap_util/MapsManager.h | 1 + rtabmap_util/src/MapsManager.cpp | 7 ++++++- 2 files changed, 7 insertions(+), 1 deletion(-) diff --git a/rtabmap_util/include/rtabmap_util/MapsManager.h b/rtabmap_util/include/rtabmap_util/MapsManager.h index 311ca24c..5c013f9b 100644 --- a/rtabmap_util/include/rtabmap_util/MapsManager.h +++ b/rtabmap_util/include/rtabmap_util/MapsManager.h @@ -100,6 +100,7 @@ private: bool mapCacheCleanup_; bool alwaysUpdateMap_; bool scanEmptyRayTracing_; + bool localMapsCacheLoadedOnInit_; ros::Publisher cloudMapPub_; ros::Publisher cloudGroundPub_; diff --git a/rtabmap_util/src/MapsManager.cpp b/rtabmap_util/src/MapsManager.cpp index 1ee902f2..2a1d12ec 100644 --- a/rtabmap_util/src/MapsManager.cpp +++ b/rtabmap_util/src/MapsManager.cpp @@ -70,6 +70,7 @@ MapsManager::MapsManager() : mapCacheCleanup_(true), alwaysUpdateMap_(false), scanEmptyRayTracing_(true), + localMapsCacheLoadedOnInit_(true), assembledObstacles_(new pcl::PointCloud), assembledGround_(new pcl::PointCloud), occupancyGrid_(new OccupancyGrid(&localMaps_)), @@ -122,6 +123,7 @@ void MapsManager::init(ros::NodeHandle & nh, ros::NodeHandle & pnh, const std::s } } pnh.param("map_empty_ray_tracing", scanEmptyRayTracing_, scanEmptyRayTracing_); + pnh.param("map_cache_loaded_on_init", localMapsCacheLoadedOnInit_, localMapsCacheLoadedOnInit_); if(pnh.hasParam("scan_output_voxelized")) { @@ -141,6 +143,7 @@ void MapsManager::init(ros::NodeHandle & nh, ros::NodeHandle & pnh, const std::s ROS_INFO("%s(maps): map_cleanup = %s", name.c_str(), mapCacheCleanup_?"true":"false"); ROS_INFO("%s(maps): map_always_update = %s", name.c_str(), alwaysUpdateMap_?"true":"false"); ROS_INFO("%s(maps): map_empty_ray_tracing = %s", name.c_str(), scanEmptyRayTracing_?"true":"false"); + ROS_INFO("%s(maps): map_cache_loaded_on_init = %s", name.c_str(), localMapsCacheLoadedOnInit_?"true":"false"); ROS_INFO("%s(maps): cloud_output_voxelized = %s", name.c_str(), cloudOutputVoxelized_?"true":"false"); ROS_INFO("%s(maps): cloud_subtract_filtering = %s", name.c_str(), cloudSubtractFiltering_?"true":"false"); ROS_INFO("%s(maps): cloud_subtract_filtering_min_neighbors = %d", name.c_str(), cloudSubtractFilteringMinNeighbors_); @@ -355,7 +358,9 @@ void MapsManager::set2DMap( { occupancyGrid_->setMap(map, xMin, yMin, cellSize, poses); //update cache in case the map should be updated - if(memory) + if(memory && + uStrNumCmp(memory->getDatabaseVersion(), "0.11.10")>=0 && // versions 0.11.10+ have local grids saved in db + localMapsCacheLoadedOnInit_) { for(std::map::const_iterator iter=poses.lower_bound(1); iter!=poses.end(); ++iter) { From e9bed4a6b06a86222a28e32193c6ac83f011ea7f Mon Sep 17 00:00:00 2001 From: matlabbe Date: Tue, 2 Sep 2025 14:28:55 -0700 Subject: [PATCH 05/56] Fixed build against rtabmap 0.23 --- rtabmap_util/src/MapsManager.cpp | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) diff --git a/rtabmap_util/src/MapsManager.cpp b/rtabmap_util/src/MapsManager.cpp index 2a1d12ec..9ad16c45 100644 --- a/rtabmap_util/src/MapsManager.cpp +++ b/rtabmap_util/src/MapsManager.cpp @@ -944,11 +944,19 @@ void MapsManager::publishMaps( if(graphGroundOptimized && !tmpGroundPts.empty()) { +#if RTABMAP_VERSION_MAJOR>0 || (RTABMAP_VERSION_MAJOR==0 && RTABMAP_VERSION_MINOR>=23) + assembledGroundIndex_.buildIndex(FlannIndex::FLANN_INDEX_KDTREE_SINGLE, tmpGroundPts); +#else assembledGroundIndex_.buildKDTreeSingleIndex(tmpGroundPts, 15); +#endif } if(graphObstacleOptimized && !tmpObstaclePts.empty()) { +#if RTABMAP_VERSION_MAJOR>0 || (RTABMAP_VERSION_MAJOR==0 && RTABMAP_VERSION_MINOR>=23) + assembledObstacleIndex_.buildIndex(FlannIndex::FLANN_INDEX_KDTREE_SINGLE, tmpObstaclePts); +#else assembledObstacleIndex_.buildKDTreeSingleIndex(tmpObstaclePts, 15); +#endif } double indexingTime = t.ticks(); ROS_INFO("Graph optimized! Time recreating clouds (%d ground, %d obstacles) = %f s (indexing %fs)", countGrounds, countObstacles, addingPointsTime+indexingTime, indexingTime); @@ -989,7 +997,11 @@ void MapsManager::publishMaps( } if(!assembledGroundIndex_.isBuilt()) { +#if RTABMAP_VERSION_MAJOR>0 || (RTABMAP_VERSION_MAJOR==0 && RTABMAP_VERSION_MINOR>=23) + assembledGroundIndex_.buildIndex(FlannIndex::FLANN_INDEX_KDTREE_SINGLE, pts); +#else assembledGroundIndex_.buildKDTreeSingleIndex(pts, 15); +#endif } else { @@ -1036,7 +1048,11 @@ void MapsManager::publishMaps( } if(!assembledObstacleIndex_.isBuilt()) { +#if RTABMAP_VERSION_MAJOR>0 || (RTABMAP_VERSION_MAJOR==0 && RTABMAP_VERSION_MINOR>=23) + assembledObstacleIndex_.buildIndex(FlannIndex::FLANN_INDEX_KDTREE_SINGLE, pts); +#else assembledObstacleIndex_.buildKDTreeSingleIndex(pts, 15); +#endif } else { From deee14e1d250eb9789355f25763321ff7870ed03 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Wed, 3 Sep 2025 16:59:24 -0700 Subject: [PATCH 06/56] Fixing map_cache_loaded_on_init loading anyway on first update (#1352) --- rtabmap_util/src/MapsManager.cpp | 37 +++++++++++++++++++------------- 1 file changed, 22 insertions(+), 15 deletions(-) diff --git a/rtabmap_util/src/MapsManager.cpp b/rtabmap_util/src/MapsManager.cpp index 9ad16c45..0429508f 100644 --- a/rtabmap_util/src/MapsManager.cpp +++ b/rtabmap_util/src/MapsManager.cpp @@ -551,32 +551,39 @@ std::map MapsManager::updateMapCaches( filteredPoses.erase(0); } + bool fullUpdateNeeded = true; +#if RTABMAP_VERSION_MAJOR>0 || (RTABMAP_VERSION_MAJOR==0 && RTABMAP_VERSION_MINOR>=23) + fullUpdateNeeded = (updateGrid && occupancyGrid_->fullUpdateNeeded(filteredPoses)) +#if defined(WITH_OCTOMAP_MSGS) and defined(RTABMAP_OCTOMAP) + || (updateOctomap && octomap_->fullUpdateNeeded(filteredPoses)) +#endif +#if defined(WITH_GRID_MAP_ROS) and defined(RTABMAP_GRIDMAP) + || (updateElevation && elevationMap_->fullUpdateNeeded(filteredPoses)) +#endif + ; + if(fullUpdateNeeded) { + ROS_INFO("Full occupancy grid map update needed"); + } + else { + ROS_DEBUG("Full occupancy grid map update not needed"); + } +#endif + bool longUpdate = false; UTimer longUpdateTimer; - if(filteredPoses.size() > 20) + if(fullUpdateNeeded && filteredPoses.size() > 20 & localMaps_.size() < 5) { - if(updateGridCache && localMaps_.size() < 5) - { - ROS_WARN("Many occupancy grids should be loaded (~%d), this may take a while to update the map(s)...", int(filteredPoses.size()-localMaps_.size())); - longUpdate = true; - } -#if defined(WITH_OCTOMAP_MSGS) and defined(RTABMAP_OCTOMAP) - if(updateOctomap && octomap_->addedNodes().size() < 5) - { - ROS_WARN("Many clouds should be added to octomap (~%d), this may take a while to update the map(s)...", int(filteredPoses.size()-octomap_->addedNodes().size())); - longUpdate = true; - } -#endif + ROS_WARN("Many occupancy grids should be loaded (~%d), this may take a while to update the map(s)...", int(filteredPoses.size()-localMaps_.size())); + longUpdate = true; } bool occupancySavedInDB = memory && uStrNumCmp(memory->getDatabaseVersion(), "0.11.10")>=0?true:false; - for(std::map::iterator iter=filteredPoses.begin(); iter!=filteredPoses.end(); ++iter) { if(!iter->second.isNull()) { rtabmap::SensorData data; - if(updateGridCache && (iter->first == 0 || !uContains(localMaps_.localGrids(), iter->first))) + if(iter->first == 0 || (fullUpdateNeeded && !uContains(localMaps_.localGrids(), iter->first))) { ROS_DEBUG("Data required for %d", iter->first); std::map::const_iterator findIter = signatures.find(iter->first); From 3fd34c64f47de42a53aad6cfbe2932a8d7f13b54 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sun, 7 Sep 2025 11:49:38 -0700 Subject: [PATCH 07/56] fixed typo --- rtabmap_util/src/MapsManager.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rtabmap_util/src/MapsManager.cpp b/rtabmap_util/src/MapsManager.cpp index 0429508f..59db3fa6 100644 --- a/rtabmap_util/src/MapsManager.cpp +++ b/rtabmap_util/src/MapsManager.cpp @@ -571,7 +571,7 @@ std::map MapsManager::updateMapCaches( bool longUpdate = false; UTimer longUpdateTimer; - if(fullUpdateNeeded && filteredPoses.size() > 20 & localMaps_.size() < 5) + if(fullUpdateNeeded && filteredPoses.size() > 20 && localMaps_.size() < 5) { ROS_WARN("Many occupancy grids should be loaded (~%d), this may take a while to update the map(s)...", int(filteredPoses.size()-localMaps_.size())); longUpdate = true; From 0ad034680be3949fe374f89f1eaff60291f3529d Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sun, 7 Sep 2025 19:08:58 -0700 Subject: [PATCH 08/56] Odom: added parameter "always_process_most_recent_frame" (default true like before) so that we can disable aggressive frame dropping in case input frames are published with flaky latency (e.g., with large rosbag having issue to replay in time topics) --- .../include/rtabmap_odom/OdometryROS.h | 5 +- .../include/rtabmap_odom/icp_odometry.hpp | 1 + rtabmap_odom/src/OdometryROS.cpp | 63 ++++++++++++++++--- rtabmap_odom/src/nodelets/icp_odometry.cpp | 7 ++- .../include/rtabmap_sync/SyncDiagnostic.h | 47 +++++++------- rtabmap_util/CMakeLists.txt | 2 + .../include/rtabmap_util/lidar_deskewing.hpp | 4 ++ rtabmap_util/package.xml | 1 + rtabmap_util/src/nodelets/lidar_deskewing.cpp | 24 +++++++ rtabmap_viz/src/GuiWrapper.cpp | 12 +++- 10 files changed, 132 insertions(+), 34 deletions(-) diff --git a/rtabmap_odom/include/rtabmap_odom/OdometryROS.h b/rtabmap_odom/include/rtabmap_odom/OdometryROS.h index 4a1b7a81..582d0bc5 100644 --- a/rtabmap_odom/include/rtabmap_odom/OdometryROS.h +++ b/rtabmap_odom/include/rtabmap_odom/OdometryROS.h @@ -98,7 +98,7 @@ protected: virtual void postProcessData(const rtabmap::SensorData & /*data*/, const std_msgs::msg::Header & /*header*/) const {} private: - + void processData(); virtual void mainLoop(); virtual void mainLoopKill(); virtual void updateParameters(rtabmap::ParametersMap &) {} @@ -174,9 +174,12 @@ private: rtabmap::Transform guessPreviousPose_; double previousStamp_; double previousClockTime_; + double lastReceivedTopicClock_; + double lastReceivedTopicStamp_; double expectedUpdateRate_; double maxUpdateRate_; double minUpdateRate_; + bool alwaysProcessMostRecentFrame_; std::string compressionImgFormat_; bool compressionParallelized_; int odomStrategy_; diff --git a/rtabmap_odom/include/rtabmap_odom/icp_odometry.hpp b/rtabmap_odom/include/rtabmap_odom/icp_odometry.hpp index 28ab0e55..dc50b450 100644 --- a/rtabmap_odom/include/rtabmap_odom/icp_odometry.hpp +++ b/rtabmap_odom/include/rtabmap_odom/icp_odometry.hpp @@ -78,6 +78,7 @@ private: double scanNormalGroundUp_; bool deskewing_; bool deskewingSlerp_; + int topicQueueSize_; //std::vector > plugins_; //pluginlib::ClassLoader plugin_loader_; bool scanReceived_ = false; diff --git a/rtabmap_odom/src/OdometryROS.cpp b/rtabmap_odom/src/OdometryROS.cpp index c9955d99..d7624e62 100644 --- a/rtabmap_odom/src/OdometryROS.cpp +++ b/rtabmap_odom/src/OdometryROS.cpp @@ -89,9 +89,12 @@ OdometryROS::OdometryROS(const std::string & name, const rclcpp::NodeOptions & o icpParams_(false), previousStamp_(0.0), previousClockTime_(0.0), + lastReceivedTopicClock_(0.0), + lastReceivedTopicStamp_(0.0), expectedUpdateRate_(0.0), maxUpdateRate_(0.0), minUpdateRate_(0.0), + alwaysProcessMostRecentFrame_(true), compressionImgFormat_(".jpg"), compressionParallelized_(true), odomStrategy_(Parameters::defaultOdomStrategy()), @@ -144,6 +147,7 @@ OdometryROS::OdometryROS(const std::string & name, const rclcpp::NodeOptions & o expectedUpdateRate_ = this->declare_parameter("expected_update_rate", expectedUpdateRate_); maxUpdateRate_ = this->declare_parameter("max_update_rate", maxUpdateRate_); minUpdateRate_ = this->declare_parameter("min_update_rate", minUpdateRate_); + alwaysProcessMostRecentFrame_ = this->declare_parameter("always_process_most_recent_frame", alwaysProcessMostRecentFrame_); compressionImgFormat_ = this->declare_parameter("sensor_data_compression_format", compressionImgFormat_); compressionParallelized_ = this->declare_parameter("sensor_data_parallel_compression", compressionParallelized_); @@ -461,25 +465,47 @@ void OdometryROS::callbackIMU(const sensor_msgs::msg::Imu::SharedPtr msg) void OdometryROS::processData(SensorData & data, const std_msgs::msg::Header & header) { //RCLCPP_WARN(get_logger(), "Received image: %f delay=%f", data.stamp(), (now() - header.stamp).seconds()); + double clockNow = rtabmap_conversions::timestampFromROS(now()); if(dataMutex_.lockTry() == 0) { if(bufferedDataToProcess_) { - RCLCPP_ERROR(this->get_logger(), "We didn't receive IMU newer than previous image (%f) and we just received a new image (%f). The previous image is dropped!", + RCLCPP_ERROR(this->get_logger(), "We didn't receive IMU newer than previous image/scan (%f) and we just received a new image/scan (%f). The previous image/scan is dropped!", rtabmap_conversions::timestampFromROS(dataHeaderToProcess_.stamp), rtabmap_conversions::timestampFromROS(header.stamp)); ++droppedMsgs_; } dataToProcess_ = data; dataHeaderToProcess_ = header; bufferedDataToProcess_ = false; - dataReady_.release(); + if(alwaysProcessMostRecentFrame_) { + dataReady_.release(); + } dataMutex_.unlock(); ++processedMsgs_; + if(!alwaysProcessMostRecentFrame_) { + processData(); + } } else { - //RCLCPP_WARN(get_logger(), "Dropping image/scan data"); + double estimatedPeriod = clockNow - lastReceivedTopicClock_; + double topicPeriod = rtabmap_conversions::timestampFromROS(header.stamp) - lastReceivedTopicStamp_; + if(estimatedPeriod>0.0 && topicPeriod>0.0 && estimatedPeriod < topicPeriod*0.9) { + RCLCPP_WARN(get_logger(), + "Dropping image/scan data with stamp %f (delay=%f). Something is wrong " + "because the clock difference with the previous topic received (%fs) is much lower than the " + "expected one (%fs) estimated from the topic stamps (previous stamp=%f). If you are processing " + "a large bag with flaky replaying delay, consider setting parameter \"always_process_most_recent_frame:=false\" " + "to avoid aggressively dropping data.", + rtabmap_conversions::timestampFromROS(header.stamp), + clockNow - rtabmap_conversions::timestampFromROS(header.stamp), + estimatedPeriod, + topicPeriod, + lastReceivedTopicStamp_); + } ++droppedMsgs_; } + lastReceivedTopicStamp_ = rtabmap_conversions::timestampFromROS(header.stamp); + lastReceivedTopicClock_ = clockNow; } void OdometryROS::mainLoopKill() @@ -497,7 +523,10 @@ void OdometryROS::mainLoop() // thread killed return; } - + processData(); +} +void OdometryROS::processData() +{ UScopeMutex lock(dataMutex_); // aliases @@ -516,21 +545,37 @@ void OdometryROS::mainLoop() if(waitIMUToinit_ && (imus_.empty() || imus_.rbegin()->first < rtabmap_conversions::timestampFromROS(header.stamp))) { - RCLCPP_WARN(this->get_logger(), "Make sure IMU is published faster than data rate! (last image stamp=%f and last imu stamp received=%f). Buffering the image until an imu with same or greater stamp is received.", - data.stamp(), imus_.empty()?0:imus_.rbegin()->first); + if(!imus_.empty()) { + RCLCPP_WARN(this->get_logger(), "Make sure IMU is published faster than data rate! (last image/scan stamp=%f and last imu stamp received=%f). Buffering the image/scan until an imu with same or greater stamp is received.", + data.stamp(), imus_.rbegin()->first); + } + else { + // If empty, it is an error! + RCLCPP_ERROR(this->get_logger(), "Make sure IMU is published faster than data rate! (last image/scan stamp=%f and imu buffer is empty). Buffering the image/scan until an imu with same or greater stamp is received.", + data.stamp()); + } bufferedDataToProcess_ = true; return; } // process all imu data up to current image stamp (or just after so that underlying odom approach can do interpolation of imu at image stamp) std::map::iterator iterEnd = imus_.lower_bound(rtabmap_conversions::timestampFromROS(header.stamp)); + std::map::iterator iterLast = iterEnd; if(iterEnd!= imus_.end()) { ++iterEnd; } for(std::map::iterator iter=imus_.begin(); iter!=iterEnd;) { - imus.push_back(*iter); - imus_.erase(iter++); + // Because we always keep the last processed imu in the buffer, skip the first one when processing again the buffer. + if(iter!=imus_.begin()) { + imus.push_back(*iter); + } + if(iter!=iterLast) { + imus_.erase(iter++); + } + else { + ++iter; + } } } // end imu lock @@ -1241,6 +1286,8 @@ void OdometryROS::reset(const Transform & pose) guessPreviousPose_.setNull(); previousStamp_ = 0.0; previousClockTime_ = 0.0; + lastReceivedTopicClock_ = 0.0; + lastReceivedTopicStamp_ = 0.0; resetCurrentCount_ = resetCountdown_; imuProcessed_ = false; dataToProcess_ = SensorData(); diff --git a/rtabmap_odom/src/nodelets/icp_odometry.cpp b/rtabmap_odom/src/nodelets/icp_odometry.cpp index d30b68aa..990976ec 100644 --- a/rtabmap_odom/src/nodelets/icp_odometry.cpp +++ b/rtabmap_odom/src/nodelets/icp_odometry.cpp @@ -60,6 +60,7 @@ ICPOdometry::ICPOdometry(const rclcpp::NodeOptions & options) : scanNormalGroundUp_(0.0), deskewing_(false), deskewingSlerp_(false), + topicQueueSize_(1), scanReceived_(false), cloudReceived_(false) { @@ -83,6 +84,7 @@ void ICPOdometry::onOdomInit() scanNormalGroundUp_ = this->declare_parameter("scan_normal_ground_up", scanNormalGroundUp_); deskewing_ = this->declare_parameter("deskewing", deskewing_); deskewingSlerp_ = this->declare_parameter("deskewing_slerp", deskewingSlerp_); + topicQueueSize_ = this->declare_parameter("topic_queue_size", topicQueueSize_); RCLCPP_INFO(this->get_logger(), "IcpOdometry: qos = %d", (int)qos()); RCLCPP_INFO(this->get_logger(), "IcpOdometry: scan_cloud_max_points = %d", scanCloudMaxPoints_); @@ -96,12 +98,13 @@ void ICPOdometry::onOdomInit() RCLCPP_INFO(this->get_logger(), "IcpOdometry: scan_normal_ground_up = %f", scanNormalGroundUp_); RCLCPP_INFO(this->get_logger(), "IcpOdometry: deskewing = %s", deskewing_?"true":"false"); RCLCPP_INFO(this->get_logger(), "IcpOdometry: deskewing_slerp = %s", deskewingSlerp_?"true":"false"); + RCLCPP_INFO(this->get_logger(), "IcpOdometry: topic_queue_size = %d", topicQueueSize_); rclcpp::SubscriptionOptions options; options.callback_group = dataCallbackGroup_; - scan_sub_ = create_subscription("scan", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()), std::bind(&ICPOdometry::callbackScan, this, std::placeholders::_1), options); - cloud_sub_ = create_subscription("scan_cloud", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()), std::bind(&ICPOdometry::callbackCloud, this, std::placeholders::_1), options); + scan_sub_ = create_subscription("scan", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()), std::bind(&ICPOdometry::callbackScan, this, std::placeholders::_1), options); + cloud_sub_ = create_subscription("scan_cloud", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()), std::bind(&ICPOdometry::callbackCloud, this, std::placeholders::_1), options); filtered_scan_pub_ = create_publisher("odom_filtered_input_scan", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos())); diff --git a/rtabmap_sync/include/rtabmap_sync/SyncDiagnostic.h b/rtabmap_sync/include/rtabmap_sync/SyncDiagnostic.h index 03b49348..f9411519 100644 --- a/rtabmap_sync/include/rtabmap_sync/SyncDiagnostic.h +++ b/rtabmap_sync/include/rtabmap_sync/SyncDiagnostic.h @@ -62,7 +62,7 @@ class SyncDiagnostic { diagnosticTimer_ = node_->create_wall_timer(5s, std::bind(&SyncDiagnostic::diagnosticTimerCallback, this), nullptr); } - void tickInput(const rclcpp::Time & stamp, double expectedFrequency = 0) + void tickInput(const rclcpp::Time & stamp, double expectedFrequency = 0.0) { updateFrequency( stamp, @@ -74,9 +74,12 @@ class SyncDiagnostic { lastTickInputStamp_); } - void tickOutput(const rclcpp::Time & stamp, double expectedFrequency = 0) + void tickOutput(const rclcpp::Time & stamp, double expectedFrequency = 0.0) { - double lastTickOutputStamp; + if(expectedFrequency == 0.0) { + outTargetFrequency_ = inTargetFrequency_; + } + double lastTickOutputStamp = 0.0; updateFrequency( stamp, expectedFrequency, @@ -112,31 +115,33 @@ private: timeStatus.tick(stamp); double stampSec = rtabmap_conversions::timestampFromROS(stamp); - double singlePeriod = stampSec - lastTickStamp; - window.push_back(singlePeriod); - if(window.size() > windowSize_) + if(expectedFrequency>0) { - window.pop_front(); + targetFrequency = expectedFrequency; + } + else if(lastTickStamp > 0.0) { + double singlePeriod = stampSec - lastTickStamp; - double period = 0.0; - if(window.size() == windowSize_) + window.push_back(singlePeriod); + if(window.size() > windowSize_) { - for(size_t i=0; i0.0 && expectedFrequency == 0 && (targetFrequency == 0.0 || period < 1.0/targetFrequency)) - { - targetFrequency = 1.0/period; - } - else if(expectedFrequency>0) - { - targetFrequency = expectedFrequency; - + if(period>0.0 && (targetFrequency == 0.0 || period < 1.0/targetFrequency)) + { + targetFrequency = 1.0/period; + } } } diff --git a/rtabmap_util/CMakeLists.txt b/rtabmap_util/CMakeLists.txt index 525d239e..15fc4274 100644 --- a/rtabmap_util/CMakeLists.txt +++ b/rtabmap_util/CMakeLists.txt @@ -26,6 +26,7 @@ find_package(pcl_ros REQUIRED) find_package(message_filters REQUIRED) find_package(rtabmap_msgs REQUIRED) find_package(rtabmap_conversions REQUIRED) +find_package(rtabmap_sync REQUIRED) # Optional components find_package(octomap_msgs) @@ -54,6 +55,7 @@ SET(Libraries message_filters rtabmap_msgs rtabmap_conversions + rtabmap_sync ) if("$ENV{ROS_DISTRO}" STRLESS "jazzy") diff --git a/rtabmap_util/include/rtabmap_util/lidar_deskewing.hpp b/rtabmap_util/include/rtabmap_util/lidar_deskewing.hpp index 3f19b09c..a1def1c0 100644 --- a/rtabmap_util/include/rtabmap_util/lidar_deskewing.hpp +++ b/rtabmap_util/include/rtabmap_util/lidar_deskewing.hpp @@ -28,6 +28,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include "rclcpp/rclcpp.hpp" +#include + #include #include @@ -58,6 +60,8 @@ private: bool slerp_; std::shared_ptr tfBuffer_; std::shared_ptr tfListener_; + std::unique_ptr scanSyncDiagnostic_; + std::unique_ptr cloudSyncDiagnostic_; }; } diff --git a/rtabmap_util/package.xml b/rtabmap_util/package.xml index a802b4f0..21d5df95 100644 --- a/rtabmap_util/package.xml +++ b/rtabmap_util/package.xml @@ -32,6 +32,7 @@ message_filters rtabmap_msgs rtabmap_conversions + rtabmap_sync grid_map_ros diff --git a/rtabmap_util/src/nodelets/lidar_deskewing.cpp b/rtabmap_util/src/nodelets/lidar_deskewing.cpp index 2ebd00a7..d15e189b 100644 --- a/rtabmap_util/src/nodelets/lidar_deskewing.cpp +++ b/rtabmap_util/src/nodelets/lidar_deskewing.cpp @@ -46,6 +46,16 @@ LidarDeskewing::~LidarDeskewing() void LidarDeskewing::callbackScan(const sensor_msgs::msg::LaserScan::ConstSharedPtr msg) { + if(scanSyncDiagnostic_.get() == 0) { + scanSyncDiagnostic_.reset(new rtabmap_sync::SyncDiagnostic(this, 0.5)); + scanSyncDiagnostic_->init(subScan_->get_topic_name(), + uFormat("%s: Did not receive data since 5 seconds! Make sure the input topic \"%s\" is " + "published (\"$ rostopic hz my_topic\") and the timestamps in their " + "header are set.", + this->get_name(), + subScan_->get_topic_name())); + } + scanSyncDiagnostic_->tickInput(msg->header.stamp); // make sure the frame of the laser is updated during the whole scan time rtabmap::Transform tmpT = rtabmap_conversions::getMovingTransform( msg->header.frame_id, @@ -75,10 +85,23 @@ void LidarDeskewing::callbackScan(const sensor_msgs::msg::LaserScan::ConstShared rtabmap_conversions::transformPointCloud(t.toEigen4f(), scanOut, scanOutDeskewed); scanOutDeskewed.header.frame_id = msg->header.frame_id; pubScan_->publish(scanOutDeskewed); + + scanSyncDiagnostic_->tickOutput(msg->header.stamp); } void LidarDeskewing::callbackCloud(const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg) { + if(cloudSyncDiagnostic_.get() == 0) { + cloudSyncDiagnostic_.reset(new rtabmap_sync::SyncDiagnostic(this, 0.5)); + cloudSyncDiagnostic_->init(subCloud_->get_topic_name(), + uFormat("%s: Did not receive data since 5 seconds! Make sure the input topic \"%s\" is " + "published (\"$ rostopic hz my_topic\") and the timestamps in their " + "header are set.", + this->get_name(), + subCloud_->get_topic_name())); + } + cloudSyncDiagnostic_->tickInput(msg->header.stamp); + sensor_msgs::msg::PointCloud2 msgDeskewed; if(rtabmap_conversions::deskew(*msg, msgDeskewed, fixedFrameId_, *tfBuffer_, waitForTransformDuration_, slerp_)) { @@ -91,6 +114,7 @@ void LidarDeskewing::callbackCloud(const sensor_msgs::msg::PointCloud2::ConstSha RCLCPP_WARN(this->get_logger(), "deskewing failed! returning possible skewed cloud!"); pubCloud_->publish(*msg); } + cloudSyncDiagnostic_->tickOutput(msg->header.stamp); } } diff --git a/rtabmap_viz/src/GuiWrapper.cpp b/rtabmap_viz/src/GuiWrapper.cpp index 44ac1c97..e0283d7c 100644 --- a/rtabmap_viz/src/GuiWrapper.cpp +++ b/rtabmap_viz/src/GuiWrapper.cpp @@ -203,7 +203,11 @@ void GuiWrapper::infoMapCallback( this->post(new RtabmapEvent(stat)); - tick(infoMsg->header.stamp); + ParametersMap allParameters = prefDialog_->getAllParameters(); + float detectionRate = Parameters::defaultRtabmapDetectionRate(); + Parameters::parse(allParameters, Parameters::kRtabmapDetectionRate(), detectionRate); + + tick(infoMsg->header.stamp, detectionRate); } @@ -236,7 +240,11 @@ void GuiWrapper::infoCallback( this->post(new RtabmapEvent(stat)); - tick(infoMsg->header.stamp); + ParametersMap allParameters = prefDialog_->getAllParameters(); + float detectionRate = Parameters::defaultRtabmapDetectionRate(); + Parameters::parse(allParameters, Parameters::kRtabmapDetectionRate(), detectionRate); + + tick(infoMsg->header.stamp, detectionRate); } void GuiWrapper::goalPathCallback( From f64fb02eab3333daa8875180a4758a5f3da976fb Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sun, 7 Sep 2025 20:06:13 -0700 Subject: [PATCH 09/56] Fixed imus ignored (bug previous commit) --- rtabmap_odom/src/OdometryROS.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/rtabmap_odom/src/OdometryROS.cpp b/rtabmap_odom/src/OdometryROS.cpp index d7624e62..4318d213 100644 --- a/rtabmap_odom/src/OdometryROS.cpp +++ b/rtabmap_odom/src/OdometryROS.cpp @@ -564,10 +564,11 @@ void OdometryROS::processData() { ++iterEnd; } - for(std::map::iterator iter=imus_.begin(); iter!=iterEnd;) + std::map::iterator iterFirst = imus_.begin(); + for(std::map::iterator iter=iterFirst; iter!=iterEnd;) { // Because we always keep the last processed imu in the buffer, skip the first one when processing again the buffer. - if(iter!=imus_.begin()) { + if(iter!=iterFirst) { imus.push_back(*iter); } if(iter!=iterLast) { From bed951751b97fc8fc63776f120ba4f99ceba626a Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sat, 13 Sep 2025 10:20:53 -0700 Subject: [PATCH 10/56] Update README.md Fixed some hyperlinks --- rtabmap_legacy/launch/jfr2018/README.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/rtabmap_legacy/launch/jfr2018/README.md b/rtabmap_legacy/launch/jfr2018/README.md index 8124c2e4..2f2eec64 100644 --- a/rtabmap_legacy/launch/jfr2018/README.md +++ b/rtabmap_legacy/launch/jfr2018/README.md @@ -1,7 +1,7 @@ **TODO: currently committed here for backup, though a comprehensive step-by-step procedure would be required so that anyone could reproduce the results** -This is the raw files used to generate results for MIT Stata Center dataset of this paper (for KITTI, EuRoC and TUM datasets, see this [page](https://github.com/introlab/rtabmap/blob/docker/jfr2018)): +This is the raw files used to generate results for MIT Stata Center dataset of this paper (for KITTI, EuRoC and TUM datasets, see this [page](https://github.com/introlab/rtabmap/tree/master/docker/jfr2018)): * M. Labbé and F. Michaud, “RTAB-Map as an Open-Source Lidar and Visual SLAM Library for Large-Scale and Long-Term Online Operation,” in Journal of Field Robotics, accepted, 2018. ([pdf](https://introlab.3it.usherbrooke.ca/mediawiki-introlab/images/7/7a/Labbe18JFR_preprint.pdf)) ([Wiley](https://doi.org/10.1002/rob.21831)) -Manual instructions are in [launch_lidar](https://github.com/introlab/rtabmap_ros/blob/master/launch/jfr2018/launch_lidar), [launch_stereo](https://github.com/introlab/rtabmap_ros/blob/master/launch/jfr2018/launch_stereo) and [launch_rgbd](https://github.com/introlab/rtabmap_ros/blob/master/launch/jfr2018/launch_rgbd) files depending on the sensor configuration. Refer also to explanations in the paper. +Manual instructions are in [launch_lidar](https://github.com/introlab/rtabmap_ros/blob/master/rtabmap_legacy/launch/jfr2018/launch_lidar), [launch_stereo](https://github.com/introlab/rtabmap_ros/blob/master/rtabmap_legacy/launch/jfr2018/launch_stereo) and [launch_rgbd](https://github.com/introlab/rtabmap_ros/blob/master/rtabmap_legacy/launch/jfr2018/launch_rgbd) files depending on the sensor configuration. Refer also to explanations in the paper. From e355deca4391403dc1763bb6ed41e4fe3956ea26 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sat, 13 Sep 2025 17:52:54 +0000 Subject: [PATCH 11/56] devcontainer: nopasswd for vscode user --- .devcontainer/Dockerfile | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/.devcontainer/Dockerfile b/.devcontainer/Dockerfile index 3eec0a03..d4d636ad 100644 --- a/.devcontainer/Dockerfile +++ b/.devcontainer/Dockerfile @@ -8,7 +8,9 @@ ARG USER_GID=1000 RUN set -ex && \ groupadd --gid ${USER_GID} ${USERNAME} && \ useradd --uid ${USER_UID} --gid ${USER_GID} -m ${USERNAME} && \ - usermod -a -G sudo ${USERNAME} + usermod -a -G sudo ${USERNAME} && \ + echo "${USERNAME} ALL=(ALL) NOPASSWD:ALL" > /etc/sudoers.d/${USERNAME} && \ + chmod 0440 /etc/sudoers.d/${USERNAME} RUN mkdir -p /home/${USERNAME}/catkin_ws/src && \ chown -R ${USERNAME}:${USERNAME} /home/${USERNAME}/catkin_ws From 3fa20f56fb07f493837d5b49e1c681fdf539d44d Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sun, 14 Sep 2025 23:44:46 +0000 Subject: [PATCH 12/56] Backporting ros2 commit 0ad034680be3949fe374f89f1eaff60291f3529d --- rtabmap_launch/launch/rtabmap.launch | 4 ++ .../include/rtabmap_odom/OdometryROS.h | 4 ++ rtabmap_odom/src/OdometryROS.cpp | 71 +++++++++++++++---- rtabmap_odom/src/nodelets/icp_odometry.cpp | 14 +++- 4 files changed, 80 insertions(+), 13 deletions(-) diff --git a/rtabmap_launch/launch/rtabmap.launch b/rtabmap_launch/launch/rtabmap.launch index 19ea0b14..fc6dc4b5 100644 --- a/rtabmap_launch/launch/rtabmap.launch +++ b/rtabmap_launch/launch/rtabmap.launch @@ -127,6 +127,7 @@ + @@ -264,6 +265,7 @@ + @@ -295,6 +297,7 @@ + @@ -323,6 +326,7 @@ + diff --git a/rtabmap_odom/include/rtabmap_odom/OdometryROS.h b/rtabmap_odom/include/rtabmap_odom/OdometryROS.h index 24d20160..f21262b2 100644 --- a/rtabmap_odom/include/rtabmap_odom/OdometryROS.h +++ b/rtabmap_odom/include/rtabmap_odom/OdometryROS.h @@ -95,6 +95,7 @@ private: virtual void onOdomInit() = 0; virtual void updateParameters(rtabmap::ParametersMap & parameters) {} + void processData(); virtual void mainLoop(); virtual void mainLoopKill(); @@ -160,9 +161,12 @@ private: rtabmap::Transform guessPreviousPose_; ros::Time previousStamp_; ros::Time previousClockTime_; + double lastReceivedTopicClock_; + double lastReceivedTopicStamp_; double expectedUpdateRate_; double maxUpdateRate_; double minUpdateRate_; + bool alwaysProcessMostRecentFrame_; std::string compressionImgFormat_; bool compressionParallelized_; int odomStrategy_; diff --git a/rtabmap_odom/src/OdometryROS.cpp b/rtabmap_odom/src/OdometryROS.cpp index 81d08076..beff2a20 100644 --- a/rtabmap_odom/src/OdometryROS.cpp +++ b/rtabmap_odom/src/OdometryROS.cpp @@ -78,9 +78,12 @@ OdometryROS::OdometryROS(bool stereoParams, bool visParams, bool icpParams) : stereoParams_(stereoParams), visParams_(visParams), icpParams_(icpParams), + lastReceivedTopicClock_(0.0), + lastReceivedTopicStamp_(0.0), expectedUpdateRate_(0.0), maxUpdateRate_(0.0), minUpdateRate_(0.0), + alwaysProcessMostRecentFrame_(true), compressionImgFormat_(".jpg"), compressionParallelized_(true), odomStrategy_(Parameters::defaultOdomStrategy()), @@ -148,6 +151,7 @@ void OdometryROS::onInit() pnh.param("expected_update_rate", expectedUpdateRate_, expectedUpdateRate_); // expected sensor rate pnh.param("max_update_rate", maxUpdateRate_, maxUpdateRate_); pnh.param("min_update_rate", minUpdateRate_, minUpdateRate_); + pnh.param("always_process_most_recent_frame", alwaysProcessMostRecentFrame_, alwaysProcessMostRecentFrame_); pnh.param("sensor_data_compression_format", compressionImgFormat_, compressionImgFormat_); pnh.param("sensor_data_parallel_compression", compressionParallelized_, compressionParallelized_); @@ -373,10 +377,11 @@ void OdometryROS::onInit() Parameters::parse(this->parameters(), Parameters::kOdomStrategy(), odomStrategy_); if(waitIMUToinit_) { - int queueSize = 10; - pnh.param("queue_size", queueSize, queueSize); - imuSub_ = nh.subscribe("imu", queueSize*5, &OdometryROS::callbackIMU, this); - NODELET_INFO("odometry: Subscribing to IMU topic %s", imuSub_.getTopic().c_str()); + int queueSize = 50; + pnh.param("imu_queue_size", queueSize, queueSize); + imuSub_ = nh.subscribe("imu", queueSize, &OdometryROS::callbackIMU, this); + NODELET_INFO("odometry: Subscribing to IMU topic %s (imu_queue_size=%d)", + imuSub_.getTopic().c_str(), queueSize); } this->start(); @@ -460,22 +465,43 @@ void OdometryROS::callbackIMU(const sensor_msgs::ImuConstPtr& msg) void OdometryROS::processData(SensorData & data, const std_msgs::Header & header) { //NODELET_WARN("Received image: %f delay=%f", data.stamp(), (ros::Time::now() - header.stamp).toSec()); + double clockNow = ros::Time::now().toSec(); if(dataMutex_.lockTry() == 0) { if(bufferedDataToProcess_) { - NODELET_ERROR("We didn't receive IMU newer than previous image (%f) and we just received a new image (%f). The previous image is dropped!", + NODELET_ERROR("We didn't receive IMU newer than previous image/scan (%f) and we just received a new image/scan (%f). The previous image/scan is dropped! Make sure IMU is published faster and with less delay than the image/scan.", dataHeaderToProcess_.stamp.toSec(), header.stamp.toSec()); } dataToProcess_ = data; dataHeaderToProcess_ = header; bufferedDataToProcess_ = false; - dataReady_.release(); + if(alwaysProcessMostRecentFrame_) { + dataReady_.release(); + } dataMutex_.unlock(); + if(!alwaysProcessMostRecentFrame_) { + processData(); + } } else { - NODELET_DEBUG("Dropping image/scan data"); + double estimatedPeriod = clockNow - lastReceivedTopicClock_; + double topicPeriod = header.stamp.toSec() - lastReceivedTopicStamp_; + if(estimatedPeriod>0.0 && topicPeriod>0.0 && estimatedPeriod < topicPeriod*0.9) { + NODELET_WARN("Dropping image/scan data with stamp %f (delay=%f). Something is wrong " + "because the clock difference with the previous topic received (%fs) is much lower than the " + "expected one (%fs) estimated from the topic stamps (previous stamp=%f). If you are processing " + "a large bag with flaky replaying delay, consider setting parameter \"always_process_most_recent_frame:=false\" " + "to avoid aggressively dropping data.", + header.stamp.toSec(), + clockNow - header.stamp.toSec(), + estimatedPeriod, + topicPeriod, + lastReceivedTopicStamp_); + } } + lastReceivedTopicStamp_ = header.stamp.toSec(); + lastReceivedTopicClock_ = clockNow; } void OdometryROS::mainLoopKill() @@ -493,7 +519,11 @@ void OdometryROS::mainLoop() // thread killed return; } + processData(); +} +void OdometryROS::processData() +{ UScopeMutex lock(dataMutex_); // aliases @@ -512,21 +542,36 @@ void OdometryROS::mainLoop() if(waitIMUToinit_ && (imus_.empty() || imus_.rbegin()->first < header.stamp.toSec())) { - NODELET_WARN("Make sure IMU is published faster than data rate! (last image stamp=%f and last imu stamp received=%f). Buffering the image until an imu with same or greater stamp is received.", - data.stamp(), imus_.empty()?0:imus_.rbegin()->first); + if(imus_.empty()) { + // If empty, it is an error! + NODELET_ERROR("Make sure IMU is published faster than data rate! (last image/scan stamp=%f and imu buffer is empty). Buffering the image/scan until an imu with same or greater stamp is received.", + data.stamp()); + } bufferedDataToProcess_ = true; return; } // process all imu data up to current image stamp (or just after so that underlying odom approach can do interpolation of imu at image stamp) std::map::iterator iterEnd = imus_.lower_bound(header.stamp.toSec()); + std::map::iterator iterLast = iterEnd; if(iterEnd!= imus_.end()) { ++iterEnd; } - for(std::map::iterator iter=imus_.begin(); iter!=iterEnd;) + std::map::iterator iterFirst = imus_.begin(); + for(std::map::iterator iter=iterFirst; iter!=iterEnd;) { - imus.push_back(*iter); - imus_.erase(iter++); + // Because we always keep the last processed imu in the buffer, skip the first + // one when processing again the buffer unless its time is lower/equal to image + // current stamp (could happen on initialization). + if(iter!=iterFirst || iter->first <= header.stamp.toSec()) { + imus.push_back(*iter); + } + if(iter!=iterLast) { + imus_.erase(iter++); + } + else { + ++iter; + } } } @@ -1193,6 +1238,8 @@ void OdometryROS::reset(const Transform & pose) guessPreviousPose_.setNull(); previousStamp_ = ros::Time(); previousClockTime_ = ros::Time(); + lastReceivedTopicClock_ = 0.0; + lastReceivedTopicStamp_ = 0.0; resetCurrentCount_ = resetCountdown_; imuProcessed_ = false; dataToProcess_ = SensorData(); diff --git a/rtabmap_odom/src/nodelets/icp_odometry.cpp b/rtabmap_odom/src/nodelets/icp_odometry.cpp index 1c712e1c..ebe904bd 100644 --- a/rtabmap_odom/src/nodelets/icp_odometry.cpp +++ b/rtabmap_odom/src/nodelets/icp_odometry.cpp @@ -91,7 +91,19 @@ private: ros::NodeHandle & pnh = getPrivateNodeHandle(); int queueSize = 1; - pnh.param("queue_size", queueSize, queueSize); + pnh.param("topic_queue_size", queueSize, queueSize); + if(pnh.hasParam("queue_size") && !pnh.hasParam("topic_queue_size")) + { + pnh.param("queue_size", queueSize, queueSize); + ROS_WARN("Parameter \"queue_size\" has been renamed " + "to \"topic_queue_size\" and will be removed " + "in future versions! The value (%d) is still copied to " + "\"topic_queue_size\".", queueSize); + } + else + { + pnh.param("topic_queue_size", queueSize, queueSize); + } pnh.param("scan_cloud_max_points", scanCloudMaxPoints_, scanCloudMaxPoints_); pnh.param("scan_cloud_is_2d", scanCloudIs2d_, scanCloudIs2d_); pnh.param("scan_downsampling_step", scanDownsamplingStep_, scanDownsamplingStep_); From 3d827e002d2321ddab57d635151a4e1c48ead139 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sun, 14 Sep 2025 23:46:16 +0000 Subject: [PATCH 13/56] Fixing internal VINS-fusion not in par to external vins fusion (#911) --- rtabmap_examples/launch/euroc_datasets.launch | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/rtabmap_examples/launch/euroc_datasets.launch b/rtabmap_examples/launch/euroc_datasets.launch index 5cbdb787..4775f87d 100644 --- a/rtabmap_examples/launch/euroc_datasets.launch +++ b/rtabmap_examples/launch/euroc_datasets.launch @@ -108,13 +108,13 @@ Examples: + - @@ -128,6 +128,11 @@ Examples: + + + + + From f601fd43bf3435bc99dea8b273532631bc69d44d Mon Sep 17 00:00:00 2001 From: matlabbe Date: Mon, 15 Sep 2025 00:57:06 +0000 Subject: [PATCH 14/56] fixed some deprecated compiler warnings --- rtabmap_legacy/src/CameraNode.cpp | 8 ++++---- rtabmap_util/src/DbPlayerNode.cpp | 2 +- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/rtabmap_legacy/src/CameraNode.cpp b/rtabmap_legacy/src/CameraNode.cpp index 5f2f8233..036005d3 100644 --- a/rtabmap_legacy/src/CameraNode.cpp +++ b/rtabmap_legacy/src/CameraNode.cpp @@ -209,7 +209,7 @@ public: //usb device camera_ = new rtabmap::CameraVideo(deviceId, false, frameRate); } - cameraThread_ = new rtabmap::CameraThread(camera_); + cameraThread_ = new rtabmap::SensorCaptureThread(camera_); init(); if(!pause) { @@ -221,9 +221,9 @@ public: protected: virtual bool handleEvent(UEvent * event) { - if(event->getClassName().compare("CameraEvent") == 0) + if(event->getClassName().compare("SensorEvent") == 0) { - rtabmap::CameraEvent * e = (rtabmap::CameraEvent*)event; + rtabmap::SensorEvent * e = (rtabmap::SensorEvent*)event; const cv::Mat & image = e->data().imageRaw(); if(!image.empty() && image.depth() == CV_8U) { @@ -248,7 +248,7 @@ protected: private: image_transport::Publisher rosPublisher_; - rtabmap::CameraThread * cameraThread_; + rtabmap::SensorCaptureThread * cameraThread_; rtabmap::Camera * camera_; ros::ServiceServer startSrv_; ros::ServiceServer stopSrv_; diff --git a/rtabmap_util/src/DbPlayerNode.cpp b/rtabmap_util/src/DbPlayerNode.cpp index 80d9eb8a..28d30501 100644 --- a/rtabmap_util/src/DbPlayerNode.cpp +++ b/rtabmap_util/src/DbPlayerNode.cpp @@ -223,7 +223,7 @@ int main(int argc, char** argv) } UTimer timer; - rtabmap::CameraInfo cameraInfo; + rtabmap::SensorCaptureInfo cameraInfo; rtabmap::SensorData data = reader.takeImage(&cameraInfo); rtabmap::OdometryInfo odomInfo; odomInfo.reg.covariance = cameraInfo.odomCovariance; From f9ad71a7afda843011dae31cc80f82e0ecc034ce Mon Sep 17 00:00:00 2001 From: matlabbe Date: Thu, 18 Sep 2025 04:43:24 +0000 Subject: [PATCH 15/56] Updated README with examples of usage of the MIT stata center dataset based on jfr2018 paper. #1350 --- rtabmap_legacy/launch/jfr2018/README.md | 188 +++++++++++++++++- rtabmap_legacy/launch/jfr2018/extract_rgbd.py | 6 +- .../launch/jfr2018/extract_stereo.py | 26 +-- .../launch/jfr2018/throttle_bag.launch | 13 +- rtabmap_legacy/src/nodelets/data_throttle.cpp | 33 ++- .../src/nodelets/stereo_throttle.cpp | 32 ++- rtabmap_odom/src/OdometryROS.cpp | 2 +- rtabmap_odom/src/nodelets/rgbd_odometry.cpp | 47 ++--- .../src/nodelets/rgbdicp_odometry.cpp | 21 +- rtabmap_odom/src/nodelets/stereo_odometry.cpp | 47 ++--- rtabmap_sync/src/nodelets/rgbd_sync.cpp | 15 +- rtabmap_sync/src/nodelets/stereo_sync.cpp | 15 +- 12 files changed, 324 insertions(+), 121 deletions(-) diff --git a/rtabmap_legacy/launch/jfr2018/README.md b/rtabmap_legacy/launch/jfr2018/README.md index 2f2eec64..d1cd1f0c 100644 --- a/rtabmap_legacy/launch/jfr2018/README.md +++ b/rtabmap_legacy/launch/jfr2018/README.md @@ -1,7 +1,191 @@ -**TODO: currently committed here for backup, though a comprehensive step-by-step procedure would be required so that anyone could reproduce the results** This is the raw files used to generate results for MIT Stata Center dataset of this paper (for KITTI, EuRoC and TUM datasets, see this [page](https://github.com/introlab/rtabmap/tree/master/docker/jfr2018)): * M. Labbé and F. Michaud, “RTAB-Map as an Open-Source Lidar and Visual SLAM Library for Large-Scale and Long-Term Online Operation,” in Journal of Field Robotics, accepted, 2018. ([pdf](https://introlab.3it.usherbrooke.ca/mediawiki-introlab/images/7/7a/Labbe18JFR_preprint.pdf)) ([Wiley](https://doi.org/10.1002/rob.21831)) -Manual instructions are in [launch_lidar](https://github.com/introlab/rtabmap_ros/blob/master/rtabmap_legacy/launch/jfr2018/launch_lidar), [launch_stereo](https://github.com/introlab/rtabmap_ros/blob/master/rtabmap_legacy/launch/jfr2018/launch_stereo) and [launch_rgbd](https://github.com/introlab/rtabmap_ros/blob/master/rtabmap_legacy/launch/jfr2018/launch_rgbd) files depending on the sensor configuration. Refer also to explanations in the paper. +Manual instructions are in [launch_lidar](https://github.com/introlab/rtabmap_ros/blob/master/rtabmap_legacy/launch/jfr2018/launch_lidar), [launch_stereo](https://github.com/introlab/rtabmap_ros/blob/master/rtabmap_legacy/launch/jfr2018/launch_stereo) and [launch_rgbd](https://github.com/introlab/rtabmap_ros/blob/master/rtabmap_legacy/launch/jfr2018/launch_rgbd) files depending on the sensor configuration. Refer also to explanations in the paper. Below are some examples of usage per sensor type. + +# Long-Range LiDAR with WheelIMU→S2M odometry + +1. Launch rtabmap following config "Long-Range LiDAR with WheelIMU→S2M odometry" + ``` + roslaunch rtabmap_launch rtabmap.launch \ + args:="\ + -d \ + --Rtabmap/PublishRAMUsage true \ + --Rtabmap/StartNewMapOnLoopClosure true \ + --Reg/Force3DoF false \ + --RGBD/ProximityPathMaxNeighbors 0 \ + --Mem/STMSize 15 \ + --Mem/BinDataKept true \ + --Kp/FlannRebalancingFactor 1.0 \ + --RGBD/LinearUpdate 0 \ + --RGBD/ProximityBySpace true \ + --RGBD/OptimizeMaxError 3 \ + --FAST/Threshold 7" \ + odom_args:="\ + --Odom/Strategy 0 \ + --Vis/CorType 0 \ + --Odom/KeyFrameThr 0.3 \ + --OdomF2M/MaxSize 2000 \ + --uwarn" \ + rgbd_sync:=true \ + depth_scale:=1.043 \ + frame_id:=base_footprint \ + ground_truth_frame_id:=world \ + ground_truth_base_frame_id:=scan_gt \ + use_sim_time:=true \ + odom_topic:=odom \ + rgb_topic:=/camera/rgb/image_raw \ + depth_topic:=/camera/depth/image_raw \ + camera_info_topic:=/camera/rgb/camera_info \ + approx_sync:=false \ + odom_guess_frame_id:=odom_combined \ + odom_always_process_most_recent_frame:=false + ``` + +2. Publish the ground truth in TF + ``` + python3 gt_tf_broadcaster.py \ + _file:=2012-01-25-12-33-29_part1_floor2.gt.laser.poses \ + _frame_id:=scan_gt \ + _fixed_frame_id:=world \ + _offset_time:=82.2 \ + _offset_x:=-0.275 + ``` + +3. Launch the rosbag. Make sure you are running the rosbag on a SSD directly connected inside the computer (not USB SSD) to limit the lags. Note that in contrast to visual odometry approaches below, the lags seem affecting less lidar odometry, so we are still using the original rosbag here. + ``` + rosbag play --clock --pause 2012-01-25-12-33-29.bag + ``` + +# RGB-D Camera with F2M odometry +1. To avoid lags when replaying the original rosbag, just extract the RGB-D data into another rosbag. With the script in this folder, do: + ``` + python3 extract_rgbd.py 2012-01-25-12-33-29 + ``` + This will create a new rosbag called `2012-01-25-12-33-29_rgbd.bag`. + +2. Launch rtabmap following config "RGB-D Camera with F2M odometry" + ``` + roslaunch rtabmap_launch rtabmap.launch \ + args:="-d \ + --Rtabmap/PublishRAMUsage true \ + --Rtabmap/StartNewMapOnLoopClosure true \ + --Reg/Force3DoF false \ + --RGBD/ProximityPathMaxNeighbors 0 \ + --Mem/STMSize 15 \ + --Mem/BinDataKept true \ + --Kp/FlannRebalancingFactor 1.0 \ + --RGBD/LinearUpdate 0 \ + --RGBD/ProximityBySpace true \ + --RGBD/OptimizeMaxError 3 \ + --GFTT/QualityLevel 0.01 \ + --Vis/MinInliers 10" \ + odom_args:="\ + --Odom/Strategy 0 \ + --Vis/CorType 0 \ + --Odom/KeyFrameThr 0.3 \ + --OdomF2M/MaxSize 2000 \ + --uwarn" \ + rgbd_sync:=true \ + depth_scale:=1.043 \ + frame_id:=base_footprint \ + ground_truth_frame_id:=world \ + ground_truth_base_frame_id:=scan_gt \ + use_sim_time:=true \ + odom_topic:=odom \ + rgb_topic:=/camera/rgb/image_raw \ + depth_topic:=/camera/depth/image_raw \ + camera_info_topic:=/camera/rgb/camera_info \ + approx_sync:=false \ + approx_sync_max_interval:=0.015 \ + odom_guess_frame_id:=odom_combined \ + odom_always_process_most_recent_frame:=false + ``` +3. Publish the ground truth in TF + ``` + python3 gt_tf_broadcaster.py \ + _file:=2012-01-25-12-33-29_part1_floor2.gt.laser.poses \ + _frame_id:=scan_gt \ + _fixed_frame_id:=world \ + _offset_time:=82.2 \ + _offset_x:=-0.275 + ``` + +4. Launch the rosbag. Make sure you are running the rosbag on a SSD directly connected inside the computer (not USB SSD) to limit the lags. + ``` + rosbag play --clock --pause 2012-01-25-12-33-29_rgbd.bag + ``` + +# Stereo Camera with F2M odometry +1. To avoid lags when replaying the original rosbag, just extract the stereo data into another rosbag. With the script in this folder, do: + ``` + python3 extract_stereo.py 2012-01-25-12-33-29 + ``` + This will create a new rosbag called `2012-01-25-12-33-29_stereo.bag`. + +2. Launch rtabmap following config "Stereo Camera with F2M odometry" + ``` + roslaunch rtabmap_launch rtabmap.launch \ + args:="-d \ + --Rtabmap/PublishRAMUsage true \ + --Rtabmap/StartNewMapOnLoopClosure true \ + --Reg/Force3DoF false \ + --RGBD/ProximityPathMaxNeighbors 0 \ + --Mem/STMSize 15 \ + --Mem/BinDataKept true \ + --Kp/FlannRebalancingFactor 1.0 \ + --RGBD/LinearUpdate 0 \ + --RGBD/ProximityBySpace true \ + --Odom/KeyFrameThr 0.3 \ + --Odom/Strategy 0 \ + --OdomF2M/MaxSize 2000 \ + --RGBD/OptimizeMaxError 3 \ + --GFTT/QualityLevel 0.01 \ + --Vis/MinInliers 10" \ + odom_args:="--Vis/CorType 0 --uwarn" \ + frame_id:=base_footprint \ + ground_truth_frame_id:=world \ + ground_truth_base_frame_id:=scan_gt \ + use_sim_time:=true \ + stereo:=true \ + stereo_namespace:=/wide_stereo \ + left_camera_info_topic:=/wide_stereo/left/camera_info \ + right_camera_info_topic:=/wide_stereo/right/camera_info_scaled \ + odom_topic:=odom \ + approx_sync:=false \ + odom_guess_frame_id:=odom_combined \ + odom_always_process_most_recent_frame:=false + ``` +3. Publish the ground truth in TF + ``` + python3 gt_tf_broadcaster.py \ + _file:=2012-01-25-12-33-29_part1_floor2.gt.laser.poses \ + _frame_id:=scan_gt \ + _fixed_frame_id:=world \ + _offset_time:=82.2 \ + _offset_x:=-0.275 + ``` + +4. Republish the stereo camera calibration with baseline correctly scaled + ``` + python3 republish_camera_info.py \ + camera_info_in:=/wide_stereo/right/camera_info \ + camera_info_out:=/wide_stereo/right/camera_info_scaled + ``` + +5. Rectify the raw stereo images + ``` + export ROS_NAMESPACE=wide_stereo + rosrun stereo_image_proc stereo_image_proc \ + left/image_raw:=left/image_raw \ + right/image_raw:=right/image_raw \ + left/camera_info:=left/camera_info \ + right/camera_info:=right/camera_info_scaled + ``` + +6. Launch the rosbag. Make sure you are running the rosbag on a SSD directly connected inside the computer (not USB SSD) to limit the lags. + ``` + rosbag play --clock --pause 2012-01-25-12-33-29_stereo.bag + ``` diff --git a/rtabmap_legacy/launch/jfr2018/extract_rgbd.py b/rtabmap_legacy/launch/jfr2018/extract_rgbd.py index e204ea59..e35511fb 100755 --- a/rtabmap_legacy/launch/jfr2018/extract_rgbd.py +++ b/rtabmap_legacy/launch/jfr2018/extract_rgbd.py @@ -4,13 +4,13 @@ import sys from tf.msg import tfMessage if len(sys.argv) < 2: - print 'Usage: $ python extract_rgbd.py "2012-01-25-12-14-25"' + print('Usage: $ python extract_rgbd.py "2012-01-25-12-14-25"') sys.exit(0) bagName = sys.argv[1] with rosbag.Bag(bagName + '_rgbd.bag', 'w') as outbag: - print 'Processing ' + bagName + '.bag...' + print(f'Processing {bagName}.bag...') for topic, msg, t in rosbag.Bag(bagName + '.bag').read_messages(): if topic == "/tf" or topic == "/camera/depth/image_raw" or topic == "/camera/rgb/camera_info" or topic == "/camera/rgb/image_raw": outbag.write(topic, msg, t) -print 'Output: ' + bagName + '_out.bag' +print(f'Output: {bagName}_rgbd.bag') diff --git a/rtabmap_legacy/launch/jfr2018/extract_stereo.py b/rtabmap_legacy/launch/jfr2018/extract_stereo.py index 78f24b06..565ad26b 100755 --- a/rtabmap_legacy/launch/jfr2018/extract_stereo.py +++ b/rtabmap_legacy/launch/jfr2018/extract_stereo.py @@ -4,34 +4,22 @@ import sys from tf.msg import tfMessage if len(sys.argv) < 2: - print 'Usage: $ python extract_stereo.py "2012-01-25-12-14-25"' + print('Usage: $ python extract_stereo.py "2012-01-25-12-14-25"') sys.exit(0) bagName = sys.argv[1] with rosbag.Bag(bagName + '_stereo.bag', 'w') as outbag: - print 'Processing ' + bagName + '.bag...' - leftCamInfoStatus = True - leftImageStatus = True - rightCamInfoStatus = True - rightImageStatus = True + print(f'Processing {bagName}.bag...') for topic, msg, t in rosbag.Bag(bagName + '.bag').read_messages(): if topic == "/tf": outbag.write(topic, msg, t) elif topic == "/wide_stereo/left/camera_info": - if leftCamInfoStatus: - outbag.write(topic, msg, t) - leftCamInfoStatus = not leftCamInfoStatus + outbag.write(topic, msg, t) elif topic == "/wide_stereo/right/camera_info": - if rightCamInfoStatus: - outbag.write(topic, msg, t) - rightCamInfoStatus = not rightCamInfoStatus + outbag.write(topic, msg, t) elif topic == "/wide_stereo/left/image_raw": - if leftImageStatus: - outbag.write(topic, msg, t) - leftImageStatus = not leftImageStatus + outbag.write(topic, msg, t) elif topic == "/wide_stereo/right/image_raw": - if rightImageStatus: - outbag.write(topic, msg, t) - rightImageStatus = not rightImageStatus + outbag.write(topic, msg, t) -print 'Output: ' + bagName + '_out.bag' +print(f'Output: {bagName}_stereo.bag') diff --git a/rtabmap_legacy/launch/jfr2018/throttle_bag.launch b/rtabmap_legacy/launch/jfr2018/throttle_bag.launch index b1ab7f79..5a89eff4 100755 --- a/rtabmap_legacy/launch/jfr2018/throttle_bag.launch +++ b/rtabmap_legacy/launch/jfr2018/throttle_bag.launch @@ -3,21 +3,20 @@ - + - + + - - - + @@ -25,6 +24,10 @@ + + + + diff --git a/rtabmap_legacy/src/nodelets/data_throttle.cpp b/rtabmap_legacy/src/nodelets/data_throttle.cpp index aa4d8971..74ed30e7 100644 --- a/rtabmap_legacy/src/nodelets/data_throttle.cpp +++ b/rtabmap_legacy/src/nodelets/data_throttle.cpp @@ -51,7 +51,7 @@ class DataThrottleNodelet : public nodelet::Nodelet public: //Constructor DataThrottleNodelet(): - rate_(0), + period_(0), approxSync_(0), exactSync_(0), decimation_(1) @@ -72,7 +72,7 @@ public: private: ros::Time last_update_; - double rate_; + double period_; virtual void onInit() { ros::NodeHandle& nh = getNodeHandle(); @@ -90,22 +90,33 @@ private: int queueSize = 10; bool approxSync = true; double approxSyncMaxInterval = 0.0; - if(private_nh.getParam("max_rate", rate_)) + double rate = 0.0; + double expectedInputRate = 0.0; + if(private_nh.getParam("max_rate", rate)) { NODELET_WARN("\"max_rate\" is now known as \"rate\"."); } - private_nh.param("rate", rate_, rate_); + private_nh.param("rate", rate, rate); + private_nh.param("expected_input_rate", expectedInputRate, expectedInputRate); private_nh.param("queue_size", queueSize, queueSize); private_nh.param("approx_sync", approxSync, approxSync); private_nh.param("approx_sync_max_interval", approxSyncMaxInterval, approxSyncMaxInterval); private_nh.param("decimation", decimation_, decimation_); ROS_ASSERT(decimation_ >= 1); - NODELET_INFO("rate=%f Hz", rate_); + NODELET_INFO("rate=%f Hz", rate); + NODELET_INFO("expected_input_rate=%f Hz", expectedInputRate); NODELET_INFO("decimation=%d", decimation_); NODELET_INFO("approx_sync = %s", approxSync?"true":"false"); if(approxSync) NODELET_INFO("approx_sync_max_interval = %f", approxSyncMaxInterval); + if(rate>0){ + period_ = 1.0/rate; + } + if(expectedInputRate > 0 && expectedInputRate >= rate) { + period_ -= (1.0/expectedInputRate)/2.0; + } + if(approxSync) { approxSync_ = new message_filters::Synchronizer(MyApproxSyncPolicy(queueSize), image_sub_, image_depth_sub_, info_sub_); @@ -132,19 +143,19 @@ private: const sensor_msgs::ImageConstPtr& imageDepth, const sensor_msgs::CameraInfoConstPtr& camInfo) { - if (rate_ > 0.0) + if (period_ > 0.0) { - NODELET_DEBUG("update set to %f", rate_); - if ( last_update_ + ros::Duration(1.0/rate_) > ros::Time::now()) + if (last_update_ != ros::Time() && (image->header.stamp - last_update_).toSec() < period_) { - NODELET_DEBUG("throttle last update at %f skipping", last_update_.toSec()); + NODELET_DEBUG("throttle last update at %f skipping (ref=%f)", image->header.stamp.toSec(), last_update_.toSec()); return; } } - else + else { NODELET_DEBUG("rate unset continuing"); + } - last_update_ = ros::Time::now(); + last_update_ = image->header.stamp; double rgbStamp = image->header.stamp.toSec(); double depthStamp = imageDepth->header.stamp.toSec(); diff --git a/rtabmap_legacy/src/nodelets/stereo_throttle.cpp b/rtabmap_legacy/src/nodelets/stereo_throttle.cpp index ca7c95f7..96911019 100644 --- a/rtabmap_legacy/src/nodelets/stereo_throttle.cpp +++ b/rtabmap_legacy/src/nodelets/stereo_throttle.cpp @@ -50,7 +50,7 @@ class StereoThrottleNodelet : public nodelet::Nodelet public: //Constructor StereoThrottleNodelet(): - rate_(0), + period_(0), approxSync_(0), exactSync_(0), decimation_(1) @@ -71,7 +71,7 @@ public: private: ros::Time last_update_; - double rate_; + double period_; virtual void onInit() { ros::NodeHandle& nh = getNodeHandle(); @@ -89,18 +89,29 @@ private: int queueSize = 5; bool approxSync = false; double approxSyncMaxInterval = 0.0; + double rate = 0.0; + double expectedInputRate = 0.0; pnh.param("approx_sync", approxSync, approxSync); pnh.param("approx_sync_max_interval", approxSyncMaxInterval, approxSyncMaxInterval); - pnh.param("rate", rate_, rate_); + pnh.param("rate", rate, rate); + pnh.param("expected_input_rate", expectedInputRate, expectedInputRate); pnh.param("queue_size", queueSize, queueSize); pnh.param("decimation", decimation_, decimation_); ROS_ASSERT(decimation_ >= 1); - NODELET_INFO("rate=%f Hz", rate_); + NODELET_INFO("rate=%f Hz", rate); + NODELET_INFO("expected_input_rate=%f Hz", expectedInputRate); NODELET_INFO("decimation=%d", decimation_); NODELET_INFO("approx_sync = %s", approxSync?"true":"false"); if(approxSync) NODELET_INFO("approx_sync_max_interval = %f", approxSyncMaxInterval); + if(rate>0){ + period_ = 1.0/rate; + } + if(expectedInputRate > 0 && expectedInputRate >= rate) { + period_ -= (1.0/expectedInputRate)/2.0; + } + if(approxSync) { approxSync_ = new message_filters::Synchronizer(MyApproxSyncPolicy(queueSize), imageLeft_, imageRight_, cameraInfoLeft_, cameraInfoRight_); @@ -130,20 +141,21 @@ private: const sensor_msgs::CameraInfoConstPtr& camInfoLeft, const sensor_msgs::CameraInfoConstPtr& camInfoRight) { - if (rate_ > 0.0) + if (period_ > 0.0) { - NODELET_DEBUG("update set to %f", rate_); - if ( last_update_ + ros::Duration(1.0/rate_) > ros::Time::now()) + if (last_update_ != ros::Time() && (imageLeft->header.stamp - last_update_).toSec() < period_) { - NODELET_DEBUG("throttle last update at %f skipping", last_update_.toSec()); + NODELET_DEBUG("throttle last update at %f skipping (ref=%f)", imageLeft->header.stamp.toSec(), last_update_.toSec()); return; } } - else + else { NODELET_DEBUG("rate unset continuing"); + } - last_update_ = ros::Time::now(); + last_update_ = imageLeft->header.stamp; + double leftStamp = imageLeft->header.stamp.toSec(); double rightStamp = imageRight->header.stamp.toSec(); double leftInfoStamp = camInfoLeft->header.stamp.toSec(); diff --git a/rtabmap_odom/src/OdometryROS.cpp b/rtabmap_odom/src/OdometryROS.cpp index beff2a20..40b2a5e9 100644 --- a/rtabmap_odom/src/OdometryROS.cpp +++ b/rtabmap_odom/src/OdometryROS.cpp @@ -487,7 +487,7 @@ void OdometryROS::processData(SensorData & data, const std_msgs::Header & header { double estimatedPeriod = clockNow - lastReceivedTopicClock_; double topicPeriod = header.stamp.toSec() - lastReceivedTopicStamp_; - if(estimatedPeriod>0.0 && topicPeriod>0.0 && estimatedPeriod < topicPeriod*0.9) { + if(estimatedPeriod>0.0 && topicPeriod>0.0 && estimatedPeriod < topicPeriod*0.5) { NODELET_WARN("Dropping image/scan data with stamp %f (delay=%f). Something is wrong " "because the clock difference with the previous topic received (%fs) is much lower than the " "expected one (%fs) estimated from the topic stamps (previous stamp=%f). If you are processing " diff --git a/rtabmap_odom/src/nodelets/rgbd_odometry.cpp b/rtabmap_odom/src/nodelets/rgbd_odometry.cpp index bda1e1a9..7ba7936f 100644 --- a/rtabmap_odom/src/nodelets/rgbd_odometry.cpp +++ b/rtabmap_odom/src/nodelets/rgbd_odometry.cpp @@ -76,7 +76,8 @@ public: exactSync6_(0), topicQueueSize_(1), syncQueueSize_(5), - keepColor_(false) + keepColor_(false), + approxSyncMaxInterval_(0.0) { } @@ -108,9 +109,8 @@ private: int rgbdCameras = 1; bool approxSync = true; bool subscribeRGBD = false; - double approxSyncMaxInterval = 0.0; pnh.param("approx_sync", approxSync, approxSync); - pnh.param("approx_sync_max_interval", approxSyncMaxInterval, approxSyncMaxInterval); + pnh.param("approx_sync_max_interval", approxSyncMaxInterval_, approxSyncMaxInterval_); pnh.param("topic_queue_size", topicQueueSize_, topicQueueSize_); if(pnh.hasParam("queue_size") && !pnh.hasParam("sync_queue_size")) { @@ -138,7 +138,7 @@ private: NODELET_INFO("RGBDOdometry: approx_sync = %s", approxSync?"true":"false"); if(approxSync) - NODELET_INFO("RGBDOdometry: approx_sync_max_interval = %f", approxSyncMaxInterval); + NODELET_INFO("RGBDOdometry: approx_sync_max_interval = %f", approxSyncMaxInterval_); NODELET_INFO("RGBDOdometry: topic_queue_size = %d", topicQueueSize_); NODELET_INFO("RGBDOdometry: sync_queue_size = %d", syncQueueSize_); NODELET_INFO("RGBDOdometry: subscribe_rgbd = %s", subscribeRGBD?"true":"false"); @@ -178,8 +178,8 @@ private: MyApproxSync2Policy(syncQueueSize_), rgbd_image1_sub_, rgbd_image2_sub_); - if(approxSyncMaxInterval > 0.0) - approxSync2_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval)); + if(approxSyncMaxInterval_ > 0.0) + approxSync2_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval_)); approxSync2_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD2, this, boost::placeholders::_1, boost::placeholders::_2)); } else @@ -193,7 +193,7 @@ private: subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s \\\n %s", getName().c_str(), approxSync?"approx":"exact", - approxSync&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"", + approxSync&&approxSyncMaxInterval_!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval_).c_str():"", rgbd_image1_sub_.getTopic().c_str(), rgbd_image2_sub_.getTopic().c_str()); } @@ -206,8 +206,8 @@ private: rgbd_image1_sub_, rgbd_image2_sub_, rgbd_image3_sub_); - if(approxSyncMaxInterval > 0.0) - approxSync3_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval)); + if(approxSyncMaxInterval_ > 0.0) + approxSync3_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval_)); approxSync3_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD3, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3)); } else @@ -222,7 +222,7 @@ private: subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s \\\n %s \\\n %s", getName().c_str(), approxSync?"approx":"exact", - approxSync&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"", + approxSync&&approxSyncMaxInterval_!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval_).c_str():"", rgbd_image1_sub_.getTopic().c_str(), rgbd_image2_sub_.getTopic().c_str(), rgbd_image3_sub_.getTopic().c_str()); @@ -237,8 +237,8 @@ private: rgbd_image2_sub_, rgbd_image3_sub_, rgbd_image4_sub_); - if(approxSyncMaxInterval > 0.0) - approxSync4_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval)); + if(approxSyncMaxInterval_ > 0.0) + approxSync4_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval_)); approxSync4_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD4, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4)); } else @@ -254,7 +254,7 @@ private: subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s \\\n %s \\\n %s \\\n %s", getName().c_str(), approxSync?"approx":"exact", - approxSync&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"", + approxSync&&approxSyncMaxInterval_!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval_).c_str():"", rgbd_image1_sub_.getTopic().c_str(), rgbd_image2_sub_.getTopic().c_str(), rgbd_image3_sub_.getTopic().c_str(), @@ -271,8 +271,8 @@ private: rgbd_image3_sub_, rgbd_image4_sub_, rgbd_image5_sub_); - if(approxSyncMaxInterval > 0.0) - approxSync5_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval)); + if(approxSyncMaxInterval_ > 0.0) + approxSync5_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval_)); approxSync5_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD5, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4, boost::placeholders::_5)); } else @@ -289,7 +289,7 @@ private: subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s \\\n %s \\\n %s \\\n %s \\\n %s", getName().c_str(), approxSync?"approx":"exact", - approxSync&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"", + approxSync&&approxSyncMaxInterval_!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval_).c_str():"", rgbd_image1_sub_.getTopic().c_str(), rgbd_image2_sub_.getTopic().c_str(), rgbd_image3_sub_.getTopic().c_str(), @@ -308,8 +308,8 @@ private: rgbd_image4_sub_, rgbd_image5_sub_, rgbd_image6_sub_); - if(approxSyncMaxInterval > 0.0) - approxSync6_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval)); + if(approxSyncMaxInterval_ > 0.0) + approxSync6_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval_)); approxSync6_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD6, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4, boost::placeholders::_5, boost::placeholders::_6)); } else @@ -327,7 +327,7 @@ private: subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s \\\n %s \\\n %s \\\n %s \\\n %s \\\n %s", getName().c_str(), approxSync?"approx":"exact", - approxSync&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"", + approxSync&&approxSyncMaxInterval_!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval_).c_str():"", rgbd_image1_sub_.getTopic().c_str(), rgbd_image2_sub_.getTopic().c_str(), rgbd_image3_sub_.getTopic().c_str(), @@ -382,8 +382,8 @@ private: if(approxSync) { approxSync_ = new message_filters::Synchronizer(MyApproxSyncPolicy(syncQueueSize_), image_mono_sub_, image_depth_sub_, info_sub_); - if(approxSyncMaxInterval > 0.0) - approxSync_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval)); + if(approxSyncMaxInterval_ > 0.0) + approxSync_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval_)); approxSync_->registerCallback(boost::bind(&RGBDOdometry::callback, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3)); } else @@ -396,7 +396,7 @@ private: subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s \\\n %s \\\n %s", getName().c_str(), approxSync?"approx":"exact", - approxSync&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"", + approxSync&&approxSyncMaxInterval_!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval_).c_str():"", image_mono_sub_.getTopic().c_str(), image_depth_sub_.getTopic().c_str(), info_sub_.getTopic().c_str()); @@ -593,7 +593,7 @@ private: infoMsgs.push_back(*cameraInfo); double stampDiff = fabs(image->header.stamp.toSec() - depth->header.stamp.toSec()); - if(stampDiff > 0.020) + if(approxSyncMaxInterval_==0.0 && stampDiff > 0.020) { NODELET_WARN("The time difference between rgb and depth frames is " "high (diff=%fs, rgb=%fs, depth=%fs). You may want " @@ -934,6 +934,7 @@ private: int topicQueueSize_; int syncQueueSize_; bool keepColor_; + double approxSyncMaxInterval_; }; PLUGINLIB_EXPORT_CLASS(rtabmap_odom::RGBDOdometry, nodelet::Nodelet); diff --git a/rtabmap_odom/src/nodelets/rgbdicp_odometry.cpp b/rtabmap_odom/src/nodelets/rgbdicp_odometry.cpp index 99b5ddb1..647e809a 100644 --- a/rtabmap_odom/src/nodelets/rgbdicp_odometry.cpp +++ b/rtabmap_odom/src/nodelets/rgbdicp_odometry.cpp @@ -75,6 +75,7 @@ public: queueSize_(1), syncQueueSize_(5), keepColor_(false), + approxSyncMaxInterval_(0.0), scanCloudMaxPoints_(0), scanVoxelSize_(0.0), scanNormalK_(0), @@ -111,9 +112,8 @@ private: bool approxSync = true; bool subscribeScanCloud = false; - double approxSyncMaxInterval = 0.0; pnh.param("approx_sync", approxSync, approxSync); - pnh.param("approx_sync_max_interval", approxSyncMaxInterval, approxSyncMaxInterval); + pnh.param("approx_sync_max_interval", approxSyncMaxInterval_, approxSyncMaxInterval_); pnh.param("topic_queue_size", queueSize_, queueSize_); if(pnh.hasParam("queue_size") && !pnh.hasParam("sync_queue_size")) { @@ -142,7 +142,7 @@ private: NODELET_INFO("RGBDIcpOdometry: approx_sync = %s", approxSync?"true":"false"); if(approxSync) - NODELET_INFO("RGBDIcpOdometry: approx_sync_max_interval = %f", approxSyncMaxInterval); + NODELET_INFO("RGBDIcpOdometry: approx_sync_max_interval = %f", approxSyncMaxInterval_); NODELET_INFO("RGBDIcpOdometry: topic_queue_size = %d", queueSize_); NODELET_INFO("RGBDIcpOdometry: sync_queue_size = %d", syncQueueSize_); NODELET_INFO("RGBDIcpOdometry: subscribe_scan_cloud = %s", subscribeScanCloud?"true":"false"); @@ -172,8 +172,8 @@ private: if(approxSync) { approxCloudSync_ = new message_filters::Synchronizer(MyApproxCloudSyncPolicy(syncQueueSize_), image_mono_sub_, image_depth_sub_, info_sub_, cloud_sub_); - if(approxSyncMaxInterval > 0.0) - approxCloudSync_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval)); + if(approxSyncMaxInterval_ > 0.0) + approxCloudSync_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval_)); approxCloudSync_->registerCallback(boost::bind(&RGBDICPOdometry::callbackCloud, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4)); } else @@ -185,7 +185,7 @@ private: subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s,\n %s,\n %s, \n %s", getName().c_str(), approxSync?"approx":"exact", - approxSync&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"", + approxSync&&approxSyncMaxInterval_!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval_).c_str():"", image_mono_sub_.getTopic().c_str(), image_depth_sub_.getTopic().c_str(), info_sub_.getTopic().c_str(), @@ -197,8 +197,8 @@ private: if(approxSync) { approxScanSync_ = new message_filters::Synchronizer(MyApproxScanSyncPolicy(syncQueueSize_), image_mono_sub_, image_depth_sub_, info_sub_, scan_sub_); - if(approxSyncMaxInterval > 0.0) - approxScanSync_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval)); + if(approxSyncMaxInterval_ > 0.0) + approxScanSync_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval_)); approxScanSync_->registerCallback(boost::bind(&RGBDICPOdometry::callbackScan, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4)); } else @@ -210,7 +210,7 @@ private: subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s \\\n %s \\\n %s \\\n %s", getName().c_str(), approxSync?"approx":"exact", - approxSync&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"", + approxSync&&approxSyncMaxInterval_!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval_).c_str():"", image_mono_sub_.getTopic().c_str(), image_depth_sub_.getTopic().c_str(), info_sub_.getTopic().c_str(), @@ -301,7 +301,7 @@ private: } double stampDiff = fabs(image->header.stamp.toSec() - depth->header.stamp.toSec()); - if(stampDiff > 0.010) + if(approxSyncMaxInterval_==0.0 && stampDiff > 0.010) { NODELET_WARN("The time difference between rgb and depth frames is " "high (diff=%fs, rgb=%fs, depth=%fs). You may want " @@ -518,6 +518,7 @@ private: double scanVoxelSize_; int scanNormalK_; double scanNormalRadius_; + double approxSyncMaxInterval_; }; PLUGINLIB_EXPORT_CLASS(rtabmap_odom::RGBDICPOdometry, nodelet::Nodelet); diff --git a/rtabmap_odom/src/nodelets/stereo_odometry.cpp b/rtabmap_odom/src/nodelets/stereo_odometry.cpp index e698c38d..e871e84c 100644 --- a/rtabmap_odom/src/nodelets/stereo_odometry.cpp +++ b/rtabmap_odom/src/nodelets/stereo_odometry.cpp @@ -76,7 +76,8 @@ public: exactSync6_(0), topicQueueSize_(1), syncQueueSize_(5), - keepColor_(false) + keepColor_(false), + approxSyncMaxInterval_(0.0) { } @@ -106,10 +107,9 @@ private: bool approxSync = false; bool subscribeRGBD = false; - double approxSyncMaxInterval = 0.0; int rgbdCameras = 1; pnh.param("approx_sync", approxSync, approxSync); - pnh.param("approx_sync_max_interval", approxSyncMaxInterval, approxSyncMaxInterval); + pnh.param("approx_sync_max_interval", approxSyncMaxInterval_, approxSyncMaxInterval_); pnh.param("topic_queue_size", topicQueueSize_, topicQueueSize_); if(pnh.hasParam("queue_size") && !pnh.hasParam("sync_queue_size")) { @@ -129,7 +129,7 @@ private: NODELET_INFO("StereoOdometry: approx_sync = %s", approxSync?"true":"false"); if(approxSync) - NODELET_INFO("StereoOdometry: approx_sync_max_interval = %f", approxSyncMaxInterval); + NODELET_INFO("StereoOdometry: approx_sync_max_interval = %f", approxSyncMaxInterval_); NODELET_INFO("StereoOdometry: topic_queue_size = %d", topicQueueSize_); NODELET_INFO("StereoOdometry: sync_queue_size = %d", syncQueueSize_); NODELET_INFO("StereoOdometry: subscribe_rgbd = %s", subscribeRGBD?"true":"false"); @@ -168,8 +168,8 @@ private: MyApproxSync2Policy(syncQueueSize_), rgbd_image1_sub_, rgbd_image2_sub_); - if(approxSyncMaxInterval > 0.0) - approxSync2_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval)); + if(approxSyncMaxInterval_ > 0.0) + approxSync2_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval_)); approxSync2_->registerCallback(boost::bind(&StereoOdometry::callbackRGBD2, this, boost::placeholders::_1, boost::placeholders::_2)); } else @@ -183,7 +183,7 @@ private: subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s \\\n %s", getName().c_str(), approxSync?"approx":"exact", - approxSync&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"", + approxSync&&approxSyncMaxInterval_!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval_).c_str():"", rgbd_image1_sub_.getTopic().c_str(), rgbd_image2_sub_.getTopic().c_str()); } @@ -196,8 +196,8 @@ private: rgbd_image1_sub_, rgbd_image2_sub_, rgbd_image3_sub_); - if(approxSyncMaxInterval > 0.0) - approxSync3_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval)); + if(approxSyncMaxInterval_ > 0.0) + approxSync3_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval_)); approxSync3_->registerCallback(boost::bind(&StereoOdometry::callbackRGBD3, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3)); } else @@ -212,7 +212,7 @@ private: subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s \\\n %s \\\n %s", getName().c_str(), approxSync?"approx":"exact", - approxSync&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"", + approxSync&&approxSyncMaxInterval_!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval_).c_str():"", rgbd_image1_sub_.getTopic().c_str(), rgbd_image2_sub_.getTopic().c_str(), rgbd_image3_sub_.getTopic().c_str()); @@ -227,8 +227,8 @@ private: rgbd_image2_sub_, rgbd_image3_sub_, rgbd_image4_sub_); - if(approxSyncMaxInterval > 0.0) - approxSync4_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval)); + if(approxSyncMaxInterval_ > 0.0) + approxSync4_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval_)); approxSync4_->registerCallback(boost::bind(&StereoOdometry::callbackRGBD4, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4)); } else @@ -244,7 +244,7 @@ private: subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s \\\n %s \\\n %s \\\n %s", getName().c_str(), approxSync?"approx":"exact", - approxSync&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"", + approxSync&&approxSyncMaxInterval_!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval_).c_str():"", rgbd_image1_sub_.getTopic().c_str(), rgbd_image2_sub_.getTopic().c_str(), rgbd_image3_sub_.getTopic().c_str(), @@ -261,8 +261,8 @@ private: rgbd_image3_sub_, rgbd_image4_sub_, rgbd_image5_sub_); - if(approxSyncMaxInterval > 0.0) - approxSync5_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval)); + if(approxSyncMaxInterval_ > 0.0) + approxSync5_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval_)); approxSync5_->registerCallback(boost::bind(&StereoOdometry::callbackRGBD5, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4, boost::placeholders::_5)); } else @@ -279,7 +279,7 @@ private: subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s \\\n %s \\\n %s \\\n %s \\\n %s", getName().c_str(), approxSync?"approx":"exact", - approxSync&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"", + approxSync&&approxSyncMaxInterval_!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval_).c_str():"", rgbd_image1_sub_.getTopic().c_str(), rgbd_image2_sub_.getTopic().c_str(), rgbd_image3_sub_.getTopic().c_str(), @@ -298,8 +298,8 @@ private: rgbd_image4_sub_, rgbd_image5_sub_, rgbd_image6_sub_); - if(approxSyncMaxInterval > 0.0) - approxSync6_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval)); + if(approxSyncMaxInterval_ > 0.0) + approxSync6_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval_)); approxSync6_->registerCallback(boost::bind(&StereoOdometry::callbackRGBD6, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4, boost::placeholders::_5, boost::placeholders::_6)); } else @@ -317,7 +317,7 @@ private: subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s \\\n %s \\\n %s \\\n %s \\\n %s \\\n %s", getName().c_str(), approxSync?"approx":"exact", - approxSync&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"", + approxSync&&approxSyncMaxInterval_!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval_).c_str():"", rgbd_image1_sub_.getTopic().c_str(), rgbd_image2_sub_.getTopic().c_str(), rgbd_image3_sub_.getTopic().c_str(), @@ -373,8 +373,8 @@ private: if(approxSync) { approxSync_ = new message_filters::Synchronizer(MyApproxSyncPolicy(syncQueueSize_), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_); - if(approxSyncMaxInterval>0.0) - approxSync_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval)); + if(approxSyncMaxInterval_>0.0) + approxSync_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval_)); approxSync_->registerCallback(boost::bind(&StereoOdometry::callback, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4)); } else @@ -387,7 +387,7 @@ private: subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s \\\n %s \\\n %s \\\n %s", getName().c_str(), approxSync?"approx":"exact", - approxSync&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"", + approxSync&&approxSyncMaxInterval_!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval_).c_str():"", imageRectLeft_.getTopic().c_str(), imageRectRight_.getTopic().c_str(), cameraInfoLeft_.getTopic().c_str(), @@ -702,7 +702,7 @@ private: rightInfoMsgs.push_back(*cameraInfoRight); double stampDiff = fabs(imageLeft->header.stamp.toSec() - imageRight->header.stamp.toSec()); - if(stampDiff > 0.010) + if(approxSyncMaxInterval_ == 0.0 && stampDiff > 0.010) { NODELET_WARN("The time difference between left and right frames is " "high (diff=%fs, left=%fs, right=%fs). If your left and right cameras are hardware " @@ -1075,6 +1075,7 @@ private: int topicQueueSize_; int syncQueueSize_; bool keepColor_; + double approxSyncMaxInterval_; }; PLUGINLIB_EXPORT_CLASS(rtabmap_odom::StereoOdometry, nodelet::Nodelet); diff --git a/rtabmap_sync/src/nodelets/rgbd_sync.cpp b/rtabmap_sync/src/nodelets/rgbd_sync.cpp index 70d7607d..aa443677 100644 --- a/rtabmap_sync/src/nodelets/rgbd_sync.cpp +++ b/rtabmap_sync/src/nodelets/rgbd_sync.cpp @@ -65,6 +65,7 @@ public: depthScale_(1.0), decimation_(1), compressedRate_(0), + approxSyncMaxInterval_(0.0), approxSyncDepth_(0), exactSyncDepth_(0) {} @@ -84,9 +85,8 @@ private: int queueSize = 1; int syncQueueSize = 10; bool approxSync = true; - double approxSyncMaxInterval = 0.0; pnh.param("approx_sync", approxSync, approxSync); - pnh.param("approx_sync_max_interval", approxSyncMaxInterval, approxSyncMaxInterval); + pnh.param("approx_sync_max_interval", approxSyncMaxInterval_, approxSyncMaxInterval_); pnh.param("topic_queue_size", queueSize, queueSize); if(pnh.hasParam("queue_size") && !pnh.hasParam("sync_queue_size")) { @@ -111,7 +111,7 @@ private: NODELET_INFO("%s: approx_sync = %s", getName().c_str(), approxSync?"true":"false"); if(approxSync) - NODELET_INFO("%s: approx_sync_max_interval = %f", getName().c_str(), approxSyncMaxInterval); + NODELET_INFO("%s: approx_sync_max_interval = %f", getName().c_str(), approxSyncMaxInterval_); NODELET_INFO("%s: topic_queue_size = %d", getName().c_str(), queueSize); NODELET_INFO("%s: sync_queue_size = %d", getName().c_str(), syncQueueSize); NODELET_INFO("%s: depth_scale = %f", getName().c_str(), depthScale_); @@ -124,8 +124,8 @@ private: if(approxSync) { approxSyncDepth_ = new message_filters::Synchronizer(MyApproxSyncDepthPolicy(syncQueueSize), imageSub_, imageDepthSub_, cameraInfoSub_); - if(approxSyncMaxInterval > 0.0) - approxSyncDepth_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval)); + if(approxSyncMaxInterval_ > 0.0) + approxSyncDepth_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval_)); approxSyncDepth_->registerCallback(boost::bind(&RGBDSync::callback, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3)); } else @@ -150,7 +150,7 @@ private: std::string subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s \\\n %s \\\n %s", getName().c_str(), approxSync?"approx":"exact", - approxSync&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"", + approxSync&&approxSyncMaxInterval_!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval_).c_str():"", imageSub_.getTopic().c_str(), imageDepthSub_.getTopic().c_str(), cameraInfoSub_.getTopic().c_str()); @@ -181,7 +181,7 @@ private: double infoStamp = cameraInfo->header.stamp.toSec(); double stampDiff = fabs(rgbStamp - depthStamp); - if(stampDiff > 0.010) + if(approxSyncMaxInterval_==0.0 && stampDiff > 0.010) { NODELET_WARN("The time difference between rgb and depth frames is " "high (diff=%fs, rgb=%fs, depth=%fs). You may want " @@ -304,6 +304,7 @@ private: double depthScale_; int decimation_; double compressedRate_; + double approxSyncMaxInterval_; ros::Time lastCompressedPublished_; diff --git a/rtabmap_sync/src/nodelets/stereo_sync.cpp b/rtabmap_sync/src/nodelets/stereo_sync.cpp index ebb6ad2c..e738974f 100644 --- a/rtabmap_sync/src/nodelets/stereo_sync.cpp +++ b/rtabmap_sync/src/nodelets/stereo_sync.cpp @@ -61,6 +61,7 @@ class StereoSync : public nodelet::Nodelet public: StereoSync() : compressedRate_(0), + approxSyncMaxInterval_(0.0), approxSync_(0), exactSync_(0) {} @@ -80,10 +81,9 @@ private: int queueSize = 1; int syncQueueSize = 10; bool approxSync = false; - double approxSyncMaxInterval = 0.0; pnh.param("approx_sync", approxSync, approxSync); if(approxSync) - pnh.param("approx_sync_max_interval", approxSyncMaxInterval, approxSyncMaxInterval); + pnh.param("approx_sync_max_interval", approxSyncMaxInterval_, approxSyncMaxInterval_); pnh.param("topic_queue_size", queueSize, queueSize); if(pnh.hasParam("queue_size") && !pnh.hasParam("sync_queue_size")) { @@ -100,7 +100,7 @@ private: pnh.param("compressed_rate", compressedRate_, compressedRate_); NODELET_INFO("%s: approx_sync = %s", getName().c_str(), approxSync?"true":"false"); - NODELET_INFO("%s: approx_sync_max_interval = %f", getName().c_str(), approxSyncMaxInterval); + NODELET_INFO("%s: approx_sync_max_interval = %f", getName().c_str(), approxSyncMaxInterval_); NODELET_INFO("%s: topic_queue_size = %d", getName().c_str(), queueSize); NODELET_INFO("%s: sync_queue_size = %d", getName().c_str(), syncQueueSize); NODELET_INFO("%s: compressed_rate = %f", getName().c_str(), compressedRate_); @@ -111,8 +111,8 @@ private: if(approxSync) { approxSync_ = new message_filters::Synchronizer(MyApproxSyncPolicy(syncQueueSize), imageLeftSub_, imageRightSub_, cameraInfoLeftSub_, cameraInfoRightSub_); - if(approxSyncMaxInterval>0.0) - approxSync_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval)); + if(approxSyncMaxInterval_>0.0) + approxSync_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval_)); approxSync_->registerCallback(boost::bind(&StereoSync::callback, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4)); } else @@ -138,7 +138,7 @@ private: std::string subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s \\\n %s \\\n %s \\\n %s", getName().c_str(), approxSync?"approx":"exact", - approxSync&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"", + approxSync&&approxSyncMaxInterval_!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval_).c_str():"", imageLeftSub_.getTopic().c_str(), imageRightSub_.getTopic().c_str(), cameraInfoLeftSub_.getTopic().c_str(), @@ -173,7 +173,7 @@ private: double rightInfoStamp = cameraInfoRight->header.stamp.toSec(); double stampDiff = fabs(leftStamp - rightStamp); - if(stampDiff > 0.010) + if(approxSyncMaxInterval_==0.0 && stampDiff > 0.010) { NODELET_WARN("The time difference between left and right frames is " "high (diff=%fs, left=%fs, right=%fs). If your left and right cameras are hardware " @@ -240,6 +240,7 @@ private: private: double compressedRate_; + double approxSyncMaxInterval_; ros::Time lastCompressedPublished_; ros::Publisher rgbdImagePub_; From 7d4de30ec78476f35bc30542a648ddd43f753c88 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sat, 20 Sep 2025 18:10:04 -0700 Subject: [PATCH 16/56] data_player: added support for ground truth Tf publishing --- rtabmap_util/src/DbPlayerNode.cpp | 27 ++++++++++++++++++++------- 1 file changed, 20 insertions(+), 7 deletions(-) diff --git a/rtabmap_util/src/DbPlayerNode.cpp b/rtabmap_util/src/DbPlayerNode.cpp index 28d30501..3add3275 100644 --- a/rtabmap_util/src/DbPlayerNode.cpp +++ b/rtabmap_util/src/DbPlayerNode.cpp @@ -145,6 +145,8 @@ int main(int argc, char** argv) std::string odomFrameId = "odom"; std::string cameraFrameId = "camera_optical_link"; std::string scanFrameId = "base_laser_link"; + std::string gtFrameId = "world"; + std::string gtBaseFrameId = "base_link_gt"; double rate = 1.0f; std::string databasePath = ""; bool publishTf = true; @@ -155,6 +157,8 @@ int main(int argc, char** argv) pnh.param("odom_frame_id", odomFrameId, odomFrameId); pnh.param("camera_frame_id", cameraFrameId, cameraFrameId); pnh.param("scan_frame_id", scanFrameId, scanFrameId); + pnh.param("ground_truth_frame_id", gtFrameId, gtFrameId); + pnh.param("ground_truth_base_frame_id", gtBaseFrameId, gtBaseFrameId); pnh.param("rate", rate, rate); // Ratio of the database stamps pnh.param("database", databasePath, databasePath); pnh.param("publish_tf", publishTf, publishTf); @@ -172,7 +176,8 @@ int main(int argc, char** argv) ROS_INFO("odom_frame_id = %s", odomFrameId.c_str()); ROS_INFO("camera_frame_id = %s", cameraFrameId.c_str()); ROS_INFO("scan_frame_id = %s", scanFrameId.c_str()); - ROS_INFO("rate = %f", rate); + ROS_INFO("ground_truth_frame_id = %s", gtFrameId.c_str()); + ROS_INFO("rate (factor) = %f", rate); ROS_INFO("publish_tf = %s", publishTf?"true":"false"); ROS_INFO("start_id = %d", startId); ROS_INFO("Publish clock (--clock): %s", publishClock?"true":"false"); @@ -394,6 +399,7 @@ int main(int argc, char** argv) { localTransform = odom.data().stereoCameraModels()[0].left().localTransform(); } + std::vector transforms; if(!localTransform.isNull()) { geometry_msgs::TransformStamped baseToCamera; @@ -401,7 +407,7 @@ int main(int argc, char** argv) baseToCamera.header.frame_id = frameId; baseToCamera.header.stamp = time; rtabmap_conversions::transformToGeometryMsg(localTransform, baseToCamera.transform); - tfBroadcaster.sendTransform(baseToCamera); + transforms.push_back(baseToCamera); } if(!odom.pose().isNull()) @@ -411,7 +417,7 @@ int main(int argc, char** argv) odomToBase.header.frame_id = odomFrameId; odomToBase.header.stamp = time; rtabmap_conversions::transformToGeometryMsg(odom.pose(), odomToBase.transform); - tfBroadcaster.sendTransform(odomToBase); + transforms.push_back(odomToBase); } if(!scanPub.getTopic().empty() || !scanCloudPub.getTopic().empty()) @@ -421,8 +427,18 @@ int main(int argc, char** argv) baseToLaserScan.header.frame_id = frameId; baseToLaserScan.header.stamp = time; rtabmap_conversions::transformToGeometryMsg(odom.data().laserScanCompressed().localTransform(), baseToLaserScan.transform); - tfBroadcaster.sendTransform(baseToLaserScan); + transforms.push_back(baseToLaserScan); } + + if(!odom.data().groundTruth().isNull()) { + geometry_msgs::TransformStamped worldToBase; + worldToBase.child_frame_id = gtBaseFrameId; + worldToBase.header.frame_id = gtFrameId; + worldToBase.header.stamp = time; + rtabmap_conversions::transformToGeometryMsg(odom.data().groundTruth(), worldToBase.transform); + transforms.push_back(worldToBase); + } + tfBroadcaster.sendTransform(transforms); } if(!odom.pose().isNull()) { @@ -517,7 +533,6 @@ int main(int argc, char** argv) if(leftPub.getNumSubscribers() && type == 1) { leftPub.publish(imageRosMsg); - leftCamInfoPub.publish(camInfoA); } } @@ -538,7 +553,6 @@ int main(int argc, char** argv) imageRosMsg->header.stamp = time; depthPub.publish(imageRosMsg); - depthCamInfoPub.publish(camInfoB); } if(rightPub.getNumSubscribers() && !odom.data().rightRaw().empty() && type==1) @@ -551,7 +565,6 @@ int main(int argc, char** argv) imageRosMsg->header.stamp = time; rightPub.publish(imageRosMsg); - rightCamInfoPub.publish(camInfoB); } if(!odom.data().laserScanRaw().isEmpty()) From 166f18c98fca951d4816bc5376d41fc21dd319a1 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sun, 21 Sep 2025 16:20:46 -0700 Subject: [PATCH 17/56] missing not staged change --- rtabmap_odom/src/nodelets/rgbd_odometry.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rtabmap_odom/src/nodelets/rgbd_odometry.cpp b/rtabmap_odom/src/nodelets/rgbd_odometry.cpp index 4c9c96ec..47b1e171 100644 --- a/rtabmap_odom/src/nodelets/rgbd_odometry.cpp +++ b/rtabmap_odom/src/nodelets/rgbd_odometry.cpp @@ -126,7 +126,7 @@ void RGBDOdometry::onOdomInit() RCLCPP_INFO(this->get_logger(), "RGBDOdometry: approx_sync = %s", approxSync?"true":"false"); if(approxSync) - RCLCPP_INFO(this->get_logger(), "RGBDOdometry: approx_sync_max_interval = %f", approxSyncMaxInterval); + RCLCPP_INFO(this->get_logger(), "RGBDOdometry: approx_sync_max_interval = %f", approxSyncMaxInterval_); RCLCPP_INFO(this->get_logger(), "RGBDOdometry: topic_queue_size = %d", topicQueueSize_); RCLCPP_INFO(this->get_logger(), "RGBDOdometry: sync_queue_size = %d", syncQueueSize_); RCLCPP_INFO(this->get_logger(), "RGBDOdometry: qos = %d", (int)qos()); From bda8f16bb81d3257025df5d0a23457b38bca1e82 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sun, 21 Sep 2025 17:08:11 -0700 Subject: [PATCH 18/56] Ported data_player node to ROS2. Fixed camera publisher namespace in rgbd_split. --- rtabmap_util/CMakeLists.txt | 12 +- .../include/rtabmap_util/db_player.hpp | 105 +++ .../include/rtabmap_util/rgbd_split.hpp | 6 +- rtabmap_util/src/DbPlayerNode.cpp | 627 +----------------- rtabmap_util/src/nodelets/db_player.cpp | 592 +++++++++++++++++ rtabmap_util/src/nodelets/rgbd_split.cpp | 12 +- 6 files changed, 743 insertions(+), 611 deletions(-) create mode 100644 rtabmap_util/include/rtabmap_util/db_player.hpp create mode 100644 rtabmap_util/src/nodelets/db_player.cpp diff --git a/rtabmap_util/CMakeLists.txt b/rtabmap_util/CMakeLists.txt index 15fc4274..eb40a8a1 100644 --- a/rtabmap_util/CMakeLists.txt +++ b/rtabmap_util/CMakeLists.txt @@ -76,6 +76,7 @@ SET(rtabmap_util_plugins_lib_src src/nodelets/point_cloud_aggregator.cpp src/nodelets/point_cloud_assembler.cpp src/nodelets/imu_to_tf.cpp + src/nodelets/db_player.cpp src/nodelets/lidar_deskewing.cpp src/nodelets/rgbd_relay.cpp src/nodelets/rgbd_split.cpp @@ -134,6 +135,7 @@ 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::DbPlayer") 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") @@ -188,10 +190,10 @@ ament_target_dependencies(rtabmap_point_cloud_xyzrgb ${Libraries}) target_link_libraries(rtabmap_point_cloud_xyzrgb rtabmap_util_plugins) set_target_properties(rtabmap_point_cloud_xyzrgb PROPERTIES OUTPUT_NAME "point_cloud_xyzrgb") -#add_executable(rtabmap_data_player src/DbPlayerNode.cpp) -#ament_target_dependencies(rtabmap_data_player ${Libraries}) -#target_link_libraries(rtabmap_data_player rtabmap_util_plugins) -#set_target_properties(rtabmap_data_player PROPERTIES OUTPUT_NAME "data_player") +add_executable(rtabmap_data_player src/DbPlayerNode.cpp) +ament_target_dependencies(rtabmap_data_player ${Libraries}) +target_link_libraries(rtabmap_data_player rtabmap_util_plugins) +set_target_properties(rtabmap_data_player PROPERTIES OUTPUT_NAME "data_player") #add_executable(rtabmap_odom_msg_to_tf src/OdomMsgToTFNode.cpp) #ament_target_dependencies(rtabmap_odom_msg_to_tf ${Libraries}) @@ -250,7 +252,7 @@ install(TARGETS ) install(TARGETS # rtabmap_map_optimizer -# rtabmap_data_player + rtabmap_data_player # rtabmap_odom_msg_to_tf rtabmap_imu_to_tf rtabmap_disparity_to_depth diff --git a/rtabmap_util/include/rtabmap_util/db_player.hpp b/rtabmap_util/include/rtabmap_util/db_player.hpp new file mode 100644 index 00000000..ca879bdd --- /dev/null +++ b/rtabmap_util/include/rtabmap_util/db_player.hpp @@ -0,0 +1,105 @@ +/* +Copyright (c) 2010-2025, 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 +#include +#include + +#include + +#include + +#include + +#include + +#include +#include + +#include + +namespace rtabmap_util +{ + +class DbPlayer : public rclcpp::Node +{ +public: + RTABMAP_UTIL_PUBLIC + explicit DbPlayer(const rclcpp::NodeOptions & options); + virtual ~DbPlayer(); + bool publishNextFrame(); + bool isPaused() const {return paused_;} + void setPaused(bool enabled) {paused_ = enabled;} + +private: + void pauseCallback(const std::shared_ptr, const std::shared_ptr, std::shared_ptr); + void resumeCallback(const std::shared_ptr, const std::shared_ptr, std::shared_ptr); + +private: + bool paused_; + std::shared_ptr reader_; + std::string frameId_; + std::string odomFrameId_; + std::string cameraFrameId_; + std::string scanFrameId_; + std::string gtFrameId_; + std::string gtBaseFrameId_; + int qos_; + double scanAngleMin_; + double scanAngleMax_; + double scanAngleIncrement_; + double scanRangeMin_; + double scanRangeMax_; + + rclcpp::Service::SharedPtr pauseSrv_; + rclcpp::Service::SharedPtr resumeSrv_; + + image_transport::Publisher imagePub_; + image_transport::Publisher rgbPub_; + image_transport::Publisher depthPub_; + image_transport::Publisher leftPub_; + image_transport::Publisher rightPub_; + rclcpp::Publisher::SharedPtr rgbInfoPub_; + rclcpp::Publisher::SharedPtr depthInfoPub_; + rclcpp::Publisher::SharedPtr leftInfoPub_; + rclcpp::Publisher::SharedPtr rightInfoPub_; + rclcpp::Publisher::SharedPtr odometryPub_; + rclcpp::Publisher::SharedPtr scanPub_; + rclcpp::Publisher::SharedPtr scanCloudPub_; + rclcpp::Publisher::SharedPtr globalPosePub_; + rclcpp::Publisher::SharedPtr gpsFixPub_; + rclcpp::Publisher::SharedPtr clockPub_; + std::shared_ptr tfBroadcaster_; +}; + +} diff --git a/rtabmap_util/include/rtabmap_util/rgbd_split.hpp b/rtabmap_util/include/rtabmap_util/rgbd_split.hpp index a735dedf..d31a736a 100644 --- a/rtabmap_util/include/rtabmap_util/rgbd_split.hpp +++ b/rtabmap_util/include/rtabmap_util/rgbd_split.hpp @@ -50,8 +50,10 @@ public: private: rclcpp::Subscription::SharedPtr rgbdImageSub_; - image_transport::CameraPublisher rgbPub_; - image_transport::CameraPublisher depthPub_; + image_transport::Publisher rgbPub_; + image_transport::Publisher depthPub_; + rclcpp::Publisher::SharedPtr rgbInfoPub_; + rclcpp::Publisher::SharedPtr depthInfoPub_; }; } diff --git a/rtabmap_util/src/DbPlayerNode.cpp b/rtabmap_util/src/DbPlayerNode.cpp index 0d4b4d2a..57a979d0 100644 --- a/rtabmap_util/src/DbPlayerNode.cpp +++ b/rtabmap_util/src/DbPlayerNode.cpp @@ -1,5 +1,5 @@ /* -Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke +Copyright (c) 2010-2019, Mathieu Labbe - IntRoLab - Universite de Sherbrooke All rights reserved. Redistribution and use in source and binary forms, with or without @@ -25,36 +25,9 @@ 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 -#ifdef PRE_ROS_IRON -#include -#else -#include -#endif -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include +#include "rtabmap_util/db_player.hpp" +#include "rtabmap/utilite/ULogger.h" +#include "rclcpp/rclcpp.hpp" #ifndef _WIN32 #include @@ -96,604 +69,58 @@ bool spacehit() } #endif -bool paused = false; -bool pauseCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&) +int main(int argc, char **argv) { - if(paused) - { - ROS_WARN("Already paused!"); - } - else - { - paused = true; - ROS_INFO("paused!"); - } - return true; -} + ULogger::setType(ULogger::kTypeConsole); -bool resumeCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&) -{ - if(!paused) - { - ROS_WARN("Already running!"); - } - else - { - paused = false; - ROS_INFO("resumed!"); - } - return true; -} - -int main(int argc, char** argv) -{ - ros::init(argc, argv, "data_player"); - - //ULogger::setType(ULogger::kTypeConsole); - //ULogger::setLevel(ULogger::kDebug); - //ULogger::setEventLevel(ULogger::kWarning); - - bool publishClock = false; + std::vector arguments; for(int i=1;i(options); - pnh.param("frame_id", frameId, frameId); - pnh.param("odom_frame_id", odomFrameId, odomFrameId); - pnh.param("camera_frame_id", cameraFrameId, cameraFrameId); - pnh.param("scan_frame_id", scanFrameId, scanFrameId); - pnh.param("ground_truth_frame_id", gtFrameId, gtFrameId); - pnh.param("ground_truth_base_frame_id", gtBaseFrameId, gtBaseFrameId); - pnh.param("rate", rate, rate); // Ratio of the database stamps - pnh.param("database", databasePath, databasePath); - pnh.param("publish_tf", publishTf, publishTf); - pnh.param("start_id", startId, startId); + rclcpp::Rate pauseRate(10); - // A general 360 lidar with 0.5 deg increment - double scanAngleMin, scanAngleMax, scanAngleIncrement, scanRangeMin, scanRangeMax; - pnh.param("scan_angle_min", scanAngleMin, -M_PI); - pnh.param("scan_angle_max", scanAngleMax, M_PI); - pnh.param("scan_angle_increment", scanAngleIncrement, M_PI / 720.0); - pnh.param("scan_range_min", scanRangeMin, 0.0); - pnh.param("scan_range_max", scanRangeMax, 60); - - ROS_INFO("frame_id = %s", frameId.c_str()); - ROS_INFO("odom_frame_id = %s", odomFrameId.c_str()); - ROS_INFO("camera_frame_id = %s", cameraFrameId.c_str()); - ROS_INFO("scan_frame_id = %s", scanFrameId.c_str()); - ROS_INFO("ground_truth_frame_id = %s", gtFrameId.c_str()); - ROS_INFO("rate (factor) = %f", rate); - ROS_INFO("publish_tf = %s", publishTf?"true":"false"); - ROS_INFO("start_id = %d", startId); - ROS_INFO("Publish clock (--clock): %s", publishClock?"true":"false"); - - if(databasePath.empty()) + while(rclcpp::ok()) { - ROS_ERROR("Parameter \"database\" must be set (path to a RTAB-Map database)."); - return -1; - } - databasePath = uReplaceChar(databasePath, '~', UDirectory::homeDir()); - if(databasePath.size() && databasePath.at(0) != '/') - { - databasePath = UDirectory::currentDir(true) + databasePath; - } - ROS_INFO("database = %s", databasePath.c_str()); - - rtabmap::DBReader reader(databasePath, -rate, false, false, false, startId); - if(!reader.init()) - { - ROS_ERROR("Cannot open database \"%s\".", databasePath.c_str()); - return -1; - } - - ros::ServiceServer pauseSrv = pnh.advertiseService("pause", pauseCallback); - ros::ServiceServer resumeSrv = pnh.advertiseService("resume", resumeCallback); - - image_transport::ImageTransport it(nh); - image_transport::Publisher imagePub; - image_transport::Publisher rgbPub; - image_transport::Publisher depthPub; - image_transport::Publisher leftPub; - image_transport::Publisher rightPub; - ros::Publisher rgbCamInfoPub; - ros::Publisher depthCamInfoPub; - ros::Publisher leftCamInfoPub; - ros::Publisher rightCamInfoPub; - ros::Publisher odometryPub; - ros::Publisher scanPub; - ros::Publisher scanCloudPub; - ros::Publisher globalPosePub; - ros::Publisher gpsFixPub; - ros::Publisher clockPub; - tf2_ros::TransformBroadcaster tfBroadcaster; - - if(publishClock) - { - clockPub = nh.advertise("/clock", 1); - } - - UTimer timer; - rtabmap::SensorCaptureInfo cameraInfo; - rtabmap::SensorData data = reader.takeImage(&cameraInfo); - rtabmap::OdometryInfo odomInfo; - odomInfo.reg.covariance = cameraInfo.odomCovariance; - rtabmap::OdometryEvent odom(data, cameraInfo.odomPose, odomInfo); - double acquisitionTime = timer.ticks(); - while(ros::ok() && odom.data().id()) - { - ROS_INFO("Reading sensor data %d...", odom.data().id()); - - ros::Time time(odom.data().stamp()); - - if(publishClock) - { - rosgraph_msgs::Clock msg; - msg.clock = time; - clockPub.publish(msg); + if(!node->publishNextFrame()) { + // end of file, exit + RCLCPP_INFO(node->get_logger(), "Last frame published, exiting!"); + break; } - sensor_msgs::CameraInfo camInfoA; //rgb or left - sensor_msgs::CameraInfo camInfoB; //depth or right - - camInfoA.K.assign(0); - camInfoA.K[0] = camInfoA.K[4] = camInfoA.K[8] = 1; - camInfoA.R.assign(0); - camInfoA.R[0] = camInfoA.R[4] = camInfoA.R[8] = 1; - camInfoA.P.assign(0); - camInfoA.P[10] = 1; - - camInfoA.header.frame_id = cameraFrameId; - camInfoA.header.stamp = time; - - camInfoB = camInfoA; - - int type = -1; - if(!odom.data().depthRaw().empty() && (odom.data().depthRaw().type() == CV_32FC1 || odom.data().depthRaw().type() == CV_16UC1)) - { - if(odom.data().cameraModels().size() > 1) - { - ROS_WARN("Multi-cameras detected in database but this node cannot send multi-images yet..."); - } - else - { - //depth - if(odom.data().cameraModels().size()) - { - camInfoA.D.resize(5,0); - - camInfoA.P[0] = odom.data().cameraModels()[0].fx(); - camInfoA.K[0] = odom.data().cameraModels()[0].fx(); - camInfoA.P[5] = odom.data().cameraModels()[0].fy(); - camInfoA.K[4] = odom.data().cameraModels()[0].fy(); - camInfoA.P[2] = odom.data().cameraModels()[0].cx(); - camInfoA.K[2] = odom.data().cameraModels()[0].cx(); - camInfoA.P[6] = odom.data().cameraModels()[0].cy(); - camInfoA.K[5] = odom.data().cameraModels()[0].cy(); - - camInfoB = camInfoA; - } - - type=0; - - if(rgbPub.getTopic().empty()) rgbPub = it.advertise("rgb/image", 1); - if(depthPub.getTopic().empty()) depthPub = it.advertise("depth_registered/image", 1); - if(rgbCamInfoPub.getTopic().empty()) rgbCamInfoPub = nh.advertise("rgb/camera_info", 1); - if(depthCamInfoPub.getTopic().empty()) depthCamInfoPub = nh.advertise("depth_registered/camera_info", 1); - } - } - else if(!odom.data().rightRaw().empty() && odom.data().rightRaw().type() == CV_8U) - { - if(odom.data().stereoCameraModels().size() > 1) - { - ROS_WARN("Multi-cameras detected in database but this node cannot send multi-images yet..."); - } - else - { - //stereo - if(odom.data().stereoCameraModels()[0].isValidForProjection()) - { - camInfoA.D.resize(8,0); - - camInfoA.P[0] = odom.data().stereoCameraModels()[0].left().fx(); - camInfoA.K[0] = odom.data().stereoCameraModels()[0].left().fx(); - camInfoA.P[5] = odom.data().stereoCameraModels()[0].left().fy(); - camInfoA.K[4] = odom.data().stereoCameraModels()[0].left().fy(); - camInfoA.P[2] = odom.data().stereoCameraModels()[0].left().cx(); - camInfoA.K[2] = odom.data().stereoCameraModels()[0].left().cx(); - camInfoA.P[6] = odom.data().stereoCameraModels()[0].left().cy(); - camInfoA.K[5] = odom.data().stereoCameraModels()[0].left().cy(); - - camInfoB = camInfoA; - camInfoB.P[3] = odom.data().stereoCameraModels()[0].right().Tx(); // Right_Tx = -baseline*fx - } - - type=1; - - if(leftPub.getTopic().empty()) leftPub = it.advertise("left/image", 1); - if(rightPub.getTopic().empty()) rightPub = it.advertise("right/image", 1); - if(leftCamInfoPub.getTopic().empty()) leftCamInfoPub = nh.advertise("left/camera_info", 1); - if(rightCamInfoPub.getTopic().empty()) rightCamInfoPub = nh.advertise("right/camera_info", 1); - } - - } - else - { - if(imagePub.getTopic().empty()) imagePub = it.advertise("image", 1); - } - - camInfoA.height = odom.data().imageRaw().rows; - camInfoA.width = odom.data().imageRaw().cols; - camInfoB.height = odom.data().depthOrRightRaw().rows; - camInfoB.width = odom.data().depthOrRightRaw().cols; - - if(!odom.data().laserScanRaw().isEmpty()) - { - if(scanPub.getTopic().empty() && odom.data().laserScanRaw().is2d()) - { - scanPub = nh.advertise("scan", 1); - if(odom.data().laserScanRaw().angleIncrement() > 0.0f) - { - ROS_INFO("Scan will be published."); - } - else - { - ROS_INFO("Scan will be published with those parameters:"); - ROS_INFO(" scan_angle_min=%f", scanAngleMin); - ROS_INFO(" scan_angle_max=%f", scanAngleMax); - ROS_INFO(" scan_angle_increment=%f", scanAngleIncrement); - ROS_INFO(" scan_range_min=%f", scanRangeMin); - ROS_INFO(" scan_range_max=%f", scanRangeMax); - } - } - else if(scanCloudPub.getTopic().empty()) - { - scanCloudPub = nh.advertise("scan_cloud", 1); - ROS_INFO("Scan cloud will be published."); - } - } - - if(!odom.data().globalPose().isNull() && - odom.data().globalPoseCovariance().cols==6 && - odom.data().globalPoseCovariance().rows==6) - { - if(globalPosePub.getTopic().empty()) - { - globalPosePub = nh.advertise("global_pose", 1); - ROS_INFO("Global pose will be published."); - } - } - - if(odom.data().gps().stamp() > 0.0) - { - if(gpsFixPub.getTopic().empty()) - { - gpsFixPub = nh.advertise("gps/fix", 1); - ROS_INFO("GPS will be published."); - } - } - - // publish transforms first - if(publishTf) - { - rtabmap::Transform localTransform; - if(odom.data().cameraModels().size() == 1) - { - localTransform = odom.data().cameraModels()[0].localTransform(); - } - else if(odom.data().stereoCameraModels().size() == 1) - { - localTransform = odom.data().stereoCameraModels()[0].left().localTransform(); - } - std::vector transforms; - if(!localTransform.isNull()) - { - geometry_msgs::TransformStamped baseToCamera; - baseToCamera.child_frame_id = cameraFrameId; - baseToCamera.header.frame_id = frameId; - baseToCamera.header.stamp = time; - rtabmap_conversions::transformToGeometryMsg(localTransform, baseToCamera.transform); - transforms.push_back(baseToCamera); - } - - if(!odom.pose().isNull()) - { - geometry_msgs::TransformStamped odomToBase; - odomToBase.child_frame_id = frameId; - odomToBase.header.frame_id = odomFrameId; - odomToBase.header.stamp = time; - rtabmap_conversions::transformToGeometryMsg(odom.pose(), odomToBase.transform); - transforms.push_back(odomToBase); - } - - if(!scanPub.getTopic().empty() || !scanCloudPub.getTopic().empty()) - { - geometry_msgs::TransformStamped baseToLaserScan; - baseToLaserScan.child_frame_id = scanFrameId; - baseToLaserScan.header.frame_id = frameId; - baseToLaserScan.header.stamp = time; - rtabmap_conversions::transformToGeometryMsg(odom.data().laserScanCompressed().localTransform(), baseToLaserScan.transform); - transforms.push_back(baseToLaserScan); - } - - if(!odom.data().groundTruth().isNull()) { - geometry_msgs::TransformStamped worldToBase; - worldToBase.child_frame_id = gtBaseFrameId; - worldToBase.header.frame_id = gtFrameId; - worldToBase.header.stamp = time; - rtabmap_conversions::transformToGeometryMsg(odom.data().groundTruth(), worldToBase.transform); - transforms.push_back(worldToBase); - } - tfBroadcaster.sendTransform(transforms); - } - if(!odom.pose().isNull()) - { - if(odometryPub.getTopic().empty()) odometryPub = nh.advertise("odom", 1); - - if(odometryPub.getNumSubscribers()) - { - nav_msgs::Odometry odomMsg; - odomMsg.child_frame_id = frameId; - odomMsg.header.frame_id = odomFrameId; - odomMsg.header.stamp = time; - rtabmap_conversions::transformToPoseMsg(odom.pose(), odomMsg.pose.pose); - UASSERT(odomMsg.pose.covariance.size() == 36 && - odom.covariance().total() == 36 && - odom.covariance().type() == CV_64FC1); - memcpy(odomMsg.pose.covariance.begin(), odom.covariance().data, 36*sizeof(double)); - odometryPub.publish(odomMsg); - } - } - - // Publish async topics first (so that they can catched by rtabmap before the image topics) - if(globalPosePub.getNumSubscribers() > 0 && - !odom.data().globalPose().isNull() && - odom.data().globalPoseCovariance().cols==6 && - odom.data().globalPoseCovariance().rows==6) - { - geometry_msgs::PoseWithCovarianceStamped msg; - rtabmap_conversions::transformToPoseMsg(odom.data().globalPose(), msg.pose.pose); - memcpy(msg.pose.covariance.data(), odom.data().globalPoseCovariance().data, 36*sizeof(double)); - msg.header.frame_id = frameId; - msg.header.stamp = time; - globalPosePub.publish(msg); - } - - if(odom.data().gps().stamp() > 0.0) - { - sensor_msgs::NavSatFix msg; - msg.longitude = odom.data().gps().longitude(); - msg.latitude = odom.data().gps().latitude(); - msg.altitude = odom.data().gps().altitude(); - msg.position_covariance_type = sensor_msgs::NavSatFix::COVARIANCE_TYPE_DIAGONAL_KNOWN; - msg.position_covariance.at(0) = msg.position_covariance.at(4) = msg.position_covariance.at(8)= odom.data().gps().error()* odom.data().gps().error(); - msg.header.frame_id = frameId; - msg.header.stamp.fromSec(odom.data().gps().stamp()); - gpsFixPub.publish(msg); - } - - if(type >= 0) - { - if(rgbCamInfoPub.getNumSubscribers() && type == 0) - { - rgbCamInfoPub.publish(camInfoA); - } - if(leftCamInfoPub.getNumSubscribers() && type == 1) - { - leftCamInfoPub.publish(camInfoA); - } - if(depthCamInfoPub.getNumSubscribers() && type == 0) - { - depthCamInfoPub.publish(camInfoB); - } - if(rightCamInfoPub.getNumSubscribers() && type == 1) - { - rightCamInfoPub.publish(camInfoB); - } - } - - if(imagePub.getNumSubscribers() || rgbPub.getNumSubscribers() || leftPub.getNumSubscribers()) - { - cv_bridge::CvImage img; - if(odom.data().imageRaw().channels() == 1) - { - img.encoding = sensor_msgs::image_encodings::MONO8; - } - else - { - img.encoding = sensor_msgs::image_encodings::BGR8; - } - img.image = odom.data().imageRaw(); - sensor_msgs::ImagePtr imageRosMsg = img.toImageMsg(); - imageRosMsg->header.frame_id = cameraFrameId; - imageRosMsg->header.stamp = time; - - if(imagePub.getNumSubscribers()) - { - imagePub.publish(imageRosMsg); - } - if(rgbPub.getNumSubscribers() && type == 0) - { - rgbPub.publish(imageRosMsg); - } - if(leftPub.getNumSubscribers() && type == 1) - { - leftPub.publish(imageRosMsg); - } - } - - if(depthPub.getNumSubscribers() && !odom.data().depthRaw().empty() && type==0) - { - cv_bridge::CvImage img; - if(odom.data().depthRaw().type() == CV_32FC1) - { - img.encoding = sensor_msgs::image_encodings::TYPE_32FC1; - } - else - { - img.encoding = sensor_msgs::image_encodings::TYPE_16UC1; - } - img.image = odom.data().depthRaw(); - sensor_msgs::ImagePtr imageRosMsg = img.toImageMsg(); - imageRosMsg->header.frame_id = cameraFrameId; - imageRosMsg->header.stamp = time; - - depthPub.publish(imageRosMsg); - } - - if(rightPub.getNumSubscribers() && !odom.data().rightRaw().empty() && type==1) - { - cv_bridge::CvImage img; - img.encoding = sensor_msgs::image_encodings::MONO8; - img.image = odom.data().rightRaw(); - sensor_msgs::ImagePtr imageRosMsg = img.toImageMsg(); - imageRosMsg->header.frame_id = cameraFrameId; - imageRosMsg->header.stamp = time; - - rightPub.publish(imageRosMsg); - } - - if(!odom.data().laserScanRaw().isEmpty()) - { - if(scanPub.getNumSubscribers() && odom.data().laserScanRaw().is2d()) - { - //inspired from pointcloud_to_laserscan package - sensor_msgs::LaserScan msg; - msg.header.frame_id = scanFrameId; - msg.header.stamp = time; - - msg.angle_min = scanAngleMin; - msg.angle_max = scanAngleMax; - msg.angle_increment = scanAngleIncrement; - msg.time_increment = 0.0; - msg.scan_time = 0; - msg.range_min = scanRangeMin; - msg.range_max = scanRangeMax; - if(odom.data().laserScanRaw().angleIncrement() > 0.0f) - { - msg.angle_min = odom.data().laserScanRaw().angleMin(); - msg.angle_max = odom.data().laserScanRaw().angleMax(); - msg.angle_increment = odom.data().laserScanRaw().angleIncrement(); - msg.range_min = odom.data().laserScanRaw().rangeMin(); - msg.range_max = odom.data().laserScanRaw().rangeMax(); - } - - uint32_t rangesSize = std::ceil((msg.angle_max - msg.angle_min) / msg.angle_increment); - msg.ranges.assign(rangesSize, 0.0); - - const cv::Mat & scan = odom.data().laserScanRaw().data(); - for (int i=0; i(0,i); - double range = hypot(ptr[0], ptr[1]); - if (range >= msg.range_min && range <=msg.range_max) - { - double angle = atan2(ptr[1], ptr[0]); - if (angle >= msg.angle_min && angle <= msg.angle_max) - { - int index = (angle - msg.angle_min) / msg.angle_increment; - if (index>=0 && index= 7 && // including null str ending - odom.data().userDataRaw().rows == 1 && - memcmp(odom.data().userDataRaw().data, "GOAL:", 5) == 0) - { - //GOAL format detected, remove it from the user data and send it as goal event - std::string goalStr = (const char *)odom.data().userDataRaw().data; - if(!goalStr.empty()) - { - std::list strs = uSplit(goalStr, ':'); - if(strs.size() == 2) - { - int goalId = atoi(strs.rbegin()->c_str()); - - if(goalId > 0) - { - ROS_WARN("Goal %d detected, calling rtabmap's set_goal service!", goalId); - rtabmap_msgs::SetGoal setGoalSrv; - setGoalSrv.request.node_id = goalId; - setGoalSrv.request.node_label = ""; - if(!ros::service::call("set_goal", setGoalSrv)) - { - ROS_ERROR("Can't call \"set_goal\" service"); - } - } - } - } - } - - ros::spinOnce(); - - while(ros::ok()) + while(rclcpp::ok()) { #ifndef _WIN32 if (spacehit()) { - paused = !paused; - if(paused) + node->setPaused(!node->isPaused()); + if(node->isPaused()) { - ROS_INFO("paused!"); + RCLCPP_INFO(node->get_logger(), "paused!"); } else { - ROS_INFO("resumed!"); + RCLCPP_INFO(node->get_logger(), "resumed!"); } } #endif - if(!paused) + if(!node->isPaused()) { break; } - uSleep(100); - ros::spinOnce(); + pauseRate.sleep(); + rclcpp::spin_some(node); } - - timer.restart(); - cameraInfo = rtabmap::CameraInfo(); - data = reader.takeImage(&cameraInfo); - odomInfo.reg.covariance = cameraInfo.odomCovariance; - odom = rtabmap::OdometryEvent(data, cameraInfo.odomPose, odomInfo); - acquisitionTime = timer.ticks(); } - + rclcpp::shutdown(); return 0; } diff --git a/rtabmap_util/src/nodelets/db_player.cpp b/rtabmap_util/src/nodelets/db_player.cpp new file mode 100644 index 00000000..54d3cb2d --- /dev/null +++ b/rtabmap_util/src/nodelets/db_player.cpp @@ -0,0 +1,592 @@ +/* +Copyright (c) 2010-2025, 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 + +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#ifdef PRE_ROS_IRON +#include +#else +#include +#endif + +namespace rtabmap_util +{ + +DbPlayer::DbPlayer(const rclcpp::NodeOptions & options) : + rclcpp::Node("db_player", options), + paused_(false), + frameId_("base_link"), + odomFrameId_("odom"), + cameraFrameId_("camera_optical_link"), + scanFrameId_("base_laser_link"), + gtFrameId_("world"), + gtBaseFrameId_("base_link_gt"), + qos_(RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT) +{ + //ULogger::setType(ULogger::kTypeConsole); + //ULogger::setLevel(ULogger::kDebug); + //ULogger::setEventLevel(ULogger::kWarning); + + //parse input arguments + bool publishClock = false; + publishClock = this->declare_parameter("publish_clock", publishClock); + std::vector tmpList = get_node_options().arguments(); + std::vector argList; + for(unsigned int i=0; ideclare_parameter("frame_id", frameId_); + odomFrameId_ = this->declare_parameter("odom_frame_id", odomFrameId_); + cameraFrameId_ = this->declare_parameter("camera_frame_id", cameraFrameId_); + scanFrameId_ = this->declare_parameter("scan_frame_id", scanFrameId_); + gtFrameId_ = this->declare_parameter("ground_truth_frame_id", gtFrameId_); + gtBaseFrameId_ = this->declare_parameter("ground_truth_base_frame_id", gtBaseFrameId_); + rate = this->declare_parameter("rate", rate); // Ratio of the database stamps + databasePath = this->declare_parameter("database", databasePath); + publishTf = this->declare_parameter("publish_tf", publishTf); + ignoreOdom = this->declare_parameter("ignore_odom", ignoreOdom); + startId = this->declare_parameter("start_id", startId); + qos_ = this->declare_parameter("qos", qos_); + + // A general 360 lidar with 0.5 deg increment + scanAngleMin_ = this->declare_parameter("scan_angle_min", -M_PI); + scanAngleMax_ = this->declare_parameter("scan_angle_max", M_PI); + scanAngleIncrement_ = this->declare_parameter("scan_angle_increment", M_PI / 720.0); + scanRangeMin_ = this->declare_parameter("scan_range_min", 0.0); + scanRangeMax_ = this->declare_parameter("scan_range_max", 60); + + RCLCPP_INFO(get_logger(), "frame_id = %s", frameId_.c_str()); + RCLCPP_INFO(get_logger(), "odom_frame_id = %s", odomFrameId_.c_str()); + RCLCPP_INFO(get_logger(), "camera_frame_id = %s", cameraFrameId_.c_str()); + RCLCPP_INFO(get_logger(), "scan_frame_id = %s", scanFrameId_.c_str()); + RCLCPP_INFO(get_logger(), "ground_truth_frame_id = %s", gtFrameId_.c_str()); + RCLCPP_INFO(get_logger(), "rate (factor) = %f", rate); + RCLCPP_INFO(get_logger(), "publish_tf = %s", publishTf?"true":"false"); + RCLCPP_INFO(get_logger(), "start_id = %d", startId); + RCLCPP_INFO(get_logger(), "Publish clock (--clock): %s", publishClock?"true":"false"); + RCLCPP_INFO(get_logger(), "qos = %d", qos_); + + if(databasePath.empty()) + { + RCLCPP_ERROR(get_logger(), "Parameter \"database\" must be set (path to a RTAB-Map database)."); + exit(-1); + } + + databasePath = uReplaceChar(databasePath, '~', UDirectory::homeDir()); + if(databasePath.size() && databasePath.at(0) != '/') + { + databasePath = UDirectory::currentDir(true) + databasePath; + } + RCLCPP_INFO(get_logger(), "database = %s", databasePath.c_str()); + + reader_.reset(new rtabmap::DBReader(databasePath, -rate, ignoreOdom, false, false, startId)); + if(!reader_->init()) + { + RCLCPP_ERROR(get_logger(), "Cannot open database \"%s\".", databasePath.c_str()); + exit(-1); + } + + const std::string servicePrefix = get_name() + std::string("/"); + pauseSrv_ = this->create_service(servicePrefix + "pause", std::bind(&DbPlayer::pauseCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + resumeSrv_ = this->create_service(servicePrefix + "resume", std::bind(&DbPlayer::resumeCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + + if(publishTf) { + tfBroadcaster_ = std::make_shared(this); + } + + if(publishClock) + { + clockPub_ = this->create_publisher("/clock", 1); + } +} + +DbPlayer::~DbPlayer(){} + +void DbPlayer::pauseCallback( + const std::shared_ptr, + const std::shared_ptr, + std::shared_ptr) +{ + if(paused_) + { + RCLCPP_WARN(get_logger(), "Already paused!"); + } + else + { + paused_ = true; + RCLCPP_INFO(get_logger(), "paused!"); + } +} +void DbPlayer::resumeCallback( + const std::shared_ptr, + const std::shared_ptr, + std::shared_ptr) +{ + if(!paused_) + { + RCLCPP_WARN(get_logger(), "Already running!"); + } + else + { + paused_ = false; + RCLCPP_INFO(get_logger(), "resumed!"); + } +} + +bool DbPlayer::publishNextFrame() +{ + rtabmap::SensorCaptureInfo cameraInfo; + rtabmap::SensorData data = reader_->takeImage(&cameraInfo); + rtabmap::OdometryInfo odomInfo; + odomInfo.reg.covariance = cameraInfo.odomCovariance; + rtabmap::OdometryEvent odom(data, cameraInfo.odomPose, odomInfo); + if(!odom.data().id()) + { + return false; + } + + RCLCPP_INFO(get_logger(), "Reading sensor data %d...", odom.data().id()); + + rclcpp::Time time = rtabmap_conversions::timestampToROS(odom.data().stamp()); + + if(clockPub_.get()) + { + rosgraph_msgs::msg::Clock msg; + msg.clock = time; + clockPub_->publish(msg); + } + + sensor_msgs::msg::CameraInfo camInfoA; //rgb or left + sensor_msgs::msg::CameraInfo camInfoB; //depth or right + + camInfoA.k.fill(0); + camInfoA.k[0] = camInfoA.k[4] = camInfoA.k[8] = 1; + camInfoA.r.fill(0); + camInfoA.r[0] = camInfoA.r[4] = camInfoA.r[8] = 1; + camInfoA.p.fill(0); + camInfoA.p[10] = 1; + + camInfoA.header.frame_id = cameraFrameId_; + camInfoA.header.stamp = time; + + camInfoB = camInfoA; + + if(!odom.data().depthRaw().empty() && (odom.data().depthRaw().type() == CV_32FC1 || odom.data().depthRaw().type() == CV_16UC1)) + { + if(odom.data().cameraModels().size() > 1) + { + RCLCPP_WARN(get_logger(), "Multi-cameras detected in database but this node cannot send multi-images yet..."); + } + else + { + //depth + if(odom.data().cameraModels().size()) + { + camInfoA.d.resize(5,0); + + camInfoA.p[0] = odom.data().cameraModels()[0].fx(); + camInfoA.k[0] = odom.data().cameraModels()[0].fx(); + camInfoA.p[5] = odom.data().cameraModels()[0].fy(); + camInfoA.k[4] = odom.data().cameraModels()[0].fy(); + camInfoA.p[2] = odom.data().cameraModels()[0].cx(); + camInfoA.k[2] = odom.data().cameraModels()[0].cx(); + camInfoA.p[6] = odom.data().cameraModels()[0].cy(); + camInfoA.k[5] = odom.data().cameraModels()[0].cy(); + + camInfoB = camInfoA; + } + + if(rgbPub_.getTopic().empty()) rgbPub_ = image_transport::create_publisher(this, "rgb/image", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos_).get_rmw_qos_profile()); + if(depthPub_.getTopic().empty()) depthPub_ = image_transport::create_publisher(this, "depth/image", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos_).get_rmw_qos_profile()); + if(!rgbInfoPub_.get()) rgbInfoPub_ = this->create_publisher("rgb/camera_info", 1); + if(!depthInfoPub_.get()) depthInfoPub_ = this->create_publisher("depth/camera_info", 1); + } + } + else if(!odom.data().rightRaw().empty() && odom.data().rightRaw().type() == CV_8U) + { + if(odom.data().stereoCameraModels().size() > 1) + { + RCLCPP_WARN(get_logger(), "Multi-cameras detected in database but this node cannot send multi-images yet..."); + } + else + { + //stereo + if(odom.data().stereoCameraModels()[0].isValidForProjection()) + { + camInfoA.d.resize(8,0); + + camInfoA.p[0] = odom.data().stereoCameraModels()[0].left().fx(); + camInfoA.k[0] = odom.data().stereoCameraModels()[0].left().fx(); + camInfoA.p[5] = odom.data().stereoCameraModels()[0].left().fy(); + camInfoA.k[4] = odom.data().stereoCameraModels()[0].left().fy(); + camInfoA.p[2] = odom.data().stereoCameraModels()[0].left().cx(); + camInfoA.k[2] = odom.data().stereoCameraModels()[0].left().cx(); + camInfoA.p[6] = odom.data().stereoCameraModels()[0].left().cy(); + camInfoA.k[5] = odom.data().stereoCameraModels()[0].left().cy(); + + camInfoB = camInfoA; + camInfoB.p[3] = odom.data().stereoCameraModels()[0].right().Tx(); // Right_Tx = -baseline*fx + } + + if(leftPub_.getTopic().empty()) leftPub_ = image_transport::create_publisher(this, "left/image", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos_).get_rmw_qos_profile()); + if(rightPub_.getTopic().empty()) rightPub_ = image_transport::create_publisher(this, "right/image", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos_).get_rmw_qos_profile()); + if(!leftInfoPub_.get()) leftInfoPub_ = this->create_publisher("left/camera_info", 1); + if(!rightInfoPub_.get()) rightInfoPub_ = this->create_publisher("right/camera_info", 1); + } + + } + else + { + if(imagePub_.getTopic().empty()) imagePub_ = image_transport::create_publisher(this, "image", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos_).get_rmw_qos_profile()); + } + + camInfoA.height = odom.data().imageRaw().rows; + camInfoA.width = odom.data().imageRaw().cols; + camInfoB.height = odom.data().depthOrRightRaw().rows; + camInfoB.width = odom.data().depthOrRightRaw().cols; + + if(!odom.data().laserScanRaw().isEmpty()) + { + if(!scanPub_.get() && odom.data().laserScanRaw().is2d()) + { + scanPub_ = this->create_publisher("scan", 1); + if(odom.data().laserScanRaw().angleIncrement() > 0.0f) + { + RCLCPP_INFO(get_logger(), "Scan will be published."); + } + else + { + RCLCPP_INFO(get_logger(), "Scan will be published with those parameters:"); + RCLCPP_INFO(get_logger(), " scan_angle_min=%f", scanAngleMin_); + RCLCPP_INFO(get_logger(), " scan_angle_max=%f", scanAngleMax_); + RCLCPP_INFO(get_logger(), " scan_angle_increment=%f", scanAngleIncrement_); + RCLCPP_INFO(get_logger(), " scan_range_min=%f", scanRangeMin_); + RCLCPP_INFO(get_logger(), " scan_range_max=%f", scanRangeMax_); + } + } + else if(!scanCloudPub_.get()) + { + scanCloudPub_ = this->create_publisher("scan_cloud", 1); + RCLCPP_INFO(get_logger(), "Scan cloud will be published."); + } + } + + if(!odom.data().globalPose().isNull() && + odom.data().globalPoseCovariance().cols==6 && + odom.data().globalPoseCovariance().rows==6) + { + if(!globalPosePub_.get()) + { + globalPosePub_ = this->create_publisher("global_pose", 1); + RCLCPP_INFO(get_logger(), "Global pose will be published."); + } + } + + if(odom.data().gps().stamp() > 0.0) + { + if(!gpsFixPub_.get()) + { + gpsFixPub_ = this->create_publisher("gps/fix", 1); + RCLCPP_INFO(get_logger(), "GPS will be published."); + } + } + + // publish transforms first + if(tfBroadcaster_.get()) + { + rtabmap::Transform localTransform; + if(odom.data().cameraModels().size() == 1) + { + localTransform = odom.data().cameraModels()[0].localTransform(); + } + else if(odom.data().stereoCameraModels().size() == 1) + { + localTransform = odom.data().stereoCameraModels()[0].left().localTransform(); + } + std::vector transforms; + if(!localTransform.isNull()) + { + geometry_msgs::msg::TransformStamped baseToCamera; + baseToCamera.child_frame_id = cameraFrameId_; + baseToCamera.header.frame_id = frameId_; + baseToCamera.header.stamp = time; + rtabmap_conversions::transformToGeometryMsg(localTransform, baseToCamera.transform); + transforms.push_back(baseToCamera); + } + + if(!odom.pose().isNull()) + { + geometry_msgs::msg::TransformStamped odomToBase; + odomToBase.child_frame_id = frameId_; + odomToBase.header.frame_id = odomFrameId_; + odomToBase.header.stamp = time; + rtabmap_conversions::transformToGeometryMsg(odom.pose(), odomToBase.transform); + transforms.push_back(odomToBase); + } + + if(scanPub_.get() || scanCloudPub_.get()) + { + geometry_msgs::msg::TransformStamped baseToLaserScan; + baseToLaserScan.child_frame_id = scanFrameId_; + baseToLaserScan.header.frame_id = frameId_; + baseToLaserScan.header.stamp = time; + rtabmap_conversions::transformToGeometryMsg(odom.data().laserScanCompressed().localTransform(), baseToLaserScan.transform); + transforms.push_back(baseToLaserScan); + } + + if(!odom.data().groundTruth().isNull()) { + geometry_msgs::msg::TransformStamped worldToBase; + worldToBase.child_frame_id = gtBaseFrameId_; + worldToBase.header.frame_id = gtFrameId_; + worldToBase.header.stamp = time; + rtabmap_conversions::transformToGeometryMsg(odom.data().groundTruth(), worldToBase.transform); + transforms.push_back(worldToBase); + } + tfBroadcaster_->sendTransform(transforms); + } + + if(!odom.pose().isNull()) + { + if(!odometryPub_.get()) odometryPub_ = this->create_publisher("odom", 1); + + if(odometryPub_->get_subscription_count()) + { + nav_msgs::msg::Odometry odomMsg; + odomMsg.child_frame_id = frameId_; + odomMsg.header.frame_id = odomFrameId_; + odomMsg.header.stamp = time; + rtabmap_conversions::transformToPoseMsg(odom.pose(), odomMsg.pose.pose); + UASSERT(odomMsg.pose.covariance.size() == 36 && + odom.covariance().total() == 36 && + odom.covariance().type() == CV_64FC1); + memcpy(odomMsg.pose.covariance.begin(), odom.covariance().data, 36*sizeof(double)); + odometryPub_->publish(odomMsg); + } + } + + // Publish async topics first (so that they can catched by rtabmap before the image topics) + if( globalPosePub_.get() && + globalPosePub_->get_subscription_count() > 0 && + !odom.data().globalPose().isNull() && + odom.data().globalPoseCovariance().cols==6 && + odom.data().globalPoseCovariance().rows==6) + { + geometry_msgs::msg::PoseWithCovarianceStamped msg; + rtabmap_conversions::transformToPoseMsg(odom.data().globalPose(), msg.pose.pose); + memcpy(msg.pose.covariance.data(), odom.data().globalPoseCovariance().data, 36*sizeof(double)); + msg.header.frame_id = frameId_; + msg.header.stamp = time; + globalPosePub_->publish(msg); + } + + if( gpsFixPub_.get() && + gpsFixPub_->get_subscription_count() > 0 && + odom.data().gps().stamp() > 0.0) + { + sensor_msgs::msg::NavSatFix msg; + msg.longitude = odom.data().gps().longitude(); + msg.latitude = odom.data().gps().latitude(); + msg.altitude = odom.data().gps().altitude(); + msg.position_covariance_type = sensor_msgs::msg::NavSatFix::COVARIANCE_TYPE_DIAGONAL_KNOWN; + msg.position_covariance.at(0) = msg.position_covariance.at(4) = msg.position_covariance.at(8)= odom.data().gps().error()* odom.data().gps().error(); + msg.header.frame_id = frameId_; + msg.header.stamp = rtabmap_conversions::timestampToROS(odom.data().gps().stamp()); + gpsFixPub_->publish(msg); + } + + if( (imagePub_.getNumSubscribers()) || + (rgbPub_.getNumSubscribers()) || + (leftPub_.getNumSubscribers())) + { + cv_bridge::CvImage img; + if(odom.data().imageRaw().channels() == 1) + { + img.encoding = sensor_msgs::image_encodings::MONO8; + } + else + { + img.encoding = sensor_msgs::image_encodings::BGR8; + } + img.image = odom.data().imageRaw(); + sensor_msgs::msg::Image imageRosMsg; + img.toImageMsg(imageRosMsg); + imageRosMsg.header.frame_id = cameraFrameId_; + imageRosMsg.header.stamp = time; + + if(imagePub_.getNumSubscribers()) + { + imagePub_.publish(imageRosMsg); + } + if(rgbPub_.getNumSubscribers()) + { + rgbPub_.publish(imageRosMsg); + rgbInfoPub_->publish(camInfoA); + } + if(leftPub_.getNumSubscribers()) + { + leftPub_.publish(imageRosMsg); + leftInfoPub_->publish(camInfoA); + } + } + + if(depthPub_.getNumSubscribers() && !odom.data().depthRaw().empty()) + { + cv_bridge::CvImage img; + if(odom.data().depthRaw().type() == CV_32FC1) + { + img.encoding = sensor_msgs::image_encodings::TYPE_32FC1; + } + else + { + img.encoding = sensor_msgs::image_encodings::TYPE_16UC1; + } + img.image = odom.data().depthRaw(); + sensor_msgs::msg::Image imageRosMsg; + img.toImageMsg(imageRosMsg); + imageRosMsg.header.frame_id = cameraFrameId_; + imageRosMsg.header.stamp = time; + + depthPub_.publish(imageRosMsg); + depthInfoPub_->publish(camInfoB); + } + + if(rightPub_.getNumSubscribers() && !odom.data().rightRaw().empty()) + { + cv_bridge::CvImage img; + if(odom.data().imageRaw().channels() == 1) + { + img.encoding = sensor_msgs::image_encodings::MONO8; + } + else + { + img.encoding = sensor_msgs::image_encodings::BGR8; + } + img.image = odom.data().rightRaw(); + sensor_msgs::msg::Image imageRosMsg; + img.toImageMsg(imageRosMsg); + imageRosMsg.header.frame_id = cameraFrameId_; + imageRosMsg.header.stamp = time; + + rightPub_.publish(imageRosMsg); + rightInfoPub_->publish(camInfoB); + } + + if(!odom.data().laserScanRaw().isEmpty()) + { + if(scanPub_.get() && scanPub_->get_subscription_count() && odom.data().laserScanRaw().is2d()) + { + //inspired from pointcloud_to_laserscan package + sensor_msgs::msg::LaserScan msg; + msg.header.frame_id = scanFrameId_; + msg.header.stamp = time; + + msg.angle_min = scanAngleMin_; + msg.angle_max = scanAngleMax_; + msg.angle_increment = scanAngleIncrement_; + msg.time_increment = 0.0; + msg.scan_time = 0; + msg.range_min = scanRangeMin_; + msg.range_max = scanRangeMax_; + if(odom.data().laserScanRaw().angleIncrement() > 0.0f) + { + msg.angle_min = odom.data().laserScanRaw().angleMin(); + msg.angle_max = odom.data().laserScanRaw().angleMax(); + msg.angle_increment = odom.data().laserScanRaw().angleIncrement(); + msg.range_min = odom.data().laserScanRaw().rangeMin(); + msg.range_max = odom.data().laserScanRaw().rangeMax(); + } + + int rangesSize = std::ceil((msg.angle_max - msg.angle_min) / msg.angle_increment); + msg.ranges.assign(rangesSize, 0.0); + + const cv::Mat & scan = odom.data().laserScanRaw().data(); + for (int i=0; i(0,i); + double range = hypot(ptr[0], ptr[1]); + if (range >= msg.range_min && range <=msg.range_max) + { + double angle = atan2(ptr[1], ptr[0]); + if (angle >= msg.angle_min && angle <= msg.angle_max) + { + int index = (angle - msg.angle_min) / msg.angle_increment; + if (index>=0 && indexpublish(msg); + } + else if(scanCloudPub_.get() && scanCloudPub_->get_subscription_count()) + { + sensor_msgs::msg::PointCloud2 msg; + pcl_conversions::moveFromPCL(*rtabmap::util3d::laserScanToPointCloud2(odom.data().laserScanRaw()), msg); + msg.header.frame_id = scanFrameId_; + msg.header.stamp = time; + scanCloudPub_->publish(msg); + } + } + return true; +} + +} + +#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::DbPlayer) diff --git a/rtabmap_util/src/nodelets/rgbd_split.cpp b/rtabmap_util/src/nodelets/rgbd_split.cpp index d1b85f97..d1ebc4ed 100644 --- a/rtabmap_util/src/nodelets/rgbd_split.cpp +++ b/rtabmap_util/src/nodelets/rgbd_split.cpp @@ -46,8 +46,10 @@ RGBDSplit::RGBDSplit(const rclcpp::NodeOptions & options) : rgbdImageSub_ = create_subscription("rgbd_image", rclcpp::QoS(5).reliability((rmw_qos_reliability_policy_t)qos), std::bind(&RGBDSplit::callback, this, std::placeholders::_1)); - rgbPub_ = image_transport::create_camera_publisher(this, 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(this, std::string(rgbdImageSub_->get_topic_name()) + "/depth", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile()); + rgbPub_ = image_transport::create_publisher(this, std::string(rgbdImageSub_->get_topic_name()) + "/rgb/image", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile()); + depthPub_ = image_transport::create_publisher(this, std::string(rgbdImageSub_->get_topic_name()) + "/depth/image", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile()); + rgbInfoPub_ = this->create_publisher(std::string(rgbdImageSub_->get_topic_name()) + "/rgb/camera_info", 1); + depthInfoPub_ = this->create_publisher(std::string(rgbdImageSub_->get_topic_name()) + "/depth/camera_info", 1); } @@ -73,7 +75,8 @@ void RGBDSplit::callback(const rtabmap_msgs::msg::RGBDImage::SharedPtr input) co cv_bridge::toCvCopy(input->rgb_compressed)->toImageMsg(outputImage); #endif } - rgbPub_.publish(outputImage, outputCameraInfo); + rgbPub_.publish(outputImage); + rgbInfoPub_->publish(outputCameraInfo); } if(depthPub_.getNumSubscribers()) @@ -96,7 +99,8 @@ void RGBDSplit::callback(const rtabmap_msgs::msg::RGBDImage::SharedPtr input) co #endif } outputImage.header = outputCameraInfo.header = input->header; - depthPub_.publish(outputImage, outputCameraInfo); + depthPub_.publish(outputImage); + depthInfoPub_->publish(outputCameraInfo); } } From 46f4443baa696fde23c1a3dbca8310d2d925f9a8 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Tue, 23 Sep 2025 14:10:57 -0700 Subject: [PATCH 19/56] Stale inputs detection (#1358) * Added stale_update_detection parameter to detect if upstream is stale for too long, trigger new map * On too small guess motion, still publish odom topic along tf * Added parameters when guess odom topic is sent * fixed stale disabled logic * Added info when odometry update is skipped. * Making vo publishing the expected output data even if odometry update was skip by not enough motion * fixed skipping odom * refactored tooOldPreviousData to work with guess * renamed stale_update_detection to staleness_factor * cleanup * reverting a change --------- Co-authored-by: mathieu86 --- .../include/rtabmap_odom/OdometryROS.h | 2 + rtabmap_odom/src/OdometryROS.cpp | 120 ++++++++++++------ .../include/rtabmap_slam/CoreWrapper.h | 1 + rtabmap_slam/src/CoreWrapper.cpp | 50 ++++++++ 4 files changed, 132 insertions(+), 41 deletions(-) diff --git a/rtabmap_odom/include/rtabmap_odom/OdometryROS.h b/rtabmap_odom/include/rtabmap_odom/OdometryROS.h index f21262b2..82063cb4 100644 --- a/rtabmap_odom/include/rtabmap_odom/OdometryROS.h +++ b/rtabmap_odom/include/rtabmap_odom/OdometryROS.h @@ -114,6 +114,8 @@ private: double guessMinTranslation_; double guessMinRotation_; double guessMinTime_; + double guessLinearVariance_; + double guessAngularVariance_; bool publishTf_; bool waitForTransform_; double waitForTransformDuration_; diff --git a/rtabmap_odom/src/OdometryROS.cpp b/rtabmap_odom/src/OdometryROS.cpp index 40b2a5e9..9843afeb 100644 --- a/rtabmap_odom/src/OdometryROS.cpp +++ b/rtabmap_odom/src/OdometryROS.cpp @@ -67,6 +67,8 @@ OdometryROS::OdometryROS(bool stereoParams, bool visParams, bool icpParams) : guessMinTranslation_(0.0), guessMinRotation_(0.0), guessMinTime_(0.0), + guessLinearVariance_(0.001), + guessAngularVariance_(0.001), publishTf_(true), waitForTransform_(true), waitForTransformDuration_(0.1), // 100 ms @@ -147,6 +149,8 @@ void OdometryROS::onInit() pnh.param("guess_min_translation", guessMinTranslation_, guessMinTranslation_); pnh.param("guess_min_rotation", guessMinRotation_, guessMinRotation_); pnh.param("guess_min_time", guessMinTime_, guessMinTime_); + pnh.param("guess_linear_variance", guessLinearVariance_, guessLinearVariance_); + pnh.param("guess_angular_variance", guessAngularVariance_, guessAngularVariance_); pnh.param("expected_update_rate", expectedUpdateRate_, expectedUpdateRate_); // expected sensor rate pnh.param("max_update_rate", maxUpdateRate_, maxUpdateRate_); @@ -186,6 +190,8 @@ void OdometryROS::onInit() NODELET_INFO("Odometry: guess_min_translation = %f", guessMinTranslation_); NODELET_INFO("Odometry: guess_min_rotation = %f", guessMinRotation_); NODELET_INFO("Odometry: guess_min_time = %f", guessMinTime_); + NODELET_INFO("Odometry: guess_linear_variance = %f", guessLinearVariance_); + NODELET_INFO("Odometry: guess_angular_variance = %f", guessAngularVariance_); NODELET_INFO("Odometry: expected_update_rate = %f Hz", expectedUpdateRate_); NODELET_INFO("Odometry: max_update_rate = %f Hz", maxUpdateRate_); NODELET_INFO("Odometry: min_update_rate = %f Hz", minUpdateRate_); @@ -672,6 +678,44 @@ void OdometryROS::processData() } } + bool tooOldPreviousData = minUpdateRate_ > 0 && previousStamp_.toSec() > 0 && (header.stamp-previousStamp_).toSec() > 1.0/minUpdateRate_; + if(tooOldPreviousData) + { + NODELET_WARN( "Odometry lost! Odometry will be reset because last update " + "is %fs too old (>%fs, min_update_rate = %f Hz). Previous data stamp is %f while new data stamp is %f.", + (header.stamp - previousStamp_).toSec(), 1.0/minUpdateRate_, minUpdateRate_, previousStamp_.toSec(), header.stamp.toSec()); + + if(!guess_.isNull()) + { + NODELET_WARN( "Odometry automatically reset based on latest guess available from TF (%s->%s, moved %s since got lost)!", + guessFrameId_.c_str(), frameId_.c_str(), guess_.prettyPrint().c_str()); + odometry_->reset(odometry_->getPose() * guess_); + guess_.setNull(); + guessPreviousPose_.setNull(); + } + else + { + // Check TF to see if sensor fusion is used (e.g., the output of robot_localization) + Transform tfPose = rtabmap_conversions::getTransform(odomFrameId_, frameId_, header.stamp, this->tfListener(), this->waitForTransformDuration()); + if(tfPose.isNull()) + { + NODELET_WARN( "Odometry automatically reset to latest computed pose!"); + odometry_->reset(odometry_->getPose()); + } + else + { + NODELET_WARN( "Odometry automatically reset to latest odometry pose available from TF (%s->%s)!", + odomFrameId_.c_str(), frameId_.c_str()); + odometry_->reset(tfPose); + } + } + } + + bool skipOdometryUpdate = false; + + rtabmap::Transform pose; + rtabmap::OdometryInfo info; + rtabmap::Transform guessVelocity; Transform guessCurrentPose; if(!guessFrameId_.empty()) @@ -708,27 +752,22 @@ void OdometryROS::processData() (guessMinTime_ <= 0.0 || (previousStamp_.toSec()>0.0 && (header.stamp-previousStamp_).toSec() < guessMinTime_))) { // Ignore odometry update, we didn't move enough - if(publishTf_) - { - geometry_msgs::TransformStamped correctionMsg; - correctionMsg.child_frame_id = guessFrameId_; - correctionMsg.header.frame_id = odomFrameId_; - correctionMsg.header.stamp = header.stamp; - Transform correction = odometry_->getPose() * guess_ * guessCurrentPose.inverse(); - rtabmap_conversions::transformToGeometryMsg(correction, correctionMsg.transform); - ros::Time time_now = ros::Time::now(); - if(time_now >= previousClockTime_) { - tfBroadcaster_.sendTransform(correctionMsg); - } - else { - ROS_WARN("TF %s->%s is not published because we detected a time jump in the past of %f sec.", - correctionMsg.header.frame_id.c_str(), - correctionMsg.child_frame_id.c_str(), - (previousClockTime_ - time_now).toSec()); - } - } - guessPreviousPose_ = guessCurrentPose; - return; + pose = odometry_->getPose() * guess_; + info.reg.covariance = cv::Mat::zeros(6,6,CV_64FC1); + info.reg.covariance.at(0,0) = guessLinearVariance_; // xx + info.reg.covariance.at(1,1) = guessLinearVariance_; // yy + info.reg.covariance.at(2,2) = guessLinearVariance_; // zz + info.reg.covariance.at(3,3) = guessAngularVariance_; // rr + info.reg.covariance.at(4,4) = guessAngularVariance_; // pp + info.reg.covariance.at(5,5) = guessAngularVariance_; // yawyaw + + //set velocity + double dt = (header.stamp-previousStamp_).toSec(); + UASSERT(dt>0.0); + // use part of guess matching dt + (previousPose.inverse() * guessCurrentPose).getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw); + guessVelocity = rtabmap::Transform(x/dt, y/dt, z/dt, roll/dt, pitch/dt, yaw/dt); + skipOdometryUpdate = true; } } guessPreviousPose_ = guessCurrentPose; @@ -740,23 +779,21 @@ void OdometryROS::processData() } } - bool tooOldPreviousData = minUpdateRate_ > 0 && previousStamp_.toSec() > 0 && (header.stamp-previousStamp_).toSec() > 1.0/minUpdateRate_; - // process data ros::WallTime time = ros::WallTime::now(); - rtabmap::OdometryInfo info; if(!groundTruth.isNull()) { data.setGroundTruth(groundTruth); } - rtabmap::Transform pose; - if(!tooOldPreviousData) + if(!skipOdometryUpdate) { pose = odometry_->process(data, guess_, &info); } if(!pose.isNull()) { - guess_.setNull(); + if(!skipOdometryUpdate) { + guess_.setNull(); + } resetCurrentCount_ = resetCountdown_; //********************* @@ -829,11 +866,16 @@ void OdometryROS::processData() odom.pose.covariance.at(35) = info.reg.covariance.at(5,5)*2; // yawyaw //set velocity - bool setTwist = !odometry_->getVelocityGuess().isNull(); + bool setTwist = !guessVelocity.isNull() || !odometry_->getVelocityGuess().isNull(); if(setTwist) { float x,y,z,roll,pitch,yaw; - odometry_->getVelocityGuess().getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw); + if(skipOdometryUpdate) { + UASSERT(!guessVelocity.isNull()); + guessVelocity.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw); + } else { + odometry_->getVelocityGuess().getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw); + } odom.twist.twist.linear.x = x; odom.twist.twist.linear.y = y; odom.twist.twist.linear.z = z; @@ -878,7 +920,7 @@ void OdometryROS::processData() odomLocalMap_.publish(cloudMsg); } - if(odomLastFrame_.getNumSubscribers()) + if(!skipOdometryUpdate && odomLastFrame_.getNumSubscribers()) { // check which type of Odometry is using if(odometry_->getType() == Odometry::kTypeF2M) // If it's Frame to Map Odometry @@ -1008,20 +1050,14 @@ void OdometryROS::processData() } } - if(pose.isNull() && (resetCurrentCount_ > 0 || tooOldPreviousData)) + if(pose.isNull() && resetCurrentCount_ > 0) { - if(tooOldPreviousData) - { - NODELET_WARN( "Odometry lost! Odometry will be reset because last update " - "is %fs too old (>%fs, min_update_rate = %f Hz). Previous data stamp is %f while new data stamp is %f.", - (header.stamp - previousStamp_).toSec(), 1.0/minUpdateRate_, minUpdateRate_, previousStamp_.toSec(), header.stamp.toSec()); - } - else if(--resetCurrentCount_>0) + if(--resetCurrentCount_>0) { NODELET_WARN( "Odometry lost! Odometry will be reset after next %d consecutive unsuccessful odometry updates...", resetCurrentCount_); } - if(resetCurrentCount_ == 0 || tooOldPreviousData) + if(resetCurrentCount_ == 0) { if(!guess_.isNull()) { @@ -1184,9 +1220,11 @@ void OdometryROS::processData() msg.header.stamp = header.stamp; // use corresponding time stamp to image odomSensorDataCompressedPub_.publish(msg); } - double delay = (ros::Time::now() - header.stamp).toSec(); - if(visParams_) + if(skipOdometryUpdate) { + NODELET_INFO( "Odom: , std dev=%fm|%frad, update time=%fs, delay=%fs", pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at(0,0)), pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at(5,5)), (ros::WallTime::now()-time).toSec(), delay); + } + else if(visParams_) { if(icpParams_) { diff --git a/rtabmap_slam/include/rtabmap_slam/CoreWrapper.h b/rtabmap_slam/include/rtabmap_slam/CoreWrapper.h index 37a8bfc1..2f5d7c72 100644 --- a/rtabmap_slam/include/rtabmap_slam/CoreWrapper.h +++ b/rtabmap_slam/include/rtabmap_slam/CoreWrapper.h @@ -278,6 +278,7 @@ private: double landmarkDefaultLinVariance_; bool waitForTransform_; double waitForTransformDuration_; + double stalenessFactor_; bool useActionForGoal_; bool useSavedMap_; bool genScan_; diff --git a/rtabmap_slam/src/CoreWrapper.cpp b/rtabmap_slam/src/CoreWrapper.cpp index 6a3ab207..0a96897a 100644 --- a/rtabmap_slam/src/CoreWrapper.cpp +++ b/rtabmap_slam/src/CoreWrapper.cpp @@ -104,6 +104,7 @@ CoreWrapper::CoreWrapper() : landmarkDefaultLinVariance_(0.001), waitForTransform_(true), waitForTransformDuration_(0.2), // 200 ms + stalenessFactor_(0.0), useActionForGoal_(false), useSavedMap_(true), genScan_(false), @@ -197,6 +198,7 @@ void CoreWrapper::onInit() pnh.param("pub_loc_pose_only_when_localizing", pubLocPoseOnlyWhenLocalizing_,pubLocPoseOnlyWhenLocalizing_); pnh.param("wait_for_transform", waitForTransform_, waitForTransform_); pnh.param("wait_for_transform_duration", waitForTransformDuration_, waitForTransformDuration_); + pnh.param("staleness_factor", stalenessFactor_, stalenessFactor_); pnh.param("initial_pose", initialPoseStr, initialPoseStr); pnh.param("use_action_for_goal", useActionForGoal_, useActionForGoal_); pnh.param("use_saved_map", useSavedMap_, useSavedMap_); @@ -248,6 +250,14 @@ void CoreWrapper::onInit() NODELET_INFO("rtabmap: tf_tolerance = %f", tfTolerance); NODELET_INFO("rtabmap: odom_sensor_sync = %s", odomSensorSync_?"true":"false"); NODELET_INFO("rtabmap: pub_loc_pose_only_when_localizing = %s", pubLocPoseOnlyWhenLocalizing_?"true":"false"); + NODELET_INFO("rtabmap: wait_for_transform = %s", waitForTransform_?"true":"false"); + NODELET_INFO("rtabmap: wait_for_transform_duration = %f", waitForTransformDuration_); + if(stalenessFactor_!=0.0 && stalenessFactor_ < 1.0) { + NODELET_ERROR("rtabmap: staleness_factor should be 0 (disabled) or >= 1 (value that multiplies the detection update period). Current value is %f, setting it to 0...", + stalenessFactor_); + stalenessFactor_ = 0.0; + } + NODELET_INFO("rtabmap: staleness_factor = %f", stalenessFactor_); bool subscribeStereo = false; pnh.param("subscribe_stereo", subscribeStereo, subscribeStereo); if(subscribeStereo) @@ -1054,6 +1064,26 @@ bool CoreWrapper::odomUpdate(const nav_msgs::OdometryConstPtr & odomMsg, ros::Ti UWARN("Odometry is reset (identity pose or high variance (%f) detected). Increment map id!", MAX(odomMsg->pose.covariance[0], odomMsg->twist.covariance[0])); rtabmap_.triggerNewMap(); covariance_ = cv::Mat(); + } + else if(stalenessFactor_>0.0 && + previousStamp_.toSec() > 0.0 && + rate_>0.0f && + stamp.toSec() - previousStamp_.toSec() > stalenessFactor_/rate_) + { + UWARN("The time difference (%f s) between the new timestamp received (%f) and " + "the previous one (%f) is way over than the expected update period (%s=%f Hz) " + "%f x staleness_factor (%f) = %f s. Triggering a new map! Set staleness_factor to 0 " + "to avoid triggering a new map when this happens.", + stamp.toSec() - previousStamp_.toSec(), + stamp.toSec(), + previousStamp_.toSec(), + Parameters::kRtabmapDetectionRate().c_str(), + rate_, + 1.0f/rate_, + stalenessFactor_, + stalenessFactor_/rate_); + rtabmap_.triggerNewMap(); + covariance_ = cv::Mat(); } lastPoseIntermediate_ = false; @@ -1147,6 +1177,26 @@ bool CoreWrapper::odomTFUpdate(const ros::Time & stamp) rtabmap_.triggerNewMap(); covariance_ = cv::Mat(); } + else if(stalenessFactor_>0.0 && + previousStamp_.toSec() > 0.0 && + rate_>0.0f && + stamp.toSec() - previousStamp_.toSec() > stalenessFactor_/rate_) + { + UWARN("The time difference (%f s) between the new timestamp received (%f) and " + "the previous one (%f) is way over than the expected update period (%s=%f Hz) " + "%f x staleness_factor (%f) = %f s. Triggering a new map! Set staleness_factor to 0 " + "to avoid triggering a new map when this happens.", + stamp.toSec() - previousStamp_.toSec(), + stamp.toSec(), + previousStamp_.toSec(), + Parameters::kRtabmapDetectionRate().c_str(), + rate_, + 1.0f/rate_, + stalenessFactor_, + stalenessFactor_/rate_); + rtabmap_.triggerNewMap(); + covariance_ = cv::Mat(); + } lastPoseIntermediate_ = false; lastPose_ = odom; From 6e84c77d9046ed67fcca9a96567a19e7f8cce405 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Thu, 25 Sep 2025 00:50:00 -0700 Subject: [PATCH 20/56] Fixed regression #1361 added from #1352 --- rtabmap_util/src/MapsManager.cpp | 24 +++++++++++++++++++----- 1 file changed, 19 insertions(+), 5 deletions(-) diff --git a/rtabmap_util/src/MapsManager.cpp b/rtabmap_util/src/MapsManager.cpp index 95228800..28209840 100644 --- a/rtabmap_util/src/MapsManager.cpp +++ b/rtabmap_util/src/MapsManager.cpp @@ -480,16 +480,30 @@ std::map MapsManager::updateMapCaches( filteredPoses.erase(0); } + const std::map emptyNodes; + const std::map * addedNodes = &emptyNodes; bool fullUpdateNeeded = true; #if RTABMAP_VERSION_MAJOR>0 || (RTABMAP_VERSION_MAJOR==0 && RTABMAP_VERSION_MINOR>=23) - fullUpdateNeeded = (updateGrid && occupancyGrid_->fullUpdateNeeded(filteredPoses)) + if(updateGrid) { + fullUpdateNeeded = occupancyGrid_->fullUpdateNeeded(filteredPoses); + addedNodes = &occupancyGrid_->addedNodes(); + } #if defined(WITH_OCTOMAP_MSGS) and defined(RTABMAP_OCTOMAP) - || (updateOctomap && octomap_->fullUpdateNeeded(filteredPoses)) + if(updateOctomap) { + fullUpdateNeeded = fullUpdateNeeded || octomap_->fullUpdateNeeded(filteredPoses); + if(octomap_->addedNodes().size() < addedNodes->size()) { + addedNodes = &octomap_->addedNodes(); + } + } #endif #if defined(WITH_GRID_MAP_ROS) and defined(RTABMAP_GRIDMAP) - || (updateElevation && elevationMap_->fullUpdateNeeded(filteredPoses)) + if(updateElevation) { + fullUpdateNeeded = fullUpdateNeeded || elevationMap_->fullUpdateNeeded(filteredPoses); + if(elevationMap_->addedNodes().size() < addedNodes->size()) { + addedNodes = &elevationMap_->addedNodes(); + } + } #endif - ; if(fullUpdateNeeded) { UINFO("Full occupancy grid map update needed"); } @@ -512,7 +526,7 @@ std::map MapsManager::updateMapCaches( if(!iter->second.isNull()) { rtabmap::SensorData data; - if(iter->first == 0 || (fullUpdateNeeded && !uContains(localMaps_.localGrids(), iter->first))) + if(iter->first == 0 || (addedNodes->find(iter->first) == addedNodes->end() && !uContains(localMaps_.localGrids(), iter->first))) { UDEBUG("Data required for %d", iter->first); std::map::const_iterator findIter = signatures.find(iter->first); From 4b0067d875575d5c9f9aa26c2b800528818ccfd2 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Thu, 25 Sep 2025 08:10:58 +0000 Subject: [PATCH 21/56] Fixed empty /map topic (regression from #1352) --- rtabmap_util/src/MapsManager.cpp | 26 ++++++++++++++++++++++---- 1 file changed, 22 insertions(+), 4 deletions(-) diff --git a/rtabmap_util/src/MapsManager.cpp b/rtabmap_util/src/MapsManager.cpp index 59db3fa6..ffd42926 100644 --- a/rtabmap_util/src/MapsManager.cpp +++ b/rtabmap_util/src/MapsManager.cpp @@ -551,14 +551,30 @@ std::map MapsManager::updateMapCaches( filteredPoses.erase(0); } + + const std::map emptyNodes; + const std::map * addedNodes = &emptyNodes; bool fullUpdateNeeded = true; #if RTABMAP_VERSION_MAJOR>0 || (RTABMAP_VERSION_MAJOR==0 && RTABMAP_VERSION_MINOR>=23) - fullUpdateNeeded = (updateGrid && occupancyGrid_->fullUpdateNeeded(filteredPoses)) + if(updateGrid) { + fullUpdateNeeded = occupancyGrid_->fullUpdateNeeded(filteredPoses); + addedNodes = &occupancyGrid_->addedNodes(); + } #if defined(WITH_OCTOMAP_MSGS) and defined(RTABMAP_OCTOMAP) - || (updateOctomap && octomap_->fullUpdateNeeded(filteredPoses)) + if(updateOctomap) { + fullUpdateNeeded = fullUpdateNeeded || octomap_->fullUpdateNeeded(filteredPoses); + if(octomap_->addedNodes().size() < addedNodes->size()) { + addedNodes = &octomap_->addedNodes(); + } + } #endif #if defined(WITH_GRID_MAP_ROS) and defined(RTABMAP_GRIDMAP) - || (updateElevation && elevationMap_->fullUpdateNeeded(filteredPoses)) + if(updateElevation) { + fullUpdateNeeded = fullUpdateNeeded || elevationMap_->fullUpdateNeeded(filteredPoses); + if(elevationMap_->addedNodes().size() < addedNodes->size()) { + addedNodes = &elevationMap_->addedNodes(); + } + } #endif ; if(fullUpdateNeeded) { @@ -583,7 +599,9 @@ std::map MapsManager::updateMapCaches( if(!iter->second.isNull()) { rtabmap::SensorData data; - if(iter->first == 0 || (fullUpdateNeeded && !uContains(localMaps_.localGrids(), iter->first))) + if(iter->first == 0 || ( + (fullUpdateNeeded || addedNodes->find(iter->first) == addedNodes->end()) && + !uContains(localMaps_.localGrids(), iter->first))) { ROS_DEBUG("Data required for %d", iter->first); std::map::const_iterator findIter = signatures.find(iter->first); From 1c5df12674f867607660caa42d12980e32be3a4c Mon Sep 17 00:00:00 2001 From: matlabbe Date: Thu, 25 Sep 2025 18:46:35 -0700 Subject: [PATCH 22/56] Disabling missing grid_map_ros on kilted to pass CI --- .github/workflows/ros2.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/ros2.yml b/.github/workflows/ros2.yml index a15f79e0..2720381d 100644 --- a/.github/workflows/ros2.yml +++ b/.github/workflows/ros2.yml @@ -23,7 +23,7 @@ jobs: - ros_distro: jazzy skip_keys: '' - ros_distro: kilted - skip_keys: '' + skip_keys: 'grid_map_ros' - ros_distro: rolling skip_keys: 'nav2_bringup nav2_msgs velodyne' fail-fast: false From bc5f0a7146c76856ae56e49e9b4ef1c185b24aad Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sun, 28 Sep 2025 13:37:07 -0700 Subject: [PATCH 23/56] Data player multicam support (#1365) * Data player multicam support * viewer: fixed pause button --- rtabmap_conversions/src/MsgConversion.cpp | 17 +- .../include/rtabmap_util/db_player.hpp | 31 +- rtabmap_util/src/nodelets/db_player.cpp | 659 +++++++++++------- rtabmap_util/src/nodelets/rgbd_split.cpp | 17 +- rtabmap_viz/CMakeLists.txt | 11 + .../include/rtabmap_viz/rgbd_image_viewer.hpp | 84 +++ rtabmap_viz/src/RGBDImageViewerNode.cpp | 83 +++ rtabmap_viz/src/rgbd_image_viewer.cpp | 171 +++++ 8 files changed, 804 insertions(+), 269 deletions(-) create mode 100644 rtabmap_viz/include/rtabmap_viz/rgbd_image_viewer.hpp create mode 100644 rtabmap_viz/src/RGBDImageViewerNode.cpp create mode 100644 rtabmap_viz/src/rgbd_image_viewer.cpp diff --git a/rtabmap_conversions/src/MsgConversion.cpp b/rtabmap_conversions/src/MsgConversion.cpp index 32382ecf..6eaf84ea 100644 --- a/rtabmap_conversions/src/MsgConversion.cpp +++ b/rtabmap_conversions/src/MsgConversion.cpp @@ -869,7 +869,12 @@ void cameraModelToROS( sensor_msgs::msg::CameraInfo & camInfo) { UASSERT(model.K_raw().empty() || model.K_raw().total() == 9); - if(model.K_raw().empty()) + UASSERT(model.P().empty() || model.P().total() == 12); + if(!model.P().empty()) + { + model.P().colRange(0,3).copyTo(cv::Mat(3,3,CV_64FC1, camInfo.k.data())); + } + else if(model.K_raw().empty()) { memset(camInfo.k.data(), 0.0, 9*sizeof(double)); } @@ -878,7 +883,12 @@ void cameraModelToROS( memcpy(camInfo.k.data(), model.K_raw().data, 9*sizeof(double)); } - if(model.D_raw().total() == 6) + if(!model.P().empty()) { + camInfo.d = std::vector(model.D().cols); + memcpy(camInfo.d.data(), model.D().data, model.D().cols*sizeof(double)); + camInfo.distortion_model = "plumb_bob"; + } + else if(model.D_raw().total() == 6) { camInfo.d = std::vector(4); camInfo.d[0] = model.D_raw().at(0,0); @@ -902,7 +912,7 @@ void cameraModelToROS( } UASSERT(model.R().empty() || model.R().total() == 9); - if(model.R().empty()) + if(model.R().empty() || countNonZero(model.R()) == 0) { cv::Mat eye = cv::Mat::eye(3,3,CV_64FC1); memcpy(camInfo.r.data(), eye.data, 9*sizeof(double)); @@ -912,7 +922,6 @@ void cameraModelToROS( memcpy(camInfo.r.data(), model.R().data, 9*sizeof(double)); } - UASSERT(model.P().empty() || model.P().total() == 12); if(model.P().empty()) { memset(camInfo.p.data(), 0.0, 12*sizeof(double)); diff --git a/rtabmap_util/include/rtabmap_util/db_player.hpp b/rtabmap_util/include/rtabmap_util/db_player.hpp index ca879bdd..3dc5311d 100644 --- a/rtabmap_util/include/rtabmap_util/db_player.hpp +++ b/rtabmap_util/include/rtabmap_util/db_player.hpp @@ -30,6 +30,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include +#include #include #include #include @@ -46,7 +47,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include +#include #include +#include namespace rtabmap_util { @@ -57,13 +60,15 @@ public: RTABMAP_UTIL_PUBLIC explicit DbPlayer(const rclcpp::NodeOptions & options); virtual ~DbPlayer(); - bool publishNextFrame(); - bool isPaused() const {return paused_;} - void setPaused(bool enabled) {paused_ = enabled;} + bool publishNextFrame(); + bool isPaused() const {return paused_;} + void setPaused(bool enabled) {paused_ = enabled;} private: - void pauseCallback(const std::shared_ptr, const std::shared_ptr, std::shared_ptr); + void pauseCallback(const std::shared_ptr, const std::shared_ptr, std::shared_ptr); void resumeCallback(const std::shared_ptr, const std::shared_ptr, std::shared_ptr); + void initializePublishers(const rtabmap::OdometryEvent & odom); + bool cvImageToROS(const cv::Mat & image, sensor_msgs::msg::Image & rosImage); private: bool paused_; @@ -74,7 +79,15 @@ private: std::string scanFrameId_; std::string gtFrameId_; std::string gtBaseFrameId_; + std::string imuFrameId_; int qos_; + int qosCameraInfo_; + int qosOdom_; + int qosScan_; + int qosScanCloud_; + int qosGlobalPose_; + int qosGps_; + int qosImu_; double scanAngleMin_; double scanAngleMax_; double scanAngleIncrement_; @@ -89,15 +102,17 @@ private: image_transport::Publisher depthPub_; image_transport::Publisher leftPub_; image_transport::Publisher rightPub_; - rclcpp::Publisher::SharedPtr rgbInfoPub_; - rclcpp::Publisher::SharedPtr depthInfoPub_; - rclcpp::Publisher::SharedPtr leftInfoPub_; - rclcpp::Publisher::SharedPtr rightInfoPub_; + rclcpp::Publisher::SharedPtr rgbInfoPub_; + rclcpp::Publisher::SharedPtr depthInfoPub_; + rclcpp::Publisher::SharedPtr leftInfoPub_; + rclcpp::Publisher::SharedPtr rightInfoPub_; + std::vector::SharedPtr> rgbdImagePubs_; rclcpp::Publisher::SharedPtr odometryPub_; rclcpp::Publisher::SharedPtr scanPub_; rclcpp::Publisher::SharedPtr scanCloudPub_; rclcpp::Publisher::SharedPtr globalPosePub_; rclcpp::Publisher::SharedPtr gpsFixPub_; + rclcpp::Publisher::SharedPtr imuPub_; rclcpp::Publisher::SharedPtr clockPub_; std::shared_ptr tfBroadcaster_; }; diff --git a/rtabmap_util/src/nodelets/db_player.cpp b/rtabmap_util/src/nodelets/db_player.cpp index 54d3cb2d..8771933f 100644 --- a/rtabmap_util/src/nodelets/db_player.cpp +++ b/rtabmap_util/src/nodelets/db_player.cpp @@ -53,7 +53,7 @@ namespace rtabmap_util { DbPlayer::DbPlayer(const rclcpp::NodeOptions & options) : - rclcpp::Node("db_player", options), + rclcpp::Node("db_player", options), paused_(false), frameId_("base_link"), odomFrameId_("odom"), @@ -61,93 +61,110 @@ DbPlayer::DbPlayer(const rclcpp::NodeOptions & options) : scanFrameId_("base_laser_link"), gtFrameId_("world"), gtBaseFrameId_("base_link_gt"), + imuFrameId_("imu_link"), qos_(RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT) { //ULogger::setType(ULogger::kTypeConsole); - //ULogger::setLevel(ULogger::kDebug); - //ULogger::setEventLevel(ULogger::kWarning); + //ULogger::setLevel(ULogger::kDebug); + //ULogger::setEventLevel(ULogger::kWarning); //parse input arguments bool publishClock = false; publishClock = this->declare_parameter("publish_clock", publishClock); - std::vector tmpList = get_node_options().arguments(); - std::vector argList; - for(unsigned int i=0; i tmpList = get_node_options().arguments(); + std::vector argList; + for(unsigned int i=0; ideclare_parameter("frame_id", frameId_); odomFrameId_ = this->declare_parameter("odom_frame_id", odomFrameId_); cameraFrameId_ = this->declare_parameter("camera_frame_id", cameraFrameId_); scanFrameId_ = this->declare_parameter("scan_frame_id", scanFrameId_); gtFrameId_ = this->declare_parameter("ground_truth_frame_id", gtFrameId_); gtBaseFrameId_ = this->declare_parameter("ground_truth_base_frame_id", gtBaseFrameId_); + imuFrameId_ = this->declare_parameter("imu_frame_id", imuFrameId_); rate = this->declare_parameter("rate", rate); // Ratio of the database stamps databasePath = this->declare_parameter("database", databasePath); publishTf = this->declare_parameter("publish_tf", publishTf); ignoreOdom = this->declare_parameter("ignore_odom", ignoreOdom); startId = this->declare_parameter("start_id", startId); - qos_ = this->declare_parameter("qos", qos_); + qos_ = this->declare_parameter("qos", qos_); + qosCameraInfo_ = this->declare_parameter("qos_camera_info", qos_); + qosOdom_ = this->declare_parameter("qos_odom", qos_); + qosScan_ = this->declare_parameter("qos_scan", qos_); + qosScanCloud_ = this->declare_parameter("qos_scan_cloud", qos_); + qosGlobalPose_ = this->declare_parameter("qos_global_pose", qos_); + qosGps_ = this->declare_parameter("qos_gps", qos_); + qosImu_ = this->declare_parameter("qos_imu", qos_); // A general 360 lidar with 0.5 deg increment - scanAngleMin_ = this->declare_parameter("scan_angle_min", -M_PI); + scanAngleMin_ = this->declare_parameter("scan_angle_min", -M_PI); scanAngleMax_ = this->declare_parameter("scan_angle_max", M_PI); scanAngleIncrement_ = this->declare_parameter("scan_angle_increment", M_PI / 720.0); scanRangeMin_ = this->declare_parameter("scan_range_min", 0.0); scanRangeMax_ = this->declare_parameter("scan_range_max", 60); RCLCPP_INFO(get_logger(), "frame_id = %s", frameId_.c_str()); - RCLCPP_INFO(get_logger(), "odom_frame_id = %s", odomFrameId_.c_str()); - RCLCPP_INFO(get_logger(), "camera_frame_id = %s", cameraFrameId_.c_str()); - RCLCPP_INFO(get_logger(), "scan_frame_id = %s", scanFrameId_.c_str()); - RCLCPP_INFO(get_logger(), "ground_truth_frame_id = %s", gtFrameId_.c_str()); - RCLCPP_INFO(get_logger(), "rate (factor) = %f", rate); - RCLCPP_INFO(get_logger(), "publish_tf = %s", publishTf?"true":"false"); - RCLCPP_INFO(get_logger(), "start_id = %d", startId); - RCLCPP_INFO(get_logger(), "Publish clock (--clock): %s", publishClock?"true":"false"); + RCLCPP_INFO(get_logger(), "odom_frame_id = %s", odomFrameId_.c_str()); + RCLCPP_INFO(get_logger(), "camera_frame_id = %s", cameraFrameId_.c_str()); + RCLCPP_INFO(get_logger(), "scan_frame_id = %s", scanFrameId_.c_str()); + RCLCPP_INFO(get_logger(), "ground_truth_frame_id = %s", gtFrameId_.c_str()); + RCLCPP_INFO(get_logger(), "imu_frame_id = %s", imuFrameId_.c_str()); + RCLCPP_INFO(get_logger(), "rate (factor) = %f", rate); + RCLCPP_INFO(get_logger(), "publish_tf = %s", publishTf?"true":"false"); + RCLCPP_INFO(get_logger(), "start_id = %d", startId); + RCLCPP_INFO(get_logger(), "Publish clock (--clock): %s", publishClock?"true":"false"); RCLCPP_INFO(get_logger(), "qos = %d", qos_); + RCLCPP_INFO(get_logger(), " qos_camera_info = %d", qosCameraInfo_); + RCLCPP_INFO(get_logger(), " qos_odom = %d", qosOdom_); + RCLCPP_INFO(get_logger(), " qos_scan = %d", qosScan_); + RCLCPP_INFO(get_logger(), " qos_scan_cloud = %d", qosScanCloud_); + RCLCPP_INFO(get_logger(), " qos_global_pose = %d", qosGlobalPose_); + RCLCPP_INFO(get_logger(), " qos_gps = %d", qosGps_); + RCLCPP_INFO(get_logger(), " qos_imu = %d", qosImu_); if(databasePath.empty()) - { - RCLCPP_ERROR(get_logger(), "Parameter \"database\" must be set (path to a RTAB-Map database)."); - exit(-1); - } + { + RCLCPP_ERROR(get_logger(), "Parameter \"database\" must be set (path to a RTAB-Map database)."); + exit(-1); + } databasePath = uReplaceChar(databasePath, '~', UDirectory::homeDir()); - if(databasePath.size() && databasePath.at(0) != '/') - { - databasePath = UDirectory::currentDir(true) + databasePath; - } - RCLCPP_INFO(get_logger(), "database = %s", databasePath.c_str()); + if(databasePath.size() && databasePath.at(0) != '/') + { + databasePath = UDirectory::currentDir(true) + databasePath; + } + RCLCPP_INFO(get_logger(), "database = %s", databasePath.c_str()); - reader_.reset(new rtabmap::DBReader(databasePath, -rate, ignoreOdom, false, false, startId)); - if(!reader_->init()) - { - RCLCPP_ERROR(get_logger(), "Cannot open database \"%s\".", databasePath.c_str()); - exit(-1); - } + reader_.reset(new rtabmap::DBReader(databasePath, -rate, ignoreOdom, false, false, startId)); + if(!reader_->init()) + { + RCLCPP_ERROR(get_logger(), "Cannot open database \"%s\".", databasePath.c_str()); + exit(-1); + } const std::string servicePrefix = get_name() + std::string("/"); pauseSrv_ = this->create_service(servicePrefix + "pause", std::bind(&DbPlayer::pauseCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - resumeSrv_ = this->create_service(servicePrefix + "resume", std::bind(&DbPlayer::resumeCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + resumeSrv_ = this->create_service(servicePrefix + "resume", std::bind(&DbPlayer::resumeCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); if(publishTf) { tfBroadcaster_ = std::make_shared(this); } if(publishClock) - { - clockPub_ = this->create_publisher("/clock", 1); - } + { + clockPub_ = this->create_publisher("/clock", 1); + } } DbPlayer::~DbPlayer(){} @@ -158,14 +175,14 @@ void DbPlayer::pauseCallback( std::shared_ptr) { if(paused_) - { - RCLCPP_WARN(get_logger(), "Already paused!"); - } - else - { - paused_ = true; - RCLCPP_INFO(get_logger(), "paused!"); - } + { + RCLCPP_WARN(get_logger(), "Already paused!"); + } + else + { + paused_ = true; + RCLCPP_INFO(get_logger(), "paused!"); + } } void DbPlayer::resumeCallback( const std::shared_ptr, @@ -173,140 +190,105 @@ void DbPlayer::resumeCallback( std::shared_ptr) { if(!paused_) - { - RCLCPP_WARN(get_logger(), "Already running!"); - } - else - { - paused_ = false; - RCLCPP_INFO(get_logger(), "resumed!"); - } + { + RCLCPP_WARN(get_logger(), "Already running!"); + } + else + { + paused_ = false; + RCLCPP_INFO(get_logger(), "resumed!"); + } } -bool DbPlayer::publishNextFrame() +void DbPlayer::initializePublishers(const rtabmap::OdometryEvent & odom) { - rtabmap::SensorCaptureInfo cameraInfo; - rtabmap::SensorData data = reader_->takeImage(&cameraInfo); - rtabmap::OdometryInfo odomInfo; - odomInfo.reg.covariance = cameraInfo.odomCovariance; - rtabmap::OdometryEvent odom(data, cameraInfo.odomPose, odomInfo); - if(!odom.data().id()) - { - return false; - } - - RCLCPP_INFO(get_logger(), "Reading sensor data %d...", odom.data().id()); - - rclcpp::Time time = rtabmap_conversions::timestampToROS(odom.data().stamp()); - - if(clockPub_.get()) - { - rosgraph_msgs::msg::Clock msg; - msg.clock = time; - clockPub_->publish(msg); - } - - sensor_msgs::msg::CameraInfo camInfoA; //rgb or left - sensor_msgs::msg::CameraInfo camInfoB; //depth or right - - camInfoA.k.fill(0); - camInfoA.k[0] = camInfoA.k[4] = camInfoA.k[8] = 1; - camInfoA.r.fill(0); - camInfoA.r[0] = camInfoA.r[4] = camInfoA.r[8] = 1; - camInfoA.p.fill(0); - camInfoA.p[10] = 1; - - camInfoA.header.frame_id = cameraFrameId_; - camInfoA.header.stamp = time; - - camInfoB = camInfoA; - if(!odom.data().depthRaw().empty() && (odom.data().depthRaw().type() == CV_32FC1 || odom.data().depthRaw().type() == CV_16UC1)) { if(odom.data().cameraModels().size() > 1) { - RCLCPP_WARN(get_logger(), "Multi-cameras detected in database but this node cannot send multi-images yet..."); + if(rgbdImagePubs_.empty()) { + for(size_t i=0;icreate_publisher(uFormat("rgbd_image%ld", i), rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos_))); + RCLCPP_INFO(get_logger(), "RGB-D image \"%s\" will be published.", rgbdImagePubs_.back()->get_topic_name()); + } + } + else { + UASSERT_MSG(rgbdImagePubs_.size() == odom.data().cameraModels().size(), uFormat("%ld versus %ld", rgbdImagePubs_.size(), odom.data().cameraModels().size()).c_str()); + } } else { - //depth - if(odom.data().cameraModels().size()) - { - camInfoA.d.resize(5,0); - - camInfoA.p[0] = odom.data().cameraModels()[0].fx(); - camInfoA.k[0] = odom.data().cameraModels()[0].fx(); - camInfoA.p[5] = odom.data().cameraModels()[0].fy(); - camInfoA.k[4] = odom.data().cameraModels()[0].fy(); - camInfoA.p[2] = odom.data().cameraModels()[0].cx(); - camInfoA.k[2] = odom.data().cameraModels()[0].cx(); - camInfoA.p[6] = odom.data().cameraModels()[0].cy(); - camInfoA.k[5] = odom.data().cameraModels()[0].cy(); - - camInfoB = camInfoA; + if(rgbPub_.getTopic().empty()) { + rgbPub_ = image_transport::create_publisher(this, "rgb/image", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos_).get_rmw_qos_profile()); + RCLCPP_INFO(get_logger(), "Gray/RGB image \"%s\" will be published.", rgbPub_.getTopic().c_str()); + } + if(!rgbInfoPub_.get()) { + rgbInfoPub_ = this->create_publisher("rgb/camera_info", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosCameraInfo_)); + RCLCPP_INFO(get_logger(), "Gray/RGB calibration \"%s\" will be published.", rgbInfoPub_->get_topic_name()); + } + if(depthPub_.getTopic().empty()) { + depthPub_ = image_transport::create_publisher(this, "depth/image", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos_).get_rmw_qos_profile()); + RCLCPP_INFO(get_logger(), "Depth image \"%s\" will be published.", depthPub_.getTopic().c_str()); + } + if(!depthInfoPub_.get()) { + depthInfoPub_ = this->create_publisher("depth/camera_info", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosCameraInfo_)); + RCLCPP_INFO(get_logger(), "Depth calibration \"%s\" will be published.", depthInfoPub_->get_topic_name()); } - - if(rgbPub_.getTopic().empty()) rgbPub_ = image_transport::create_publisher(this, "rgb/image", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos_).get_rmw_qos_profile()); - if(depthPub_.getTopic().empty()) depthPub_ = image_transport::create_publisher(this, "depth/image", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos_).get_rmw_qos_profile()); - if(!rgbInfoPub_.get()) rgbInfoPub_ = this->create_publisher("rgb/camera_info", 1); - if(!depthInfoPub_.get()) depthInfoPub_ = this->create_publisher("depth/camera_info", 1); } } - else if(!odom.data().rightRaw().empty() && odom.data().rightRaw().type() == CV_8U) + else if(!odom.data().rightRaw().empty() && (odom.data().rightRaw().type() == CV_8U || odom.data().rightRaw().type() == CV_8UC3)) { if(odom.data().stereoCameraModels().size() > 1) { - RCLCPP_WARN(get_logger(), "Multi-cameras detected in database but this node cannot send multi-images yet..."); + if(rgbdImagePubs_.empty()) { + for(size_t i=0;icreate_publisher(uFormat("stereo_image%ld", i), rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos_))); + RCLCPP_INFO(get_logger(), "Stereo image \"%s\" will be published.", rgbdImagePubs_.back()->get_topic_name()); + } + } + else { + UASSERT_MSG(rgbdImagePubs_.size() == odom.data().stereoCameraModels().size(), uFormat("%ld versus %ld", rgbdImagePubs_.size(), odom.data().stereoCameraModels().size()).c_str()); + } } else { - //stereo - if(odom.data().stereoCameraModels()[0].isValidForProjection()) - { - camInfoA.d.resize(8,0); - - camInfoA.p[0] = odom.data().stereoCameraModels()[0].left().fx(); - camInfoA.k[0] = odom.data().stereoCameraModels()[0].left().fx(); - camInfoA.p[5] = odom.data().stereoCameraModels()[0].left().fy(); - camInfoA.k[4] = odom.data().stereoCameraModels()[0].left().fy(); - camInfoA.p[2] = odom.data().stereoCameraModels()[0].left().cx(); - camInfoA.k[2] = odom.data().stereoCameraModels()[0].left().cx(); - camInfoA.p[6] = odom.data().stereoCameraModels()[0].left().cy(); - camInfoA.k[5] = odom.data().stereoCameraModels()[0].left().cy(); - - camInfoB = camInfoA; - camInfoB.p[3] = odom.data().stereoCameraModels()[0].right().Tx(); // Right_Tx = -baseline*fx + if(leftPub_.getTopic().empty()) { + leftPub_ = image_transport::create_publisher(this, "left/image", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos_).get_rmw_qos_profile()); + RCLCPP_INFO(get_logger(), "Left image \"%s\" will be published.", leftPub_.getTopic().c_str()); + } + if(!leftInfoPub_.get()) { + leftInfoPub_ = this->create_publisher("left/camera_info", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosCameraInfo_)); + RCLCPP_INFO(get_logger(), "Left calibration \"%s\" will be published.", leftInfoPub_->get_topic_name()); + } + if(rightPub_.getTopic().empty()) { + rightPub_ = image_transport::create_publisher(this, "right/image", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos_).get_rmw_qos_profile()); + RCLCPP_INFO(get_logger(), "Right image \"%s\" will be published.", rightPub_.getTopic().c_str()); + } + if(!rightInfoPub_.get()) { + rightInfoPub_ = this->create_publisher("right/camera_info", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosCameraInfo_)); + RCLCPP_INFO(get_logger(), "Right calibration \"%s\" will be published.", rightInfoPub_->get_topic_name()); } - - if(leftPub_.getTopic().empty()) leftPub_ = image_transport::create_publisher(this, "left/image", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos_).get_rmw_qos_profile()); - if(rightPub_.getTopic().empty()) rightPub_ = image_transport::create_publisher(this, "right/image", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos_).get_rmw_qos_profile()); - if(!leftInfoPub_.get()) leftInfoPub_ = this->create_publisher("left/camera_info", 1); - if(!rightInfoPub_.get()) rightInfoPub_ = this->create_publisher("right/camera_info", 1); } } - else + else if(imagePub_.getTopic().empty()) { - if(imagePub_.getTopic().empty()) imagePub_ = image_transport::create_publisher(this, "image", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos_).get_rmw_qos_profile()); + imagePub_ = image_transport::create_publisher(this, "image", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos_).get_rmw_qos_profile()); + RCLCPP_INFO(get_logger(), "Image \"%s\" without calibration will be published.", imagePub_.getTopic().c_str()); } - camInfoA.height = odom.data().imageRaw().rows; - camInfoA.width = odom.data().imageRaw().cols; - camInfoB.height = odom.data().depthOrRightRaw().rows; - camInfoB.width = odom.data().depthOrRightRaw().cols; - if(!odom.data().laserScanRaw().isEmpty()) { if(!scanPub_.get() && odom.data().laserScanRaw().is2d()) { - scanPub_ = this->create_publisher("scan", 1); + scanPub_ = this->create_publisher("scan", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosScan_)); if(odom.data().laserScanRaw().angleIncrement() > 0.0f) { - RCLCPP_INFO(get_logger(), "Scan will be published."); + RCLCPP_INFO(get_logger(), "LaserScan \"%s\" will be published.", scanPub_->get_topic_name()); } else { - RCLCPP_INFO(get_logger(), "Scan will be published with those parameters:"); + RCLCPP_INFO(get_logger(), "LaserScan \"%s\" will be published with those parameters:", scanPub_->get_topic_name()); RCLCPP_INFO(get_logger(), " scan_angle_min=%f", scanAngleMin_); RCLCPP_INFO(get_logger(), " scan_angle_max=%f", scanAngleMax_); RCLCPP_INFO(get_logger(), " scan_angle_increment=%f", scanAngleIncrement_); @@ -316,8 +298,8 @@ bool DbPlayer::publishNextFrame() } else if(!scanCloudPub_.get()) { - scanCloudPub_ = this->create_publisher("scan_cloud", 1); - RCLCPP_INFO(get_logger(), "Scan cloud will be published."); + scanCloudPub_ = this->create_publisher("scan_cloud", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosScanCloud_)); + RCLCPP_INFO(get_logger(), "PointCloud2 \"%s\" will be published.", scanCloudPub_->get_topic_name()); } } @@ -327,41 +309,121 @@ bool DbPlayer::publishNextFrame() { if(!globalPosePub_.get()) { - globalPosePub_ = this->create_publisher("global_pose", 1); - RCLCPP_INFO(get_logger(), "Global pose will be published."); + globalPosePub_ = this->create_publisher("global_pose", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosGlobalPose_)); + RCLCPP_INFO(get_logger(), "Global pose \"%s\" will be published.", globalPosePub_->get_topic_name()); } } - if(odom.data().gps().stamp() > 0.0) + if(!gpsFixPub_.get() && odom.data().gps().stamp() > 0.0) { - if(!gpsFixPub_.get()) - { - gpsFixPub_ = this->create_publisher("gps/fix", 1); - RCLCPP_INFO(get_logger(), "GPS will be published."); - } + gpsFixPub_ = this->create_publisher("gps/fix", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosGps_)); + RCLCPP_INFO(get_logger(), "GPS \"%s\" will be published.", gpsFixPub_->get_topic_name()); + } + + if(!odometryPub_.get() && !odom.pose().isNull()) + { + odometryPub_ = this->create_publisher("odom", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosOdom_)); + RCLCPP_INFO(get_logger(), "Odometry \"%s\" will be published.", odometryPub_->get_topic_name()); + } + if(!imuPub_.get() && !odom.data().imu().empty()) + { + imuPub_ = this->create_publisher("imu", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosImu_)); + RCLCPP_INFO(get_logger(), "IMU \"%s\" will be published.", imuPub_->get_topic_name()); + } +} + +bool DbPlayer::cvImageToROS(const cv::Mat & image, sensor_msgs::msg::Image & rosImage) +{ + cv_bridge::CvImage img; + if(image.type() == CV_32FC1) + { + img.encoding = sensor_msgs::image_encodings::TYPE_32FC1; + } + else if(image.type() == CV_16UC1) + { + img.encoding = sensor_msgs::image_encodings::TYPE_16UC1; + } + else if(image.type() == CV_8UC1) + { + img.encoding = sensor_msgs::image_encodings::MONO8; + } + else if(image.type() == CV_8UC3) + { + img.encoding = sensor_msgs::image_encodings::BGR8; + } + else { + RCLCPP_ERROR(get_logger(), "Unsupported image format: cv type = %d", image.type()); + return false; + } + img.image = image; + img.toImageMsg(rosImage); + return true; +} + +bool DbPlayer::publishNextFrame() +{ + rtabmap::SensorCaptureInfo cameraInfo; + rtabmap::SensorData data = reader_->takeImage(&cameraInfo); + rtabmap::OdometryInfo odomInfo; + odomInfo.reg.covariance = cameraInfo.odomCovariance; + rtabmap::OdometryEvent odom(data, cameraInfo.odomPose, odomInfo); + if(!odom.data().id()) + { + return false; + } + + RCLCPP_INFO(get_logger(), "Reading sensor data %d...", odom.data().id()); + + rclcpp::Time time = rtabmap_conversions::timestampToROS(odom.data().stamp()); + + /////////////////////// + // Initialize publishers based on data in the database + /////////////////////// + initializePublishers(odom); // called everytime in case some data like global pose, imu, odometry was not available at the beggining. + + + /////////////////////// + // Publish topics + /////////////////////// + + if(clockPub_.get()) + { + rosgraph_msgs::msg::Clock msg; + msg.clock = time; + clockPub_->publish(msg); } // publish transforms first if(tfBroadcaster_.get()) { - rtabmap::Transform localTransform; - if(odom.data().cameraModels().size() == 1) - { - localTransform = odom.data().cameraModels()[0].localTransform(); - } - else if(odom.data().stereoCameraModels().size() == 1) - { - localTransform = odom.data().stereoCameraModels()[0].left().localTransform(); - } std::vector transforms; - if(!localTransform.isNull()) + const std::vector * models = &odom.data().cameraModels(); + std::vector stereoModels; + bool stereo = false; + if(odom.data().stereoCameraModels().size()) { - geometry_msgs::msg::TransformStamped baseToCamera; - baseToCamera.child_frame_id = cameraFrameId_; - baseToCamera.header.frame_id = frameId_; - baseToCamera.header.stamp = time; - rtabmap_conversions::transformToGeometryMsg(localTransform, baseToCamera.transform); - transforms.push_back(baseToCamera); + for(const auto & cam: odom.data().stereoCameraModels()) { + stereoModels.push_back(cam.left()); + stereoModels.push_back(cam.right()); + } + models = &stereoModels; + stereo = true; + } + int index = 0; + for(const auto & cam: *models) { + rtabmap::Transform localTransform = cam.localTransform(); + if(!localTransform.isNull()) { + geometry_msgs::msg::TransformStamped baseToCamera; + baseToCamera.child_frame_id = (stereo?index%2==0?"left_":"right_":"") + cameraFrameId_ + (models->size()>1?uNumber2Str(index/(stereo?2:1)):""); + baseToCamera.header.frame_id = frameId_; + baseToCamera.header.stamp = time; + if(cam.Tx() != 0) { + localTransform *= rtabmap::Transform(-cam.Tx()/cam.fx(), 0, 0); + } + rtabmap_conversions::transformToGeometryMsg(localTransform, baseToCamera.transform); + transforms.push_back(baseToCamera); + } + ++index; } if(!odom.pose().isNull()) @@ -392,26 +454,32 @@ bool DbPlayer::publishNextFrame() rtabmap_conversions::transformToGeometryMsg(odom.data().groundTruth(), worldToBase.transform); transforms.push_back(worldToBase); } + + if(!odom.data().imu().empty()) { + geometry_msgs::msg::TransformStamped baseToImu; + baseToImu.child_frame_id = imuFrameId_; + baseToImu.header.frame_id = frameId_; + baseToImu.header.stamp = time; + rtabmap_conversions::transformToGeometryMsg(odom.data().imu().localTransform(), baseToImu.transform); + transforms.push_back(baseToImu); + } tfBroadcaster_->sendTransform(transforms); } - if(!odom.pose().isNull()) + if( odometryPub_.get() && + !odom.pose().isNull() && + odometryPub_->get_subscription_count()) { - if(!odometryPub_.get()) odometryPub_ = this->create_publisher("odom", 1); - - if(odometryPub_->get_subscription_count()) - { - nav_msgs::msg::Odometry odomMsg; - odomMsg.child_frame_id = frameId_; - odomMsg.header.frame_id = odomFrameId_; - odomMsg.header.stamp = time; - rtabmap_conversions::transformToPoseMsg(odom.pose(), odomMsg.pose.pose); - UASSERT(odomMsg.pose.covariance.size() == 36 && - odom.covariance().total() == 36 && - odom.covariance().type() == CV_64FC1); - memcpy(odomMsg.pose.covariance.begin(), odom.covariance().data, 36*sizeof(double)); - odometryPub_->publish(odomMsg); - } + nav_msgs::msg::Odometry odomMsg; + odomMsg.child_frame_id = frameId_; + odomMsg.header.frame_id = odomFrameId_; + odomMsg.header.stamp = time; + rtabmap_conversions::transformToPoseMsg(odom.pose(), odomMsg.pose.pose); + UASSERT(odomMsg.pose.covariance.size() == 36 && + odom.covariance().total() == 36 && + odom.covariance().type() == CV_64FC1); + memcpy(odomMsg.pose.covariance.begin(), odom.covariance().data, 36*sizeof(double)); + odometryPub_->publish(odomMsg); } // Publish async topics first (so that they can catched by rtabmap before the image topics) @@ -444,86 +512,164 @@ bool DbPlayer::publishNextFrame() gpsFixPub_->publish(msg); } - if( (imagePub_.getNumSubscribers()) || - (rgbPub_.getNumSubscribers()) || - (leftPub_.getNumSubscribers())) + if( imuPub_.get() && + imuPub_->get_subscription_count() > 0 && + !odom.data().imu().empty()) { - cv_bridge::CvImage img; - if(odom.data().imageRaw().channels() == 1) - { - img.encoding = sensor_msgs::image_encodings::MONO8; - } - else - { - img.encoding = sensor_msgs::image_encodings::BGR8; - } - img.image = odom.data().imageRaw(); - sensor_msgs::msg::Image imageRosMsg; - img.toImageMsg(imageRosMsg); - imageRosMsg.header.frame_id = cameraFrameId_; - imageRosMsg.header.stamp = time; + sensor_msgs::msg::Imu msg; + rtabmap_conversions::imuToROS(odom.data().imu(), msg); + msg.header.frame_id = imuFrameId_; + msg.header.stamp = time; + imuPub_->publish(msg); + } - if(imagePub_.getNumSubscribers()) + // single camera + if(imagePub_.getNumSubscribers() || (odom.data().cameraModels().size() <= 1 && odom.data().stereoCameraModels().size() <= 1)) + { + if(!odom.data().imageRaw().empty() && + ((imagePub_.getNumSubscribers()) || + (rgbPub_.getNumSubscribers()) || + (leftPub_.getNumSubscribers()))) { - imagePub_.publish(imageRosMsg); + sensor_msgs::msg::Image imageRosMsg; + if(cvImageToROS(odom.data().imageRaw(), imageRosMsg)) + { + imageRosMsg.header.frame_id = cameraFrameId_; + imageRosMsg.header.stamp = time; + + if(imagePub_.getNumSubscribers()) + { + imagePub_.publish(imageRosMsg); + } + if(rgbPub_.getNumSubscribers()) + { + rgbPub_.publish(imageRosMsg); + UASSERT(odom.data().cameraModels().size() == 1); + sensor_msgs::msg::CameraInfo info; + rtabmap_conversions::cameraModelToROS(odom.data().cameraModels()[0], info); + info.header = imageRosMsg.header; + rgbInfoPub_->publish(info); + } + if(leftPub_.getNumSubscribers()) + { + imageRosMsg.header.frame_id = "left_" + cameraFrameId_; + leftPub_.publish(imageRosMsg); + UASSERT(odom.data().stereoCameraModels().size() == 1); + sensor_msgs::msg::CameraInfo info; + rtabmap_conversions::cameraModelToROS(odom.data().stereoCameraModels()[0].left(), info); + info.header = imageRosMsg.header; + leftInfoPub_->publish(info); + } + } } - if(rgbPub_.getNumSubscribers()) + + if(!odom.data().depthRaw().empty() && depthPub_.getNumSubscribers()) { - rgbPub_.publish(imageRosMsg); - rgbInfoPub_->publish(camInfoA); + sensor_msgs::msg::Image imageRosMsg; + if(cvImageToROS(odom.data().depthRaw(), imageRosMsg)) + { + imageRosMsg.header.frame_id = cameraFrameId_; + imageRosMsg.header.stamp = time; + + depthPub_.publish(imageRosMsg); + + UASSERT(odom.data().cameraModels().size() == 1); + sensor_msgs::msg::CameraInfo info; + // We assume depth is registered with the RGB camera, so they share same calibration and TF frame + rtabmap_conversions::cameraModelToROS(odom.data().cameraModels()[0], info); + info.header = imageRosMsg.header; + depthInfoPub_->publish(info); + } } - if(leftPub_.getNumSubscribers()) + + if(!odom.data().rightRaw().empty() && rightPub_.getNumSubscribers()) { - leftPub_.publish(imageRosMsg); - leftInfoPub_->publish(camInfoA); + sensor_msgs::msg::Image imageRosMsg; + if(cvImageToROS(odom.data().rightRaw(), imageRosMsg)) + { + imageRosMsg.header.frame_id = "right_" + cameraFrameId_; + imageRosMsg.header.stamp = time; + + rightPub_.publish(imageRosMsg); + + UASSERT(odom.data().stereoCameraModels().size() == 1); + sensor_msgs::msg::CameraInfo info; + rtabmap_conversions::cameraModelToROS(odom.data().stereoCameraModels()[0].right(), info); + info.header = imageRosMsg.header; + rightInfoPub_->publish(info); + } } } - if(depthPub_.getNumSubscribers() && !odom.data().depthRaw().empty()) + // Multi-cameras + if(!odom.data().imageRaw().empty()) { - cv_bridge::CvImage img; - if(odom.data().depthRaw().type() == CV_32FC1) + std::vector rgbdImages; + if(odom.data().cameraModels().size() > 1) { - img.encoding = sensor_msgs::image_encodings::TYPE_32FC1; - } - else - { - img.encoding = sensor_msgs::image_encodings::TYPE_16UC1; - } - img.image = odom.data().depthRaw(); - sensor_msgs::msg::Image imageRosMsg; - img.toImageMsg(imageRosMsg); - imageRosMsg.header.frame_id = cameraFrameId_; - imageRosMsg.header.stamp = time; + UASSERT(odom.data().cameraModels().size() == rgbdImagePubs_.size()); + int subRgbImageWidth = odom.data().imageRaw().cols / odom.data().cameraModels().size(); + int subDepthImageWidth = odom.data().depthRaw().cols / odom.data().cameraModels().size(); + for(size_t i=0; ipublish(camInfoB); - } + cvImageToROS(cv::Mat(odom.data().imageRaw(), cv::Range::all(), cv::Range(i*subRgbImageWidth, (i+1)*subRgbImageWidth)), msg.rgb); + msg.rgb.header = msg.header; + rtabmap_conversions::cameraModelToROS(odom.data().cameraModels()[i], msg.rgb_camera_info); + msg.rgb_camera_info.header = msg.header; - if(rightPub_.getNumSubscribers() && !odom.data().rightRaw().empty()) - { - cv_bridge::CvImage img; - if(odom.data().imageRaw().channels() == 1) - { - img.encoding = sensor_msgs::image_encodings::MONO8; - } - else - { - img.encoding = sensor_msgs::image_encodings::BGR8; - } - img.image = odom.data().rightRaw(); - sensor_msgs::msg::Image imageRosMsg; - img.toImageMsg(imageRosMsg); - imageRosMsg.header.frame_id = cameraFrameId_; - imageRosMsg.header.stamp = time; + if(subDepthImageWidth) { + cvImageToROS(cv::Mat(odom.data().depthRaw(), cv::Range::all(), cv::Range(i*subDepthImageWidth, (i+1)*subDepthImageWidth)), msg.depth); + msg.depth.header = msg.header; + UASSERT(subDepthImageWidth <= subRgbImageWidth); + if(subDepthImageWidth < subRgbImageWidth) { + rtabmap_conversions::cameraModelToROS(odom.data().cameraModels()[i].scaled(double(subDepthImageWidth)/double(subRgbImageWidth)), msg.depth_camera_info); + } + else { + rtabmap_conversions::cameraModelToROS(odom.data().cameraModels()[i], msg.depth_camera_info); + } + msg.depth_camera_info.header = msg.header; + } - rightPub_.publish(imageRosMsg); - rightInfoPub_->publish(camInfoB); + rgbdImagePubs_[i]->publish(msg); + } + } + else if(odom.data().stereoCameraModels().size() > 1) + { + UASSERT(odom.data().stereoCameraModels().size() == rgbdImagePubs_.size()); + int subImageWidth = odom.data().imageRaw().cols / odom.data().stereoCameraModels().size(); + UASSERT(odom.data().imageRaw().cols == odom.data().rightRaw().cols); + for(size_t i=0; ipublish(msg); + } + } } if(!odom.data().laserScanRaw().isEmpty()) { - if(scanPub_.get() && scanPub_->get_subscription_count() && odom.data().laserScanRaw().is2d()) + if(scanPub_.get() && + scanPub_->get_subscription_count() && + odom.data().laserScanRaw().is2d()) { //inspired from pointcloud_to_laserscan package sensor_msgs::msg::LaserScan msg; @@ -570,7 +716,8 @@ bool DbPlayer::publishNextFrame() scanPub_->publish(msg); } - else if(scanCloudPub_.get() && scanCloudPub_->get_subscription_count()) + else if(scanCloudPub_.get() && + scanCloudPub_->get_subscription_count()) { sensor_msgs::msg::PointCloud2 msg; pcl_conversions::moveFromPCL(*rtabmap::util3d::laserScanToPointCloud2(odom.data().laserScanRaw()), msg); diff --git a/rtabmap_util/src/nodelets/rgbd_split.cpp b/rtabmap_util/src/nodelets/rgbd_split.cpp index d1ebc4ed..623f0fac 100644 --- a/rtabmap_util/src/nodelets/rgbd_split.cpp +++ b/rtabmap_util/src/nodelets/rgbd_split.cpp @@ -98,7 +98,22 @@ void RGBDSplit::callback(const rtabmap_msgs::msg::RGBDImage::SharedPtr input) co cv_bridge::toCvCopy(input->depth_compressed)->toImageMsg(outputImage); #endif } - outputImage.header = outputCameraInfo.header = input->header; + if(outputCameraInfo.header.frame_id.empty()) { + if(outputImage.header.frame_id.empty()) { + outputCameraInfo.header = input->header; + } + else { + outputCameraInfo.header = outputImage.header; + } + } + if(outputImage.header.frame_id.empty()) { + if(outputCameraInfo.header.frame_id.empty()) { + outputImage.header = input->header; + } + else { + outputImage.header = outputCameraInfo.header; + } + } depthPub_.publish(outputImage); depthInfoPub_->publish(outputCameraInfo); } diff --git a/rtabmap_viz/CMakeLists.txt b/rtabmap_viz/CMakeLists.txt index a7707f22..60dcfd7c 100644 --- a/rtabmap_viz/CMakeLists.txt +++ b/rtabmap_viz/CMakeLists.txt @@ -68,12 +68,23 @@ SET_TARGET_PROPERTIES( AUTORCC ON ) +add_executable(rgbd_image_viewer src/RGBDImageViewerNode.cpp src/rgbd_image_viewer.cpp include/${PROJECT_NAME}/rgbd_image_viewer.hpp) +ament_target_dependencies(rgbd_image_viewer ${Libraries}) +SET_TARGET_PROPERTIES( + rgbd_image_viewer + PROPERTIES + AUTOUIC ON + AUTOMOC ON + AUTORCC ON +) + ############# ## Install ## ############# install(TARGETS rtabmap_viz + rgbd_image_viewer DESTINATION lib/${PROJECT_NAME} ) diff --git a/rtabmap_viz/include/rtabmap_viz/rgbd_image_viewer.hpp b/rtabmap_viz/include/rtabmap_viz/rgbd_image_viewer.hpp new file mode 100644 index 00000000..fe8920e4 --- /dev/null +++ b/rtabmap_viz/include/rtabmap_viz/rgbd_image_viewer.hpp @@ -0,0 +1,84 @@ +/* +Copyright (c) 2010-2025, Mathieu Labbe - IntRoLab - Universite de Sherbrooke +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. + * Neither the name of the Universite de Sherbrooke nor the + names of its contributors may be used to endorse or promote products + derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY +DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ + +#ifndef RGBDIMAGEVIEWER_H_ +#define RGBDIMAGEVIEWER_H_ + +#include +#include +#include +#include "rtabmap_msgs/msg/rgbd_image.hpp" +#include +#include +#include +#include + +namespace rtabmap +{ + class CameraViewer; +} + +class QComboBox; +class QSpinBox; +class QLabel; + +namespace rtabmap_viz { + +class RGBDImageViewer : public QMainWindow, public UEventsSender +{ + Q_OBJECT + +public: + RTABMAP_VIZ_PUBLIC + explicit RGBDImageViewer(std::shared_ptr & node, const rtabmap::ParametersMap & parameters); + virtual ~RGBDImageViewer(); + +private Q_SLOTS: + void updateTopicList(); + void topicSelected(const QString & topicName); + +private: + void callback(const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr msg); + +private: + QComboBox * topicComboBox_; + QComboBox * frameComboBox_; + QSpinBox * spinBox_; + QLabel * warningLabel_; + rtabmap::CameraViewer * cameraView_; + rclcpp::Subscription::SharedPtr rgbdImageSub_; + + std::shared_ptr node_; + std::shared_ptr tfBuffer_; + std::shared_ptr tfListener_; + + std::mutex mutex_; +}; + +} + +#endif /* RGBDIMAGEVIEWER_H_ */ diff --git a/rtabmap_viz/src/RGBDImageViewerNode.cpp b/rtabmap_viz/src/RGBDImageViewerNode.cpp new file mode 100644 index 00000000..f277ac85 --- /dev/null +++ b/rtabmap_viz/src/RGBDImageViewerNode.cpp @@ -0,0 +1,83 @@ +/* +Copyright (c) 2010-2025, 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_viz/rgbd_image_viewer.hpp" +#include "rtabmap/utilite/ULogger.h" + +#include +#include +#include +#include + +QApplication * app = 0; + +void my_handler(int){ + app->exit(-1); +} + +int main(int argc, char** argv) +{ + rclcpp::init(argc, argv); + + app = new QApplication(argc, argv); + app->connect( app, SIGNAL( lastWindowClosed() ), app, SLOT( quit() ) ); + + int r; + { + auto node = std::make_shared("rgbd_image_viewer"); + rtabmap::ParametersMap parameters = rtabmap::Parameters::parseArguments(argc, argv, true); + rtabmap_viz::RGBDImageViewer viewer(node, parameters); + viewer.show(); + + // Catch ctrl-c to close the gui + // (Place this after QApplication's constructor) + struct sigaction sigIntHandler; + sigIntHandler.sa_handler = my_handler; + sigemptyset(&sigIntHandler.sa_mask); + sigIntHandler.sa_flags = 0; + sigaction(SIGINT, &sigIntHandler, NULL); + + // Here start the ROS events loop + rclcpp::executors::SingleThreadedExecutor executor; //Use 1 thread + executor.add_node(node); + auto spin_executor = [&executor]() { + executor.spin(); + }; + + // Launch executer + std::thread execution_thread(spin_executor); + + // Now wait for application to finish + r = app->exec();// MUST be called by the Main Thread + + rclcpp::shutdown(); + execution_thread.join(); + } + delete app; + + return r; +} diff --git a/rtabmap_viz/src/rgbd_image_viewer.cpp b/rtabmap_viz/src/rgbd_image_viewer.cpp new file mode 100644 index 00000000..d9fd1953 --- /dev/null +++ b/rtabmap_viz/src/rgbd_image_viewer.cpp @@ -0,0 +1,171 @@ +/* +Copyright (c) 2010-2025, 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_viz/rgbd_image_viewer.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace rtabmap_viz { + +RGBDImageViewer::RGBDImageViewer(std::shared_ptr & node, const rtabmap::ParametersMap & parameters) : + node_(node) +{ + this->setWindowTitle("rgbd_image_viewer"); + + topicComboBox_ = new QComboBox(this); + topicComboBox_->setSizeAdjustPolicy(QComboBox::SizeAdjustPolicy::AdjustToContents); + topicComboBox_->setToolTip("Available rtabmap_msgs::RGBDImage topics"); + frameComboBox_ = new QComboBox(this); + frameComboBox_->setSizeAdjustPolicy(QComboBox::SizeAdjustPolicy::AdjustToContents); + frameComboBox_->setToolTip("Base frame of the point cloud"); + spinBox_ = new QSpinBox(this); + spinBox_->setMinimum(0); + spinBox_->setMaximum(1000); + spinBox_->setValue(10); + spinBox_->setSuffix(" ms"); + spinBox_->setToolTip("Maximum time to wait for TF to transform in base frame (0 means latest available)"); + warningLabel_ = new QLabel(this); + warningLabel_->setStyleSheet("QLabel { color : red; }"); + cameraView_ = new rtabmap::CameraViewer(this, parameters); + cameraView_->registerToEventsManager(); + QPushButton * refreshButton = new QPushButton(this); + refreshButton->setIcon(style()->standardIcon(QStyle::SP_BrowserReload)); + refreshButton->setToolTip("Refresh topics and frames"); + + connect(topicComboBox_, SIGNAL(currentTextChanged(const QString &)), this, SLOT(topicSelected(const QString &))); + connect(cameraView_, SIGNAL(finished(int)), this, SLOT(close())); + connect(refreshButton, SIGNAL(clicked()), this, SLOT(updateTopicList())); + + QWidget *centralWidget = new QWidget(this); + QVBoxLayout *layout = new QVBoxLayout(centralWidget); + + QHBoxLayout *hlayout = new QHBoxLayout(); + hlayout->addWidget(topicComboBox_); + hlayout->addWidget(frameComboBox_); + hlayout->addWidget(spinBox_); + hlayout->addWidget(refreshButton); + hlayout->addWidget(warningLabel_); + hlayout->addStretch(); + + layout->addLayout(hlayout); + layout->addWidget(cameraView_); + + this->setCentralWidget(centralWidget); + + tfBuffer_ = std::make_shared(node_->get_clock()); + tfListener_ = std::make_shared(*tfBuffer_); + + updateTopicList(); +} + +RGBDImageViewer::~RGBDImageViewer() +{ +} + +void RGBDImageViewer::updateTopicList() { + std::map> topicNames = node_->get_topic_names_and_types(); + topicComboBox_->clear(); + for(auto topic: topicNames) { + for(auto type: topic.second) { + if(type == "rtabmap_msgs/msg/RGBDImage") { + topicComboBox_->addItem(topic.first.c_str()); + } + } + } + std::vector frames = tfBuffer_->getAllFrameNames(); + frameComboBox_->clear(); + frameComboBox_->addItem(""); + for(auto & frame: frames) { + frameComboBox_->addItem(frame.c_str()); + } +} + +void RGBDImageViewer::topicSelected(const QString & topicName) { + rgbdImageSub_.reset(); + if(!topicName.isEmpty()) { + rgbdImageSub_ = node_->create_subscription(topicName.toStdString(), rclcpp::QoS(1), std::bind(&RGBDImageViewer::callback, this, std::placeholders::_1)); + } +} + +void RGBDImageViewer::callback( + const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr msg) +{ + bool warned = false; + rtabmap::SensorData data = rtabmap_conversions::rgbdImageFromROS(msg); + if(!frameComboBox_->currentText().isEmpty() && (!data.cameraModels().empty() || !data.stereoCameraModels().empty())) { + rtabmap::Transform localTransform; + if(frameComboBox_->currentText().compare("") == 0) { + localTransform = rtabmap::CameraModel::opticalRotation(); + } + else { + localTransform = rtabmap_conversions::getTransform( + frameComboBox_->currentText().toStdString(), + msg->header.frame_id, + msg->header.stamp, + *tfBuffer_, + double(spinBox_->value()) / 1000.0); + } + if(localTransform.isNull()) + { + QString log = QString("Could not get TF between \"%1\" and \"%2\" frames for stamp %3 after waiting %4 ms.") + .arg(frameComboBox_->currentText()) + .arg(msg->header.frame_id.c_str()) + .arg(QString::number(rclcpp::Time(msg->header.stamp).seconds(), 'f', 3)) + .arg(spinBox_->value()); + warningLabel_->setToolTip(log); + QMetaObject::invokeMethod(warningLabel_, "setText", Q_ARG(QString, log)); + warned = true; + } + + if(!data.cameraModels().empty()) { + rtabmap::CameraModel model = data.cameraModels()[0]; + model.setLocalTransform(localTransform); + data.setCameraModel(model); + } + else { + rtabmap::StereoCameraModel model = data.stereoCameraModels()[0]; + model.setLocalTransform(localTransform); + data.setStereoCameraModel(model); + } + } + if(!warned) { + warningLabel_->setToolTip(""); + QMetaObject::invokeMethod(warningLabel_, "clear"); + } + + this->post(new rtabmap::SensorEvent(data)); +} + +} From 3b5a4ed675038e407d7cd2b973f821b49c742f31 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sat, 18 Oct 2025 13:59:55 -0700 Subject: [PATCH 24/56] Added error messages when subscribe_odom and odom_frame_id are both not set. rgb_sync: added option "fill_empty_depth" to add fake empty depth for monocular cameras. See also #1363 --- rtabmap_conversions/src/MsgConversion.cpp | 2 +- rtabmap_odom/src/nodelets/rgbd_odometry.cpp | 4 + rtabmap_slam/src/CoreWrapper.cpp | 43 ++-- .../include/rtabmap_sync/rgb_sync.hpp | 1 + rtabmap_sync/src/CommonDataSubscriber.cpp | 8 +- rtabmap_sync/src/nodelets/rgb_sync.cpp | 22 ++ rtabmap_viz/include/rtabmap_viz/GuiWrapper.h | 14 -- rtabmap_viz/src/GuiWrapper.cpp | 208 ++---------------- 8 files changed, 82 insertions(+), 220 deletions(-) diff --git a/rtabmap_conversions/src/MsgConversion.cpp b/rtabmap_conversions/src/MsgConversion.cpp index 6eaf84ea..5a69ce00 100644 --- a/rtabmap_conversions/src/MsgConversion.cpp +++ b/rtabmap_conversions/src/MsgConversion.cpp @@ -1997,7 +1997,7 @@ rtabmap::Transform getTransform( } catch(tf2::TransformException & ex) { - UWARN("(getting transform %s -> %s) %s (wait_for_transform=%f)", fromFrameId.c_str(), toFrameId.c_str(), ex.what(), waitForTransform); + UWARN("(getting transform \"%s\" -> \"%s\") %s (wait_for_transform=%f)", fromFrameId.c_str(), toFrameId.c_str(), ex.what(), waitForTransform); } return transform; diff --git a/rtabmap_odom/src/nodelets/rgbd_odometry.cpp b/rtabmap_odom/src/nodelets/rgbd_odometry.cpp index 47b1e171..a2614e09 100644 --- a/rtabmap_odom/src/nodelets/rgbd_odometry.cpp +++ b/rtabmap_odom/src/nodelets/rgbd_odometry.cpp @@ -432,8 +432,10 @@ void RGBDOdometry::commonCallback( { UASSERT(rgbImages.size() > 0 && rgbImages.size() == depthImages.size() && rgbImages.size() == cameraInfos.size()); rclcpp::Time higherStamp; + UASSERT_MSG(rgbImages[0], "RGB image is null!"); int imageWidth = rgbImages[0]->image.cols; int imageHeight = rgbImages[0]->image.rows; + UASSERT_MSG(depthImages[0], "Depth image is null!"); int depthWidth = depthImages[0]->image.cols; int depthHeight = depthImages[0]->image.rows; @@ -447,6 +449,8 @@ void RGBDOdometry::commonCallback( std::vector cameraModels; for(unsigned int i=0; iencoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) ==0 || rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 || rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 || diff --git a/rtabmap_slam/src/CoreWrapper.cpp b/rtabmap_slam/src/CoreWrapper.cpp index 528b6065..55cd356a 100644 --- a/rtabmap_slam/src/CoreWrapper.cpp +++ b/rtabmap_slam/src/CoreWrapper.cpp @@ -233,20 +233,14 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) : stereoToDepth_ = this->declare_parameter("stereo_to_depth", stereoToDepth_); odomSensorSync_ = this->declare_parameter("odom_sensor_sync", odomSensorSync_); - RCLCPP_INFO(this->get_logger(), "rtabmap: frame_id = %s", frameId_.c_str()); - if(!odomFrameId_.empty()) - { - RCLCPP_INFO(this->get_logger(), "rtabmap: odom_frame_id = %s", odomFrameId_.c_str()); - } - if(!groundTruthFrameId_.empty()) - { - RCLCPP_INFO(this->get_logger(), "rtabmap: ground_truth_frame_id = %s -> ground_truth_base_frame_id = %s", - groundTruthFrameId_.c_str(), - groundTruthBaseFrameId_.c_str()); - } - RCLCPP_INFO(this->get_logger(), "rtabmap: map_frame_id = %s", mapFrameId_.c_str()); + RCLCPP_INFO(this->get_logger(), "rtabmap: frame_id = \"%s\"", frameId_.c_str()); + RCLCPP_INFO(this->get_logger(), "rtabmap: odom_frame_id = \"%s\"", odomFrameId_.c_str()); + RCLCPP_INFO(this->get_logger(), "rtabmap: ground_truth_frame_id = \"%s\" -> ground_truth_base_frame_id = \"%s\"", + groundTruthFrameId_.c_str(), + groundTruthBaseFrameId_.c_str()); + RCLCPP_INFO(this->get_logger(), "rtabmap: map_frame_id = \"%s\"", mapFrameId_.c_str()); RCLCPP_INFO(this->get_logger(), "rtabmap: log_to_rosout_level = %d", eventLevel); - RCLCPP_INFO(this->get_logger(), "rtabmap: initial_pose = %s", initialPoseStr.c_str()); + RCLCPP_INFO(this->get_logger(), "rtabmap: initial_pose = \"%s\"", initialPoseStr.c_str()); RCLCPP_INFO(this->get_logger(), "rtabmap: use_action_for_goal = %s", useActionForGoal_?"true":"false"); RCLCPP_INFO(this->get_logger(), "rtabmap: tf_delay = %f", tfDelay); RCLCPP_INFO(this->get_logger(), "rtabmap: tf_tolerance = %f", tfTolerance); @@ -840,6 +834,14 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) : rtabmap_.parseParameters(parameters_); } } + + if(!this->isSubscribedToOdom() && odomFrameId_.empty()) + { + bool isRGBD = uStr2Bool(parameters_.at(Parameters::kRGBDEnabled()).c_str()); + if(isRGBD) { + RCLCPP_ERROR(this->get_logger(), "\"subscribe_odom\" or \"odom_frame_id\" should be used when \"%s\" is enabled!", Parameters::kRGBDEnabled().c_str()); + } + } // Set initial pose if set if(!initialPoseStr.empty()) @@ -1356,6 +1358,11 @@ void CoreWrapper::commonMultiCameraCallback( mapToOdomMutex_.lock(); odomFrameId = odomFrameId_; mapToOdomMutex_.unlock(); + if(odomFrameId.empty()) + { + RCLCPP_ERROR(this->get_logger(), "This callback cannot be used without \"subscribe_odom\" or \"odom_frame_id\" set."); + return; + } if(!scan2dMsg.ranges.empty()) { if(!odomTFUpdate(odomFrameId, scan2dMsg.header.stamp)) @@ -1742,6 +1749,11 @@ void CoreWrapper::commonLaserScanCallback( mapToOdomMutex_.lock(); odomFrameId = odomFrameId_; mapToOdomMutex_.unlock(); + if(odomFrameId.empty()) + { + RCLCPP_ERROR(this->get_logger(), "This callback cannot be used without \"subscribe_odom\" or \"odom_frame_id\" set."); + return; + } if(!scan2dMsg.ranges.empty()) { if(!odomTFUpdate(odomFrameId, scan2dMsg.header.stamp)) @@ -1951,6 +1963,11 @@ void CoreWrapper::commonSensorDataCallback( mapToOdomMutex_.lock(); odomFrameId = odomFrameId_; mapToOdomMutex_.unlock(); + if(odomFrameId.empty()) + { + RCLCPP_ERROR(this->get_logger(), "This callback cannot be used without \"subscribe_odom\" or \"odom_frame_id\" set."); + return; + } if(!odomTFUpdate(odomFrameId, sensorDataMsg->header.stamp)) { return; diff --git a/rtabmap_sync/include/rtabmap_sync/rgb_sync.hpp b/rtabmap_sync/include/rtabmap_sync/rgb_sync.hpp index 204d7370..571ef3a1 100644 --- a/rtabmap_sync/include/rtabmap_sync/rgb_sync.hpp +++ b/rtabmap_sync/include/rtabmap_sync/rgb_sync.hpp @@ -58,6 +58,7 @@ public: private: double compressedRate_; + bool fillEmptyDepth_; rclcpp::Time lastCompressedPublished_; diff --git a/rtabmap_sync/src/CommonDataSubscriber.cpp b/rtabmap_sync/src/CommonDataSubscriber.cpp index f671b439..c8cbfb06 100644 --- a/rtabmap_sync/src/CommonDataSubscriber.cpp +++ b/rtabmap_sync/src/CommonDataSubscriber.cpp @@ -531,6 +531,13 @@ void CommonDataSubscriber::setupCallbacks( RCLCPP_INFO(node.get_logger(), "%s: subscribe_stereo = %s", name_.c_str(), subscribedToStereo_?"true":"false"); RCLCPP_INFO(node.get_logger(), "%s: subscribe_rgbd = %s (rgbd_cameras=%d)", name_.c_str(), subscribedToRGBD_?"true":"false", rgbdCameras_); RCLCPP_INFO(node.get_logger(), "%s: subscribe_sensor_data = %s", name_.c_str(), subscribedToSensorData_?"true":"false"); + if(subscribedToOdom_ && !odomFrameId_.empty()) { + RCLCPP_INFO(node.get_logger(), "%s: subscribe_odom = false (\"odom_frame_id\" is set)", name_.c_str()); + subscribedToOdom_ = false; + } + else { + RCLCPP_INFO(node.get_logger(), "%s: subscribe_odom = %s", name_.c_str(), subscribedToOdom_?"true":"false"); + } RCLCPP_INFO(node.get_logger(), "%s: subscribe_odom_info = %s", name_.c_str(), subscribedToOdomInfo_?"true":"false"); RCLCPP_INFO(node.get_logger(), "%s: subscribe_user_data = %s", name_.c_str(), subscribedToUserData_?"true":"false"); RCLCPP_INFO(node.get_logger(), "%s: subscribe_scan = %s", name_.c_str(), subscribedToScan2d_?"true":"false"); @@ -550,7 +557,6 @@ void CommonDataSubscriber::setupCallbacks( rclcpp::SubscriptionOptions callbackOptions; callbackOptions.callback_group = syncCallbackGroup_; - subscribedToOdom_ = odomFrameId_.empty() && subscribedToOdom_; if(subscribedToDepth_) { setupDepthCallbacks( diff --git a/rtabmap_sync/src/nodelets/rgb_sync.cpp b/rtabmap_sync/src/nodelets/rgb_sync.cpp index 5498135d..a7f7d740 100644 --- a/rtabmap_sync/src/nodelets/rgb_sync.cpp +++ b/rtabmap_sync/src/nodelets/rgb_sync.cpp @@ -48,6 +48,7 @@ namespace rtabmap_sync RGBSync::RGBSync(const rclcpp::NodeOptions & options) : Node("rgbd_sync", options), compressedRate_(0), + fillEmptyDepth_(false), approxSync_(0), exactSync_(0) { @@ -73,6 +74,7 @@ RGBSync::RGBSync(const rclcpp::NodeOptions & options) : int qosCaminfo = this->declare_parameter("qos_camera_info", qos); compressedRate_ = this->declare_parameter("compressed_rate", compressedRate_); std::string imageTransport = this->declare_parameter("image_transport", std::string("raw")); + fillEmptyDepth_ = this->declare_parameter("fill_empty_depth", fillEmptyDepth_); RCLCPP_INFO(this->get_logger(), "%s: approx_sync = %s", get_name(), approxSync?"true":"false"); if(approxSync) @@ -83,6 +85,7 @@ RGBSync::RGBSync(const rclcpp::NodeOptions & options) : RCLCPP_INFO(this->get_logger(), "%s: qos_camera_info = %d", get_name(), qosCaminfo); RCLCPP_INFO(this->get_logger(), "%s: compressed_rate = %f", get_name(), compressedRate_); RCLCPP_INFO(this->get_logger(), "%s: image_transport = %s", get_name(), imageTransport.c_str()); + RCLCPP_INFO(this->get_logger(), "%s: fill_empty_depth = %s", get_name(), fillEmptyDepth_?"true":"false"); rgbdImagePub_ = this->create_publisher("rgbd_image", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos)); rgbdImageCompressedPub_ = this->create_publisher("rgbd_image/compressed", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos)); @@ -150,6 +153,14 @@ void RGBSync::callback( msg.header.frame_id = cameraInfo->header.frame_id; msg.header.stamp = image->header.stamp; msg.rgb_camera_info = *cameraInfo; + cv_bridge::CvImage fakeDepthImage; + if(fillEmptyDepth_) + { + msg.depth_camera_info = *cameraInfo; + fakeDepthImage.header = image->header; + fakeDepthImage.image = cv::Mat::zeros(image->height, image->width, CV_16UC1); + fakeDepthImage.encoding = sensor_msgs::image_encodings::TYPE_16UC1; + } if(rgbdImageCompressedPub_->get_subscription_count()) { @@ -172,6 +183,13 @@ void RGBSync::callback( cv_bridge::CvImageConstPtr imagePtr = cv_bridge::toCvShare(image); imagePtr->toCompressedImageMsg(msgCompressed.rgb_compressed, cv_bridge::JPG); + if(fillEmptyDepth_) + { + msgCompressed.depth_compressed.header = image->header; + msgCompressed.depth_compressed.data = rtabmap::compressImage(fakeDepthImage.image, ".png"); + msgCompressed.depth_compressed.format = "png"; + } + rgbdImageCompressedPub_->publish(msgCompressed); } } @@ -179,6 +197,10 @@ void RGBSync::callback( if(rgbdImagePub_->get_subscription_count()) { msg.rgb = *image; + if(fillEmptyDepth_) + { + fakeDepthImage.toImageMsg(msg.depth); + } rgbdImagePub_->publish(msg); } diff --git a/rtabmap_viz/include/rtabmap_viz/GuiWrapper.h b/rtabmap_viz/include/rtabmap_viz/GuiWrapper.h index f263871d..4d7d90c5 100644 --- a/rtabmap_viz/include/rtabmap_viz/GuiWrapper.h +++ b/rtabmap_viz/include/rtabmap_viz/GuiWrapper.h @@ -89,20 +89,6 @@ private: const std::vector > & localKeyPoints = std::vector >(), const std::vector > & localPoints3d = std::vector >(), const std::vector & localDescriptors = std::vector()); - virtual void commonStereoCallback( - const nav_msgs::msg::Odometry::ConstSharedPtr & odomMsg, - const rtabmap_msgs::msg::UserData::ConstSharedPtr & userDataMsg, - const cv_bridge::CvImageConstPtr& leftImageMsg, - const cv_bridge::CvImageConstPtr& rightImageMsg, - const sensor_msgs::msg::CameraInfo& leftCamInfoMsg, - const sensor_msgs::msg::CameraInfo& rightCamInfoMsg, - const sensor_msgs::msg::LaserScan & scan2dMsg, - const sensor_msgs::msg::PointCloud2 & scan3dMsg, - const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr& odomInfoMsg, - const std::vector & globalDescriptorMsgs = std::vector(), - const std::vector & localKeyPoints = std::vector(), - const std::vector & localPoints3d = std::vector(), - const cv::Mat & localDescriptors = cv::Mat()); virtual void commonLaserScanCallback( const nav_msgs::msg::Odometry::ConstSharedPtr & odomMsg, const rtabmap_msgs::msg::UserData::ConstSharedPtr & userDataMsg, diff --git a/rtabmap_viz/src/GuiWrapper.cpp b/rtabmap_viz/src/GuiWrapper.cpp index e0283d7c..872d8b95 100644 --- a/rtabmap_viz/src/GuiWrapper.cpp +++ b/rtabmap_viz/src/GuiWrapper.cpp @@ -590,6 +590,11 @@ void GuiWrapper::commonMultiCameraCallback( } odomHeader.frame_id = odomFrameId_; + if(odomHeader.frame_id.empty()) + { + RCLCPP_ERROR(this->get_logger(), "This callback cannot be used without \"subscribe_odom\" or \"odom_frame_id\" set."); + return; + } odomT = rtabmap_conversions::getTransform(odomHeader.frame_id, frameId_, odomHeader.stamp, *tfBuffer_, waitForTransform_); if(odomT.isNull()) { @@ -747,197 +752,6 @@ void GuiWrapper::commonMultiCameraCallback( QMetaObject::invokeMethod(mainWindow_, "processOdometry", Q_ARG(rtabmap::OdometryEvent, odomEvent), Q_ARG(bool, ignoreData)); } -void GuiWrapper::commonStereoCallback( - const nav_msgs::msg::Odometry::ConstSharedPtr & odomMsg, - const rtabmap_msgs::msg::UserData::ConstSharedPtr &, - const cv_bridge::CvImageConstPtr& leftImageMsg, - const cv_bridge::CvImageConstPtr& rightImageMsg, - const sensor_msgs::msg::CameraInfo& leftCamInfoMsg, - const sensor_msgs::msg::CameraInfo& rightCamInfoMsg, - const sensor_msgs::msg::LaserScan & scan2dMsg, - const sensor_msgs::msg::PointCloud2 & scan3dMsg, - const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr& odomInfoMsg, - const std::vector &, - const std::vector &, - const std::vector &, - const cv::Mat &) -{ - std_msgs::msg::Header odomHeader; - std::string frameId = frameId_; - Transform odomT; - if(odomMsg.get()) - { - odomT = rtabmap_conversions::transformFromPoseMsg(odomMsg->pose.pose); - odomHeader = odomMsg->header; - if(!odomMsg->child_frame_id.empty()) - { - frameId = odomMsg->child_frame_id; - } - else - { - RCLCPP_WARN(get_logger(), "Received odom topic with child_frame_id not set! Using \"%s\" as base frame.", frameId_.c_str()); - } - } - else - { - if(!scan2dMsg.ranges.empty()) - { - odomHeader = scan2dMsg.header; - } - else if(!scan3dMsg.data.empty()) - { - odomHeader = scan3dMsg.header; - } - else - { - odomHeader = leftCamInfoMsg.header; - } - odomHeader.frame_id = odomFrameId_; - - odomT = rtabmap_conversions::getTransform(odomHeader.frame_id, frameId_, odomHeader.stamp, *tfBuffer_, waitForTransform_); - if(odomT.isNull()) - { - RCLCPP_WARN(this->get_logger(), "Could not get odometry pose from " - "TF for stamp %f, aborting! To show red screen in rtabmap_viz " - "when this happens (indicating potentially lost), set subscribe_odom " - "to true.", rclcpp::Time(odomHeader.stamp).seconds()); - return; - } - } - - cv::Mat covariance = cv::Mat::eye(6,6,CV_64FC1); - if(odomMsg.get()) - { - UASSERT(odomMsg->twist.covariance.size() == 36); - if(odomMsg->twist.covariance[0] != 0 && - odomMsg->twist.covariance[7] != 0 && - odomMsg->twist.covariance[14] != 0 && - odomMsg->twist.covariance[21] != 0 && - odomMsg->twist.covariance[28] != 0 && - odomMsg->twist.covariance[35] != 0) - { - covariance = cv::Mat(6,6,CV_64FC1,(void*)odomMsg->twist.covariance.data()).clone(); - } - } - else if(odomInfoMsg.get() && odomInfoMsg->covariance.size() == 36) - { - if(odomInfoMsg->covariance[0] != 0 && - odomInfoMsg->covariance[7] != 0 && - odomInfoMsg->covariance[14] != 0 && - odomInfoMsg->covariance[21] != 0 && - odomInfoMsg->covariance[28] != 0 && - odomInfoMsg->covariance[35] != 0) - { - covariance = cv::Mat(6,6,CV_64FC1,(void*)odomInfoMsg->covariance.data()).clone(); - } - } - if(odomHeader.frame_id.empty()) - { - RCLCPP_ERROR(this->get_logger(), "Odometry frame not set!?"); - return; - } - - cv::Mat left; - cv::Mat right; - LaserScan scan; - rtabmap::StereoCameraModel stereoModel; - rtabmap::OdometryInfo info; - bool ignoreData = false; - - // limit update rate - if(maxOdomUpdateRate_<=0.0 || - (UTimer::now() - lastOdomInfoUpdateTime_ > 1.0/maxOdomUpdateRate_ && - !mainWindow_->isProcessingOdometry() && - !mainWindow_->isProcessingStatistics())) - { - lastOdomInfoUpdateTime_ = UTimer::now(); - - ParametersMap allParameters = prefDialog_->getAllParameters(); - bool imagesAlreadyRectified = Parameters::defaultRtabmapImagesAlreadyRectified(); - Parameters::parse(allParameters, Parameters::kRtabmapImagesAlreadyRectified(), imagesAlreadyRectified); - - if(!rtabmap_conversions::convertStereoMsg( - leftImageMsg, - rightImageMsg, - leftCamInfoMsg, - rightCamInfoMsg, - frameId, - odomSensorSync_?odomHeader.frame_id:"", - odomHeader.stamp, - left, - right, - stereoModel, - *tfBuffer_, - waitForTransform_, - imagesAlreadyRectified)) - { - RCLCPP_ERROR(this->get_logger(), "Could not convert stereo msgs! Aborting rtabmap_viz update..."); - return; - } - - if(!scan2dMsg.ranges.empty()) - { - if(!rtabmap_conversions::convertScanMsg( - scan2dMsg, - frameId, - odomSensorSync_?odomHeader.frame_id:"", - odomHeader.stamp, - scan, - *tfBuffer_, - waitForTransform_)) - { - RCLCPP_ERROR(this->get_logger(), "Could not convert laser scan msg! Aborting rtabmap_viz update..."); - return; - } - } - else if(!scan3dMsg.data.empty()) - { - if(!rtabmap_conversions::convertScan3dMsg( - scan3dMsg, - frameId, - odomSensorSync_?odomHeader.frame_id:"", - odomHeader.stamp, - scan, - *tfBuffer_, - waitForTransform_)) - { - RCLCPP_ERROR(this->get_logger(), "Could not convert 3d laser scan msg! Aborting rtabmap_viz update..."); - return; - } - } - - if(odomInfoMsg.get()) - { - info = rtabmap_conversions::odomInfoFromROS(*odomInfoMsg); - } - ignoreData = false; - } - else if(odomInfoMsg.get()) - { - info = rtabmap_conversions::odomInfoFromROS(*odomInfoMsg).copyWithoutData(); - ignoreData = true; - } - else - { - // don't update GUI odom stuff if we don't use visual odometry - return; - } - - info.reg.covariance = covariance; - rtabmap::OdometryEvent odomEvent( - rtabmap::SensorData( - scan, - left, - right, - stereoModel, - 0, - rtabmap_conversions::timestampFromROS(odomHeader.stamp)), - odomT, - info); - - QMetaObject::invokeMethod(mainWindow_, "processOdometry", Q_ARG(rtabmap::OdometryEvent, odomEvent), Q_ARG(bool, ignoreData)); -} - void GuiWrapper::commonLaserScanCallback( const nav_msgs::msg::Odometry::ConstSharedPtr & odomMsg, const rtabmap_msgs::msg::UserData::ConstSharedPtr &, @@ -978,6 +792,12 @@ void GuiWrapper::commonLaserScanCallback( } odomHeader.frame_id = odomFrameId_; + if(odomHeader.frame_id.empty()) + { + RCLCPP_ERROR(this->get_logger(), "This callback cannot be used without \"subscribe_odom\" or \"odom_frame_id\" set."); + return; + } + odomT = rtabmap_conversions::getTransform(odomHeader.frame_id, frameId_, odomHeader.stamp, *tfBuffer_, waitForTransform_); if(odomT.isNull()) { @@ -1194,6 +1014,12 @@ void GuiWrapper::commonSensorDataCallback( odomHeader = sensorDataMsg->header; odomHeader.frame_id = odomFrameId_; + if(odomHeader.frame_id.empty()) + { + RCLCPP_ERROR(this->get_logger(), "This callback cannot be used without \"subscribe_odom\" or \"odom_frame_id\" set."); + return; + } + odomT = rtabmap_conversions::getTransform(odomHeader.frame_id, frameId_, odomHeader.stamp, *tfBuffer_, waitForTransform_); if(odomT.isNull()) { From 2477bc21c32b618f6a6d30fc0941ef340c56d0a5 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sun, 19 Oct 2025 17:02:25 -0700 Subject: [PATCH 25/56] Making odometry's services private (under odometry node namespace) like rtabmap node instead of being global. For example, "reset_odom" will be under "rgbd_odometry/reset_odom". Added new parameter for rtabmap_viz to call correctly odometry services, with "odometry_node_name" set to "rgbd_odometry" by default. --- .../launch/husky/husky_slam2d.launch.py | 3 +- .../launch/husky/husky_slam3d.launch.py | 3 +- .../husky/husky_slam3d_assemble.launch.py | 3 +- .../launch/isaac/isaac_vslam.launch.py | 3 +- .../launch/stereo_outdoor_demo.launch.py | 3 +- .../turtlebot3/turtlebot3_rgbd.launch.py | 2 +- .../turtlebot3/turtlebot3_scan.launch.py | 3 +- .../turtlebot4/turtlebot4_slam.launch.py | 3 +- .../launch/euroc_datasets.launch.py | 3 +- rtabmap_examples/launch/lidar3d.launch.py | 3 +- .../launch/lidar3d_assemble.launch.py | 3 +- .../launch/lidar3d_assemble_x2.launch.py | 3 +- .../launch/realsense_d435i_color.launch.py | 12 +++- .../launch/realsense_d435i_infra.launch.py | 11 ++- .../launch/realsense_d435i_stereo.launch.py | 23 +++++-- rtabmap_launch/launch/rtabmap.launch.py | 15 +++- rtabmap_odom/src/OdometryROS.cpp | 17 ++--- rtabmap_viz/include/rtabmap_viz/GuiWrapper.h | 2 +- rtabmap_viz/src/GuiWrapper.cpp | 69 ++++++++----------- rtabmap_viz/src/PreferencesDialogROS.cpp | 2 +- 20 files changed, 115 insertions(+), 71 deletions(-) diff --git a/rtabmap_demos/launch/husky/husky_slam2d.launch.py b/rtabmap_demos/launch/husky/husky_slam2d.launch.py index 53bfcf52..667e31ed 100644 --- a/rtabmap_demos/launch/husky/husky_slam2d.launch.py +++ b/rtabmap_demos/launch/husky/husky_slam2d.launch.py @@ -123,6 +123,7 @@ def generate_launch_description(): Node( package='rtabmap_viz', executable='rtabmap_viz', output='screen', namespace=robot_ns, - parameters=[rtabmap_parameters, shared_parameters], + parameters=[rtabmap_parameters, shared_parameters, + {"odometry_node_name": "icp_odometry"}], remappings=remappings), ]) diff --git a/rtabmap_demos/launch/husky/husky_slam3d.launch.py b/rtabmap_demos/launch/husky/husky_slam3d.launch.py index bbadcb25..513f39e0 100644 --- a/rtabmap_demos/launch/husky/husky_slam3d.launch.py +++ b/rtabmap_demos/launch/husky/husky_slam3d.launch.py @@ -141,6 +141,7 @@ def generate_launch_description(): Node( package='rtabmap_viz', executable='rtabmap_viz', output='screen', namespace=robot_ns, - parameters=[rtabmap_parameters, shared_parameters], + parameters=[rtabmap_parameters, shared_parameters, + {"odometry_node_name": "icp_odometry"}], remappings=remappings), ]) diff --git a/rtabmap_demos/launch/husky/husky_slam3d_assemble.launch.py b/rtabmap_demos/launch/husky/husky_slam3d_assemble.launch.py index 62b4116f..0c412ebf 100644 --- a/rtabmap_demos/launch/husky/husky_slam3d_assemble.launch.py +++ b/rtabmap_demos/launch/husky/husky_slam3d_assemble.launch.py @@ -129,6 +129,7 @@ def generate_launch_description(): Node( package='rtabmap_viz', executable='rtabmap_viz', output='screen', namespace=robot_ns, - parameters=[rtabmap_parameters, shared_parameters], + parameters=[rtabmap_parameters, shared_parameters, + {"odometry_node_name": "icp_odometry"}], remappings=remappings + [('scan_cloud', 'sensors/lidar3d_0/points')]), ]) diff --git a/rtabmap_demos/launch/isaac/isaac_vslam.launch.py b/rtabmap_demos/launch/isaac/isaac_vslam.launch.py index 6ad55bd8..244180da 100644 --- a/rtabmap_demos/launch/isaac/isaac_vslam.launch.py +++ b/rtabmap_demos/launch/isaac/isaac_vslam.launch.py @@ -103,7 +103,8 @@ def launch_setup(context, *args, **kwargs): condition=IfCondition(rtabmap_viz), package='rtabmap_viz', executable='rtabmap_viz', output='screen', namespace='rtabmap', - parameters=[parameters], + parameters=[parameters, + {"odometry_node_name": vo_node_prefix+'_odometry'}], remappings=remappings), ] diff --git a/rtabmap_demos/launch/stereo_outdoor_demo.launch.py b/rtabmap_demos/launch/stereo_outdoor_demo.launch.py index 41036389..f3b63581 100644 --- a/rtabmap_demos/launch/stereo_outdoor_demo.launch.py +++ b/rtabmap_demos/launch/stereo_outdoor_demo.launch.py @@ -141,7 +141,8 @@ def generate_launch_description(): Node( package='rtabmap_viz', executable='rtabmap_viz', output='screen', condition=IfCondition(LaunchConfiguration("rtabmap_viz")), - parameters=[parameters], + parameters=[parameters, + {"odometry_node_name": 'stereo_odometry'}], remappings=remappings), Node( package='rviz2', executable='rviz2', name="rviz2", output='screen', diff --git a/rtabmap_demos/launch/turtlebot3/turtlebot3_rgbd.launch.py b/rtabmap_demos/launch/turtlebot3/turtlebot3_rgbd.launch.py index b9076449..e9a2fdcf 100644 --- a/rtabmap_demos/launch/turtlebot3/turtlebot3_rgbd.launch.py +++ b/rtabmap_demos/launch/turtlebot3/turtlebot3_rgbd.launch.py @@ -65,7 +65,7 @@ def generate_launch_description(): package='rtabmap_slam', executable='rtabmap', output='screen', parameters=[parameters], remappings=remappings, - arguments=['-d']), # This will delete the previous database (~/.ros/rtabmap.db) + arguments=['-d', '--udebug']), # This will delete the previous database (~/.ros/rtabmap.db) # Localization mode: Node( diff --git a/rtabmap_demos/launch/turtlebot3/turtlebot3_scan.launch.py b/rtabmap_demos/launch/turtlebot3/turtlebot3_scan.launch.py index e027c7e0..a66b55ec 100644 --- a/rtabmap_demos/launch/turtlebot3/turtlebot3_scan.launch.py +++ b/rtabmap_demos/launch/turtlebot3/turtlebot3_scan.launch.py @@ -76,7 +76,8 @@ def launch_setup(context, *args, **kwargs): # Visualization Node( package='rtabmap_viz', executable='rtabmap_viz', output='screen', - parameters=[parameters], + parameters=[parameters, + {"odometry_node_name": 'icp_odometry'}], remappings=remappings), ] diff --git a/rtabmap_demos/launch/turtlebot4/turtlebot4_slam.launch.py b/rtabmap_demos/launch/turtlebot4/turtlebot4_slam.launch.py index 85558ca8..63159b1d 100644 --- a/rtabmap_demos/launch/turtlebot4/turtlebot4_slam.launch.py +++ b/rtabmap_demos/launch/turtlebot4/turtlebot4_slam.launch.py @@ -114,6 +114,7 @@ def generate_launch_description(): Node( condition=IfCondition(rtabmap_viz), package='rtabmap_viz', executable='rtabmap_viz', output='screen', - parameters=[rtabmap_parameters, shared_parameters], + parameters=[rtabmap_parameters, shared_parameters, + {"odometry_node_name": 'icp_odometry'}], remappings=remappings), ]) diff --git a/rtabmap_examples/launch/euroc_datasets.launch.py b/rtabmap_examples/launch/euroc_datasets.launch.py index 07706deb..da324634 100644 --- a/rtabmap_examples/launch/euroc_datasets.launch.py +++ b/rtabmap_examples/launch/euroc_datasets.launch.py @@ -83,7 +83,8 @@ def generate_launch_description(): Node( package='rtabmap_viz', executable='rtabmap_viz', output='screen', - parameters=[parameters], + parameters=[parameters, + {'odometry_node_name': "stereo_odometry"}], remappings=remappings), # Image rectification and publishing synchronized camera_info diff --git a/rtabmap_examples/launch/lidar3d.launch.py b/rtabmap_examples/launch/lidar3d.launch.py index 6ddee09a..f72e82b9 100644 --- a/rtabmap_examples/launch/lidar3d.launch.py +++ b/rtabmap_examples/launch/lidar3d.launch.py @@ -154,7 +154,8 @@ def launch_setup(context: LaunchContext, *args, **kwargs): Node( package='rtabmap_viz', executable='rtabmap_viz', output='screen', - parameters=[shared_parameters, rtabmap_parameters], + parameters=[shared_parameters, rtabmap_parameters, + {'odometry_node_name': "icp_odometry"}], remappings=remappings + [('scan_cloud', 'odom_filtered_input_scan')]) ] diff --git a/rtabmap_examples/launch/lidar3d_assemble.launch.py b/rtabmap_examples/launch/lidar3d_assemble.launch.py index 9c217640..1fd3ef68 100644 --- a/rtabmap_examples/launch/lidar3d_assemble.launch.py +++ b/rtabmap_examples/launch/lidar3d_assemble.launch.py @@ -173,7 +173,8 @@ def launch_setup(context: LaunchContext, *args, **kwargs): # Just for visualization Node( package='rtabmap_viz', executable='rtabmap_viz', output='screen', - parameters=[shared_parameters, rtabmap_parameters], + parameters=[shared_parameters, rtabmap_parameters, + {'odometry_node_name': "icp_odometry"}], remappings=remappings + [('scan_cloud', viz_topic)]) ] diff --git a/rtabmap_examples/launch/lidar3d_assemble_x2.launch.py b/rtabmap_examples/launch/lidar3d_assemble_x2.launch.py index 23da663f..accc9da3 100644 --- a/rtabmap_examples/launch/lidar3d_assemble_x2.launch.py +++ b/rtabmap_examples/launch/lidar3d_assemble_x2.launch.py @@ -201,7 +201,8 @@ def launch_setup(context: LaunchContext, *args, **kwargs): # Just for visualization Node( package='rtabmap_viz', executable='rtabmap_viz', output='screen', - parameters=[shared_parameters, rtabmap_parameters], + parameters=[shared_parameters, rtabmap_parameters, + {'odometry_node_name': "icp_odometry"}], remappings=remappings + [('scan_cloud', viz_topic)]) ] diff --git a/rtabmap_examples/launch/realsense_d435i_color.launch.py b/rtabmap_examples/launch/realsense_d435i_color.launch.py index ae4e2edb..c0eb257f 100644 --- a/rtabmap_examples/launch/realsense_d435i_color.launch.py +++ b/rtabmap_examples/launch/realsense_d435i_color.launch.py @@ -37,6 +37,15 @@ def generate_launch_description(): # Make sure IR emitter is enabled SetParameter(name='depth_module.emitter_enabled', value=1), + + DeclareLaunchArgument( + 'args', default_value='', + description='Extra arguments set to rtabmap and odometry nodes.'), + + DeclareLaunchArgument( + 'odom_args', default_value='', + description='Extra arguments just for odometry node. If the same argument is already set in \"args\", it will be overwritten by the one in \"odom_args\".'), + # Launch camera driver IncludeLaunchDescription( @@ -55,13 +64,14 @@ def generate_launch_description(): Node( package='rtabmap_odom', executable='rgbd_odometry', output='screen', parameters=parameters, + arguments=[LaunchConfiguration("args"), LaunchConfiguration("odom_args")], remappings=remappings), Node( package='rtabmap_slam', executable='rtabmap', output='screen', parameters=parameters, remappings=remappings, - arguments=['-d']), + arguments=['-d', LaunchConfiguration("args")]), Node( package='rtabmap_viz', executable='rtabmap_viz', output='screen', diff --git a/rtabmap_examples/launch/realsense_d435i_infra.launch.py b/rtabmap_examples/launch/realsense_d435i_infra.launch.py index c5284b2c..a3e6c017 100644 --- a/rtabmap_examples/launch/realsense_d435i_infra.launch.py +++ b/rtabmap_examples/launch/realsense_d435i_infra.launch.py @@ -35,6 +35,14 @@ def generate_launch_description(): DeclareLaunchArgument( 'unite_imu_method', default_value='2', description='0-None, 1-copy, 2-linear_interpolation. Use unite_imu_method:="1" if imu topics stop being published.'), + + DeclareLaunchArgument( + 'args', default_value='', + description='Extra arguments set to rtabmap and odometry nodes.'), + + DeclareLaunchArgument( + 'odom_args', default_value='', + description='Extra arguments just for odometry node. If the same argument is already set in \"args\", it will be overwritten by the one in \"odom_args\".'), #Hack to disable IR emitter SetParameter(name='depth_module.emitter_enabled', value=0), @@ -56,13 +64,14 @@ def generate_launch_description(): Node( package='rtabmap_odom', executable='rgbd_odometry', output='screen', parameters=parameters, + arguments=[LaunchConfiguration("args"), LaunchConfiguration("odom_args")], remappings=remappings), Node( package='rtabmap_slam', executable='rtabmap', output='screen', parameters=parameters, remappings=remappings, - arguments=['-d']), + arguments=['-d', LaunchConfiguration("args")]), Node( package='rtabmap_viz', executable='rtabmap_viz', output='screen', diff --git a/rtabmap_examples/launch/realsense_d435i_stereo.launch.py b/rtabmap_examples/launch/realsense_d435i_stereo.launch.py index ccf865d3..9dd9595a 100644 --- a/rtabmap_examples/launch/realsense_d435i_stereo.launch.py +++ b/rtabmap_examples/launch/realsense_d435i_stereo.launch.py @@ -16,11 +16,11 @@ from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration def generate_launch_description(): - parameters=[{ + parameters={ 'frame_id':'camera_link', 'subscribe_stereo':True, 'subscribe_odom_info':True, - 'wait_imu_to_init':True}] + 'wait_imu_to_init':True} remappings=[ ('imu', '/imu/data'), @@ -38,6 +38,15 @@ def generate_launch_description(): #Hack to disable IR emitter SetParameter(name='depth_module.emitter_enabled', value=0), + + DeclareLaunchArgument( + 'args', default_value='', + description='Extra arguments set to rtabmap and odometry nodes.'), + + DeclareLaunchArgument( + 'odom_args', default_value='', + description='Extra arguments just for odometry node. If the same argument is already set in \"args\", it will be overwritten by the one in \"odom_args\".'), + # Launch camera driver IncludeLaunchDescription( @@ -55,18 +64,20 @@ def generate_launch_description(): Node( package='rtabmap_odom', executable='stereo_odometry', output='screen', - parameters=parameters, + parameters=[parameters], + arguments=[LaunchConfiguration("args"), LaunchConfiguration("odom_args")], remappings=remappings), Node( package='rtabmap_slam', executable='rtabmap', output='screen', - parameters=parameters, + parameters=[parameters], remappings=remappings, - arguments=['-d']), + arguments=['-d', LaunchConfiguration("args")]), Node( package='rtabmap_viz', executable='rtabmap_viz', output='screen', - parameters=parameters, + parameters=[parameters, + {'odometry_node_name': "stereo_odometry"}], remappings=remappings), # Compute quaternion of the IMU diff --git a/rtabmap_launch/launch/rtabmap.launch.py b/rtabmap_launch/launch/rtabmap.launch.py index baa1b928..637254d7 100644 --- a/rtabmap_launch/launch/rtabmap.launch.py +++ b/rtabmap_launch/launch/rtabmap.launch.py @@ -40,7 +40,17 @@ class ConditionalBool(Substitution): return self.text_else def launch_setup(context, *args, **kwargs): - + + rtabmap_viz_odometry_node_name = "rgbd_odometry" + use_icp_odometry = LaunchConfiguration('icp_odometry').perform(context) + use_icp_odometry = use_icp_odometry == 'true' or use_icp_odometry == 'True' + use_stereo_odometry = LaunchConfiguration('stereo').perform(context) + use_stereo_odometry = use_stereo_odometry == 'true' or use_stereo_odometry == 'True' + if use_icp_odometry: + rtabmap_viz_odometry_node_name = "icp_odometry" + elif use_stereo_odometry: + rtabmap_viz_odometry_node_name = "stereo_odometry" + return [ DeclareLaunchArgument('depth', default_value=ConditionalText('false', 'true', IfCondition(PythonExpression(["'", LaunchConfiguration('stereo'), "' == 'true'"]))._predicate_func(context)), description=''), DeclareLaunchArgument('subscribe_rgb', default_value=LaunchConfiguration('depth'), description=''), @@ -359,7 +369,8 @@ def launch_setup(context, *args, **kwargs): "qos_scan": LaunchConfiguration('qos_scan'), "qos_odom": LaunchConfiguration('qos_odom'), "qos_camera_info": LaunchConfiguration('qos_camera_info'), - "qos_user_data": LaunchConfiguration('qos_user_data') + "qos_user_data": LaunchConfiguration('qos_user_data'), + "odometry_node_name": rtabmap_viz_odometry_node_name }], remappings=[ ("rgb/image", LaunchConfiguration('rgb_topic_relay')), diff --git a/rtabmap_odom/src/OdometryROS.cpp b/rtabmap_odom/src/OdometryROS.cpp index 9f7ec9ad..4176fee7 100644 --- a/rtabmap_odom/src/OdometryROS.cpp +++ b/rtabmap_odom/src/OdometryROS.cpp @@ -370,15 +370,16 @@ void OdometryROS::init(bool stereoParams, bool visParams, bool icpParams) odometry_->reset(initialPose_); } - resetSrv_ = this->create_service("reset_odom", std::bind(&OdometryROS::resetOdom, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - resetToPoseSrv_ = this->create_service("reset_odom_to_pose", std::bind(&OdometryROS::resetToPose, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - pauseSrv_ = this->create_service("pause_odom", std::bind(&OdometryROS::pause, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - resumeSrv_ = this->create_service("resume_odom", std::bind(&OdometryROS::resume, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + const std::string servicePrefix = get_name() + std::string("/"); + resetSrv_ = this->create_service(servicePrefix + "reset_odom", std::bind(&OdometryROS::resetOdom, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + resetToPoseSrv_ = this->create_service(servicePrefix + "reset_odom_to_pose", std::bind(&OdometryROS::resetToPose, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + pauseSrv_ = this->create_service(servicePrefix + "pause_odom", std::bind(&OdometryROS::pause, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + resumeSrv_ = this->create_service(servicePrefix + "resume_odom", std::bind(&OdometryROS::resume, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - setLogDebugSrv_ = this->create_service("log_debug", std::bind(&OdometryROS::setLogDebug, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - setLogInfoSrv_ = this->create_service("log_info", std::bind(&OdometryROS::setLogInfo, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - setLogWarnSrv_ = this->create_service("log_warning", std::bind(&OdometryROS::setLogWarn, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - setLogErrorSrv_ = this->create_service("log_error", std::bind(&OdometryROS::setLogError, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + setLogDebugSrv_ = this->create_service(servicePrefix + "log_debug", std::bind(&OdometryROS::setLogDebug, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + setLogInfoSrv_ = this->create_service(servicePrefix + "log_info", std::bind(&OdometryROS::setLogInfo, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + setLogWarnSrv_ = this->create_service(servicePrefix + "log_warning", std::bind(&OdometryROS::setLogWarn, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + setLogErrorSrv_ = this->create_service(servicePrefix + "log_error", std::bind(&OdometryROS::setLogError, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); odomStrategy_ = 0; Parameters::parse(this->parameters(), Parameters::kOdomStrategy(), odomStrategy_); diff --git a/rtabmap_viz/include/rtabmap_viz/GuiWrapper.h b/rtabmap_viz/include/rtabmap_viz/GuiWrapper.h index 4d7d90c5..aa676d6a 100644 --- a/rtabmap_viz/include/rtabmap_viz/GuiWrapper.h +++ b/rtabmap_viz/include/rtabmap_viz/GuiWrapper.h @@ -116,9 +116,9 @@ private: private: rtabmap::PreferencesDialog * prefDialog_; rtabmap::MainWindow * mainWindow_; - std::string cameraNodeName_; double lastOdomInfoUpdateTime_; std::string rtabmapNodeName_; + std::string odometryNodeName_; // odometry subscription stuffs std::string frameId_; diff --git a/rtabmap_viz/src/GuiWrapper.cpp b/rtabmap_viz/src/GuiWrapper.cpp index 872d8b95..9190794b 100644 --- a/rtabmap_viz/src/GuiWrapper.cpp +++ b/rtabmap_viz/src/GuiWrapper.cpp @@ -65,9 +65,9 @@ GuiWrapper::GuiWrapper(const rclcpp::NodeOptions & options) : Node("rtabmap_viz", options), rtabmap_sync::CommonDataSubscriber(*this, true), mainWindow_(0), - cameraNodeName_(""), lastOdomInfoUpdateTime_(0), rtabmapNodeName_("rtabmap"), + odometryNodeName_("rgbd_odometry"), frameId_("base_link"), odomFrameId_(""), waitForTransform_(0.2), // 200 ms @@ -93,7 +93,10 @@ GuiWrapper::GuiWrapper(const rclcpp::NodeOptions & options) : configFile.replace('~', QDir::homePath()); - rtabmapNodeName_ = this->declare_parameter("rtabmap", rtabmapNodeName_); + rtabmapNodeName_ = this->declare_parameter("rtabmap_node_name", rtabmapNodeName_); + odometryNodeName_ = this->declare_parameter("odometry_node_name", odometryNodeName_); + RCLCPP_INFO(get_logger(), "%s: rtabmap_node_name = %s", get_name(), rtabmapNodeName_.c_str()); + RCLCPP_INFO(get_logger(), "%s: odometry_node_name = %s", get_name(), odometryNodeName_.c_str()); RCLCPP_INFO(this->get_logger(), "rtabmap_viz: Using configuration from \"%s\"", configFile.toStdString().c_str()); uSleep(500); @@ -114,9 +117,17 @@ GuiWrapper::GuiWrapper(const rclcpp::NodeOptions & options) : waitForTransform_ = this->declare_parameter("wait_for_transform", waitForTransform_); odomSensorSync_ = this->declare_parameter("odom_sensor_sync", odomSensorSync_); maxOdomUpdateRate_ = this->declare_parameter("max_odom_update_rate", maxOdomUpdateRate_); - cameraNodeName_ = this->declare_parameter("camera_node_name", cameraNodeName_); // used to pause the rtabmap_ros/camera when pausing the process subscribeInfoOnly = this->declare_parameter("subscribe_info_only", subscribeInfoOnly); initCachePath = this->declare_parameter("init_cache_path", initCachePath); + + RCLCPP_INFO(get_logger(), "%s: frame_id = \"%s\"", get_name(), frameId_.c_str()); + RCLCPP_INFO(get_logger(), "%s: odom_frame_id = \"%s\"", get_name(), odomFrameId_.c_str()); + RCLCPP_INFO(get_logger(), "%s: wait_for_transform = %f", get_name(), waitForTransform_); + RCLCPP_INFO(get_logger(), "%s: odom_sensor_sync = %s", get_name(), odomSensorSync_?"true":"false"); + RCLCPP_INFO(get_logger(), "%s: max_odom_update_rate = %f", get_name(), maxOdomUpdateRate_); + RCLCPP_INFO(get_logger(), "%s: subscribe_info_only = %s", get_name(), subscribeInfoOnly?"true":"false"); + RCLCPP_INFO(get_logger(), "%s: init_cache_path = \"%s\"", get_name(), initCachePath.c_str()); + if(initCachePath.size()) { initCachePath = uReplaceChar(initCachePath, '~', UDirectory::homeDir()); @@ -327,7 +338,7 @@ bool GuiWrapper::callMapDataService(const std::string & name, bool global, bool } else { - RCLCPP_WARN(this->get_logger(), "Service \"%s\" not available.", name.c_str()); + RCLCPP_WARN(this->get_logger(), "Service \"%s\" not available. If necessary, you can remap expected rtabmap node with \"rtabmap_node_name\" parameter.", name.c_str()); } return false; } @@ -356,7 +367,7 @@ bool GuiWrapper::handleEvent(UEvent * anEvent) RCLCPP_INFO(this->get_logger(), "Parameters updated"); auto client = std::make_shared(this, rtabmapNodeName_); if (!client->wait_for_service(std::chrono::seconds(5))) { - RCLCPP_ERROR(this->get_logger(), "Can't call rtabmap parameters service, is the node running?"); + RCLCPP_ERROR(this->get_logger(), "Can't call \"%s\" parameters service, is the node running? If necessary, you can remap expected rtabmap node with \"rtabmap_node_name\" parameter.", rtabmapNodeName_.c_str()); } else { @@ -374,28 +385,18 @@ bool GuiWrapper::handleEvent(UEvent * anEvent) { if(!callEmptyService(rtabmapNodeName_+"/reset")) { - RCLCPP_ERROR(this->get_logger(), "Can't call \"reset\" service"); + RCLCPP_ERROR(this->get_logger(), "Can't call \"%s/reset\" service. If necessary, you can remap expected rtabmap node with \"rtabmap_node_name\" parameter.", rtabmapNodeName_.c_str()); } } else if(cmd == rtabmap::RtabmapEventCmd::kCmdPause) { - // Pause the camera if the rtabmap/camera node is used - if(!cameraNodeName_.empty()) - { - std::string str = uFormat("rosrun dynamic_reconfigure dynparam set %s pause true", cameraNodeName_.c_str()); - if(system(str.c_str()) !=0) - { - RCLCPP_ERROR(this->get_logger(), "Command \"%s\" returned non zero value.", str.c_str()); - } - } - - // Pause visual_odometry - callEmptyService("pause_odom"); + // Pause visual_odometry (can fail silently) + callEmptyService(odometryNodeName_ + "/pause_odom"); // Pause rtabmap if(!callEmptyService(rtabmapNodeName_+"/pause")) { - RCLCPP_ERROR(this->get_logger(), "Can't call \"pause\" service"); + RCLCPP_ERROR(this->get_logger(), "Can't call \"%s/pause\" service. If necessary, you can remap expected rtabmap node with \"rtabmap_node_name\" parameter.", rtabmapNodeName_.c_str()); } } else if(cmd == rtabmap::RtabmapEventCmd::kCmdResume) @@ -403,27 +404,17 @@ bool GuiWrapper::handleEvent(UEvent * anEvent) // Resume rtabmap if(!callEmptyService(rtabmapNodeName_+"/resume")) { - RCLCPP_ERROR(this->get_logger(), "Can't call \"resume\" service"); + RCLCPP_ERROR(this->get_logger(), "Can't call \"%s/resume\" service. If necessary, you can remap expected rtabmap node with \"rtabmap_node_name\" parameter.", rtabmapNodeName_.c_str()); } - // Pause visual_odometry - callEmptyService("resume_odom"); - - // Resume the camera if the rtabmap/camera node is used - if(!cameraNodeName_.empty()) - { - std::string str = uFormat("rosrun dynamic_reconfigure dynparam set %s pause false", cameraNodeName_.c_str()); - if(system(str.c_str()) !=0) - { - RCLCPP_ERROR(this->get_logger(), "Command \"%s\" returned non zero value.", str.c_str()); - } - } + // Pause visual_odometry (can fail silently) + callEmptyService(odometryNodeName_ + "/resume_odom"); } else if(cmd == rtabmap::RtabmapEventCmd::kCmdTriggerNewMap) { if(!callEmptyService(rtabmapNodeName_+"/trigger_new_map")) { - RCLCPP_ERROR(this->get_logger(), "Can't call \"trigger_new_map\" service"); + RCLCPP_ERROR(this->get_logger(), "Can't call \"%s/trigger_new_map\" service. If necessary, you can remap expected rtabmap node with \"rtabmap_node_name\" parameter.", rtabmapNodeName_.c_str()); } } else if(cmd == rtabmap::RtabmapEventCmd::kCmdPublish3DMap) @@ -466,14 +457,14 @@ bool GuiWrapper::handleEvent(UEvent * anEvent) } else { - RCLCPP_WARN(this->get_logger(), "Service \"set_goal\" not available."); + RCLCPP_WARN(this->get_logger(), "Service \"%s/set_goal\" not available. If necessary, you can remap expected rtabmap node with \"rtabmap_node_name\" parameter.", rtabmapNodeName_.c_str()); } } else if(cmd == rtabmap::RtabmapEventCmd::kCmdCancelGoal) { if(!callEmptyService(rtabmapNodeName_+"/cancel_goal")) { - RCLCPP_ERROR(this->get_logger(), "Can't call \"cancel_goal\" service"); + RCLCPP_ERROR(this->get_logger(), "Can't call \"%s/cancel_goal\" service. If necessary, you can remap expected rtabmap node with \"rtabmap_node_name\" parameter.", rtabmapNodeName_.c_str()); } } else if(cmd == rtabmap::RtabmapEventCmd::kCmdLabel) @@ -492,7 +483,7 @@ bool GuiWrapper::handleEvent(UEvent * anEvent) } else { - RCLCPP_WARN(this->get_logger(), "Service \"set_label\" not available."); + RCLCPP_WARN(this->get_logger(), "Service \"%s/set_label\" not available. If necessary, you can remap expected rtabmap node with \"rtabmap_node_name\" parameter.", rtabmapNodeName_.c_str()); } } else if(cmd == rtabmap::RtabmapEventCmd::kCmdRemoveLabel) @@ -508,7 +499,7 @@ bool GuiWrapper::handleEvent(UEvent * anEvent) } else { - RCLCPP_WARN(this->get_logger(), "Service \"remove_label\" not available."); + RCLCPP_WARN(this->get_logger(), "Service \"%s/remove_label\" not available. If necessary, you can remap expected rtabmap node with \"rtabmap_node_name\" parameter.", rtabmapNodeName_.c_str()); } } else if(cmd == rtabmap::RtabmapEventCmd::kCmdRepublishData) @@ -525,9 +516,9 @@ bool GuiWrapper::handleEvent(UEvent * anEvent) } else if(anEvent->getClassName().compare("OdometryResetEvent") == 0) { - if(!callEmptyService("reset_odom")) + if(!callEmptyService(odometryNodeName_ + "/reset_odom")) { - RCLCPP_ERROR(this->get_logger(), "Can't call \"reset_odom\" service, (will only work with rtabmap/visual_odometry node.)"); + RCLCPP_ERROR(this->get_logger(), "Can't call \"%s/reset_odom\" service (will only work with rtabmap's odometry nodes, you can remap the node name with \"odometry_node_name\" parameter)", odometryNodeName_.c_str()); } } return false; diff --git a/rtabmap_viz/src/PreferencesDialogROS.cpp b/rtabmap_viz/src/PreferencesDialogROS.cpp index cecd841e..dea76bf1 100644 --- a/rtabmap_viz/src/PreferencesDialogROS.cpp +++ b/rtabmap_viz/src/PreferencesDialogROS.cpp @@ -149,7 +149,7 @@ bool PreferencesDialogROS::readCoreSettings(const QString & filePath) auto client = std::make_shared(node, rtabmapNodeName_); if (!client->wait_for_service(std::chrono::seconds(5))) { - RCLCPP_ERROR(node_->get_logger(), "Can't call rtabmap parameters service, is the node running?"); + RCLCPP_ERROR(node_->get_logger(), "Can't call \"%s\" parameters service, is the node running? If necessary, you can remap the expected rtabmap node name with \"rtabmap_node_name\" parameter.", rtabmapNodeName_.c_str()); } int readCount = 0; if(client->service_is_ready()) From dc7c03e2fc0757761de0e80dfbaaa82674f400d1 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sun, 19 Oct 2025 17:51:56 -0700 Subject: [PATCH 26/56] ROS2 SuperPoint/SuperGlue example --- docker/jazzy/superpoint/Dockerfile | 127 +++++++++++++++++++++++++++++ docker/jazzy/superpoint/README.md | 59 ++++++++++++++ docker/jazzy/superpoint/launch.sh | 39 +++++++++ 3 files changed, 225 insertions(+) create mode 100644 docker/jazzy/superpoint/Dockerfile create mode 100644 docker/jazzy/superpoint/README.md create mode 100755 docker/jazzy/superpoint/launch.sh diff --git a/docker/jazzy/superpoint/Dockerfile b/docker/jazzy/superpoint/Dockerfile new file mode 100644 index 00000000..9fbe0a1e --- /dev/null +++ b/docker/jazzy/superpoint/Dockerfile @@ -0,0 +1,127 @@ +# Latest version with CUDA 12, to be compatible with Opencv 4.12.0 +FROM nvcr.io/nvidia/pytorch:25.06-py3 + +ENV DEBIAN_FRONTEND=noninteractive + +# Install build dependencies +RUN apt-get update && apt-get install -y \ + libsqlite3-dev \ + git \ + cmake \ + libyaml-cpp-dev \ + software-properties-common \ + pkg-config \ + wget \ + curl \ + build-essential && \ + apt-get clean && rm -rf /var/lib/apt/lists/ + +# Install ros keys +RUN export ROS_APT_SOURCE_VERSION=$(curl -s https://api.github.com/repos/ros-infrastructure/ros-apt-source/releases/latest | grep -F "tag_name" | awk -F\" '{print $4}') && \ + curl -L -o /tmp/ros2-apt-source.deb "https://github.com/ros-infrastructure/ros-apt-source/releases/download/${ROS_APT_SOURCE_VERSION}/ros2-apt-source_${ROS_APT_SOURCE_VERSION}.$(. /etc/os-release && echo ${UBUNTU_CODENAME:-${VERSION_CODENAME}})_all.deb" && \ + dpkg -i /tmp/ros2-apt-source.deb + +# Install ros dependencies +RUN apt-get update && \ + apt upgrade -y && \ + apt-get install -y \ + ros-jazzy-ros-base \ + ros-jazzy-rtabmap-ros \ + ros-jazzy-ros-environment \ + ros-jazzy-ament-cmake-auto \ + ros-jazzy-camera-info-manager \ + ros-jazzy-librealsense2 \ + python3-rosdep \ + python3-flake8-docstrings \ + python3-pip \ + python3-pytest-cov \ + ros-dev-tools && \ + apt-get remove -y ros-jazzy-rtabmap* libopencv* && \ + rosdep init && \ + rosdep update && \ + apt-get clean && rm -rf /var/lib/apt/lists/ + +# Optional: MRPT +RUN add-apt-repository ppa:joseluisblancoc/mrpt-stable -y && \ + apt-get update && apt install libmrpt-poses-dev -y && \ + apt-get clean && rm -rf /var/lib/apt/lists/ + +# Optional: OpenCV with xfeatures2d, cuda and nonfree modules (use same version used by jazzy to avoid cv_bridge conflicts) +RUN git clone -b 4.12.0 https://github.com/opencv/opencv_contrib.git && \ + git clone -b 4.12.0 https://github.com/opencv/opencv.git && \ + cd opencv && \ + mkdir build && \ + cd build && \ + cmake -DOPENCV_EXTRA_MODULES_PATH=/workspace/opencv_contrib/modules \ + -DCMAKE_CXX_STANDARD=17 \ + -DCMAKE_CUDA_STANDARD=17 \ + -DCMAKE_BUILD_TYPE=Release \ + -DBUILD_SHARED_LIBS=ON \ + -DBUILD_TESTS=OFF \ + -DBUILD_PERF_TESTS=OFF \ + -DOPENCV_ENABLE_NONFREE=ON \ + -DWITH_VTK=OFF \ + -DWITH_TBB=ON \ + -DWITH_CUDA=ON .. && \ + make -j6 && \ + make install && \ + cd /workspace && \ + rm -rf opencv opencv_contrib + +# Optional: OpenGV (multi-camera support) +RUN git clone https://github.com/laurentkneip/opengv.git && \ + cd opengv && \ + git checkout 91f4b19c73450833a40e463ad3648aae80b3a7f3 && \ + wget https://gist.githubusercontent.com/matlabbe/a412cf7c4627253874f81a00745a7fbb/raw/accc3acf465d1ffd0304a46b17741f62d4d354ef/opengv_disable_march_native.patch && \ + git apply opengv_disable_march_native.patch && \ + mkdir build && \ + cd build && \ + cmake -DCMAKE_BUILD_TYPE=Release .. && \ + make -j6 && \ + make install && \ + cd /workspace && \ + rm -r opengv + +# Setup catkin workspace +RUN mkdir -p ros2_ws/src +COPY . ros2_ws/src/rtabmap_ros + +# Get rtabmap library +# Create Superpoint model with current pytorch version +# Setup Superglue +# build ros packages (rebuild all packages depending on opencv) +RUN source /opt/ros/jazzy/setup.bash && \ + git clone -b jazzy https://github.com/ros-perception/image_pipeline.git ros2_ws/src/image_pipeline && \ + git clone -b 4.1.0 https://github.com/ros-perception/vision_opencv.git ros2_ws/src/vision_opencv && \ + git clone -b jazzy https://github.com/ros-perception/image_transport_plugins.git ros2_ws/src/image_transport_plugins && \ + git clone -b r/4.56.4 https://github.com/IntelRealSense/realsense-ros.git ros2_ws/src/realsense-ros && \ + git clone https://github.com/introlab/rtabmap ros2_ws/src/rtabmap && \ + cd ros2_ws/src/rtabmap/archive/2022-IlluminationInvariant/scripts && \ + wget https://github.com/magicleap/SuperPointPretrainedNetwork/raw/master/superpoint_v1.pth && \ + wget https://raw.githubusercontent.com/magicleap/SuperPointPretrainedNetwork/master/demo_superpoint.py && \ + python3 trace.py && \ + mv superpoint_v1.pt /workspace/. && \ + cd /workspace && \ + git clone https://github.com/magicleap/SuperGluePretrainedNetwork && \ + cp ros2_ws/src/rtabmap/corelib/src/python/rtabmap_superglue.py SuperGluePretrainedNetwork/. && \ + cd ros2_ws && \ + export MAKEFLAGS="-j6" && \ + colcon build --install-base /usr/local/ros --event-handlers console_direct+ --cmake-args \ + --no-warn-unused-cli \ + -DTorch_DIR=/usr/local/lib/python3.12/dist-packages/torch/share/cmake/Torch \ + -DWITH_TORCH=ON \ + -DWITH_PYTHON=ON \ + -DRTABMAP_SYNC_MULTI_RGBD=ON \ + -DCMAKE_BUILD_TYPE=Release \ + -DBUILD_TESTING=OFF && \ + cd /workspace && \ + rm -rf ros2_ws + +# Setup ROS entrypoint +RUN rm /bin/sh && ln -s /bin/bash /bin/sh + +RUN echo -e '#!/bin/bash\nset -e\n\n# setup ros2 environment\nsource "/opt/ros/jazzy/setup.bash"\nsource "/usr/local/ros/setup.bash"\nexec "$@"' > /ros_entrypoint.sh && \ + chmod +x /ros_entrypoint.sh +ENTRYPOINT [ "/ros_entrypoint.sh" ] + +RUN source /ros_entrypoint.sh && ldconfig \ No newline at end of file diff --git a/docker/jazzy/superpoint/README.md b/docker/jazzy/superpoint/README.md new file mode 100644 index 00000000..09311cb0 --- /dev/null +++ b/docker/jazzy/superpoint/README.md @@ -0,0 +1,59 @@ +Docker image example to include pytorch/CUDA support (SuperPoint, SuperGlue, OpenCV+nonfree+xfeatures2d) + +# Create image: +```bash +cd rtabmap_ros +docker build -t rtabmap_ros:superpoint -f docker/jazzy/superpoint/Dockerfile . +``` +# Example of usage: + +We launch the [realsense_d435i_infra.launch.py](https://github.com/introlab/rtabmap_ros/blob/ros2/rtabmap_examples/launch/realsense_d435i_infra.launch.py) example with arguments to use superpoint + superglue for loop closure detection. Note that visual odometry is done with default parameters in this case. + +```bash +# X11 Setup for rtabmap_viz, not required if you don't launch any UI +XAUTH=/tmp/.docker.xauth +touch $XAUTH +xauth nlist $DISPLAY | sed -e 's/^..../ffff/' | xauth -f $XAUTH nmerge - + +# Docker Run Command +docker run -it --rm \ + --user $(id -u) \ + --privileged \ + --gpus all \ + -e LD_PRELOAD="/opt/hpcx/ucc/lib/libucc.so.1" \ + -e NVIDIA_VISIBLE_DEVICES=all \ + -e NVIDIA_DRIVER_CAPABILITIES=all \ + -e DISPLAY=$DISPLAY \ + -e QT_X11_NO_MITSHM=1 \ + -e XAUTHORITY=$XAUTH \ + -v $XAUTH:$XAUTH \ + -v /tmp/.X11-unix:/tmp/.X11-unix \ + -e ROS_HOME=/tmp/.ros \ + --network host \ + -v ~/.ros:/tmp/.ros \ + rtabmap_ros:superpoint \ + ros2 launch rtabmap_examples realsense_d435i_infra.launch.py \ + args:=" \ + --SuperPoint/ModelPath /workspace/superpoint_v1.pt \ + --PyMatcher/Path /workspace/SuperGluePretrainedNetwork/rtabmap_superglue.py \ + --Kp/DetectorStrategy 11 \ + --Kp/NndrRatio 0.6 \ + --Vis/CorNNType 6 \ + --Vis/CorNNDR 0.6 \ + --Reg/RepeatOnce false \ + --Vis/CorGuessWinSize 0" \ + odom_args:=" \ + --Vis/CorNNType 1 \ + --Reg/RepeatOnce true \ + --Vis/CorGuessWinSize 40 \ + --Vis/CorNNDR 0.8" +``` + +The resulting database will be saved to `~/.ros/rtabmap.db` on the host computer. You can also use the `launch.sh` file in this folder for convenience. + +To use superpoint for odometry, remove `odom_args` and add this to `args`: +```bash +--Vis/FeatureType 11 \ +``` + +Performance tip: to avoid extracting again in `rtabmap` superpoint features already extracted in `rgbd_odometry`, we would need to edit `realsense_d435i_infra.launch.py` and add the parameter `subscribe_sensor_data:=true` to `rtabmap` and `rtabmap_viz`, then remap `sensor_data:=odom_sensor_data/raw`. \ No newline at end of file diff --git a/docker/jazzy/superpoint/launch.sh b/docker/jazzy/superpoint/launch.sh new file mode 100755 index 00000000..56b6df97 --- /dev/null +++ b/docker/jazzy/superpoint/launch.sh @@ -0,0 +1,39 @@ +#!/bin/bash + +# X11 Setup +XAUTH=/tmp/.docker.xauth +touch $XAUTH +xauth nlist $DISPLAY | sed -e 's/^..../ffff/' | xauth -f $XAUTH nmerge - + +# Docker Run Command +docker run -it --rm \ + --user $(id -u) \ + --privileged \ + --gpus all \ + -e LD_PRELOAD="/opt/hpcx/ucc/lib/libucc.so.1" \ + -e NVIDIA_VISIBLE_DEVICES=all \ + -e NVIDIA_DRIVER_CAPABILITIES=all \ + -e DISPLAY=$DISPLAY \ + -e QT_X11_NO_MITSHM=1 \ + -e XAUTHORITY=$XAUTH \ + -v $XAUTH:$XAUTH \ + -v /tmp/.X11-unix:/tmp/.X11-unix \ + -e ROS_HOME=/tmp/.ros \ + --network host \ + -v ~/.ros:/tmp/.ros \ + rtabmap_ros:superpoint \ + ros2 launch rtabmap_examples realsense_d435i_infra.launch.py \ + args:=" \ + --SuperPoint/ModelPath /workspace/superpoint_v1.pt \ + --PyMatcher/Path /workspace/SuperGluePretrainedNetwork/rtabmap_superglue.py \ + --Kp/DetectorStrategy 11 \ + --Kp/NndrRatio 0.6 \ + --Vis/CorNNType 6 \ + --Vis/CorNNDR 0.6 \ + --Reg/RepeatOnce false \ + --Vis/CorGuessWinSize 0" \ + odom_args:=" \ + --Vis/CorNNType 1 \ + --Reg/RepeatOnce true \ + --Vis/CorGuessWinSize 40 \ + --Vis/CorNNDR 0.8" From 1ba62c224e90025f530b271bda3cb8d5b761daba Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sun, 26 Oct 2025 18:37:22 -0700 Subject: [PATCH 27/56] Porting rtabmap_costmap_plugins (Voxel Layer) to ROS2 (#1373) * Porting rtabmap_costmap_plugins (Voxel Layer) to ROS2 * fixed voxel grid * Ported voxel_marker * ported patrol.py --- rtabmap_costmap_plugins/CMakeLists.txt | 85 +++ rtabmap_costmap_plugins/costmap_plugins.xml | 5 + .../rtabmap_costmap_plugins/visibility.h | 58 ++ .../rtabmap_costmap_plugins/voxel_layer.hpp | 293 +++++++++ rtabmap_costmap_plugins/package.xml | 26 + rtabmap_costmap_plugins/src/voxel_layer.cpp | 600 ++++++++++++++++++ rtabmap_costmap_plugins/src/voxel_marker.cpp | 152 +++++ rtabmap_util/CMakeLists.txt | 2 +- rtabmap_util/scripts/patrol.py | 169 ++--- 9 files changed, 1313 insertions(+), 77 deletions(-) create mode 100644 rtabmap_costmap_plugins/CMakeLists.txt create mode 100644 rtabmap_costmap_plugins/costmap_plugins.xml create mode 100644 rtabmap_costmap_plugins/include/rtabmap_costmap_plugins/visibility.h create mode 100644 rtabmap_costmap_plugins/include/rtabmap_costmap_plugins/voxel_layer.hpp create mode 100644 rtabmap_costmap_plugins/package.xml create mode 100644 rtabmap_costmap_plugins/src/voxel_layer.cpp create mode 100644 rtabmap_costmap_plugins/src/voxel_marker.cpp diff --git a/rtabmap_costmap_plugins/CMakeLists.txt b/rtabmap_costmap_plugins/CMakeLists.txt new file mode 100644 index 00000000..390fa065 --- /dev/null +++ b/rtabmap_costmap_plugins/CMakeLists.txt @@ -0,0 +1,85 @@ +cmake_minimum_required(VERSION 3.5) +project(rtabmap_costmap_plugins) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake_ros REQUIRED) +find_package(pluginlib REQUIRED) +find_package(rclcpp REQUIRED) +find_package(nav2_costmap_2d REQUIRED) +find_package(visualization_msgs REQUIRED) + +include_directories( + ${CMAKE_CURRENT_SOURCE_DIR}/include +) + +SET(Libraries + pluginlib + rclcpp + nav2_costmap_2d + visualization_msgs +) + +########### +## Build ## +########### + +add_library(rtabmap_costmap_plugins SHARED + src/voxel_layer.cpp +) +target_include_directories(rtabmap_costmap_plugins + PUBLIC + $ + $ +) + +IF("$ENV{ROS_DISTRO}" STRLESS "jazzy") + target_compile_definitions(rtabmap_costmap_plugins PRIVATE -DPRE_ROS_JAZZY) +ENDIF() + +ament_target_dependencies(rtabmap_costmap_plugins ${Libraries}) + +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(rtabmap_costmap_plugins PRIVATE "RTABMAP_ROS_BUILDING_LIBRARY") + +# prevent pluginlib from using boost +target_compile_definitions(rtabmap_costmap_plugins PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") + +pluginlib_export_plugin_description_file(nav2_costmap_2d costmap_plugins.xml) + +add_executable(rtabmap_costmap_voxel_marker src/voxel_marker.cpp) +ament_target_dependencies(rtabmap_costmap_voxel_marker ${Libraries}) +set_target_properties(rtabmap_costmap_voxel_marker PROPERTIES OUTPUT_NAME "voxel_marker") + +############# +## Install ## +############# + +ament_export_dependencies(${Libraries}) +ament_export_include_directories(include) +ament_export_targets(${PROJECT_NAME}) # To include downstream with targets +ament_export_libraries(rtabmap_costmap_plugins) # To include downstream without targets + +install(TARGETS + rtabmap_costmap_plugins + EXPORT ${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin + INCLUDES DESTINATION include +) + +install(TARGETS + rtabmap_costmap_voxel_marker + DESTINATION lib/${PROJECT_NAME} +) + +install(DIRECTORY include/ + DESTINATION include + FILES_MATCHING PATTERN "*.h" +) + +ament_package() diff --git a/rtabmap_costmap_plugins/costmap_plugins.xml b/rtabmap_costmap_plugins/costmap_plugins.xml new file mode 100644 index 00000000..a0b3e75a --- /dev/null +++ b/rtabmap_costmap_plugins/costmap_plugins.xml @@ -0,0 +1,5 @@ + + + Similar to nav2_costmap_2d::VoxelLayer, but can also move along z-axis. + + \ No newline at end of file diff --git a/rtabmap_costmap_plugins/include/rtabmap_costmap_plugins/visibility.h b/rtabmap_costmap_plugins/include/rtabmap_costmap_plugins/visibility.h new file mode 100644 index 00000000..074d0882 --- /dev/null +++ b/rtabmap_costmap_plugins/include/rtabmap_costmap_plugins/visibility.h @@ -0,0 +1,58 @@ +// Copyright 2016 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RTABMAP_COSTMAP_PLUGINS__VISIBILITY_CONTROL_H_ +#define RTABMAP_COSTMAP_PLUGINS__VISIBILITY_CONTROL_H_ + +#ifdef __cplusplus +extern "C" +{ +#endif + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ + #ifdef __GNUC__ + #define RTABMAP_COSTMAP_PLUGINS_EXPORT __attribute__ ((dllexport)) + #define RTABMAP_COSTMAP_PLUGINS_IMPORT __attribute__ ((dllimport)) + #else + #define RTABMAP_COSTMAP_PLUGINS_EXPORT __declspec(dllexport) + #define RTABMAP_COSTMAP_PLUGINS_IMPORT __declspec(dllimport) + #endif + #ifdef RTABMAP_COSTMAP_PLUGINS_BUILDING_DLL + #define RTABMAP_COSTMAP_PLUGINS_PUBLIC RTABMAP_COSTMAP_PLUGINS_EXPORT + #else + #define RTABMAP_COSTMAP_PLUGINS_PUBLIC RTABMAP_COSTMAP_PLUGINS_IMPORT + #endif + #define RTABMAP_COSTMAP_PLUGINS_PUBLIC_TYPE RTABMAP_COSTMAP_PLUGINS_PUBLIC + #define RTABMAP_COSTMAP_PLUGINS_LOCAL +#else + #define RTABMAP_COSTMAP_PLUGINS_EXPORT __attribute__ ((visibility("default"))) + #define RTABMAP_COSTMAP_PLUGINS_IMPORT + #if __GNUC__ >= 4 + #define RTABMAP_COSTMAP_PLUGINS_PUBLIC __attribute__ ((visibility("default"))) + #define RTABMAP_COSTMAP_PLUGINS_LOCAL __attribute__ ((visibility("hidden"))) + #else + #define RTABMAP_COSTMAP_PLUGINS_PUBLIC + #define RTABMAP_COSTMAP_PLUGINS_LOCAL + #endif + #define RTABMAP_COSTMAP_PLUGINS_PUBLIC_TYPE +#endif + +#ifdef __cplusplus +} +#endif + +#endif // RTABMAP_COSTMAP_PLUGINS__VISIBILITY_CONTROL_H_ diff --git a/rtabmap_costmap_plugins/include/rtabmap_costmap_plugins/voxel_layer.hpp b/rtabmap_costmap_plugins/include/rtabmap_costmap_plugins/voxel_layer.hpp new file mode 100644 index 00000000..affc632d --- /dev/null +++ b/rtabmap_costmap_plugins/include/rtabmap_costmap_plugins/voxel_layer.hpp @@ -0,0 +1,293 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2008, 2013, Willow Garage, Inc. + * 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 Willow Garage, Inc. 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 OWNER 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. + * + * Author: Eitan Marder-Eppstein + * David V. Lu!! + *********************************************************************/ +#ifndef RTABMAP_COSTMAP_PLUGINS__VOXEL_LAYER_HPP_ +#define RTABMAP_COSTMAP_PLUGINS__VOXEL_LAYER_HPP_ + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace rtabmap_costmap_plugins +{ + +/** + * @class VoxelLayer + * @brief Takes laser and pointcloud data to populate a 3D voxel representation of the environment + */ +class VoxelLayer : public nav2_costmap_2d::ObstacleLayer +{ +public: + RTABMAP_COSTMAP_PLUGINS_PUBLIC + VoxelLayer() + : voxel_grid_(0, 0, 0) + { + costmap_ = NULL; // this is the unsigned char* member of parent class's parent class Costmap2D + } + + /** + * @brief Voxel Layer destructor + */ + virtual ~VoxelLayer(); + + /** + * @brief Initialization process of layer on startup + */ + virtual void onInitialize(); + + /** + * @brief Update the bounds of the master costmap by this layer's update dimensions + * @param robot_x X pose of robot + * @param robot_y Y pose of robot + * @param robot_yaw Robot orientation + * @param min_x X min map coord of the window to update + * @param min_y Y min map coord of the window to update + * @param max_x X max map coord of the window to update + * @param max_y Y max map coord of the window to update + */ + virtual void updateBounds( + double robot_x, double robot_y, double robot_yaw, double * min_x, + double * min_y, + double * max_x, + double * max_y); + + /** + * @brief Update the layer's origin to a new pose, often when in a rolling costmap + */ + void updateOrigin(double new_origin_x, double new_origin_y); + + /** + * @brief If layer is discretely populated + */ + bool isDiscretized() + { + return true; + } + + /** + * @brief Match the size of the master costmap + */ + virtual void matchSize(); + + /** + * @brief Reset this costmap + */ + virtual void reset(); + + /** + * @brief If clearing operations should be processed on this layer or not + */ + virtual bool isClearable() {return true;} + +protected: + /** + * @brief Reset internal maps + */ + virtual void resetMaps(); + + /** + * @brief Use raycasting between 2 points to clear freespace + */ + virtual void raytraceFreespace( + const nav2_costmap_2d::Observation & clearing_observation, + double * min_x, double * min_y, + double * max_x, + double * max_y); + + bool publish_voxel_; + std::string robot_base_frame_; + rclcpp::Publisher::SharedPtr voxel_pub_; + nav2_voxel_grid::VoxelGrid voxel_grid_; + double z_resolution_, origin_z_; + int unknown_threshold_, mark_threshold_, size_z_; + rclcpp::Publisher::SharedPtr + clearing_endpoints_pub_; + + /** + * @brief Convert world coordinates into map coordinates + */ + inline bool worldToMap3DFloat( + double wx, double wy, double wz, double & mx, double & my, + double & mz) + { + if (wx < origin_x_ || wy < origin_y_ || wz < origin_z_) { + return false; + } + mx = ((wx - origin_x_) / resolution_); + my = ((wy - origin_y_) / resolution_); + mz = ((wz - origin_z_) / z_resolution_); + if (mx < size_x_ && my < size_y_ && mz < size_z_) { + return true; + } + + return false; + } + + /** + * @brief Convert world coordinates into map coordinates + */ + inline bool worldToMap3D( + double wx, double wy, double wz, unsigned int & mx, unsigned int & my, + unsigned int & mz) + { + if (wx < origin_x_ || wy < origin_y_ || wz < origin_z_) { + return false; + } + + mx = static_cast((wx - origin_x_) / resolution_); + my = static_cast((wy - origin_y_) / resolution_); + mz = static_cast((wz - origin_z_) / z_resolution_); + + if (mx < size_x_ && my < size_y_ && mz < (unsigned int)size_z_) { + return true; + } + + return false; + } + + /** + * @brief Convert map coordinates into world coordinates + */ + inline void mapToWorld3D( + unsigned int mx, unsigned int my, unsigned int mz, double & wx, + double & wy, + double & wz) + { + // returns the center point of the cell + wx = origin_x_ + (mx + 0.5) * resolution_; + wy = origin_y_ + (my + 0.5) * resolution_; + wz = origin_z_ + (mz + 0.5) * z_resolution_; + } + + /** + * @brief Find L2 norm distance in 3D + */ + inline double dist(double x0, double y0, double z0, double x1, double y1, double z1) + { + return sqrt((x1 - x0) * (x1 - x0) + (y1 - y0) * (y1 - y0) + (z1 - z0) * (z1 - z0)); + } + + /** + * @brief Get the height of the voxel sizes in meters + */ + double getSizeInMetersZ() const + { + return (size_z_ - 1 + 0.5) * z_resolution_; + } + + /** + * @brief Copy a region of a source map into a destination map + * @param source_map The source map + * @param sm_lower_left_x The lower left x point of the source map to start the copy + * @param sm_lower_left_y The lower left y point of the source map to start the copy + * @param sm_size_x The x size of the source map + * @param dest_map The destination map + * @param dm_lower_left_x The lower left x point of the destination map to start the copy + * @param dm_lower_left_y The lower left y point of the destination map to start the copy + * @param dm_size_x The x size of the destination map + * @param region_size_x The x size of the region to copy + * @param region_size_y The y size of the region to copy + */ + template + void copyMapRegion3D( + data_type * source_map, unsigned int sm_lower_left_x, + unsigned int sm_lower_left_y, + unsigned int sm_size_x, data_type * dest_map, unsigned int dm_lower_left_x, + unsigned int dm_lower_left_y, unsigned int dm_size_x, unsigned int region_size_x, + unsigned int region_size_y, int z_shift) + { + // we'll first need to compute the starting points for each map + // this is like getting voxel column. We are not taking into account the z position of the voxel + data_type * sm_index = source_map + (sm_lower_left_y * sm_size_x + sm_lower_left_x); + data_type * dm_index = dest_map + (dm_lower_left_y * dm_size_x + dm_lower_left_x); + + uint32_t marked_bits_mask = (data_type) 0xFFFF0000; + uint32_t unknown_bits_mask = (data_type) 0x0000FFFF; + + // now, we'll copy the source map into the destination map + for (unsigned int i = 0; i < region_size_y; ++i) { + memcpy(dm_index, sm_index, region_size_x * sizeof(data_type)); + + for (unsigned int j = 0; j < region_size_x; j++) { + // known marked: 11 = 2 bits, unknown: 01 = 1 bit, known free: 00 = 0 bits + if (z_shift > 0) { + dm_index[j] = + // Shift marked cells, insert zeros for new unknowns + ((dm_index[j] & marked_bits_mask) >> z_shift & marked_bits_mask) | + // Shift empty/unknown cells, insert ones for new unknowns + (((dm_index[j] & unknown_bits_mask) >> z_shift | (~((data_type) 0) << (sizeof(data_type) * 4 - z_shift))) & unknown_bits_mask); + + } else if (z_shift < 0) { + dm_index[j] = + // Shift marked cells, insert zeros for new unknowns + (dm_index[j] & marked_bits_mask) << z_shift * -1 | + // Shift empty/unknown cells, insert ones for new unknowns + ((dm_index[j] << z_shift * -1 & unknown_bits_mask) | ~(~((data_type) 0) << z_shift * -1)); + } + } + + sm_index += sm_size_x; + dm_index += dm_size_x; + } + } + + /** + * @brief Callback executed when a parameter change is detected + * @param event ParameterEvent message + */ + rcl_interfaces::msg::SetParametersResult + dynamicParametersCallback(std::vector parameters); + + // Dynamic parameters handler + rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_; +}; + +} // namespace rtabmap_costmap_plugins + +#endif // RTABMAP_COSTMAP_PLUGINS__VOXEL_LAYER_HPP_ diff --git a/rtabmap_costmap_plugins/package.xml b/rtabmap_costmap_plugins/package.xml new file mode 100644 index 00000000..a93af65d --- /dev/null +++ b/rtabmap_costmap_plugins/package.xml @@ -0,0 +1,26 @@ + + + + rtabmap_costmap_plugins + 0.22.1 + RTAB-Map's costmap plugins. + Mathieu Labbe + Mathieu Labbe + BSD + https://github.com/introlab/rtabmap_ros/issues + https://github.com/introlab/rtabmap_ros + + ament_cmake_ros + + ros_environment + + pluginlib + rclcpp + nav2_costmap_2d + visualization_msgs + + + ament_cmake + + + diff --git a/rtabmap_costmap_plugins/src/voxel_layer.cpp b/rtabmap_costmap_plugins/src/voxel_layer.cpp new file mode 100644 index 00000000..dcb6ca3f --- /dev/null +++ b/rtabmap_costmap_plugins/src/voxel_layer.cpp @@ -0,0 +1,600 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2008, 2013, Willow Garage, Inc. + * 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 Willow Garage, Inc. 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 OWNER 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. + * + * Author: Eitan Marder-Eppstein + * David V. Lu!! + *********************************************************************/ + +#include "rtabmap_costmap_plugins/voxel_layer.hpp" + +#include +#include +#include +#include +#include + +#include "pluginlib/class_list_macros.hpp" +#include "sensor_msgs/point_cloud2_iterator.hpp" + +#define VOXEL_BITS 16 +PLUGINLIB_EXPORT_CLASS(rtabmap_costmap_plugins::VoxelLayer, nav2_costmap_2d::Layer) + +using nav2_costmap_2d::NO_INFORMATION; +using nav2_costmap_2d::LETHAL_OBSTACLE; +using nav2_costmap_2d::FREE_SPACE; +using rcl_interfaces::msg::ParameterType; + +namespace rtabmap_costmap_plugins +{ + +void VoxelLayer::onInitialize() +{ + nav2_costmap_2d::ObstacleLayer::onInitialize(); + + declareParameter("enabled", rclcpp::ParameterValue(true)); + declareParameter("footprint_clearing_enabled", rclcpp::ParameterValue(true)); + declareParameter("min_obstacle_height", rclcpp::ParameterValue(0.0)); + declareParameter("max_obstacle_height", rclcpp::ParameterValue(2.0)); + declareParameter("z_voxels", rclcpp::ParameterValue(10)); + declareParameter("origin_z", rclcpp::ParameterValue(0.0)); + declareParameter("z_resolution", rclcpp::ParameterValue(0.2)); + declareParameter("unknown_threshold", rclcpp::ParameterValue(15)); + declareParameter("mark_threshold", rclcpp::ParameterValue(0)); + declareParameter("combination_method", rclcpp::ParameterValue(1)); + declareParameter("publish_voxel_map", rclcpp::ParameterValue(false)); + declareParameter("robot_base_frame", rclcpp::ParameterValue("base_link")); + + auto node = node_.lock(); + if (!node) { + throw std::runtime_error{"Failed to lock node"}; + } + + node->get_parameter(name_ + "." + "enabled", enabled_); + node->get_parameter(name_ + "." + "footprint_clearing_enabled", footprint_clearing_enabled_); + node->get_parameter(name_ + "." + "min_obstacle_height", min_obstacle_height_); + node->get_parameter(name_ + "." + "max_obstacle_height", max_obstacle_height_); + node->get_parameter(name_ + "." + "z_voxels", size_z_); + node->get_parameter(name_ + "." + "origin_z", origin_z_); + node->get_parameter(name_ + "." + "z_resolution", z_resolution_); + node->get_parameter(name_ + "." + "unknown_threshold", unknown_threshold_); + node->get_parameter(name_ + "." + "mark_threshold", mark_threshold_); + node->get_parameter(name_ + "." + "publish_voxel_map", publish_voxel_); + node->get_parameter(name_ + "." + "robot_base_frame", robot_base_frame_); + + int combination_method_param{}; + node->get_parameter(name_ + "." + "combination_method", combination_method_param); +#ifdef PRE_ROS_JAZZY + combination_method_ = combination_method_param; +#else + combination_method_ = combination_method_from_int(combination_method_param); +#endif + + if (publish_voxel_) { + voxel_pub_ = node->create_publisher( + "voxel_grid", rclcpp::QoS(1).transient_local()); + //voxel_pub_->on_activate(); + } + + clearing_endpoints_pub_ = node->create_publisher( + "clearing_endpoints", rclcpp::QoS(1).transient_local()); + //clearing_endpoints_pub_->on_activate(); + + unknown_threshold_ += (VOXEL_BITS - size_z_); + matchSize(); + + // Add callback for dynamic parameters + dyn_params_handler_ = node->add_on_set_parameters_callback( + std::bind( + &VoxelLayer::dynamicParametersCallback, + this, std::placeholders::_1)); +} + +VoxelLayer::~VoxelLayer() +{ + auto node = node_.lock(); + if (dyn_params_handler_ && node) { + node->remove_on_set_parameters_callback(dyn_params_handler_.get()); + } + dyn_params_handler_.reset(); +} + +void VoxelLayer::matchSize() +{ + std::lock_guard guard(*getMutex()); + ObstacleLayer::matchSize(); + voxel_grid_.resize(size_x_, size_y_, size_z_); + assert(voxel_grid_.sizeX() == size_x_ && voxel_grid_.sizeY() == size_y_); +} + +void VoxelLayer::reset() +{ + // Call the base class method before adding our own functionality + ObstacleLayer::reset(); + resetMaps(); +} + +void VoxelLayer::resetMaps() +{ + // Call the base class method before adding our own functionality + // Note: at the time this was written, ObstacleLayer doesn't implement + // resetMaps so this goes to the next layer down Costmap2DLayer which also + // doesn't implement this, so it actually goes all the way to Costmap2D + ObstacleLayer::resetMaps(); + voxel_grid_.reset(); +} + +void VoxelLayer::updateBounds( + double robot_x, double robot_y, double robot_yaw, double * min_x, + double * min_y, double * max_x, double * max_y) +{ + std::lock_guard guard(*getMutex()); + + if (rolling_window_) { + updateOrigin(robot_x - getSizeInMetersX() / 2, robot_y - getSizeInMetersY() / 2); + } + if (!enabled_) { + return; + } + useExtraBounds(min_x, min_y, max_x, max_y); + + bool current = true; + std::vector observations, clearing_observations; + + // get the marking observations + current = getMarkingObservations(observations) && current; + + // get the clearing observations + current = getClearingObservations(clearing_observations) && current; + + // update the global current status + current_ = current; + + // raytrace freespace + for (unsigned int i = 0; i < clearing_observations.size(); ++i) { + raytraceFreespace(clearing_observations[i], min_x, min_y, max_x, max_y); + } + + // place the new obstacles into a priority queue... each with a priority of zero to begin with + for (std::vector::const_iterator it = observations.begin(); it != observations.end(); + ++it) + { + const nav2_costmap_2d::Observation & obs = *it; + + const sensor_msgs::msg::PointCloud2 & cloud = *(obs.cloud_); + + double sq_obstacle_max_range = obs.obstacle_max_range_ * obs.obstacle_max_range_; + double sq_obstacle_min_range = obs.obstacle_min_range_ * obs.obstacle_min_range_; + + sensor_msgs::PointCloud2ConstIterator iter_x(cloud, "x"); + sensor_msgs::PointCloud2ConstIterator iter_y(cloud, "y"); + sensor_msgs::PointCloud2ConstIterator iter_z(cloud, "z"); + + for (; iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z) { + // if the obstacle is too low, we won't add it + if (*iter_z < min_obstacle_height_) { + continue; + } + + // if the obstacle is too high or too far away from the robot we won't add it + if (*iter_z > max_obstacle_height_) { + continue; + } + + // compute the squared distance from the hitpoint to the pointcloud's origin + double sq_dist = (*iter_x - obs.origin_.x) * (*iter_x - obs.origin_.x) + + (*iter_y - obs.origin_.y) * (*iter_y - obs.origin_.y) + + (*iter_z - obs.origin_.z) * (*iter_z - obs.origin_.z); + + // if the point is far enough away... we won't consider it + if (sq_dist >= sq_obstacle_max_range) { + continue; + } + + // If the point is too close, do not consider it + if (sq_dist < sq_obstacle_min_range) { + continue; + } + + // now we need to compute the map coordinates for the observation + unsigned int mx, my, mz; + if (!worldToMap3D(*iter_x, *iter_y, *iter_z, mx, my, mz)) { + continue; + } + + // mark the cell in the voxel grid and check if we should also mark it in the costmap + if (voxel_grid_.markVoxelInMap(mx, my, mz, mark_threshold_)) { + unsigned int index = getIndex(mx, my); + + costmap_[index] = LETHAL_OBSTACLE; + touch( + static_cast(*iter_x), static_cast(*iter_y), + min_x, min_y, max_x, max_y); + } + } + } + + if (publish_voxel_) { + auto grid_msg = std::make_unique(); + unsigned int size = voxel_grid_.sizeX() * voxel_grid_.sizeY(); + grid_msg->size_x = voxel_grid_.sizeX(); + grid_msg->size_y = voxel_grid_.sizeY(); + grid_msg->size_z = voxel_grid_.sizeZ(); + grid_msg->data.resize(size); + memcpy(&grid_msg->data[0], voxel_grid_.getData(), size * sizeof(unsigned int)); + + grid_msg->origin.x = origin_x_; + grid_msg->origin.y = origin_y_; + grid_msg->origin.z = origin_z_; + + grid_msg->resolutions.x = resolution_; + grid_msg->resolutions.y = resolution_; + grid_msg->resolutions.z = z_resolution_; + grid_msg->header.frame_id = global_frame_; + grid_msg->header.stamp = clock_->now(); + + voxel_pub_->publish(std::move(grid_msg)); + } + + updateFootprint(robot_x, robot_y, robot_yaw, min_x, min_y, max_x, max_y); +} + +void VoxelLayer::raytraceFreespace( + const nav2_costmap_2d::Observation & clearing_observation, double * min_x, + double * min_y, + double * max_x, + double * max_y) +{ + auto clearing_endpoints_ = std::make_unique(); + + if (clearing_observation.cloud_->height == 0 || clearing_observation.cloud_->width == 0) { + return; + } + + double sensor_x, sensor_y, sensor_z; + double ox = clearing_observation.origin_.x; + double oy = clearing_observation.origin_.y; + double oz = clearing_observation.origin_.z; + + if (!worldToMap3DFloat(ox, oy, oz, sensor_x, sensor_y, sensor_z)) { + RCLCPP_WARN( + logger_, + "Sensor origin at (%.2f, %.2f %.2f) is out of map bounds " + "(%.2f, %.2f, %.2f) to (%.2f, %.2f, %.2f). " + "The costmap cannot raytrace for it.", + ox, oy, oz, + origin_x_, origin_y_, origin_z_, + origin_x_ + getSizeInMetersX(), origin_y_ + getSizeInMetersY(), + origin_z_ + getSizeInMetersZ()); + + return; + } + + bool publish_clearing_points; + + { + auto node = node_.lock(); + if (!node) { + throw std::runtime_error{"Failed to lock node"}; + } + publish_clearing_points = (node->count_subscribers("clearing_endpoints") > 0); + } + + clearing_endpoints_->data.clear(); + clearing_endpoints_->width = clearing_observation.cloud_->width; + clearing_endpoints_->height = clearing_observation.cloud_->height; + clearing_endpoints_->is_dense = true; + clearing_endpoints_->is_bigendian = false; + + sensor_msgs::PointCloud2Modifier modifier(*clearing_endpoints_); + modifier.setPointCloud2Fields( + 3, "x", 1, sensor_msgs::msg::PointField::FLOAT32, + "y", 1, sensor_msgs::msg::PointField::FLOAT32, + "z", 1, sensor_msgs::msg::PointField::FLOAT32); + + sensor_msgs::PointCloud2Iterator clearing_endpoints_iter_x(*clearing_endpoints_, "x"); + sensor_msgs::PointCloud2Iterator clearing_endpoints_iter_y(*clearing_endpoints_, "y"); + sensor_msgs::PointCloud2Iterator clearing_endpoints_iter_z(*clearing_endpoints_, "z"); + + // we can pre-compute the endpoints of the map outside of the inner loop... we'll need these later + double map_end_x = origin_x_ + getSizeInMetersX(); + double map_end_y = origin_y_ + getSizeInMetersY(); + double map_end_z = origin_z_ + getSizeInMetersZ(); + + sensor_msgs::PointCloud2ConstIterator iter_x(*(clearing_observation.cloud_), "x"); + sensor_msgs::PointCloud2ConstIterator iter_y(*(clearing_observation.cloud_), "y"); + sensor_msgs::PointCloud2ConstIterator iter_z(*(clearing_observation.cloud_), "z"); + + for (; iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z) { + double wpx = *iter_x; + double wpy = *iter_y; + double wpz = *iter_z; + + double distance = dist(ox, oy, oz, wpx, wpy, wpz); + double scaling_fact = 1.0; + scaling_fact = std::max(std::min(scaling_fact, (distance - 2 * resolution_) / distance), 0.0); + wpx = scaling_fact * (wpx - ox) + ox; + wpy = scaling_fact * (wpy - oy) + oy; + wpz = scaling_fact * (wpz - oz) + oz; + + double a = wpx - ox; + double b = wpy - oy; + double c = wpz - oz; + double t = 1.0; + bool wp_outside = false; + + // we can only raytrace to a maximum z height + if (wpz > map_end_z) { + // we know we want the vector's z value to be max_z + t = std::max(0.0, std::min(t, (map_end_z - 0.01 - oz) / c)); + wp_outside = true; + } else if (wpz < origin_z_) { + // and we can only raytrace down to the floor + // we know we want the vector's z value to be 0.0 + t = std::min(t, (origin_z_ - oz) / c); + wp_outside = true; + } + + // the minimum value to raytrace from is the origin + if (wpx < origin_x_) { + t = std::min(t, (origin_x_ - ox) / a); + wp_outside = true; + } + if (wpy < origin_y_) { + t = std::min(t, (origin_y_ - oy) / b); + wp_outside = true; + } + + // the maximum value to raytrace to is the end of the map + if (wpx > map_end_x) { + t = std::min(t, (map_end_x - ox) / a); + wp_outside = true; + } + if (wpy > map_end_y) { + t = std::min(t, (map_end_y - oy) / b); + wp_outside = true; + } + + constexpr double wp_epsilon = 1e-5; + if (wp_outside) { + if (t > 0.0) { + t -= wp_epsilon; + } else if (t < 0.0) { + t += wp_epsilon; + } + } + + wpx = ox + a * t; + wpy = oy + b * t; + wpz = oz + c * t; + + double point_x, point_y, point_z; + if (worldToMap3DFloat(wpx, wpy, wpz, point_x, point_y, point_z)) { + unsigned int cell_raytrace_max_range = cellDistance(clearing_observation.raytrace_max_range_); + unsigned int cell_raytrace_min_range = cellDistance(clearing_observation.raytrace_min_range_); + + + // voxel_grid_.markVoxelLine(sensor_x, sensor_y, sensor_z, point_x, point_y, point_z); + voxel_grid_.clearVoxelLineInMap( + sensor_x, sensor_y, sensor_z, point_x, point_y, point_z, + costmap_, + unknown_threshold_, mark_threshold_, FREE_SPACE, NO_INFORMATION, + cell_raytrace_max_range, cell_raytrace_min_range); + + updateRaytraceBounds( + ox, oy, wpx, wpy, clearing_observation.raytrace_max_range_, + clearing_observation.raytrace_min_range_, min_x, min_y, + max_x, + max_y); + + if (publish_clearing_points) { + *clearing_endpoints_iter_x = wpx; + *clearing_endpoints_iter_y = wpy; + *clearing_endpoints_iter_z = wpz; + + ++clearing_endpoints_iter_x; + ++clearing_endpoints_iter_y; + ++clearing_endpoints_iter_z; + } + } + } + + if (publish_clearing_points) { + clearing_endpoints_->header.frame_id = global_frame_; + clearing_endpoints_->header.stamp = clearing_observation.cloud_->header.stamp; + + clearing_endpoints_pub_->publish(std::move(clearing_endpoints_)); + } +} + +void VoxelLayer::updateOrigin(double new_origin_x, double new_origin_y) +{ + int cell_oz; + // get the global pose of the robot + try + { + geometry_msgs::msg::TransformStamped transformStamped; + + transformStamped = tf_->lookupTransform(global_frame_, robot_base_frame_, rclcpp::Time(0)); + + const double robot_z = transformStamped.transform.translation.z; + const double z_grid_height = z_resolution_ * size_z_; + const double new_origin_z = robot_z - z_grid_height / 2; + cell_oz = int((new_origin_z - origin_z_) / z_resolution_); + } + catch (tf2::TransformException& ex) + { + RCLCPP_ERROR(logger_, "%s", ex.what()); + // If the robot pose is not detected, the origin_z_ will remain the same. + cell_oz = 0; + } + + // project the new origin into the grid + int cell_ox, cell_oy; + cell_ox = static_cast((new_origin_x - origin_x_) / resolution_); + cell_oy = static_cast((new_origin_y - origin_y_) / resolution_); + + // compute the associated world coordinates for the origin cell + // because we want to keep things grid-aligned + double new_grid_ox, new_grid_oy, new_grid_oz; + new_grid_ox = origin_x_ + cell_ox * resolution_; + new_grid_oy = origin_y_ + cell_oy * resolution_; + new_grid_oz = origin_z_ + cell_oz * z_resolution_; + + // To save casting from unsigned int to int a bunch of times + int size_x = size_x_; + int size_y = size_y_; + + // we need to compute the overlap of the new and existing windows + int lower_left_x, lower_left_y, upper_right_x, upper_right_y; + lower_left_x = std::min(std::max(cell_ox, 0), size_x); + lower_left_y = std::min(std::max(cell_oy, 0), size_y); + upper_right_x = std::min(std::max(cell_ox + size_x, 0), size_x); + upper_right_y = std::min(std::max(cell_oy + size_y, 0), size_y); + + unsigned int cell_size_x = upper_right_x - lower_left_x; + unsigned int cell_size_y = upper_right_y - lower_left_y; + + // we need a map to store the obstacles in the window temporarily + unsigned char * local_map = new unsigned char[cell_size_x * cell_size_y]; + unsigned int * local_voxel_map = new unsigned int[cell_size_x * cell_size_y]; + unsigned int * voxel_map = voxel_grid_.getData(); + + // copy the local window in the costmap to the local map + copyMapRegion( + costmap_, lower_left_x, lower_left_y, size_x_, local_map, 0, 0, cell_size_x, + cell_size_x, + cell_size_y); + copyMapRegion( + voxel_map, lower_left_x, lower_left_y, size_x_, local_voxel_map, 0, 0, cell_size_x, + cell_size_x, + cell_size_y); + + // we'll reset our maps to unknown space if appropriate + resetMaps(); + + // update the origin with the appropriate world coordinates + origin_x_ = new_grid_ox; + origin_y_ = new_grid_oy; + origin_z_ = new_grid_oz; + + // compute the starting cell location for copying data back in + int start_x = lower_left_x - cell_ox; + int start_y = lower_left_y - cell_oy; + + // now we want to copy the overlapping information back into the map, but in its new location + copyMapRegion( + local_map, 0, 0, cell_size_x, costmap_, start_x, start_y, size_x_, cell_size_x, + cell_size_y); + copyMapRegion3D( + local_voxel_map, 0, 0, cell_size_x, voxel_map, start_x, start_y, size_x_, + cell_size_x, + cell_size_y, + cell_oz); + + // make sure to clean up + delete[] local_map; + delete[] local_voxel_map; +} + +/** + * @brief Callback executed when a parameter change is detected + * @param event ParameterEvent message + */ +rcl_interfaces::msg::SetParametersResult +VoxelLayer::dynamicParametersCallback( + std::vector parameters) +{ + std::lock_guard guard(*getMutex()); + rcl_interfaces::msg::SetParametersResult result; + bool resize_map_needed = false; + + for (auto parameter : parameters) { + const auto & param_type = parameter.get_type(); + const auto & param_name = parameter.get_name(); + if (param_name.find(name_ + ".") != 0) { + continue; + } + + if (param_type == ParameterType::PARAMETER_DOUBLE) { + if (param_name == name_ + "." + "min_obstacle_height") { + min_obstacle_height_ = parameter.as_double(); + } else if (param_name == name_ + "." + "max_obstacle_height") { + max_obstacle_height_ = parameter.as_double(); + } else if (param_name == name_ + "." + "origin_z") { + origin_z_ = parameter.as_double(); + resize_map_needed = true; + } else if (param_name == name_ + "." + "z_resolution") { + z_resolution_ = parameter.as_double(); + resize_map_needed = true; + } + } else if (param_type == ParameterType::PARAMETER_BOOL) { + if (param_name == name_ + "." + "enabled") { + enabled_ = parameter.as_bool(); + current_ = false; + } else if (param_name == name_ + "." + "footprint_clearing_enabled") { + footprint_clearing_enabled_ = parameter.as_bool(); + } else if (param_name == name_ + "." + "publish_voxel_map") { + RCLCPP_WARN( + logger_, "publish voxel map is not a dynamic parameter " + "cannot be changed while running. Rejecting parameter update."); + continue; + } + + } else if (param_type == ParameterType::PARAMETER_INTEGER) { + if (param_name == name_ + "." + "z_voxels") { + size_z_ = parameter.as_int(); + resize_map_needed = true; + } else if (param_name == name_ + "." + "unknown_threshold") { + unknown_threshold_ = parameter.as_int() + (VOXEL_BITS - size_z_); + } else if (param_name == name_ + "." + "mark_threshold") { + mark_threshold_ = parameter.as_int(); + } else if (param_name == name_ + "." + "combination_method") { +#ifdef PRE_ROS_JAZZY + combination_method_ = parameter.as_int(); +#else + combination_method_ = combination_method_from_int(parameter.as_int()); +#endif + } + } + } + + if (resize_map_needed) { + matchSize(); + } + + result.successful = true; + return result; +} + +} // namespace rtabmap_costmap_plugins diff --git a/rtabmap_costmap_plugins/src/voxel_marker.cpp b/rtabmap_costmap_plugins/src/voxel_marker.cpp new file mode 100644 index 00000000..5f114901 --- /dev/null +++ b/rtabmap_costmap_plugins/src/voxel_marker.cpp @@ -0,0 +1,152 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2008, 2013, Willow Garage, Inc. + * 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 Willow Garage, Inc. 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 OWNER 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. + * + * Author: Eitan Marder-Eppstein + * David V. Lu!! + *********************************************************************/ + +/** + * Modified matlabbe: + * Added option to choose between unknown, free and marked cells + */ + +#include +#include +#include +#include + +namespace rtabmap_costmap_plugins +{ + +// FREE, UNKNOWN, MARKED +double g_voxel_colors_r[] = {0.0, 1.0, 1.0}; +double g_voxel_colors_g[] = {1.0, 1.0, 0.0}; +double g_voxel_colors_b[] = {1.0, 1.0, 0.0}; +double g_voxel_colors_a[] = {0.5, 0.1, 0.5}; + +class VoxelMarker: public rclcpp::Node +{ +public: + explicit VoxelMarker(const rclcpp::NodeOptions & options) : + rclcpp::Node("voxel_marker", options) + { + cell_type_ = this->declare_parameter("cell_type", (int)nav2_voxel_grid::VoxelStatus::MARKED); + color_r_ = this->declare_parameter("r", g_voxel_colors_r[cell_type_]); + color_g_ = this->declare_parameter("g", g_voxel_colors_g[cell_type_]); + color_b_ = this->declare_parameter("b", g_voxel_colors_b[cell_type_]); + color_a_ = this->declare_parameter("a", g_voxel_colors_a[cell_type_]); + + voxel_sub_ = this->create_subscription("voxel_grid", rclcpp::QoS(1), std::bind(&VoxelMarker::voxelCallback, this, std::placeholders::_1)); + marker_pub_ = this->create_publisher("visualization_marker", rclcpp::QoS(1)); + + } + virtual ~VoxelMarker() {} + + void voxelCallback(const nav2_msgs::msg::VoxelGrid::SharedPtr grid) + { + if (grid->data.empty()) + { + RCLCPP_ERROR(get_logger(), "Received empty voxel grid"); + return; + } + + visualization_msgs::msg::Marker m; + m.header.frame_id = grid->header.frame_id; + m.header.stamp = grid->header.stamp; + m.ns = "voxel_grid"; + m.id = 0; + m.type = visualization_msgs::msg::Marker::CUBE_LIST; + m.action = visualization_msgs::msg::Marker::ADD; + m.pose.orientation.w = 1.0; + m.color.r = color_r_; + m.color.g = color_g_; + m.color.b = color_b_; + m.color.a = color_a_; + + const uint32_t* data = &grid->data.front(); + const double x_origin = grid->origin.x; + const double y_origin = grid->origin.y; + const double z_origin = grid->origin.z; + const double x_res = grid->resolutions.x; + const double y_res = grid->resolutions.y; + const double z_res = grid->resolutions.z; + const uint32_t x_size = grid->size_x; + const uint32_t y_size = grid->size_y; + const uint32_t z_size = grid->size_z; + for (uint32_t y_grid = 0; y_grid < y_size; ++y_grid) + { + for (uint32_t x_grid = 0; x_grid < x_size; ++x_grid) + { + for (uint32_t z_grid = 0; z_grid < z_size; ++z_grid) + { + nav2_voxel_grid::VoxelStatus status = nav2_voxel_grid::VoxelGrid::getVoxel(x_grid, y_grid, z_grid, x_size, y_size, z_size, + data); + + if (status == (nav2_voxel_grid::VoxelStatus)cell_type_) + { + geometry_msgs::msg::Point p; + p.x = x_origin + (x_grid + 0.5) * x_res; + p.y = y_origin + (y_grid + 0.5) * y_res; + p.z = z_origin + (z_grid + 0.5) * z_res; + m.points.push_back(p); + } + } + } + } + m.scale.x = x_res; + m.scale.y = y_res; + m.scale.z = z_res; + + marker_pub_->publish(m); + } + +private: + int cell_type_; + double color_r_; + double color_g_; + double color_b_; + double color_a_; + + rclcpp::Publisher::SharedPtr marker_pub_; + rclcpp::Subscription::SharedPtr voxel_sub_; +}; + +} // rtabmap_costmap_plugins + +int main(int argc, char** argv) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared(rclcpp::NodeOptions())); + rclcpp::shutdown(); +} diff --git a/rtabmap_util/CMakeLists.txt b/rtabmap_util/CMakeLists.txt index eb40a8a1..e9edf324 100644 --- a/rtabmap_util/CMakeLists.txt +++ b/rtabmap_util/CMakeLists.txt @@ -232,7 +232,7 @@ ament_export_libraries(rtabmap_util_plugins) # To include downstream without tar # Install Python executables install(PROGRAMS -# scripts/patrol.py + scripts/patrol.py # scripts/objects_to_tags.py # scripts/point_to_tf.py # scripts/netvlad_tf_ros.py diff --git a/rtabmap_util/scripts/patrol.py b/rtabmap_util/scripts/patrol.py index ee131258..e0252a29 100755 --- a/rtabmap_util/scripts/patrol.py +++ b/rtabmap_util/scripts/patrol.py @@ -1,87 +1,104 @@ -#!/usr/bin/env python -import rospy +#!/usr/bin/env python3 import sys +import time +import rclpy +from rclpy.node import Node from std_msgs.msg import Bool -from rtabmap_ros.msg import Goal +from rtabmap_msgs.msg import Goal -pub = rospy.Publisher('rtabmap/goal_node', Goal, queue_size=1) -waypoints = [] -currentIndex = 0 -waitingTime = 1.0 -frameId = "" -def callback(data): - global currentIndex - global waitingTime - global frameId - if data.data: - rospy.loginfo(rospy.get_caller_id() + ": Goal '%s' reached! Publishing next goal in %.1f sec...", waypoints[currentIndex], waitingTime) - else: - rospy.loginfo(rospy.get_caller_id() + ": Goal '%s' failed! Publishing next goal in %.1f sec...", waypoints[currentIndex], waitingTime) +class PatrolNode(Node): + def __init__(self, waypoints): + super().__init__('patrol') - currentIndex = (currentIndex+1) % len(waypoints) + # --- Parameters --- + self.declare_parameter('time', 1.0) + self.declare_parameter('frame_id', '') + self.waiting_time = self.get_parameter('time').value + self.frame_id = self.get_parameter('frame_id').value - # Waiting time before sending next goal - rospy.sleep(waitingTime) + # --- Variables --- + self.waypoints = waypoints + self.current_index = 0 + + # --- Publisher & Subscriber --- + self.pub = self.create_publisher(Goal, 'rtabmap/goal_node', 10) + self.sub = self.create_subscription(Bool, 'rtabmap/goal_reached', self.callback, 10) + + self.get_logger().info(f"Waypoints: {self.waypoints}") + self.get_logger().info(f"Waiting time: {self.waiting_time:.1f} sec") + self.get_logger().info(f"Publishing goals on: {self.pub.topic_name}") + self.get_logger().info(f"Receiving goal status on: {self.sub.topic_name}") + + # Delay before sending first goal (ensure discovery) + time.sleep(1.0) + + # Send first goal + self.send_goal() + + def callback(self, msg: Bool): + """Called when goal_reached is received.""" + if msg.data: + self.get_logger().info( + f"Goal '{self.waypoints[self.current_index]}' reached! " + f"Publishing next goal in {self.waiting_time:.1f} sec..." + ) + else: + self.get_logger().info( + f"Goal '{self.waypoints[self.current_index]}' failed! " + f"Publishing next goal in {self.waiting_time:.1f} sec..." + ) + + # Move to next waypoint + self.current_index = (self.current_index + 1) % len(self.waypoints) + + # Wait before sending next goal + time.sleep(self.waiting_time) + self.send_goal() + + def send_goal(self): + """Send current goal to RTAB-Map.""" + waypoint = self.waypoints[self.current_index] + msg = Goal() + msg.header.stamp = self.get_clock().now().to_msg() + msg.frame_id = self.frame_id + + # Check if waypoint is a node id (int) or a label (string) + try: + msg.node_id = int(waypoint) + msg.node_label = "" + except ValueError: + msg.node_id = 0 + msg.node_label = waypoint + + self.get_logger().info( + f"Publishing goal '{waypoint}' ({self.current_index + 1}/{len(self.waypoints)})" + ) + self.pub.publish(msg) + + +def main(args=None): + rclpy.init(args=args) + + # Extract waypoints from command-line args + if len(sys.argv) < 3: + print( + "Usage: patrol.py waypointA waypointB waypointC ... " + "[--ros-args -p time:=1.0 -p frame_id:=base_footprint]" + ) + return + + waypoints = [x for x in sys.argv[1:] if not x.startswith('--') and not x.startswith('_')] + node = PatrolNode(waypoints) - msg = Goal() - msg.frame_id = frameId try: - int(waypoints[currentIndex]) - is_dig = True - except ValueError: - is_dig = False - if is_dig: - msg.node_id = int(waypoints[currentIndex]) - msg.node_label = "" - else: - msg.node_id = 0 - msg.node_label = waypoints[currentIndex] - - rospy.loginfo(rospy.get_caller_id() + ": Publishing goal '%s'! (%d/%d)", waypoints[currentIndex], currentIndex+1, len(waypoints)) - msg.header.stamp = rospy.get_rostime() - pub.publish(msg) + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() -def main(): - rospy.init_node('patrol', anonymous=False) - sub = rospy.Subscriber("rtabmap/goal_reached", Bool, callback) - global waitingTime - global frameId - waitingTime = rospy.get_param('~time', waitingTime) - frameId = rospy.get_param('~frame_id', frameId) - rospy.sleep(1.) # make sure that subscribers have seen this node before sending a goal - - rospy.loginfo(rospy.get_caller_id() + ": Waypoints: [%s]", str(waypoints).strip('[]')) - rospy.loginfo(rospy.get_caller_id() + ": time: %f", waitingTime) - rospy.loginfo(rospy.get_caller_id() + ": publish goal on %s", pub.resolved_name) - rospy.loginfo(rospy.get_caller_id() + ": receive goal status on %s", sub.resolved_name) - - # send the first goal - msg = Goal() - msg.frame_id = frameId - try: - int(waypoints[currentIndex]) - is_dig = True - except ValueError: - is_dig = False - if is_dig: - msg.node_id = int(waypoints[currentIndex]) - msg.node_label = "" - else: - msg.node_id = 0 - msg.node_label = waypoints[currentIndex] - while rospy.Time.now().secs == 0: - rospy.loginfo(rospy.get_caller_id() + ": Waiting clock...") - rospy.sleep(.1) - msg.header.stamp = rospy.Time.now() - rospy.loginfo(rospy.get_caller_id() + ": Publishing goal '%s'! (%d/%d)", waypoints[currentIndex], currentIndex+1, len(waypoints)) - pub.publish(msg) - rospy.spin() if __name__ == '__main__': - if len(sys.argv) < 3: - print("usage: patrol.py waypointA waypointB waypointC ... [_time:=1 frame_id:=base_footprint] [topic remaps] (at least 2 waypoints, can be node id, landmark or label)") - else: - waypoints = sys.argv[1:] - waypoints = [x for x in waypoints if not x.startswith('/') and not x.startswith('_')] - main() + main() \ No newline at end of file From 9963bc06af8ffd852f2b944ea463c59d422debc2 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sat, 1 Nov 2025 13:09:39 -0700 Subject: [PATCH 28/56] Added VINS-Fusion example. --- .../launch/realsense_d435i_stereo.launch.py | 16 +++++++++++++++- 1 file changed, 15 insertions(+), 1 deletion(-) diff --git a/rtabmap_examples/launch/realsense_d435i_stereo.launch.py b/rtabmap_examples/launch/realsense_d435i_stereo.launch.py index 9dd9595a..9631f300 100644 --- a/rtabmap_examples/launch/realsense_d435i_stereo.launch.py +++ b/rtabmap_examples/launch/realsense_d435i_stereo.launch.py @@ -3,7 +3,21 @@ # Install realsense2 ros2 package (ros-$ROS_DISTRO-realsense2-camera) # Example: # $ ros2 launch rtabmap_examples realsense_d435i_stereo.launch.py - +# +# +# +# +# VINS-Fusion example: +# Add to your ros2 workspace the package https://github.com/zinuok/VINS-Fusion-ROS2 +# Apply this patch https://gist.github.com/matlabbe/ebbb343cd744da9d6d6d6ded2e1557fd +# -> Revert "#define USE_GPU" change if you want to build VINS-Fusion with GPU support. +# That may be counterintuitive, but we need to build VINS-Fusion first, then rebuild rtabmap with VINS-Fusion support. +# -> in your ros2 workspace, do "colcon build --packages-select vins" +# -> go back under rtabmap library repo, then rebuild/install with "cmake -DWITH_VINS_FUSION=ON ..." +# -> do "colcon build" in your ros2 workspace again to rebuild rtabmap_ros +# $ ros2 launch rtabmap_examples realsense_d435i_stereo.launch.py odom_args:="--Odom/Strategy 9 OdomVINSFusion/ConfigPath ~/ros2_ws/src/VINS-Fusion-ROS2/config/realsense_d435i/realsense_stereo_imu_config.yaml" +# -> set "imu: 1" in realsense_stereo_imu_config.yaml to do stereo inertial odometry, otherwise only stereo odometry is done. +# import os from ament_index_python.packages import get_package_share_directory From f3cf0d20d08597d00cf527334ffe7ee10eb47f29 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sun, 2 Nov 2025 15:27:53 -0800 Subject: [PATCH 29/56] Try fixing qt6-qt5 rtabmap-rviz-plugins build issue. (#1376) * Try fixing qt6-qt5 rtabmap-rviz-plugins build issue. Disabled rtabmap-costmap-plugins for rolling. * fixed ci * Seems to fix the issue * fixed rtabmap_python test failing --- .github/workflows/ros2.yml | 7 +- .../rtabmap_conversions/MsgConversion.h | 2 +- rtabmap_python/rtabmap_python/__init__.py | 29 +++++++- rtabmap_python/rtabmap_python/compression.py | 68 +++++++++++++++---- rtabmap_python/setup.py | 4 +- rtabmap_ros/package.xml | 1 + rtabmap_rviz_plugins/CMakeLists.txt | 4 +- 7 files changed, 94 insertions(+), 21 deletions(-) diff --git a/.github/workflows/ros2.yml b/.github/workflows/ros2.yml index 2720381d..715f5f3f 100644 --- a/.github/workflows/ros2.yml +++ b/.github/workflows/ros2.yml @@ -20,12 +20,17 @@ jobs: include: - ros_distro: humble skip_keys: '' + packages: 'rtabmap_ros' - ros_distro: jazzy skip_keys: '' + packages: 'rtabmap_ros' - ros_distro: kilted skip_keys: 'grid_map_ros' + packages: 'rtabmap_ros' - ros_distro: rolling skip_keys: 'nav2_bringup nav2_msgs velodyne' + # rtabmap_costmap_plugins cannot be built, missing nav2 on rolling, build other packages: + packages: 'rtabmap_launch rtabmap_demos rtabmap_python rtabmap_examples rtabmap_rviz_plugins' fail-fast: false container: image: osrf/ros:${{ matrix.ros_distro }}-desktop-full @@ -39,7 +44,7 @@ jobs: cat /tmp/deps.repos - uses: ros-tooling/action-ros-ci@v0.4 with: - package-name: rtabmap_ros + package-name: ${{ matrix.packages }} target-ros2-distro: ${{ matrix.ros_distro }} vcs-repo-file-url: /tmp/deps.repos rosdep-skip-keys: "${{ matrix.skip_keys }}" diff --git a/rtabmap_conversions/include/rtabmap_conversions/MsgConversion.h b/rtabmap_conversions/include/rtabmap_conversions/MsgConversion.h index 05ea8a6c..4e82e388 100644 --- a/rtabmap_conversions/include/rtabmap_conversions/MsgConversion.h +++ b/rtabmap_conversions/include/rtabmap_conversions/MsgConversion.h @@ -29,7 +29,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #define MSGCONVERSION_H_ #include "rclcpp/time.hpp" -#include "tf2_ros/buffer.h" +#include "tf2_ros/buffer.hpp" #include #include #include diff --git a/rtabmap_python/rtabmap_python/__init__.py b/rtabmap_python/rtabmap_python/__init__.py index 139597f9..8b2e94fe 100644 --- a/rtabmap_python/rtabmap_python/__init__.py +++ b/rtabmap_python/rtabmap_python/__init__.py @@ -1,2 +1,27 @@ - - +# Copyright 2025 matlabbe +# +# 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 matlabbe 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. diff --git a/rtabmap_python/rtabmap_python/compression.py b/rtabmap_python/rtabmap_python/compression.py index 140f7f6f..0b8e1604 100644 --- a/rtabmap_python/rtabmap_python/compression.py +++ b/rtabmap_python/rtabmap_python/compression.py @@ -1,8 +1,38 @@ +# Copyright 2025 matlabbe +# +# 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 matlabbe 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. + -import zlib import struct +import zlib + import numpy as np + def compress(data): assert data.ndim == 1 or data.ndim == 2 @@ -14,21 +44,35 @@ def compress(data): dim1 = data.shape[0] dim2 = data.shape[1] - numpy_type_to_cvtype = {'uint8': 0, 'int8': 1, 'uint16': 2, - 'int16': 3, 'int32': 4, 'float32': 5, - 'float64': 6} + numpy_type_to_cvtype = { + 'uint8': 0, + 'int8': 1, + 'uint16': 2, + 'int16': 3, + 'int32': 4, + 'float32': 5, + 'float64': 6, + } compressed_data = bytearray(zlib.compress(data.tobytes())) - compressed_data.extend(struct.pack("iii", dim1, dim2, numpy_type_to_cvtype[data.dtype.name])) + compressed_data.extend( + struct.pack('iii', dim1, dim2, numpy_type_to_cvtype[data.dtype.name]) + ) return compressed_data -def uncompress(bytes): - cvtype_to_numpy_type = {0: 'uint8', 1: 'int8', 2: 'uint16', - 3: 'int16', 4: 'int32', 5: 'float32', - 6: 'float64'} - out = zlib.decompress(bytes[:len(bytes)-3*4]) - rows, cols, datatype = struct.unpack_from("iii", bytes, offset=len(bytes)-3*4) + +def uncompress(data): + cvtype_to_numpy_type = { + 0: 'uint8', + 1: 'int8', + 2: 'uint16', + 3: 'int16', + 4: 'int32', + 5: 'float32', + 6: 'float64', + } + out = zlib.decompress(data[: len(data) - 3 * 4]) + rows, cols, datatype = struct.unpack_from('iii', data, offset=len(data) - 3 * 4) data = np.frombuffer(out, dtype=cvtype_to_numpy_type[datatype]) return data.reshape((rows, cols)) - diff --git a/rtabmap_python/setup.py b/rtabmap_python/setup.py index a6e87e0f..7ed11455 100644 --- a/rtabmap_python/setup.py +++ b/rtabmap_python/setup.py @@ -15,11 +15,11 @@ setup( zip_safe=True, maintainer='Mathieu Labbe', maintainer_email='matlabbe@gmail.com', - description='RTAB-Map\'s python package.', + description="RTAB-Map's python package.", license='BSD', tests_require=['pytest'], entry_points={ 'console_scripts': [ ], }, -) \ No newline at end of file +) diff --git a/rtabmap_ros/package.xml b/rtabmap_ros/package.xml index 7088e8b7..9402a7b0 100644 --- a/rtabmap_ros/package.xml +++ b/rtabmap_ros/package.xml @@ -26,6 +26,7 @@ rtabmap_sync rtabmap_util rtabmap_viz + rtabmap_costmap_plugins ament_cmake diff --git a/rtabmap_rviz_plugins/CMakeLists.txt b/rtabmap_rviz_plugins/CMakeLists.txt index c1808ebe..c26c7e1a 100644 --- a/rtabmap_rviz_plugins/CMakeLists.txt +++ b/rtabmap_rviz_plugins/CMakeLists.txt @@ -18,6 +18,7 @@ find_package(ament_cmake_ros REQUIRED) find_package(pcl_conversions REQUIRED) find_package(pluginlib REQUIRED) find_package(rclcpp REQUIRED) +find_package(QT NAMES Qt5 QUIET COMPONENTS Widgets) find_package(rviz_common REQUIRED) find_package(rviz_rendering REQUIRED) find_package(rviz_default_plugins REQUIRED) @@ -45,9 +46,6 @@ SET(Libraries rtabmap_msgs ) -MESSAGE(STATUS "rtabmap_conversions=${rtabmap_conversions_LIBRARIES}") - - ########### ## Build ## ########### From e245b7751edf3f1c3b74578527c707b5aa3e630c Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sun, 9 Nov 2025 20:44:28 -0800 Subject: [PATCH 30/56] Fixed first imu of a batch used 2x internally --- rtabmap_odom/src/OdometryROS.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/rtabmap_odom/src/OdometryROS.cpp b/rtabmap_odom/src/OdometryROS.cpp index 4176fee7..bc34b042 100644 --- a/rtabmap_odom/src/OdometryROS.cpp +++ b/rtabmap_odom/src/OdometryROS.cpp @@ -571,9 +571,8 @@ void OdometryROS::processData() for(std::map::iterator iter=iterFirst; iter!=iterEnd;) { // Because we always keep the last processed imu in the buffer, skip the first - // one when processing again the buffer unless its time is lower/equal to image - // current stamp (could happen on initialization). - if(iter!=iterFirst || iter->first <= rtabmap_conversions::timestampFromROS(header.stamp)) { + // one when processing again the buffer + if(iter!=iterFirst) { imus.push_back(*iter); } if(iter!=iterLast) { From 515eb50d83ad71bbe11a3e7292e9c349a8fe3ed6 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Mon, 10 Nov 2025 04:48:44 +0000 Subject: [PATCH 31/56] cherry picked https://github.com/introlab/rtabmap_ros/commit/e245b7751edf3f1c3b74578527c707b5aa3e630c for ros1 port --- rtabmap_odom/src/OdometryROS.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/rtabmap_odom/src/OdometryROS.cpp b/rtabmap_odom/src/OdometryROS.cpp index 9843afeb..d8ebe76d 100644 --- a/rtabmap_odom/src/OdometryROS.cpp +++ b/rtabmap_odom/src/OdometryROS.cpp @@ -567,9 +567,8 @@ void OdometryROS::processData() for(std::map::iterator iter=iterFirst; iter!=iterEnd;) { // Because we always keep the last processed imu in the buffer, skip the first - // one when processing again the buffer unless its time is lower/equal to image - // current stamp (could happen on initialization). - if(iter!=iterFirst || iter->first <= header.stamp.toSec()) { + // one when processing again the buffer + if(iter!=iterFirst) { imus.push_back(*iter); } if(iter!=iterLast) { From 2f258bdae024cb65dd4cb494b879b0ae8c23df03 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Tue, 11 Nov 2025 23:56:08 +0000 Subject: [PATCH 32/56] Plumbing env_sensor topic to rtabmap --- rtabmap_demos/scripts/wifi_signal_pub.py | 13 ++++++- rtabmap_demos/src/WifiSignalPubNode.cpp | 18 ++++++++-- rtabmap_launch/launch/rtabmap.launch | 3 ++ rtabmap_msgs/msg/EnvSensor.msg | 23 ++++++++++++- .../include/rtabmap_slam/CoreWrapper.h | 5 ++- rtabmap_slam/src/CoreWrapper.cpp | 34 ++++++++++++------- rtabmap_util/src/DbPlayerNode.cpp | 24 +++++++++++++ 7 files changed, 102 insertions(+), 18 deletions(-) diff --git a/rtabmap_demos/scripts/wifi_signal_pub.py b/rtabmap_demos/scripts/wifi_signal_pub.py index 7aefef5d..677cfe3d 100755 --- a/rtabmap_demos/scripts/wifi_signal_pub.py +++ b/rtabmap_demos/scripts/wifi_signal_pub.py @@ -2,11 +2,13 @@ import rospy import struct import os -from rtabmap_ros.msg import UserData +from rtabmap_msgs.msg import UserData, EnvSensor def loop(): rospy.init_node('wifi_signal_pub', anonymous=True) pub = rospy.Publisher('wifi_signal', UserData, queue_size=10) + envPub = rospy.Publisher('wifi_signal/env_sensor', EnvSensor, queue_size=10) + frameId = rospy.get_param('frame_id', 'base_link') rate = rospy.Rate(0.5) # 0.5hz while not rospy.is_shutdown(): @@ -34,7 +36,16 @@ def loop(): # to get precise position in the graph afterward. msg.data = struct.pack(b'dd', dBm, rospy.get_time()) + msg.header.frame_id = frameId + msg.header.stamp = rospy.Time.now() pub.publish(msg) + + # Example using env sensor: + envMsg = EnvSensor() + envMsg.type = 1 + envMsg.value = dBm + envMsg = msg.header + envPub.publish(envMsg) else: rospy.logerr("Cannot get info from wireless!") rate.sleep() diff --git a/rtabmap_demos/src/WifiSignalPubNode.cpp b/rtabmap_demos/src/WifiSignalPubNode.cpp index 50dbedc8..13f630e4 100644 --- a/rtabmap_demos/src/WifiSignalPubNode.cpp +++ b/rtabmap_demos/src/WifiSignalPubNode.cpp @@ -34,14 +34,20 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include +#include -// Demo: +// Demo 1 (User Data): // $ roslaunch freenect_launch freenect.launch depth_registration:=true -// $ roslaunch rtabmap_launch rtabmap.launch rtabmap_args:="--delete_db_on_start" user_data_async_topic:=/wifi_signal rtabmapviz:=false rviz:=true +// $ roslaunch rtabmap_launch rtabmap.launch rtabmap_args:="--delete_db_on_start" user_data_async_topic:=/wifi_signal rtabmap_viz:=false rviz:=true // $ rosrun rtabmap_demos wifi_signal_pub interface:="wlan0" // $ rosrun rtabmap_demos wifi_signal_sub // In RVIZ add PointCloud2 "wifi_signals" +// Demo 2 (Env Sensor): +// $ roslaunch freenect_launch freenect.launch depth_registration:=true +// $ roslaunch rtabmap_launch rtabmap.launch rtabmap_args:="--delete_db_on_start" env_sensor_topic:=/wifi_signal/env_sensor rtabmap_viz:=false rviz:=true +// $ rosrun rtabmap_demos wifi_signal_pub interface:="wlan0" + // A percentage value that represents the signal quality // of the network. WLAN_SIGNAL_QUALITY is of type ULONG. // This member contains a value between 0 and 100. A value @@ -78,6 +84,7 @@ int main(int argc, char** argv) ros::Rate rate(rateHz); ros::Publisher wifiPub = nh.advertise("wifi_signal", 1); + ros::Publisher envSensorPub = nh.advertise("wifi_signal/env_sensor", 1); while(ros::ok()) { @@ -140,6 +147,13 @@ int main(int argc, char** argv) dataMsg.header.stamp = stamp; rtabmap_conversions::userDataToROS(data, dataMsg, false); wifiPub.publish(dataMsg); + + // Example with env sensor + rtabmap_msgs::EnvSensor envMsg; + envMsg.header = dataMsg.header; + envMsg.type = rtabmap_msgs::EnvSensor::TYPE_WIFI_SIGNAL_STRENGTH; + envMsg.value = double(dBm); + envSensorPub.publish(envMsg); } ros::spinOnce(); rate.sleep(); diff --git a/rtabmap_launch/launch/rtabmap.launch b/rtabmap_launch/launch/rtabmap.launch index fc6dc4b5..e48f470a 100644 --- a/rtabmap_launch/launch/rtabmap.launch +++ b/rtabmap_launch/launch/rtabmap.launch @@ -153,6 +153,8 @@ + + @@ -421,6 +423,7 @@ + diff --git a/rtabmap_msgs/msg/EnvSensor.msg b/rtabmap_msgs/msg/EnvSensor.msg index 23240d9d..f8506d3e 100644 --- a/rtabmap_msgs/msg/EnvSensor.msg +++ b/rtabmap_msgs/msg/EnvSensor.msg @@ -1,6 +1,27 @@ Header header -# EnvSensor +# Environmental sensor + +# built-in types +int32 TYPE_UNDEFINED=0 +int32 TYPE_WIFI_SIGNAL_STRENGTH=1 # dBm +int32 TYPE_AMBIENT_TEMPERATURE=2 # Celcius +int32 TYPE_AMBIENT_AIR_PRESSURE=3 # hPa +int32 TYPE_AMBIENT_LIGHT=4 # lx +int32 TYPE_AMBIENT_RELATIVE_HUMIDITY=5 # % + +# user types +int32 TYPE_CUSTOM1=100 +int32 TYPE_CUSTOM2=101 +int32 TYPE_CUSTOM3=102 +int32 TYPE_CUSTOM4=103 +int32 TYPE_CUSTOM5=104 +int32 TYPE_CUSTOM6=105 +int32 TYPE_CUSTOM7=106 +int32 TYPE_CUSTOM8=107 +int32 TYPE_CUSTOM9=108 + int32 type + float64 value \ No newline at end of file diff --git a/rtabmap_slam/include/rtabmap_slam/CoreWrapper.h b/rtabmap_slam/include/rtabmap_slam/CoreWrapper.h index 2f5d7c72..e97a980c 100644 --- a/rtabmap_slam/include/rtabmap_slam/CoreWrapper.h +++ b/rtabmap_slam/include/rtabmap_slam/CoreWrapper.h @@ -68,6 +68,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "rtabmap_msgs/DetectMoreLoopClosures.h" #include "rtabmap_msgs/GlobalBundleAdjustment.h" #include "rtabmap_msgs/CleanupLocalGrids.h" +#include "rtabmap_msgs/EnvSensor.h" #include "rtabmap_util/MapsManager.h" #include "rtabmap_util/ULogToRosout.h" @@ -161,6 +162,7 @@ private: void userDataAsyncCallback(const rtabmap_msgs::UserDataConstPtr & dataMsg); void globalPoseAsyncCallback(const geometry_msgs::PoseWithCovarianceStampedConstPtr & globalPoseMsg); void gpsFixAsyncCallback(const sensor_msgs::NavSatFixConstPtr & gpsFixMsg); + void envSensorCallback(const rtabmap_msgs::EnvSensorConstPtr & envSensorMsg); #ifdef WITH_APRILTAG_ROS void tagDetectionsAsyncCallback(const apriltag_ros::AprilTagDetectionArray & tagDetections); #endif @@ -372,12 +374,13 @@ private: ros::Subscriber userDataAsyncSub_; cv::Mat userData_; - UMutex userDataMutex_; ros::Subscriber globalPoseAsyncSub_; geometry_msgs::PoseWithCovarianceStamped globalPose_; ros::Subscriber gpsFixAsyncSub_; rtabmap::GPS gps_; + ros::Subscriber envSensorSub_; + rtabmap::EnvSensors envSensors_; ros::Subscriber tagDetectionsSub_; ros::Subscriber fiducialTransfromsSub_; std::map > tags_; // id, diff --git a/rtabmap_slam/src/CoreWrapper.cpp b/rtabmap_slam/src/CoreWrapper.cpp index 0a96897a..b87aaeb3 100644 --- a/rtabmap_slam/src/CoreWrapper.cpp +++ b/rtabmap_slam/src/CoreWrapper.cpp @@ -865,6 +865,7 @@ void CoreWrapper::onInit() userDataAsyncSub_ = nh.subscribe("user_data_async", 1, &CoreWrapper::userDataAsyncCallback, this); globalPoseAsyncSub_ = nh.subscribe("global_pose", 1, &CoreWrapper::globalPoseAsyncCallback, this); gpsFixAsyncSub_ = nh.subscribe("gps/fix", 1, &CoreWrapper::gpsFixAsyncCallback, this); + envSensorSub_ = nh.subscribe("env_sensor", 1, &CoreWrapper::envSensorCallback, this); #ifdef WITH_APRILTAG_ROS tagDetectionsSub_ = nh.subscribe("tag_detections", 1, &CoreWrapper::tagDetectionsAsyncCallback, this); #endif @@ -1542,7 +1543,6 @@ void CoreWrapper::commonMultiCameraCallbackImpl( if(userDataMsg.get()) { userData = rtabmap_conversions::userDataFromROS(*userDataMsg); - UScopeMutex lock(userDataMutex_); if(!userData_.empty()) { NODELET_WARN("Synchronized and asynchronized user data topics cannot be used at the same time. Async user data dropped!"); @@ -1551,7 +1551,6 @@ void CoreWrapper::commonMultiCameraCallbackImpl( } else { - UScopeMutex lock(userDataMutex_); userData = userData_; userData_ = cv::Mat(); } @@ -1701,7 +1700,6 @@ void CoreWrapper::commonLaserScanCallback( if(userDataMsg.get()) { userData = rtabmap_conversions::userDataFromROS(*userDataMsg); - UScopeMutex lock(userDataMutex_); if(!userData_.empty()) { NODELET_WARN("Synchronized and asynchronized user data topics cannot be used at the same time. Async user data dropped!"); @@ -1710,7 +1708,6 @@ void CoreWrapper::commonLaserScanCallback( } else { - UScopeMutex lock(userDataMutex_); userData = userData_; userData_ = cv::Mat(); } @@ -1766,7 +1763,6 @@ void CoreWrapper::commonOdomCallback( if(userDataMsg.get()) { userData = rtabmap_conversions::userDataFromROS(*userDataMsg); - UScopeMutex lock(userDataMutex_); if(!userData_.empty()) { NODELET_WARN("Synchronized and asynchronized user data topics cannot be used at the same time. Async user data dropped!"); @@ -1775,7 +1771,6 @@ void CoreWrapper::commonOdomCallback( } else { - UScopeMutex lock(userDataMutex_); userData = userData_; userData_ = cv::Mat(); } @@ -2033,6 +2028,13 @@ void CoreWrapper::process( data.setLandmarks(landmarks); } + // Env sensors + if(!envSensors_.empty()) + { + data.setEnvSensors(envSensors_); + envSensors_.clear(); + } + // IMU if(!imus_.empty()) { @@ -2400,7 +2402,6 @@ void CoreWrapper::userDataAsyncCallback(const rtabmap_msgs::UserDataConstPtr & d { if(!paused_) { - UScopeMutex lock(userDataMutex_); static bool warningShow = false; if(!userData_.empty() && !warningShow) { @@ -2446,6 +2447,16 @@ void CoreWrapper::gpsFixAsyncCallback(const sensor_msgs::NavSatFixConstPtr & gps } } +void CoreWrapper::envSensorCallback(const rtabmap_msgs::EnvSensorConstPtr & envSensorMsg) +{ + if(!paused_) + { + // Can only insert one value for each type per node, keep the most recent + EnvSensor value = rtabmap_conversions::envSensorFromROS(*envSensorMsg); + uInsert(envSensors_, std::make_pair(value.type(), value)); + } +} + #ifdef WITH_APRILTAG_ROS void CoreWrapper::tagDetectionsAsyncCallback(const apriltag_ros::AprilTagDetectionArray & tagDetections) { @@ -2889,10 +2900,9 @@ bool CoreWrapper::resetRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empt previousStamp_ = ros::Time(0); globalPose_.header.stamp = ros::Time(0); gps_ = rtabmap::GPS(); + envSensors_.clear(); tags_.clear(); - userDataMutex_.lock(); userData_ = cv::Mat(); - userDataMutex_.unlock(); imus_.clear(); imuFrameId_.clear(); interOdoms_.clear(); @@ -2978,10 +2988,9 @@ bool CoreWrapper::loadDatabaseCallback(rtabmap_msgs::LoadDatabase::Request& req, previousStamp_ = ros::Time(0); globalPose_.header.stamp = ros::Time(0); gps_ = rtabmap::GPS(); + envSensors_.clear(); tags_.clear(); - userDataMutex_.lock(); userData_ = cv::Mat(); - userDataMutex_.unlock(); imus_.clear(); imuFrameId_.clear(); interOdoms_.clear(); @@ -3117,11 +3126,10 @@ bool CoreWrapper::backupDatabaseCallback(std_srvs::Empty::Request&, std_srvs::Em goalFrameId_.clear(); latestNodeWasReached_ = false; graphLatched_ = false; - userDataMutex_.lock(); userData_ = cv::Mat(); - userDataMutex_.unlock(); globalPose_.header.stamp = ros::Time(0); gps_ = rtabmap::GPS(); + envSensors_.clear(); tags_.clear(); NODELET_INFO("Backup: Saving \"%s\" to \"%s\"...", databasePath_.c_str(), (databasePath_+".back").c_str()); diff --git a/rtabmap_util/src/DbPlayerNode.cpp b/rtabmap_util/src/DbPlayerNode.cpp index 3add3275..ec63320c 100644 --- a/rtabmap_util/src/DbPlayerNode.cpp +++ b/rtabmap_util/src/DbPlayerNode.cpp @@ -219,6 +219,7 @@ int main(int argc, char** argv) ros::Publisher scanCloudPub; ros::Publisher globalPosePub; ros::Publisher gpsFixPub; + ros::Publisher envSensorPub; ros::Publisher clockPub; tf2_ros::TransformBroadcaster tfBroadcaster; @@ -387,6 +388,15 @@ int main(int argc, char** argv) } } + if(!odom.data().envSensors().empty()) + { + if(envSensorPub.getTopic().empty()) + { + envSensorPub = nh.advertise("env_sensor", 1); + ROS_INFO("EnvSensor will be published."); + } + } + // publish transforms first if(publishTf) { @@ -486,6 +496,20 @@ int main(int argc, char** argv) gpsFixPub.publish(msg); } + if(!odom.data().envSensors().empty()) + { + for(rtabmap::EnvSensors::const_iterator iter=odom.data().envSensors().begin(); iter!=odom.data().envSensors().end(); ++iter) + { + rtabmap_msgs::EnvSensor msg; + rtabmap_conversions::envSensorToROS(iter->second, msg); + msg.header.frame_id = frameId; + if(iter->second.stamp() == 0.0) { + msg.header.stamp = ros::Time(data.stamp()); + } + envSensorPub.publish(msg); + } + } + if(type >= 0) { if(rgbCamInfoPub.getNumSubscribers() && type == 0) From 001ed0d5125e126244ff7e6442fad555a162dded Mon Sep 17 00:00:00 2001 From: matlabbe Date: Tue, 2 Dec 2025 21:11:25 -0800 Subject: [PATCH 33/56] Fixing duplicated code added from that merge commit https://github.com/introlab/rtabmap_ros/commit/c204ed61427475f028fe2cd435b31cd2cbcfc1fd --- rtabmap_odom/src/nodelets/stereo_odometry.cpp | 45 +------------------ 1 file changed, 1 insertion(+), 44 deletions(-) diff --git a/rtabmap_odom/src/nodelets/stereo_odometry.cpp b/rtabmap_odom/src/nodelets/stereo_odometry.cpp index 6a91743a..84a1ced0 100644 --- a/rtabmap_odom/src/nodelets/stereo_odometry.cpp +++ b/rtabmap_odom/src/nodelets/stereo_odometry.cpp @@ -500,49 +500,6 @@ void StereoOdometry::commonCallback( return; } else - { - stereoTransform = rtabmap_conversions::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_conversions::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 = rtabmap_conversions::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) @@ -576,7 +533,7 @@ void StereoOdometry::commonCallback( { 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\".", + "the cameras is valid: \"ros2 run tf2_ros tf_echo %s %s\".", Parameters::kRtabmapImagesAlreadyRectified().c_str(), rightCameraInfos[i].header.frame_id.c_str(), leftCameraInfos[i].header.frame_id.c_str()); From 64fa848dbac8c2c295bc0ccad2642343d004d0e0 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Wed, 3 Dec 2025 21:43:29 -0800 Subject: [PATCH 34/56] dbplayer: fixed camera index suffix added when there is only a single stereo camera --- rtabmap_util/src/nodelets/db_player.cpp | 28 ++++++++++++++++++++++++- 1 file changed, 27 insertions(+), 1 deletion(-) diff --git a/rtabmap_util/src/nodelets/db_player.cpp b/rtabmap_util/src/nodelets/db_player.cpp index ca6c1614..de08e506 100644 --- a/rtabmap_util/src/nodelets/db_player.cpp +++ b/rtabmap_util/src/nodelets/db_player.cpp @@ -417,11 +417,12 @@ bool DbPlayer::publishNextFrame() stereo = true; } int index = 0; + static bool firstTimeCamMsg = true; for(const auto & cam: *models) { rtabmap::Transform localTransform = cam.localTransform(); if(!localTransform.isNull()) { geometry_msgs::msg::TransformStamped baseToCamera; - baseToCamera.child_frame_id = (stereo?index%2==0?"left_":"right_":"") + cameraFrameId_ + (models->size()>1?uNumber2Str(index/(stereo?2:1)):""); + baseToCamera.child_frame_id = (stereo?index%2==0?"left_":"right_":"") + cameraFrameId_ + (((stereo && models->size()>2) || (!stereo && models->size()>1))?uNumber2Str(index/(stereo?2:1)):""); baseToCamera.header.frame_id = frameId_; baseToCamera.header.stamp = time; if(cam.Tx() != 0) { @@ -429,9 +430,13 @@ bool DbPlayer::publishNextFrame() } rtabmap_conversions::transformToGeometryMsg(localTransform, baseToCamera.transform); transforms.push_back(baseToCamera); + if(firstTimeCamMsg) { + RCLCPP_INFO(get_logger(), "Will publish tf %s -> %s", baseToCamera.header.frame_id.c_str(), baseToCamera.child_frame_id.c_str()); + } } ++index; } + firstTimeCamMsg = firstTimeCamMsg && models->empty()?true:false; if(!odom.pose().isNull()) { @@ -441,6 +446,12 @@ bool DbPlayer::publishNextFrame() odomToBase.header.stamp = time; rtabmap_conversions::transformToGeometryMsg(odom.pose(), odomToBase.transform); transforms.push_back(odomToBase); + static bool firstTimeMsg = true; + if(firstTimeMsg) { + RCLCPP_INFO(get_logger(), "Will publish tf %s -> %s", odomToBase.header.frame_id.c_str(), odomToBase.child_frame_id.c_str()); + } + firstTimeMsg = false; + } if(scanPub_.get() || scanCloudPub_.get()) @@ -451,6 +462,11 @@ bool DbPlayer::publishNextFrame() baseToLaserScan.header.stamp = time; rtabmap_conversions::transformToGeometryMsg(odom.data().laserScanCompressed().localTransform(), baseToLaserScan.transform); transforms.push_back(baseToLaserScan); + static bool firstTimeMsg = true; + if(firstTimeMsg) { + RCLCPP_INFO(get_logger(), "Will publish tf %s -> %s", baseToLaserScan.header.frame_id.c_str(), baseToLaserScan.child_frame_id.c_str()); + } + firstTimeMsg = false; } if(!odom.data().groundTruth().isNull()) { @@ -460,6 +476,11 @@ bool DbPlayer::publishNextFrame() worldToBase.header.stamp = time; rtabmap_conversions::transformToGeometryMsg(odom.data().groundTruth(), worldToBase.transform); transforms.push_back(worldToBase); + static bool firstTimeMsg = true; + if(firstTimeMsg) { + RCLCPP_INFO(get_logger(), "Will publish tf %s -> %s", worldToBase.header.frame_id.c_str(), worldToBase.child_frame_id.c_str()); + } + firstTimeMsg = false; } if(!odom.data().imu().empty()) { @@ -469,6 +490,11 @@ bool DbPlayer::publishNextFrame() baseToImu.header.stamp = time; rtabmap_conversions::transformToGeometryMsg(odom.data().imu().localTransform(), baseToImu.transform); transforms.push_back(baseToImu); + static bool firstTimeMsg = true; + if(firstTimeMsg) { + RCLCPP_INFO(get_logger(), "Will publish tf %s -> %s", baseToImu.header.frame_id.c_str(), baseToImu.child_frame_id.c_str()); + } + firstTimeMsg = false; } tfBroadcaster_->sendTransform(transforms); } From d63accf1812835f593928879ac646944779609e8 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sat, 20 Dec 2025 13:06:23 -0800 Subject: [PATCH 35/56] removed --udebug arg in a turtlebot3 demo launch --- rtabmap_demos/launch/turtlebot3/turtlebot3_rgbd.launch.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rtabmap_demos/launch/turtlebot3/turtlebot3_rgbd.launch.py b/rtabmap_demos/launch/turtlebot3/turtlebot3_rgbd.launch.py index e9a2fdcf..b9076449 100644 --- a/rtabmap_demos/launch/turtlebot3/turtlebot3_rgbd.launch.py +++ b/rtabmap_demos/launch/turtlebot3/turtlebot3_rgbd.launch.py @@ -65,7 +65,7 @@ def generate_launch_description(): package='rtabmap_slam', executable='rtabmap', output='screen', parameters=[parameters], remappings=remappings, - arguments=['-d', '--udebug']), # This will delete the previous database (~/.ros/rtabmap.db) + arguments=['-d']), # This will delete the previous database (~/.ros/rtabmap.db) # Localization mode: Node( From 758b7fa4cd72723800a63493368a03deea21905b Mon Sep 17 00:00:00 2001 From: matlabbe Date: Thu, 22 Jan 2026 16:00:40 -0800 Subject: [PATCH 36/56] read-only support (#1397) --- rtabmap_conversions/CMakeLists.txt | 2 +- rtabmap_slam/src/CoreWrapper.cpp | 21 +++++++++++++++------ 2 files changed, 16 insertions(+), 7 deletions(-) diff --git a/rtabmap_conversions/CMakeLists.txt b/rtabmap_conversions/CMakeLists.txt index 33c8a0e0..d0cc59ed 100644 --- a/rtabmap_conversions/CMakeLists.txt +++ b/rtabmap_conversions/CMakeLists.txt @@ -7,7 +7,7 @@ find_package(catkin REQUIRED COMPONENTS image_geometry rtabmap_msgs ) -find_package(RTABMap 0.21.13 REQUIRED) +find_package(RTABMap 0.23.4 REQUIRED) catkin_package( INCLUDE_DIRS include diff --git a/rtabmap_slam/src/CoreWrapper.cpp b/rtabmap_slam/src/CoreWrapper.cpp index b87aaeb3..83009168 100644 --- a/rtabmap_slam/src/CoreWrapper.cpp +++ b/rtabmap_slam/src/CoreWrapper.cpp @@ -888,15 +888,24 @@ CoreWrapper::~CoreWrapper() this->saveParameters(configPath_); printf("rtabmap: Saving database/long-term memory... (located at %s)\n", databasePath_.c_str()); + bool saveDatabase = true; if(rtabmap_.getMemory()) { - // save the grid map - float xMin=0.0f, yMin=0.0f, gridCellSize = 0.05f; - cv::Mat pixels = mapsManager_.getGridMap(xMin, yMin, gridCellSize); - if(!pixels.empty()) + if(!rtabmap_.getMemory()->isReadOnly()) { - printf("rtabmap: 2D occupancy grid map saved.\n"); - rtabmap_.getMemory()->save2DMap(pixels, xMin, yMin, gridCellSize); + // save the grid map + float xMin=0.0f, yMin=0.0f, gridCellSize = 0.05f; + cv::Mat pixels = mapsManager_.getGridMap(xMin, yMin, gridCellSize); + if(!pixels.empty()) + { + printf("rtabmap: 2D occupancy grid map saved.\n"); + rtabmap_.getMemory()->save2DMap(pixels, xMin, yMin, gridCellSize); + } + } + else + { + printf("rtabmap: Database is read-only, the current state of the memory is not saved.\n"); + saveDatabase = false; } } From 9c003ccae5119890491b29eb667d5950d7eecd0b Mon Sep 17 00:00:00 2001 From: matlabbe Date: Fri, 30 Jan 2026 22:03:31 +0000 Subject: [PATCH 37/56] Fixed loadDatabase and backup services fatal error when database is read-only --- rtabmap_slam/src/CoreWrapper.cpp | 49 +++++++++++++++++++++----------- 1 file changed, 33 insertions(+), 16 deletions(-) diff --git a/rtabmap_slam/src/CoreWrapper.cpp b/rtabmap_slam/src/CoreWrapper.cpp index 83009168..e2a5b1f8 100644 --- a/rtabmap_slam/src/CoreWrapper.cpp +++ b/rtabmap_slam/src/CoreWrapper.cpp @@ -908,8 +908,7 @@ CoreWrapper::~CoreWrapper() saveDatabase = false; } } - - rtabmap_.close(); + rtabmap_.close(saveDatabase); printf("rtabmap: Saving database/long-term memory...done! (located at %s, %ld MB)\n", databasePath_.c_str(), UFile::length(databasePath_)/(1024*1024)); delete interOdomSync_; @@ -2970,18 +2969,27 @@ bool CoreWrapper::loadDatabaseCallback(rtabmap_msgs::LoadDatabase::Request& req, // Close old database NODELET_INFO("LoadDatabase: Saving current map (%s)...", databasePath_.c_str()); + bool saveDatabase = true; if(rtabmap_.getMemory()) { - // save the grid map - float xMin=0.0f, yMin=0.0f, gridCellSize = 0.05f; - cv::Mat pixels = mapsManager_.getGridMap(xMin, yMin, gridCellSize); - if(!pixels.empty()) + if(!rtabmap_.getMemory()->isReadOnly()) { - printf("rtabmap: 2D occupancy grid map saved.\n"); - rtabmap_.getMemory()->save2DMap(pixels, xMin, yMin, gridCellSize); + // save the grid map + float xMin=0.0f, yMin=0.0f, gridCellSize = 0.05f; + cv::Mat pixels = mapsManager_.getGridMap(xMin, yMin, gridCellSize); + if(!pixels.empty()) + { + printf("rtabmap: 2D occupancy grid map saved.\n"); + rtabmap_.getMemory()->save2DMap(pixels, xMin, yMin, gridCellSize); + } + } + else + { + printf("rtabmap: Database is read-only, the current state of the memory is not saved.\n"); + saveDatabase = false; } } - rtabmap_.close(); + rtabmap_.close(saveDatabase); NODELET_INFO("LoadDatabase: Saving current map (%s, %ld MB)... done!", databasePath_.c_str(), UFile::length(databasePath_)/(1024*1024)); covariance_ = cv::Mat(); @@ -3113,18 +3121,27 @@ bool CoreWrapper::triggerNewMapCallback(std_srvs::Empty::Request&, std_srvs::Emp bool CoreWrapper::backupDatabaseCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&) { NODELET_INFO("Backup: Saving memory..."); + bool saveDatabase = true; if(rtabmap_.getMemory()) { - // save the grid map - float xMin=0.0f, yMin=0.0f, gridCellSize = 0.05f; - cv::Mat pixels = mapsManager_.getGridMap(xMin, yMin, gridCellSize); - if(!pixels.empty()) + if(!rtabmap_.getMemory()->isReadOnly()) { - printf("rtabmap: 2D occupancy grid map saved.\n"); - rtabmap_.getMemory()->save2DMap(pixels, xMin, yMin, gridCellSize); + // save the grid map + float xMin=0.0f, yMin=0.0f, gridCellSize = 0.05f; + cv::Mat pixels = mapsManager_.getGridMap(xMin, yMin, gridCellSize); + if(!pixels.empty()) + { + printf("rtabmap: 2D occupancy grid map saved.\n"); + rtabmap_.getMemory()->save2DMap(pixels, xMin, yMin, gridCellSize); + } + } + else + { + printf("rtabmap: Database is read-only, the current state of the memory is not saved.\n"); + saveDatabase = false; } } - rtabmap_.close(); + rtabmap_.close(saveDatabase); NODELET_INFO("Backup: Saving memory... done!"); covariance_ = cv::Mat(); From 3283309e766a24180f926b1bd8a2107ac9f3eb2f Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sun, 8 Feb 2026 12:10:47 -0800 Subject: [PATCH 38/56] Added realsense D435i combined IR stereo VO and RGB-D SLAM example --- .../launch/realsense_d435i_combined.launch.py | 106 ++++++++++++++++++ 1 file changed, 106 insertions(+) create mode 100644 rtabmap_examples/launch/realsense_d435i_combined.launch.py diff --git a/rtabmap_examples/launch/realsense_d435i_combined.launch.py b/rtabmap_examples/launch/realsense_d435i_combined.launch.py new file mode 100644 index 00000000..89efdffc --- /dev/null +++ b/rtabmap_examples/launch/realsense_d435i_combined.launch.py @@ -0,0 +1,106 @@ +# Requirements: +# A realsense D435i +# Install realsense2 ros2 package (ros-$ROS_DISTRO-realsense2-camera) +# Example: +# $ ros2 launch rtabmap_examples realsense_d435i_combined.launch.py +# +# Description: In this example, we feed visual odometry with IR stereo images +# for better pose estimation while seding RGB-D data to slam for +# a colored map. +# +import os + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch_ros.actions import Node, SetParameter +from launch.actions import IncludeLaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration + +def generate_launch_description(): + vo_parameters={ + 'frame_id':'camera_link', + 'wait_imu_to_init':True} + + vo_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')] + + slam_parameters={ + 'frame_id':'camera_link', + 'subscribe_depth':True, + 'subscribe_odom_info':True, + 'approx_sync':False} + + slam_remappings=[ + ('imu', '/imu/data'), + ('rgb/image', '/camera/color/image_raw'), + ('rgb/camera_info', '/camera/color/camera_info'), + ('depth/image', '/camera/aligned_depth_to_color/image_raw')] + + return LaunchDescription([ + + # Launch arguments + DeclareLaunchArgument( + 'unite_imu_method', default_value='2', + description='0-None, 1-copy, 2-linear_interpolation. Use unite_imu_method:="1" if imu topics stop being published.'), + + #Hack to disable IR emitter + SetParameter(name='depth_module.emitter_enabled', value=0), + + DeclareLaunchArgument( + 'args', default_value='', + description='Extra arguments set to rtabmap and odometry nodes.'), + + DeclareLaunchArgument( + 'odom_args', default_value='', + description='Extra arguments just for odometry node. If the same argument is already set in \"args\", it will be overwritten by the one in \"odom_args\".'), + + + # Launch camera driver + IncludeLaunchDescription( + PythonLaunchDescriptionSource([os.path.join( + get_package_share_directory('realsense2_camera'), 'launch'), + '/rs_launch.py']), + launch_arguments={'camera_namespace': '', + 'enable_gyro': 'true', + 'enable_accel': 'true', + 'unite_imu_method': LaunchConfiguration('unite_imu_method'), + 'enable_infra1': 'true', + 'enable_infra2': 'true', + 'align_depth.enable': 'true', + 'enable_sync': 'true', + 'rgb_camera.profile': '640x360x30'}.items(), + ), + + Node( + package='rtabmap_odom', executable='stereo_odometry', output='screen', + parameters=[vo_parameters], + arguments=[LaunchConfiguration("args"), LaunchConfiguration("odom_args")], + remappings=vo_remappings), + + Node( + package='rtabmap_slam', executable='rtabmap', output='screen', + parameters=[slam_parameters], + remappings=slam_remappings, + arguments=['-d', LaunchConfiguration("args")]), + + Node( + package='rtabmap_viz', executable='rtabmap_viz', output='screen', + parameters=[slam_parameters, + {'odometry_node_name': "stereo_odometry"}], + remappings=slam_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')]), + ]) From 6ed02b65e446ee21421b711222465f2499d78f45 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sun, 8 Feb 2026 14:54:33 -0800 Subject: [PATCH 39/56] Added new depthai examples (color and stereo) --- rtabmap_examples/launch/depthai.launch.py | 8 +- .../launch/depthai_color.launch.py | 81 +++++++++++++++++ .../launch/depthai_stereo.launch.py | 90 +++++++++++++++++++ 3 files changed, 178 insertions(+), 1 deletion(-) create mode 100644 rtabmap_examples/launch/depthai_color.launch.py create mode 100644 rtabmap_examples/launch/depthai_stereo.launch.py diff --git a/rtabmap_examples/launch/depthai.launch.py b/rtabmap_examples/launch/depthai.launch.py index 8918bba0..41b077d6 100644 --- a/rtabmap_examples/launch/depthai.launch.py +++ b/rtabmap_examples/launch/depthai.launch.py @@ -1,8 +1,14 @@ # Requirements: # A OAK-D camera -# Install depthai-ros package (https://github.com/luxonis/depthai-ros) +# Install depthai-ros package (https://github.com/luxonis/depthai-ros). Tested on Humble branch! # Example: # $ ros2 launch rtabmap_examples depthai.launch.py camera_model:=OAK-D +# +# Description: In this example, we feed IR-D images to rtabmap +# +# Note: The first frames may be too bright or too dark till camera exposure adjusts +# to an appriopriate level. Do "Detection->Reset odometry", then +# "Edit->Delete memory" if tracking is lost on start. import os diff --git a/rtabmap_examples/launch/depthai_color.launch.py b/rtabmap_examples/launch/depthai_color.launch.py new file mode 100644 index 00000000..6fc0202b --- /dev/null +++ b/rtabmap_examples/launch/depthai_color.launch.py @@ -0,0 +1,81 @@ +# Requirements: +# A OAK-D camera +# Install depthai-ros package (https://github.com/luxonis/depthai-ros). Tested on Humble branch! +# Example: +# $ ros2 launch rtabmap_examples depthai_color.launch.py camera_model:=OAK-D +# +# Description: In this example, we feed RGB-D images to rtabmap +# +# Note: The first frames may be too bright or too dark till camera exposure adjusts +# to an appriopriate level. Do "Detection->Reset odometry", then +# "Edit->Delete memory" if tracking is lost on start. + +import os + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource + + +def generate_launch_description(): + parameters=[{'frame_id':'oak-d-base-frame', + 'subscribe_rgbd':True, + 'subscribe_odom_info':True, + 'approx_sync':False}] + + sync_parameters=[{'approx_sync':True, + 'approx_sync_max_interval':0.005}] + + remappings=[('imu', '/imu/data')] + + return LaunchDescription([ + + # Launch camera driver + IncludeLaunchDescription( + PythonLaunchDescriptionSource([os.path.join( + get_package_share_directory('depthai_examples'), 'launch'), + '/stereo_inertial_node.launch.py']), + launch_arguments={'enableRviz': 'false', + 'rgbResolution': '1080p', + 'rgbScaleNumerator': '2', # Convert to 720p (same size than depth) + 'rgbScaleDinominator': '3'}.items(), + ), + + # Sync right/depth/camera_info together + Node( + package='rtabmap_sync', executable='rgbd_sync', output='screen', + parameters=sync_parameters, + remappings=[('rgb/image', '/color/image'), + ('rgb/camera_info', '/color/camera_info'), + ('depth/image', '/stereo/depth')]), + + # 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', '/imu')]), + + # Visual odometry + Node( + package='rtabmap_odom', executable='rgbd_odometry', output='screen', + parameters=parameters, + remappings=remappings), + + # VSLAM + Node( + package='rtabmap_slam', executable='rtabmap', output='screen', + parameters=parameters, + remappings=remappings, + arguments=['-d']), + + # Visualization + Node( + package='rtabmap_viz', executable='rtabmap_viz', output='screen', + parameters=parameters, + remappings=remappings) + ]) diff --git a/rtabmap_examples/launch/depthai_stereo.launch.py b/rtabmap_examples/launch/depthai_stereo.launch.py new file mode 100644 index 00000000..a5479607 --- /dev/null +++ b/rtabmap_examples/launch/depthai_stereo.launch.py @@ -0,0 +1,90 @@ +# Requirements: +# A OAK-D camera +# Install depthai-ros package (https://github.com/luxonis/depthai-ros). Tested on Humble branch! +# +# Issue: To have compatible stereo camera info with rtabmap (P[0,3] should be positive on left camera info or negative in right camera info), +# we should add the following line here (https://github.com/luxonis/depthai-ros/blob/887248d72cc6b9515793f828645346408b9cad47/depthai_examples/src/stereo_inertial_publisher.cpp#L593) +# so that left camera info has a positive P[0,3] instead of negative to correctly compute the baseline: +# +# leftCameraInfo.p[3] *=-1; +# +# Example: +# $ ros2 launch rtabmap_examples depthai_stereo.launch.py camera_model:=OAK-D +# +# Description: In this example, we feed stereo IR images to rtabmap +# +# Note: The first frames may be too bright or too dark till camera exposure adjusts +# to an appriopriate level. Do "Detection->Reset odometry", then +# "Edit->Delete memory" if tracking is lost on start. + +import os + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource + + +def generate_launch_description(): + parameters={'frame_id':'oak-d-base-frame', + 'subscribe_rgbd':True, + 'subscribe_odom_info':True, + 'approx_sync':False, + 'wait_imu_to_init':True} + + sync_parameters=[{'approx_sync':True, + 'approx_sync_max_interval':0.005}] + + remappings=[('imu', '/imu/data')] + + return LaunchDescription([ + + # Launch camera driver + IncludeLaunchDescription( + PythonLaunchDescriptionSource([os.path.join( + get_package_share_directory('depthai_examples'), 'launch'), + '/stereo_inertial_node.launch.py']), + launch_arguments={'depth_aligned': 'false', # not color mode + 'enableRviz': 'false', + 'monoResolution': '400p'}.items(), + ), + + # Sync right/depth/camera_info together + Node( + package='rtabmap_sync', executable='stereo_sync', output='screen', + parameters=sync_parameters, + remappings=[('left/image', '/left/image_rect'), + ('left/camera_info', '/left/camera_info'), + ('right/image', '/right/image_rect'), + ('right/camera_info', '/right/camera_info')]), + + # 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', '/imu')]), + + # Visual odometry + Node( + package='rtabmap_odom', executable='stereo_odometry', output='screen', + parameters=[parameters], + remappings=remappings), + + # VSLAM + Node( + package='rtabmap_slam', executable='rtabmap', output='screen', + parameters=[parameters], + remappings=remappings, + arguments=['-d']), + + # Visualization + Node( + package='rtabmap_viz', executable='rtabmap_viz', output='screen', + parameters=[parameters, + {'odometry_node_name': "stereo_odometry"}], + remappings=remappings) + ]) From 9d7fb44723021cd266aed4776232a9e32a6748ed Mon Sep 17 00:00:00 2001 From: matlabbe Date: Thu, 19 Feb 2026 19:18:11 -0800 Subject: [PATCH 40/56] Added multi_rgbd_inertial_dataset.launch example (https://github.com/introlab/rtabmap/issues/1654). Also fixed dev container not showing GUI apps. --- .devcontainer/devcontainer.json | 8 +- .../config/multi_rgbd_inertial_dataset.ini | 453 ++++++++++++++++++ .../multi_rgbd_inertial_dataset_front.yaml | 27 ++ .../multi_rgbd_inertial_dataset_left.yaml | 27 ++ .../multi_rgbd_inertial_dataset_rear.yaml | 27 ++ .../multi_rgbd_inertial_dataset_right.yaml | 27 ++ .../launch/multi_rgbd_inertial_dataset.launch | 131 +++++ 7 files changed, 698 insertions(+), 2 deletions(-) create mode 100644 rtabmap_examples/launch/config/multi_rgbd_inertial_dataset.ini create mode 100644 rtabmap_examples/launch/config/multi_rgbd_inertial_dataset_front.yaml create mode 100644 rtabmap_examples/launch/config/multi_rgbd_inertial_dataset_left.yaml create mode 100644 rtabmap_examples/launch/config/multi_rgbd_inertial_dataset_rear.yaml create mode 100644 rtabmap_examples/launch/config/multi_rgbd_inertial_dataset_right.yaml create mode 100644 rtabmap_examples/launch/multi_rgbd_inertial_dataset.launch diff --git a/.devcontainer/devcontainer.json b/.devcontainer/devcontainer.json index 4990bced..091c51e4 100644 --- a/.devcontainer/devcontainer.json +++ b/.devcontainer/devcontainer.json @@ -22,6 +22,10 @@ }, "workspaceMount": "source=${localWorkspaceFolder},target=/home/vscode/catkin_ws/src/rtabmap_ros,type=bind", "workspaceFolder": "/home/vscode/catkin_ws", - "postCreateCommand": "cd /home/vscode/catkin_ws/src && catkin_init_workspace" - //"runArgs": ["--privileged", "--runtime=nvidia", "--network=host"] + "postCreateCommand": "cd /home/vscode/catkin_ws/src && catkin_init_workspace", + "runArgs": ["--privileged", + "--runtime=nvidia", + "--env=DISPLAY", + "--env=QT_X11_NO_MITSHM=1", + "--volume=/tmp/.X11-unix:/tmp/.X11-unix"] } diff --git a/rtabmap_examples/launch/config/multi_rgbd_inertial_dataset.ini b/rtabmap_examples/launch/config/multi_rgbd_inertial_dataset.ini new file mode 100644 index 00000000..47189c4e --- /dev/null +++ b/rtabmap_examples/launch/config/multi_rgbd_inertial_dataset.ini @@ -0,0 +1,453 @@ +[Core] +Rtabmap\WorkingDirectory=/home/vscode/.ros + +[Gui] +AboutDialog\geometry=@ByteArray(\x1\xd9\xd0\xcb\0\x3\0\0\0\0\0\0\0\0\0/\0\0\x2\xf7\0\0\x3\x10\0\0\0\0\0\0\0/\0\0\x2\xf7\0\0\x3\x10\0\0\0\0\0\0\0\0\a\x80\0\0\0\0\0\0\0/\0\0\x2\xf7\0\0\x3\x10) +DepthCalibrationDialog\bin_depth=2 +DepthCalibrationDialog\bin_height=6 +DepthCalibrationDialog\bin_width=8 +DepthCalibrationDialog\cone_radius=0.02 +DepthCalibrationDialog\cone_stddev_thresh=0.1 +DepthCalibrationDialog\decimation=1 +DepthCalibrationDialog\laser_scan=false +DepthCalibrationDialog\max_depth=3.5 +DepthCalibrationDialog\max_model_depth=10 +DepthCalibrationDialog\min_depth=0 +DepthCalibrationDialog\smoothing=1 +DepthCalibrationDialog\voxel=0.01 +ExportBundlerDialog\exportPoints=false +ExportBundlerDialog\laplacianThr=0 +ExportBundlerDialog\maxAngularSpeed=0 +ExportBundlerDialog\maxLinearSpeed=0 +ExportBundlerDialog\sba_iterations=20 +ExportBundlerDialog\sba_rematch_features=true +ExportBundlerDialog\sba_type=0 +ExportBundlerDialog\sba_variance=1 +ExportCloudsDialog\assemble=true +ExportCloudsDialog\assemble_samples=0 +ExportCloudsDialog\assemble_voxel=0.01 +ExportCloudsDialog\bilateral=false +ExportCloudsDialog\bilateral_sigma_r=0.1 +ExportCloudsDialog\bilateral_sigma_s=10 +ExportCloudsDialog\binary=true +ExportCloudsDialog\cam_proj=false +ExportCloudsDialog\cam_proj_decimation=1 +ExportCloudsDialog\cam_proj_distance_policy=true +ExportCloudsDialog\cam_proj_export_format=0 +ExportCloudsDialog\cam_proj_keep_points=false +ExportCloudsDialog\cam_proj_mask= +ExportCloudsDialog\cam_proj_max_angle=0 +ExportCloudsDialog\cam_proj_max_depth_error=0 +ExportCloudsDialog\cam_proj_max_distance=0 +ExportCloudsDialog\cam_proj_recolor_points=true +ExportCloudsDialog\cam_proj_roi_ratios=0.0 0.0 0.0 0.0 +ExportCloudsDialog\cputsdf_flattenRadius=0.005 +ExportCloudsDialog\cputsdf_minWeight=0 +ExportCloudsDialog\cputsdf_randomSplit=1 +ExportCloudsDialog\cputsdf_resolution=0.01 +ExportCloudsDialog\cputsdf_size=12 +ExportCloudsDialog\cputsdf_truncNeg=0.03 +ExportCloudsDialog\cputsdf_truncPos=0.03 +ExportCloudsDialog\filtering=false +ExportCloudsDialog\filtering_min_neighbors=5 +ExportCloudsDialog\filtering_radius=0 +ExportCloudsDialog\frame=0 +ExportCloudsDialog\from_depth=true +ExportCloudsDialog\gain=false +ExportCloudsDialog\gain_beta=10 +ExportCloudsDialog\gain_full=false +ExportCloudsDialog\gain_overlap=0 +ExportCloudsDialog\gain_radius=0.02 +ExportCloudsDialog\gain_rgb=true +ExportCloudsDialog\intensity_colormap=0 +ExportCloudsDialog\mesh=false +ExportCloudsDialog\mesh_angle_tolerance=15 +ExportCloudsDialog\mesh_clean=true +ExportCloudsDialog\mesh_color_radius=0.05 +ExportCloudsDialog\mesh_decimation_factor=0 +ExportCloudsDialog\mesh_dense_strategy=1 +ExportCloudsDialog\mesh_max_polygons=0 +ExportCloudsDialog\mesh_min_cluster_size=0 +ExportCloudsDialog\mesh_mu=2.5 +ExportCloudsDialog\mesh_quad=false +ExportCloudsDialog\mesh_radius=0.2 +ExportCloudsDialog\mesh_texture=false +ExportCloudsDialog\mesh_textureBlending=true +ExportCloudsDialog\mesh_textureBlendingDecimation=0 +ExportCloudsDialog\mesh_textureBrightnessConstrastRatioHigh=5 +ExportCloudsDialog\mesh_textureBrightnessConstrastRatioLow=0 +ExportCloudsDialog\mesh_textureCameraFiltering=false +ExportCloudsDialog\mesh_textureCameraFilteringAngle=30 +ExportCloudsDialog\mesh_textureCameraFilteringLaplacian=0 +ExportCloudsDialog\mesh_textureCameraFilteringRadius=0 +ExportCloudsDialog\mesh_textureCameraFilteringVel=0 +ExportCloudsDialog\mesh_textureCameraFilteringVelRad=0 +ExportCloudsDialog\mesh_textureDistanceToCamPolicy=false +ExportCloudsDialog\mesh_textureExposureFusion=false +ExportCloudsDialog\mesh_textureFormat=0 +ExportCloudsDialog\mesh_textureMaxAngle=0 +ExportCloudsDialog\mesh_textureMaxCount=1 +ExportCloudsDialog\mesh_textureMaxDepthError=0 +ExportCloudsDialog\mesh_textureMaxDistance=3 +ExportCloudsDialog\mesh_textureMinCluster=50 +ExportCloudsDialog\mesh_textureMultiband=false +ExportCloudsDialog\mesh_textureMultibandAngleHardThr=90 +ExportCloudsDialog\mesh_textureMultibandBestScoreThr=0.1 +ExportCloudsDialog\mesh_textureMultibandDownScale=2 +ExportCloudsDialog\mesh_textureMultibandFillHoles=false +ExportCloudsDialog\mesh_textureMultibandForceVisible=false +ExportCloudsDialog\mesh_textureMultibandNbContrib=1 5 10 0 +ExportCloudsDialog\mesh_textureMultibandPadding=5 +ExportCloudsDialog\mesh_textureMultibandUnwrap=0 +ExportCloudsDialog\mesh_textureRoiRatios=0.0 0.0 0.0 0.0 +ExportCloudsDialog\mesh_textureSize=6 +ExportCloudsDialog\mesh_textureVertexColorPolicy=0 +ExportCloudsDialog\mesh_triangle_size=1 +ExportCloudsDialog\mls=false +ExportCloudsDialog\mls_dilation_iterations=1 +ExportCloudsDialog\mls_dilation_voxel_size=0.005 +ExportCloudsDialog\mls_output_voxel_size=0 +ExportCloudsDialog\mls_point_density=10 +ExportCloudsDialog\mls_polygonial_order=2 +ExportCloudsDialog\mls_radius=0.04 +ExportCloudsDialog\mls_upsampling_method=0 +ExportCloudsDialog\mls_upsampling_radius=0.01 +ExportCloudsDialog\mls_upsampling_step=0.005 +ExportCloudsDialog\nodes_filtering=false +ExportCloudsDialog\nodes_filtering_xmax=0 +ExportCloudsDialog\nodes_filtering_xmin=0 +ExportCloudsDialog\nodes_filtering_ymax=0 +ExportCloudsDialog\nodes_filtering_ymin=0 +ExportCloudsDialog\nodes_filtering_zmax=0 +ExportCloudsDialog\nodes_filtering_zmin=0 +ExportCloudsDialog\normals_ground_normals_up=0 +ExportCloudsDialog\normals_k=20 +ExportCloudsDialog\normals_radius=0 +ExportCloudsDialog\openchisel_carving_dist_m=0.05 +ExportCloudsDialog\openchisel_chunk_size_x=16 +ExportCloudsDialog\openchisel_chunk_size_y=16 +ExportCloudsDialog\openchisel_chunk_size_z=16 +ExportCloudsDialog\openchisel_far_plane_dist=1.1 +ExportCloudsDialog\openchisel_integration_weight=1 +ExportCloudsDialog\openchisel_merge_vertices=true +ExportCloudsDialog\openchisel_near_plane_dist=0.05 +ExportCloudsDialog\openchisel_truncation_constant=0.001504 +ExportCloudsDialog\openchisel_truncation_linear=0.00152 +ExportCloudsDialog\openchisel_truncation_quadratic=0.0019 +ExportCloudsDialog\openchisel_truncation_scale=10 +ExportCloudsDialog\openchisel_use_voxel_carving=false +ExportCloudsDialog\pipeline=1 +ExportCloudsDialog\poisson_depth=0 +ExportCloudsDialog\poisson_iso=8 +ExportCloudsDialog\poisson_manifold=true +ExportCloudsDialog\poisson_minDepth=5 +ExportCloudsDialog\poisson_outputPolygons=false +ExportCloudsDialog\poisson_pointWeight=4 +ExportCloudsDialog\poisson_polygon_size=0.03 +ExportCloudsDialog\poisson_samples=1 +ExportCloudsDialog\poisson_scale=1.1 +ExportCloudsDialog\poisson_solver=8 +ExportCloudsDialog\regenerate=false +ExportCloudsDialog\regenerate_ceiling=0 +ExportCloudsDialog\regenerate_decimation=1 +ExportCloudsDialog\regenerate_distortion_model= +ExportCloudsDialog\regenerate_edge_bleeding_error=0 +ExportCloudsDialog\regenerate_fill_error=2 +ExportCloudsDialog\regenerate_fill_size=0 +ExportCloudsDialog\regenerate_floor=0 +ExportCloudsDialog\regenerate_footprint_height=0 +ExportCloudsDialog\regenerate_footprint_length=0 +ExportCloudsDialog\regenerate_footprint_width=0 +ExportCloudsDialog\regenerate_max_depth=4 +ExportCloudsDialog\regenerate_min_depth=0 +ExportCloudsDialog\regenerate_min_depth_confidence=0 +ExportCloudsDialog\regenerate_offaxis_filtering=false +ExportCloudsDialog\regenerate_offaxis_filtering_angle=10 +ExportCloudsDialog\regenerate_offaxis_filtering_neg_x=true +ExportCloudsDialog\regenerate_offaxis_filtering_neg_y=true +ExportCloudsDialog\regenerate_offaxis_filtering_neg_z=true +ExportCloudsDialog\regenerate_offaxis_filtering_pos_x=true +ExportCloudsDialog\regenerate_offaxis_filtering_pos_y=true +ExportCloudsDialog\regenerate_offaxis_filtering_pos_z=true +ExportCloudsDialog\regenerate_roi=0.0 0.0 0.0 0.0 +ExportCloudsDialog\regenerate_scan_decimation=1 +ExportCloudsDialog\regenerate_scan_max_range=0 +ExportCloudsDialog\regenerate_scan_min_range=0 +ExportCloudsDialog\subtract=false +ExportCloudsDialog\subtract_min_neighbors=5 +ExportCloudsDialog\subtract_point_angle=0 +ExportCloudsDialog\subtract_point_radius=0.02 +Figures\counts= +Figures\curves= +Figures\thresholds= +General\beep=false +General\cloudCeilingHeight=0 +General\cloudFiltering=false +General\cloudFilteringAngle=30 +General\cloudFilteringRadius=0.1 +General\cloudFloorHeight=0 +General\cloudNoiseMinNeighbors=5 +General\cloudNoiseRadius=0 +General\cloudVoxel=0 +General\cloudsKept=true +General\colorScheme0=0 +General\colorScheme1=0 +General\colorSchemeScan0=0 +General\colorSchemeScan1=0 +General\decimation0=8 +General\decimation1=4 +General\depthConf0=0 +General\depthConf1=0 +General\downsamplingScan0=1 +General\downsamplingScan1=1 +General\elevationMapShown=0 +General\figure_cache=true +General\figure_time=true +General\gravityLength0=1 +General\gravityLength1=1 +General\gravityShown0=false +General\gravityShown1=true +General\gridMapOpacity=0.75 +General\gridMapShown=false +General\gridUIResolution=0 +General\gtAlign=true +General\imageHighestHypShown=false +General\imageRejectedShown=true +General\imagesKept=true +General\landmarkSize=0 +General\localizationsGraphView=false +General\localizationsGraphViewOdomCache=false +General\loggerEventLevel=3 +General\loggerLevel=2 +General\loggerPauseLevel=3 +General\loggerPrintThreadId=false +General\loggerPrintTime=true +General\loggerType=1 +General\maxDepth0=5 +General\maxDepth1=0 +General\maxRange0=0 +General\maxRange1=0 +General\meshing=false +General\meshing_angle=15 +General\meshing_quad=true +General\meshing_texture=false +General\meshing_triangle_size=2 +General\minDepth0=0 +General\minDepth1=0 +General\minRange0=0 +General\minRange1=0 +General\missingRepublished=true +General\noFiltering=true +General\nochangeGraphView=false +General\normalKSearch=10 +General\normalRadiusSearch=0 +General\notifyNewGlobalPath=false +General\octomap=false +General\octomap_2dgrid=true +General\octomap_3dmap=true +General\octomap_depth=16 +General\octomap_point_size=5 +General\octomap_rendering_type=0 +General\odomDisabled=false +General\odomF2MGravitySigma=-1 +General\odomOnlyInliersShown=false +General\odomQualityThr=50 +General\odomRegistration=3 +General\opacity0=1 +General\opacity1=0.75 +General\opacityScan0=1 +General\opacityScan1=0.5 +General\posteriorGraphView=true +General\ptSize0=1 +General\ptSize1=2 +General\ptSizeFeatures0=3 +General\ptSizeFeatures1=3 +General\ptSizeScan0=1 +General\ptSizeScan1=2 +General\roiRatios0=0.0 0.0 0.0 0.0 +General\roiRatios1=0.0 0.0 0.0 0.0 +General\scanCeilingHeight=0 +General\scanFloorHeight=0 +General\scanNormalKSearch=0 +General\scanNormalRadiusSearch=0 +General\showClouds0=true +General\showClouds1=false +General\showFeatures0=false +General\showFeatures1=true +General\showFrames=false +General\showFrustums0=false +General\showFrustums1=false +General\showGraphs=true +General\showIMUAcc=false +General\showIMUGravity=false +General\showLabels=false +General\showLandmarks=true +General\showScans0=true +General\showScans1=true +General\subtractFiltering=false +General\subtractFilteringAngle=0 +General\subtractFilteringMinPts=5 +General\subtractFilteringRadius=0.02 +General\verticalLayoutUsed=true +General\voxelSizeScan0=0 +General\voxelSizeScan1=0 +General\wordsGraphView=false +MainWindow\geometry=@ByteArray(\x1\xd9\xd0\xcb\0\x3\0\0\0\0\0\x32\0\0\0\x1b\0\0\x6\xf2\0\0\x3\xf\0\0\0\x32\0\0\0@\0\0\x6\xf2\0\0\x3\xf\0\0\0\0\0\0\0\0\a\x80\0\0\0\x32\0\0\0@\0\0\x6\xf2\0\0\x3\xf) +MainWindow\maximized=false +MainWindow\state="@ByteArray(\0\0\0\xff\0\0\0\0\xfd\0\0\0\x3\0\0\0\0\0\0\x3\\\0\0\x2\x95\xfc\x2\0\0\0\x3\xfb\0\0\0$\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0s\0t\0\x61\0t\0s\0V\0\x32\x2\0\0\0\x32\0\0\0\x1b\0\0\0\xc8\0\0\0\xc1\xfb\0\0\0(\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0i\0m\0\x61\0g\0\x65\0V\0i\0\x65\0w\x1\0\0\0;\0\0\x1\xaa\0\0\0\x37\0\xff\xff\xff\xfb\0\0\0&\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0o\0\x64\0o\0m\0\x65\0t\0r\0y\x1\0\0\x1\xeb\0\0\0\xe5\0\0\0\x13\0\xff\xff\xff\0\0\0\x1\0\0\x3_\0\0\x2\x95\xfc\x2\0\0\0\x4\xfb\0\0\0,\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0\x63\0l\0o\0u\0\x64\0V\0i\0\x65\0w\0\x65\0r\x1\0\0\0;\0\0\x2\x95\0\0\0\xdb\0\xff\xff\xff\xfb\0\0\0\x38\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0l\0o\0o\0p\0\x43\0l\0o\0s\0u\0r\0\x65\0V\0i\0\x65\0w\0\x65\0r\0\0\0\0\0\xff\xff\xff\xff\0\0\0\xf0\0\xff\xff\xff\xfb\0\0\0,\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0g\0r\0\x61\0p\0h\0V\0i\0\x65\0w\0\x65\0r\x2\0\0\x4\xf2\0\0\x1\x39\0\0\x2}\0\0\x1\x90\xfb\0\0\0\x34\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0m\0u\0l\0t\0i\0S\0\x65\0s\0s\0i\0o\0n\0L\0o\0\x63\0\0\0\0\0\xff\xff\xff\xff\0\0\0\x13\0\xff\xff\xff\0\0\0\x3\0\0\0\0\0\0\0\0\xfc\x1\0\0\0\x5\xfb\0\0\0(\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0p\0o\0s\0t\0\x65\0r\0i\0o\0r\0\0\0\0\0\xff\xff\xff\xff\0\0\0\xcb\0\xff\xff\xff\xfb\0\0\0*\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0l\0i\0k\0\x65\0l\0i\0h\0o\0o\0\x64\0\0\0\0\0\xff\xff\xff\xff\0\0\0\xcb\0\xff\xff\xff\xfb\0\0\0$\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0\x63\0o\0n\0s\0o\0l\0\x65\0\0\0\0\0\xff\xff\xff\xff\0\0\x1(\0\xff\xff\xff\xfb\0\0\0\x30\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0r\0\x61\0w\0l\0i\0k\0\x65\0l\0i\0h\0o\0o\0\x64\0\0\0\0\0\xff\xff\xff\xff\0\0\0\xcb\0\xff\xff\xff\xfb\0\0\0\x30\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0m\0\x61\0p\0V\0i\0s\0i\0\x62\0i\0l\0i\0t\0y\x2\0\0\0\x32\0\0\0\x1b\0\0\0\xc8\0\0\0\x88\0\0\0\0\0\0\x2\x95\0\0\0\x4\0\0\0\x4\0\0\0\b\0\0\0\b\xfc\0\0\0\x1\0\0\0\x2\0\0\0\x2\0\0\0\xe\0t\0o\0o\0l\0\x42\0\x61\0r\0\0\0\0\0\xff\xff\xff\xff\0\0\0\0\0\0\0\0\0\0\0\x12\0t\0o\0o\0l\0\x42\0\x61\0r\0_\0\x32\x1\0\0\0\0\xff\xff\xff\xff\0\0\0\0\0\0\0\0)" +MainWindow\status_bar=false +PostProcessingDialog\cluster_angle=30 +PostProcessingDialog\cluster_radius=1 +PostProcessingDialog\detect_more_lc=true +PostProcessingDialog\inter_session=true +PostProcessingDialog\intra_session=true +PostProcessingDialog\iterations=5 +PostProcessingDialog\refine_lc=false +PostProcessingDialog\refine_neigbors=false +PostProcessingDialog\sba=false +PostProcessingDialog\sba_iterations=20 +PostProcessingDialog\sba_rematch_features=true +PostProcessingDialog\sba_type=1 +PostProcessingDialog\sba_variance=1 +PreferencesDialog\geometry=@ByteArray(\x1\xd9\xd0\xcb\0\x3\0\0\0\0\x1\xa8\xff\xff\xff\xf6\0\0\x5{\0\0\x3\xb7\0\0\x1\xa8\0\0\0\x1b\0\0\x5{\0\0\x3\xb7\0\0\0\0\0\0\0\0\a\x80\0\0\x1\xa8\0\0\0\x1b\0\0\x5{\0\0\x3\xb7) +graphicsView_graphView\current_goal_color=@Variant(\0\0\0\x43\x1\xff\xff\x80\x80\0\0\x80\x80\0\0) +graphicsView_graphView\ensure_frame_visible=1 +graphicsView_graphView\global_color=@Variant(\0\0\0\x43\x1\xff\xff\xff\xff\0\0\0\0\0\0) +graphicsView_graphView\global_path_color=@Variant(\0\0\0\x43\x1\xff\xff\x80\x80\0\0\x80\x80\0\0) +graphicsView_graphView\global_path_visible=true +graphicsView_graphView\gps_color=@Variant(\0\0\0\x43\x1\xff\xff\0\0\x80\x80\x80\x80\0\0) +graphicsView_graphView\gps_graph_visible=true +graphicsView_graphView\graph_visible=true +graphicsView_graphView\grid_visible=true +graphicsView_graphView\gt_color=@Variant(\0\0\0\x43\x1\xff\xff\xa0\xa0\xa0\xa0\xa4\xa4\0\0) +graphicsView_graphView\gt_graph_visible=true +graphicsView_graphView\highlighting_color_0=@Variant(\0\0\0\x43\x1\xff\xff\xff\xff\xff\xff\0\0\0\0) +graphicsView_graphView\highlighting_color_1=@Variant(\0\0\0\x43\x1\xff\xff\xff\xff\0\0\xff\xff\0\0) +graphicsView_graphView\inter_session_color=@Variant(\0\0\0\x43\x1\xff\xff\0\0\xff\xff\0\0\0\0) +graphicsView_graphView\intra_inter_session_colors_enabled=false +graphicsView_graphView\intra_session_color=@Variant(\0\0\0\x43\x1\xff\xff\xff\xff\0\0\0\0\0\0) +graphicsView_graphView\link_width=0 +graphicsView_graphView\local_color=@Variant(\0\0\0\x43\x1\xff\xff\xff\xff\xff\xff\0\0\0\0) +graphicsView_graphView\local_path_color=@Variant(\0\0\0\x43\x1\xff\xff\0\0\xff\xff\xff\xff\0\0) +graphicsView_graphView\local_path_visible=true +graphicsView_graphView\local_radius_visible=false +graphicsView_graphView\loop_closure_outlier_thr=@Variant(\0\0\0\x87\0\0\0\0) +graphicsView_graphView\min_link_length=@Variant(\0\0\0\x87<\xa3\xd7\n) +graphicsView_graphView\neighbor_color=@Variant(\0\0\0\x43\x1\xff\xff\0\0\0\0\xff\xff\0\0) +graphicsView_graphView\neighbor_merged_color=@Variant(\0\0\0\x43\x1\xff\xff\xff\xff\xaa\xaa\0\0\0\0) +graphicsView_graphView\node_color=@Variant(\0\0\0\x43\x1\xff\xff\0\0\0\0\xff\xff\0\0) +graphicsView_graphView\node_odom_cache_color=@Variant(\0\0\0\x43\x1\xff\xff\0\0\x80\x80\0\0\0\0) +graphicsView_graphView\node_radius=0.009999999776482582 +graphicsView_graphView\node_visible=true +graphicsView_graphView\odom_cache_overlay=true +graphicsView_graphView\orientation_ENU=false +graphicsView_graphView\origin_visible=true +graphicsView_graphView\referential_visible=true +graphicsView_graphView\rejected_color=@Variant(\0\0\0\x43\x1\xff\xff\0\0\0\0\0\0\0\0) +graphicsView_graphView\user_color=@Variant(\0\0\0\x43\x1\xff\xff\xff\xff\0\0\0\0\0\0) +graphicsView_graphView\view_plane=0 +graphicsView_graphView\virtual_color=@Variant(\0\0\0\x43\x1\xff\xff\xff\xff\0\0\xff\xff\0\0) +imageView_loopClosure\alpha=100 +imageView_loopClosure\bg_color=@Variant(\0\0\0\x43\x1\xff\xff\0\0\0\0\0\0\0\0) +imageView_loopClosure\colormap=2 +imageView_loopClosure\colormap_camera_frame=true +imageView_loopClosure\colormap_max_range=@Variant(\0\0\0\x87\0\0\0\0) +imageView_loopClosure\colormap_min_range=@Variant(\0\0\0\x87\0\0\0\0) +imageView_loopClosure\confidence_shown=false +imageView_loopClosure\depth_shown=false +imageView_loopClosure\feature_color=@Variant(\0\0\0\x43\x1\xff\xff\xff\xff\xff\xff\0\0\0\0) +imageView_loopClosure\features_shown=true +imageView_loopClosure\features_size=0 +imageView_loopClosure\graphics_view=false +imageView_loopClosure\graphics_view_scale=true +imageView_loopClosure\graphics_view_scale_to_height=false +imageView_loopClosure\image_shown=true +imageView_loopClosure\lines_shown=true +imageView_loopClosure\lines_width=0 +imageView_loopClosure\matching_feature_color=@Variant(\0\0\0\x43\x1\xff\xff\xff\xff\0\0\xff\xff\0\0) +imageView_loopClosure\matching_line_color=@Variant(\0\0\0\x43\x1\xff\xff\0\0\xff\xff\xff\xff\0\0) +imageView_odometry\alpha=200 +imageView_odometry\bg_color=@Variant(\0\0\0\x43\x1\xff\xff\0\0\0\0\0\0\0\0) +imageView_odometry\colormap=2 +imageView_odometry\colormap_camera_frame=true +imageView_odometry\colormap_max_range=@Variant(\0\0\0\x87\0\0\0\0) +imageView_odometry\colormap_min_range=@Variant(\0\0\0\x87\0\0\0\0) +imageView_odometry\confidence_shown=false +imageView_odometry\depth_shown=false +imageView_odometry\feature_color=@Variant(\0\0\0\x43\x1\xff\xff\xff\xff\xff\xff\0\0\0\0) +imageView_odometry\features_shown=true +imageView_odometry\features_size=0 +imageView_odometry\graphics_view=false +imageView_odometry\graphics_view_scale=true +imageView_odometry\graphics_view_scale_to_height=false +imageView_odometry\image_shown=true +imageView_odometry\lines_shown=true +imageView_odometry\lines_width=0 +imageView_odometry\matching_feature_color=@Variant(\0\0\0\x43\x1\xff\xff\xff\xff\0\0\xff\xff\0\0) +imageView_odometry\matching_line_color=@Variant(\0\0\0\x43\x1\xff\xff\0\0\xff\xff\xff\xff\0\0) +imageView_source\alpha=100 +imageView_source\bg_color=@Variant(\0\0\0\x43\x1\xff\xff\0\0\0\0\0\0\0\0) +imageView_source\colormap=2 +imageView_source\colormap_camera_frame=true +imageView_source\colormap_max_range=@Variant(\0\0\0\x87\0\0\0\0) +imageView_source\colormap_min_range=@Variant(\0\0\0\x87\0\0\0\0) +imageView_source\confidence_shown=false +imageView_source\depth_shown=false +imageView_source\feature_color=@Variant(\0\0\0\x43\x1\xff\xff\xff\xff\xff\xff\0\0\0\0) +imageView_source\features_shown=true +imageView_source\features_size=0 +imageView_source\graphics_view=false +imageView_source\graphics_view_scale=true +imageView_source\graphics_view_scale_to_height=false +imageView_source\image_shown=true +imageView_source\lines_shown=true +imageView_source\lines_width=0 +imageView_source\matching_feature_color=@Variant(\0\0\0\x43\x1\xff\xff\xff\xff\0\0\xff\xff\0\0) +imageView_source\matching_line_color=@Variant(\0\0\0\x43\x1\xff\xff\0\0\xff\xff\xff\xff\0\0) +multisession_imageview\alpha=100 +multisession_imageview\bg_color=@Variant(\0\0\0\x43\x1\xff\xff\0\0\0\0\0\0\0\0) +multisession_imageview\colormap=2 +multisession_imageview\colormap_camera_frame=true +multisession_imageview\colormap_max_range=@Variant(\0\0\0\x87\0\0\0\0) +multisession_imageview\colormap_min_range=@Variant(\0\0\0\x87\0\0\0\0) +multisession_imageview\confidence_shown=false +multisession_imageview\depth_shown=false +multisession_imageview\feature_color=@Variant(\0\0\0\x43\x1\xff\xff\xff\xff\xff\xff\0\0\0\0) +multisession_imageview\features_shown=true +multisession_imageview\features_size=0 +multisession_imageview\graphics_view=false +multisession_imageview\graphics_view_scale=true +multisession_imageview\graphics_view_scale_to_height=false +multisession_imageview\image_shown=true +multisession_imageview\lines_shown=true +multisession_imageview\lines_width=0 +multisession_imageview\matching_feature_color=@Variant(\0\0\0\x43\x1\xff\xff\xff\xff\0\0\xff\xff\0\0) +multisession_imageview\matching_line_color=@Variant(\0\0\0\x43\x1\xff\xff\0\0\xff\xff\xff\xff\0\0) +widget_cloudViewer\bg_color=@Variant(\0\0\0\x43\x1\xff\xff\0\0\0\0\0\0\0\0) +widget_cloudViewer\camera_axis_shown=true +widget_cloudViewer\camera_focal=@Variant(\0\0\0T8\xb6\0\0\x37s\0\0\xb5\xd5\0\0) +widget_cloudViewer\camera_free=false +widget_cloudViewer\camera_lockZ=true +widget_cloudViewer\camera_pose=@Variant(\0\0\0T\xc0\xc0\x9b\x36@\x80\xf7\xea\x41\x17p\xe) +widget_cloudViewer\camera_target_follow=true +widget_cloudViewer\camera_target_locked=false +widget_cloudViewer\camera_up=@Variant(\0\0\0T\0\0\0\0\0\0\0\0?\x80\0\0) +widget_cloudViewer\color_range_inverted=0 +widget_cloudViewer\color_range_max=0 +widget_cloudViewer\color_range_min=0 +widget_cloudViewer\coordinate_frame_scale=1 +widget_cloudViewer\frustum_color=@Variant(\0\0\0\x43\x1\xff\xff\0\0\xff\xff\0\0\0\0) +widget_cloudViewer\frustum_scale=@Variant(\0\0\0\x87?\0\0\0) +widget_cloudViewer\frustum_shown=true +widget_cloudViewer\grid=false +widget_cloudViewer\grid_cell_count=50 +widget_cloudViewer\grid_cell_size=1 +widget_cloudViewer\intensity_max=100 +widget_cloudViewer\intensity_rainbow_colormap=false +widget_cloudViewer\intensity_red_colormap=true +widget_cloudViewer\normals=false +widget_cloudViewer\normals_scale=0.20000000298023224 +widget_cloudViewer\normals_step=1 +widget_cloudViewer\rendering_rate=5 +widget_cloudViewer\trajectory_shown=true +widget_cloudViewer\trajectory_size=100 diff --git a/rtabmap_examples/launch/config/multi_rgbd_inertial_dataset_front.yaml b/rtabmap_examples/launch/config/multi_rgbd_inertial_dataset_front.yaml new file mode 100644 index 00000000..2c3c2ece --- /dev/null +++ b/rtabmap_examples/launch/config/multi_rgbd_inertial_dataset_front.yaml @@ -0,0 +1,27 @@ +#%YAML:1.0 +--- +camera_name: MULTI_RGBD_INERTIAL_DATASET_FRONT +image_width: 640 +image_height: 360 +camera_matrix: + rows: 3 + cols: 3 + data: [ 319.1610, 0., 329.5844, 0., + 319.0881, 182.1654, 0., 0., 1. ] +distortion_coefficients: + rows: 1 + cols: 4 + data: [ -0.0535, 0.0589, -0.0176, 0.0 ] +distortion_model: plumb_bob +rectification_matrix: + rows: 3 + cols: 3 + data: [ 1, 0, 0, + 0, 1, 0, + 0, 0, 1 ] +projection_matrix: + rows: 3 + cols: 4 + data: [ 319.1610, 0., 329.5844, 0., 0., + 319.0881, 182.1654, 0., 0., 0., 1., + 0. ] \ No newline at end of file diff --git a/rtabmap_examples/launch/config/multi_rgbd_inertial_dataset_left.yaml b/rtabmap_examples/launch/config/multi_rgbd_inertial_dataset_left.yaml new file mode 100644 index 00000000..7c2ffe86 --- /dev/null +++ b/rtabmap_examples/launch/config/multi_rgbd_inertial_dataset_left.yaml @@ -0,0 +1,27 @@ +#%YAML:1.0 +--- +camera_name: MULTI_RGBD_INERTIAL_DATASET_LEFT +image_width: 640 +image_height: 360 +camera_matrix: + rows: 3 + cols: 3 + data: [ 319.1506, 0., 321.8367, 0., + 318.9474, 180.0959, 0., 0., 1. ] +distortion_coefficients: + rows: 1 + cols: 4 + data: [ -0.0556, 0.0576, -0.0174, 0.0 ] +distortion_model: plumb_bob +rectification_matrix: + rows: 3 + cols: 3 + data: [ 1, 0, 0, + 0, 1, 0, + 0, 0, 1 ] +projection_matrix: + rows: 3 + cols: 4 + data: [ 319.1506, 0., 321.8367, 0., 0., + 318.9474, 180.0959, 0., 0., 0., 1., + 0. ] diff --git a/rtabmap_examples/launch/config/multi_rgbd_inertial_dataset_rear.yaml b/rtabmap_examples/launch/config/multi_rgbd_inertial_dataset_rear.yaml new file mode 100644 index 00000000..b6869e87 --- /dev/null +++ b/rtabmap_examples/launch/config/multi_rgbd_inertial_dataset_rear.yaml @@ -0,0 +1,27 @@ +#%YAML:1.0 +--- +camera_name: MULTI_RGBD_INERTIAL_DATASET_REAR +image_width: 640 +image_height: 360 +camera_matrix: + rows: 3 + cols: 3 + data: [ 319.3360, 0., 323.9265, 0., + 319.1947, 181.9133, 0., 0., 1. ] +distortion_coefficients: + rows: 1 + cols: 4 + data: [ -0.0534, 0.0542, -0.0155, 0.0 ] +distortion_model: plumb_bob +rectification_matrix: + rows: 3 + cols: 3 + data: [ 1, 0, 0, + 0, 1, 0, + 0, 0, 1 ] +projection_matrix: + rows: 3 + cols: 4 + data: [ 319.3360, 0., 323.9265, 0., 0., + 319.1947, 181.9133, 0., 0., 0., 1., + 0. ] \ No newline at end of file diff --git a/rtabmap_examples/launch/config/multi_rgbd_inertial_dataset_right.yaml b/rtabmap_examples/launch/config/multi_rgbd_inertial_dataset_right.yaml new file mode 100644 index 00000000..b6987c80 --- /dev/null +++ b/rtabmap_examples/launch/config/multi_rgbd_inertial_dataset_right.yaml @@ -0,0 +1,27 @@ +#%YAML:1.0 +--- +camera_name: MULTI_RGBD_INERTIAL_DATASET_RIGHT +image_width: 640 +image_height: 360 +camera_matrix: + rows: 3 + cols: 3 + data: [ 320.7208, 0., 325.4179, 0., + 320.7359, 184.8089, 0., 0., 1. ] +distortion_coefficients: + rows: 1 + cols: 4 + data: [ -0.0531, 0.0549, -0.0166, 0.0 ] +distortion_model: plumb_bob +rectification_matrix: + rows: 3 + cols: 3 + data: [ 1, 0, 0, + 0, 1, 0, + 0, 0, 1 ] +projection_matrix: + rows: 3 + cols: 4 + data: [ 320.7208, 0., 325.4179, 0., 0., + 320.7359, 184.8089, 0., 0., 0., 1., + 0. ] diff --git a/rtabmap_examples/launch/multi_rgbd_inertial_dataset.launch b/rtabmap_examples/launch/multi_rgbd_inertial_dataset.launch new file mode 100644 index 00000000..5c3716df --- /dev/null +++ b/rtabmap_examples/launch/multi_rgbd_inertial_dataset.launch @@ -0,0 +1,131 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file From 95004363b447bedd4457b1b1fcde2bd6d409e4d7 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Thu, 19 Feb 2026 19:51:33 -0800 Subject: [PATCH 41/56] multi_rgbd_inertial_dataset.launch: record intermediate poses --- rtabmap_examples/launch/multi_rgbd_inertial_dataset.launch | 1 + 1 file changed, 1 insertion(+) diff --git a/rtabmap_examples/launch/multi_rgbd_inertial_dataset.launch b/rtabmap_examples/launch/multi_rgbd_inertial_dataset.launch index 5c3716df..4284f82d 100644 --- a/rtabmap_examples/launch/multi_rgbd_inertial_dataset.launch +++ b/rtabmap_examples/launch/multi_rgbd_inertial_dataset.launch @@ -114,6 +114,7 @@ + From 3bc26e1a415971d3cecb58fee12d9d053e0ec88a Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sat, 21 Mar 2026 14:06:02 -0700 Subject: [PATCH 42/56] Updated euroc example to use rectify_node instead of image_proc (#1404) --- rtabmap_examples/launch/euroc_datasets.launch.py | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/rtabmap_examples/launch/euroc_datasets.launch.py b/rtabmap_examples/launch/euroc_datasets.launch.py index da324634..71b08fb8 100644 --- a/rtabmap_examples/launch/euroc_datasets.launch.py +++ b/rtabmap_examples/launch/euroc_datasets.launch.py @@ -105,15 +105,13 @@ def generate_launch_description(): namespace='stereo_camera'), Node( - package='image_proc', executable='image_proc', output='screen', + package='image_proc', executable='rectify_node', output='screen', remappings=[ - ('image_raw', '/cam0/image_raw'), ('image', '/cam0/image_raw')], namespace='stereo_camera/left'), Node( - package='image_proc', executable='image_proc', output='screen', + package='image_proc', executable='rectify_node', output='screen', remappings=[ - ('image_raw', '/cam1/image_raw'), ('image', '/cam1/image_raw')], namespace='stereo_camera/right'), From 45c586c3792c90601527f0bce86b14bde94f3695 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sat, 18 Apr 2026 14:45:56 -0700 Subject: [PATCH 43/56] Added fatal errors on cv exceptions caused by possible multiple installed opencv versions --- rtabmap_conversions/src/MsgConversion.cpp | 269 ++++++++++-------- rtabmap_odom/src/nodelets/rgbd_odometry.cpp | 9 +- rtabmap_odom/src/nodelets/stereo_odometry.cpp | 9 +- rtabmap_sync/src/nodelets/rgbd_sync.cpp | 10 +- rtabmap_sync/src/nodelets/stereo_sync.cpp | 16 +- rtabmap_util/src/nodelets/point_cloud_xyz.cpp | 8 +- .../src/nodelets/point_cloud_xyzrgb.cpp | 84 ++++-- 7 files changed, 251 insertions(+), 154 deletions(-) diff --git a/rtabmap_conversions/src/MsgConversion.cpp b/rtabmap_conversions/src/MsgConversion.cpp index 5a69ce00..0c46bcb9 100644 --- a/rtabmap_conversions/src/MsgConversion.cpp +++ b/rtabmap_conversions/src/MsgConversion.cpp @@ -191,39 +191,45 @@ void toCvShare(const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr & image, cv_br void toCvShare(const rtabmap_msgs::msg::RGBDImage & image, const std::shared_ptr& trackedObject, cv_bridge::CvImageConstPtr & rgb, cv_bridge::CvImageConstPtr & depth) { - if(!image.rgb.data.empty()) + try { - rgb = cv_bridge::toCvShare(image.rgb, trackedObject); - } - else if(!image.rgb_compressed.data.empty()) - { - rgb = cv_bridge::toCvCopy(image.rgb_compressed); - } - else - { - // empty - rgb = std::make_shared(); - } - - if(!image.depth.data.empty()) - { - depth = cv_bridge::toCvShare(image.depth, trackedObject); - } - else if(!image.depth_compressed.data.empty()) - { - if(image.depth_compressed.format.compare("jpg")==0) + if(!image.rgb.data.empty()) { - depth = cv_bridge::toCvCopy(image.depth_compressed); + rgb = cv_bridge::toCvShare(image.rgb, trackedObject); + } + else if(!image.rgb_compressed.data.empty()) + { + rgb = cv_bridge::toCvCopy(image.rgb_compressed); } else { - cv_bridge::CvImagePtr ptr = std::make_shared(); - ptr->header = image.depth_compressed.header; - ptr->image = rtabmap::uncompressImage(image.depth_compressed.data); - UASSERT(ptr->image.empty() || ptr->image.type() == CV_32FC1 || ptr->image.type() == CV_16UC1); - ptr->encoding = ptr->image.empty()?"":ptr->image.type() == CV_32FC1?sensor_msgs::image_encodings::TYPE_32FC1:sensor_msgs::image_encodings::TYPE_16UC1; - depth = ptr; + // empty + rgb = std::make_shared(); } + + if(!image.depth.data.empty()) + { + depth = cv_bridge::toCvShare(image.depth, trackedObject); + } + else if(!image.depth_compressed.data.empty()) + { + if(image.depth_compressed.format.compare("jpg")==0) + { + depth = cv_bridge::toCvCopy(image.depth_compressed); + } + else + { + cv_bridge::CvImagePtr ptr = std::make_shared(); + ptr->header = image.depth_compressed.header; + ptr->image = rtabmap::uncompressImage(image.depth_compressed.data); + UASSERT(ptr->image.empty() || ptr->image.type() == CV_32FC1 || ptr->image.type() == CV_16UC1); + ptr->encoding = ptr->image.empty()?"":ptr->image.type() == CV_32FC1?sensor_msgs::image_encodings::TYPE_32FC1:sensor_msgs::image_encodings::TYPE_16UC1; + depth = ptr; + } + } + } + catch(cv::Exception& e) { + UFATAL("Fatal error while converting rgbd image (do you have multiple opencv versions? if so, make sure cv_bridge is loading the right opencv libraries on runtime): %s", e.what()); } } @@ -348,27 +354,32 @@ rtabmap::SensorData rgbdImageFromROS(const rtabmap_msgs::msg::RGBDImage::ConstSh } cv::Mat left, right; - if(imageRectLeft->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) == 0 || - imageRectLeft->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0) - { - left = imageRectLeft->image; + try { + if( imageRectLeft->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) == 0 || + imageRectLeft->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0) + { + left = imageRectLeft->image; + } + else if(imageRectLeft->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0) + { + left = cv_bridge::cvtColor(imageRectLeft, "mono8")->image; + } + else + { + left = cv_bridge::cvtColor(imageRectLeft, "bgr8")->image; + } + if( imageRectRight->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) == 0 || + imageRectRight->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0) + { + right = imageRectRight->image; + } + else + { + right = cv_bridge::cvtColor(imageRectRight, "mono8")->image; + } } - else if(imageRectLeft->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0) - { - left = cv_bridge::cvtColor(imageRectLeft, "mono8")->image; - } - else - { - left = cv_bridge::cvtColor(imageRectLeft, "bgr8")->image; - } - if(imageRectRight->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) == 0 || - imageRectRight->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0) - { - right = imageRectRight->image; - } - else - { - right = cv_bridge::cvtColor(imageRectRight, "mono8")->image; + catch(cv::Exception& e) { + UFATAL("Fatal error while converting images (do you have multiple opencv versions? if so, make sure cv_bridge is loading the right opencv libraries on runtime): %s", e.what()); } // @@ -420,19 +431,24 @@ rtabmap::SensorData rgbdImageFromROS(const rtabmap_msgs::msg::RGBDImage::ConstSh } cv_bridge::CvImageConstPtr ptrImage = imageMsg; - if(imageMsg->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1)==0 || - imageMsg->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 || - imageMsg->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0) - { - // do nothing + try { + if(imageMsg->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1)==0 || + imageMsg->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 || + imageMsg->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0) + { + // do nothing + } + else if(imageMsg->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0) + { + ptrImage = cv_bridge::cvtColor(imageMsg, "mono8"); + } + else + { + ptrImage = cv_bridge::cvtColor(imageMsg, "bgr8"); + } } - else if(imageMsg->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0) - { - ptrImage = cv_bridge::cvtColor(imageMsg, "mono8"); - } - else - { - ptrImage = cv_bridge::cvtColor(imageMsg, "bgr8"); + catch(cv::Exception& e) { + UFATAL("Fatal error while converting image (do you have multiple opencv versions? if so, make sure cv_bridge is loading the right opencv libraries on runtime): %s", e.what()); } cv_bridge::CvImageConstPtr ptrDepth = depthMsg; @@ -1136,18 +1152,23 @@ rtabmap::SensorData sensorDataFromROS(const rtabmap_msgs::msg::SensorData & msg) } else { - if(leftRawPtr->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) == 0 || - leftRawPtr->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0) - { - left = leftRawPtr->image.clone(); + try { + if( leftRawPtr->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) == 0 || + leftRawPtr->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0) + { + left = leftRawPtr->image.clone(); + } + else if(leftRawPtr->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0) + { + left = cv_bridge::cvtColor(leftRawPtr, "mono8")->image; + } + else + { + left = cv_bridge::cvtColor(leftRawPtr, "bgr8")->image; + } } - else if(leftRawPtr->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0) - { - left = cv_bridge::cvtColor(leftRawPtr, "mono8")->image; - } - else - { - left = cv_bridge::cvtColor(leftRawPtr, "bgr8")->image; + catch(cv::Exception& e) { + UFATAL("Fatal error while converting left image (do you have multiple opencv versions? if so, make sure cv_bridge is loading the right opencv libraries on runtime): %s", e.what()); } } } @@ -1169,18 +1190,23 @@ rtabmap::SensorData sensorDataFromROS(const rtabmap_msgs::msg::SensorData & msg) } else { - if(rightRawPtr->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) == 0 || - rightRawPtr->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 || - (!isStereo && - (rightRawPtr->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0|| - rightRawPtr->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1) == 0 || - rightRawPtr->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1) == 0))) - { - right = rightRawPtr->image.clone(); + try{ + if( rightRawPtr->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) == 0 || + rightRawPtr->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 || + (!isStereo && + (rightRawPtr->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0|| + rightRawPtr->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1) == 0 || + rightRawPtr->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1) == 0))) + { + right = rightRawPtr->image.clone(); + } + else + { + right = cv_bridge::cvtColor(rightRawPtr, "mono8")->image; + } } - else - { - right = cv_bridge::cvtColor(rightRawPtr, "mono8")->image; + catch(cv::Exception& e) { + UFATAL("Fatal error while converting right image (do you have multiple opencv versions? if so, make sure cv_bridge is loading the right opencv libraries on runtime): %s", e.what()); } } } @@ -2197,19 +2223,24 @@ bool convertRGBDMsgs( if(!imageMsgs.empty()) { cv_bridge::CvImageConstPtr ptrImage = imageMsgs[i]; - if(imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1)==0 || - imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 || - imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0) - { - // do nothing + try { + if(imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1)==0 || + imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 || + imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0) + { + // do nothing + } + else if(imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0) + { + ptrImage = cv_bridge::cvtColor(imageMsgs[i], "mono8"); + } + else + { + ptrImage = cv_bridge::cvtColor(imageMsgs[i], "bgr8"); + } } - else if(imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0) - { - ptrImage = cv_bridge::cvtColor(imageMsgs[i], "mono8"); - } - else - { - ptrImage = cv_bridge::cvtColor(imageMsgs[i], "bgr8"); + catch(cv::Exception& e) { + UFATAL("Fatal error while converting image (do you have multiple opencv versions? if so, make sure cv_bridge is loading the right opencv libraries on runtime): %s", e.what()); } // initialize @@ -2260,7 +2291,12 @@ bool convertRGBDMsgs( } else { - ptrImage = cv_bridge::cvtColor(depthMsgs[i], "mono8"); + try{ + ptrImage = cv_bridge::cvtColor(depthMsgs[i], "mono8"); + } + catch(cv::Exception& e) { + UFATAL("Fatal error while converting image (do you have multiple opencv versions? if so, make sure cv_bridge is loading the right opencv libraries on runtime): %s", e.what()); + } } // initialize @@ -2460,27 +2496,32 @@ bool convertStereoMsg( return false; } - if(leftImageMsg->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) == 0 || - leftImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0) - { - left = leftImageMsg->image.clone(); + try{ + if( leftImageMsg->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) == 0 || + leftImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0) + { + left = leftImageMsg->image.clone(); + } + else if(leftImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0) + { + left = cv_bridge::cvtColor(leftImageMsg, "mono8")->image; + } + else + { + left = cv_bridge::cvtColor(leftImageMsg, "bgr8")->image; + } + if( rightImageMsg->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) == 0 || + rightImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0) + { + right = rightImageMsg->image.clone(); + } + else + { + right = cv_bridge::cvtColor(rightImageMsg, "mono8")->image; + } } - else if(leftImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0) - { - left = cv_bridge::cvtColor(leftImageMsg, "mono8")->image; - } - else - { - left = cv_bridge::cvtColor(leftImageMsg, "bgr8")->image; - } - if(rightImageMsg->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) == 0 || - rightImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0) - { - right = rightImageMsg->image.clone(); - } - else - { - right = cv_bridge::cvtColor(rightImageMsg, "mono8")->image; + catch(cv::Exception& e) { + UFATAL("Fatal error while converting images (do you have multiple opencv versions? if so, make sure cv_bridge is loading the right opencv libraries on runtime): %s", e.what()); } rtabmap::Transform localTransform = getTransform(frameId, leftImageMsg->header.frame_id, leftImageMsg->header.stamp, listener, waitForTransform); diff --git a/rtabmap_odom/src/nodelets/rgbd_odometry.cpp b/rtabmap_odom/src/nodelets/rgbd_odometry.cpp index a2614e09..6944150f 100644 --- a/rtabmap_odom/src/nodelets/rgbd_odometry.cpp +++ b/rtabmap_odom/src/nodelets/rgbd_odometry.cpp @@ -595,8 +595,13 @@ void RGBDOdometry::callback( std::vector imageMsgs(1); std::vector depthMsgs(1); std::vector infoMsgs; - imageMsgs[0] = cv_bridge::toCvShare(image); - depthMsgs[0] = cv_bridge::toCvShare(depth); + try{ + imageMsgs[0] = cv_bridge::toCvShare(image); + depthMsgs[0] = cv_bridge::toCvShare(depth); + } + catch(cv::Exception& e) { + UFATAL("Fatal error while converting images (do you have multiple opencv versions? if so, make sure cv_bridge is loading the right opencv libraries on runtime): %s", e.what()); + } infoMsgs.push_back(*cameraInfo); double stampDiff = fabs(rtabmap_conversions::timestampFromROS(image->header.stamp) - rtabmap_conversions::timestampFromROS(depth->header.stamp)); diff --git a/rtabmap_odom/src/nodelets/stereo_odometry.cpp b/rtabmap_odom/src/nodelets/stereo_odometry.cpp index 84a1ced0..e406c635 100644 --- a/rtabmap_odom/src/nodelets/stereo_odometry.cpp +++ b/rtabmap_odom/src/nodelets/stereo_odometry.cpp @@ -684,8 +684,13 @@ void StereoOdometry::callback( std::vector rightMsgs(1); std::vector leftInfoMsgs; std::vector rightInfoMsgs; - leftMsgs[0] = cv_bridge::toCvShare(imageRectLeft); - rightMsgs[0] = cv_bridge::toCvShare(imageRectRight); + try{ + leftMsgs[0] = cv_bridge::toCvShare(imageRectLeft); + rightMsgs[0] = cv_bridge::toCvShare(imageRectRight); + } + catch(cv::Exception& e) { + UFATAL("Fatal error while converting images (do you have multiple opencv versions? if so, make sure cv_bridge is loading the right opencv libraries on runtime): %s", e.what()); + } leftInfoMsgs.push_back(*cameraInfoLeft); rightInfoMsgs.push_back(*cameraInfoRight); diff --git a/rtabmap_sync/src/nodelets/rgbd_sync.cpp b/rtabmap_sync/src/nodelets/rgbd_sync.cpp index 1bc213ec..48925287 100644 --- a/rtabmap_sync/src/nodelets/rgbd_sync.cpp +++ b/rtabmap_sync/src/nodelets/rgbd_sync.cpp @@ -215,8 +215,14 @@ void RGBDSync::callback( cv::Mat rgbMat; cv::Mat depthMat; - cv_bridge::CvImageConstPtr imagePtr = cv_bridge::toCvShare(image); - cv_bridge::CvImageConstPtr imageDepthPtr = cv_bridge::toCvShare(depth); + cv_bridge::CvImageConstPtr imagePtr, imageDepthPtr; + try { + imagePtr = cv_bridge::toCvShare(image); + imageDepthPtr = cv_bridge::toCvShare(depth); + } + catch(cv::Exception& e) { + UFATAL("Fatal error while converting images (do you have multiple opencv versions? if so, make sure cv_bridge is loading the right opencv libraries on runtime): %s", e.what()); + } rgbMat = imagePtr->image; depthMat = imageDepthPtr->image; diff --git a/rtabmap_sync/src/nodelets/stereo_sync.cpp b/rtabmap_sync/src/nodelets/stereo_sync.cpp index 505ab958..7b05a08e 100644 --- a/rtabmap_sync/src/nodelets/stereo_sync.cpp +++ b/rtabmap_sync/src/nodelets/stereo_sync.cpp @@ -185,10 +185,22 @@ void StereoSync::callback( rtabmap_msgs::msg::RGBDImage::UniquePtr msgCompressed(new rtabmap_msgs::msg::RGBDImage); *msgCompressed = *msg; - cv_bridge::CvImageConstPtr imagePtr = cv_bridge::toCvShare(imageLeft); + cv_bridge::CvImageConstPtr imagePtr; + try { + imagePtr = cv_bridge::toCvShare(imageLeft); + } + catch(cv::Exception& e) { + UFATAL("Fatal error while converting left image (do you have multiple opencv versions? if so, make sure cv_bridge is loading the right opencv libraries on runtime): %s", e.what()); + } imagePtr->toCompressedImageMsg(msgCompressed->rgb_compressed, cv_bridge::JPG); - cv_bridge::CvImageConstPtr imageDepthPtr = cv_bridge::toCvShare(imageRight); + cv_bridge::CvImageConstPtr imageDepthPtr; + try { + imageDepthPtr = cv_bridge::toCvShare(imageRight); + } + catch(cv::Exception& e) { + UFATAL("Fatal error while converting right image (do you have multiple opencv versions? if so, make sure cv_bridge is loading the right opencv libraries on runtime): %s", e.what()); + } imageDepthPtr->toCompressedImageMsg(msgCompressed->depth_compressed, cv_bridge::JPG); rgbdImageCompressedPub_->publish(std::move(msgCompressed)); diff --git a/rtabmap_util/src/nodelets/point_cloud_xyz.cpp b/rtabmap_util/src/nodelets/point_cloud_xyz.cpp index acc88973..f2e9b454 100644 --- a/rtabmap_util/src/nodelets/point_cloud_xyz.cpp +++ b/rtabmap_util/src/nodelets/point_cloud_xyz.cpp @@ -189,7 +189,13 @@ void PointCloudXYZ::callback( { rclcpp::Time time = now(); - cv_bridge::CvImageConstPtr imageDepthPtr = cv_bridge::toCvShare(depthMsg); + cv_bridge::CvImageConstPtr imageDepthPtr; + try{ + imageDepthPtr = cv_bridge::toCvShare(depthMsg); + } + catch(cv::Exception& e) { + UFATAL("Fatal error while converting depth image (do you have multiple opencv versions? if so, make sure cv_bridge is loading the right opencv libraries on runtime): %s", e.what()); + } rtabmap::CameraModel model = rtabmap_conversions::cameraModelFromROS(*cameraInfo); diff --git a/rtabmap_util/src/nodelets/point_cloud_xyzrgb.cpp b/rtabmap_util/src/nodelets/point_cloud_xyzrgb.cpp index eb60b24f..4d2c7044 100644 --- a/rtabmap_util/src/nodelets/point_cloud_xyzrgb.cpp +++ b/rtabmap_util/src/nodelets/point_cloud_xyzrgb.cpp @@ -240,21 +240,32 @@ void PointCloudXYZRGB::depthCallback( rclcpp::Time time = now(); cv_bridge::CvImageConstPtr imagePtr; - if(image->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1)==0) - { - imagePtr = cv_bridge::toCvShare(image); + try { + if(image->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1)==0) + { + imagePtr = cv_bridge::toCvShare(image); + } + else if(image->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 || + image->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0) + { + imagePtr = cv_bridge::toCvShare(image, "mono8"); + } + else + { + imagePtr = cv_bridge::toCvShare(image, "bgr8"); + } } - else if(image->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 || - image->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0) - { - imagePtr = cv_bridge::toCvShare(image, "mono8"); - } - else - { - imagePtr = cv_bridge::toCvShare(image, "bgr8"); + catch(cv::Exception& e) { + UFATAL("Fatal error while converting RGB image (do you have multiple opencv versions? if so, make sure cv_bridge is loading the right opencv libraries on runtime): %s", e.what()); } - cv_bridge::CvImageConstPtr imageDepthPtr = cv_bridge::toCvShare(imageDepth); + cv_bridge::CvImageConstPtr imageDepthPtr; + try { + imageDepthPtr = cv_bridge::toCvShare(imageDepth); + } + catch(cv::Exception& e) { + UFATAL("Fatal error while converting Depth image (do you have multiple opencv versions? if so, make sure cv_bridge is loading the right opencv libraries on runtime): %s", e.what()); + } rtabmap::CameraModel model = rtabmap_conversions::cameraModelFromROS(*cameraInfo); @@ -318,18 +329,24 @@ void PointCloudXYZRGB::disparityCallback( const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfo) { cv_bridge::CvImageConstPtr imagePtr; - if(image->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1)==0) - { - imagePtr = cv_bridge::toCvShare(image); + try { + if(image->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1)==0) + { + imagePtr = cv_bridge::toCvShare(image); + } + else if(image->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 || + image->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0) + { + imagePtr = cv_bridge::toCvShare(image, "mono8"); + } + else + { + imagePtr = cv_bridge::toCvShare(image, "bgr8"); + } } - else if(image->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 || - image->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0) - { - imagePtr = cv_bridge::toCvShare(image, "mono8"); - } - else - { - imagePtr = cv_bridge::toCvShare(image, "bgr8"); + catch(cv::Exception& e) { + UFATAL("Fatal error while converting RGB image (do you have multiple opencv versions? if so, make sure cv_bridge is loading the right opencv libraries on runtime): %s", e.what()); + return; } if(imageDisparity->image.encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1) !=0 && @@ -404,16 +421,21 @@ void PointCloudXYZRGB::stereoCallback( rclcpp::Time time = now(); cv_bridge::CvImageConstPtr ptrLeftImage, ptrRightImage; - if(imageLeft->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 || - imageLeft->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0) - { - ptrLeftImage = cv_bridge::toCvShare(imageLeft, "mono8"); + try { + if(imageLeft->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 || + imageLeft->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0) + { + ptrLeftImage = cv_bridge::toCvShare(imageLeft, "mono8"); + } + else + { + ptrLeftImage = cv_bridge::toCvShare(imageLeft, "bgr8"); + } + ptrRightImage = cv_bridge::toCvShare(imageRight, "mono8"); } - else - { - ptrLeftImage = cv_bridge::toCvShare(imageLeft, "bgr8"); + catch(cv::Exception& e) { + UFATAL("Fatal error converting images (do you have multiple opencv versions? if so, make sure cv_bridge is loading the right opencv libraries on runtime): %s", e.what()); } - ptrRightImage = cv_bridge::toCvShare(imageRight, "mono8"); if(roiRatios_[0]!=0.0f || roiRatios_[1]!=0.0f || roiRatios_[2]!=0.0f || roiRatios_[3]!=0.0f) { From fb27ac0eda9a82215050ea5d35d5c03505407727 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sat, 18 Apr 2026 16:22:50 -0700 Subject: [PATCH 44/56] Fixed not all CoreWrapper's parameters updated when parameters callback is called --- rtabmap_slam/include/rtabmap_slam/CoreWrapper.h | 2 ++ rtabmap_slam/src/CoreWrapper.cpp | 17 ++++++----------- 2 files changed, 8 insertions(+), 11 deletions(-) diff --git a/rtabmap_slam/include/rtabmap_slam/CoreWrapper.h b/rtabmap_slam/include/rtabmap_slam/CoreWrapper.h index 901cc255..adb7b8e9 100644 --- a/rtabmap_slam/include/rtabmap_slam/CoreWrapper.h +++ b/rtabmap_slam/include/rtabmap_slam/CoreWrapper.h @@ -245,6 +245,8 @@ private: std::map filterNodesToAssemble( const std::map & nodes, const rtabmap::Transform & currentPose); + + void applyParameters(); void updateRtabmapCallback(const std::shared_ptr, const std::shared_ptr, std::shared_ptr); void resetRtabmapCallback(const std::shared_ptr, const std::shared_ptr, std::shared_ptr); diff --git a/rtabmap_slam/src/CoreWrapper.cpp b/rtabmap_slam/src/CoreWrapper.cpp index 628aa6d4..6c302c22 100644 --- a/rtabmap_slam/src/CoreWrapper.cpp +++ b/rtabmap_slam/src/CoreWrapper.cpp @@ -964,17 +964,7 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) : parameters_.at(key) = vStr; } } - RCLCPP_INFO(this->get_logger(), "rtabmap: Updating parameters"); - if(parameters_.find(Parameters::kRtabmapDetectionRate()) != parameters_.end()) - { - rate_ = uStr2Float(parameters_.at(Parameters::kRtabmapDetectionRate())); - RCLCPP_INFO(this->get_logger(), "RTAB-Map rate detection = %f Hz", rate_); - } - rtabmap_.parseParameters(parameters_); - // Don't reset map in localization mode - if(rtabmap_.getMemory()->isIncremental()) { - mapsManager_.setParameters(parameters_); - } + applyParameters(); } }; @@ -3219,6 +3209,11 @@ void CoreWrapper::updateRtabmapCallback( } } } + applyParameters(); +} + +void CoreWrapper::applyParameters() +{ RCLCPP_INFO(get_logger(), "rtabmap: Updating parameters"); if(parameters_.find(Parameters::kRtabmapDetectionRate()) != parameters_.end()) { From 08e7d636788de73f2e0e9701adfdb53740986bd0 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sun, 19 Apr 2026 23:07:07 +0000 Subject: [PATCH 45/56] Support upstream 0.23.5 --- rtabmap_conversions/CMakeLists.txt | 2 +- rtabmap_slam/src/CoreWrapper.cpp | 18 +++++++++++------- 2 files changed, 12 insertions(+), 8 deletions(-) diff --git a/rtabmap_conversions/CMakeLists.txt b/rtabmap_conversions/CMakeLists.txt index d0cc59ed..17e44954 100644 --- a/rtabmap_conversions/CMakeLists.txt +++ b/rtabmap_conversions/CMakeLists.txt @@ -7,7 +7,7 @@ find_package(catkin REQUIRED COMPONENTS image_geometry rtabmap_msgs ) -find_package(RTABMap 0.23.4 REQUIRED) +find_package(RTABMap 0.23.5 REQUIRED) catkin_package( INCLUDE_DIRS include diff --git a/rtabmap_slam/src/CoreWrapper.cpp b/rtabmap_slam/src/CoreWrapper.cpp index e2a5b1f8..82ae6fc6 100644 --- a/rtabmap_slam/src/CoreWrapper.cpp +++ b/rtabmap_slam/src/CoreWrapper.cpp @@ -2196,8 +2196,8 @@ void CoreWrapper::process( if(rtabmap_.getMemory() == 0 || filteredPoses.size() == 0 || rtabmap_.getMemory()->getLastSignatureId() != filteredPoses.rbegin()->first || - rtabmap_.getMemory()->getLastWorkingSignature() == 0 || - rtabmap_.getMemory()->getLastWorkingSignature()->sensorData().gridCellSize() == 0 || + rtabmap_.getMemory()->getLastWorkingSignature(true) == 0 || + rtabmap_.getMemory()->getLastWorkingSignature(true)->sensorData().gridCellSize() == 0 || (!mapsManager_.getLocalMapMaker()->isGridFromDepth() && data.laserScanRaw().is2d())) // 2d laser scan would fill empty space for latest data { SensorData tmpData = data; @@ -3421,16 +3421,16 @@ bool CoreWrapper::setLogError(std_srvs::Empty::Request&, std_srvs::Empty::Respon bool CoreWrapper::getNodeDataCallback(rtabmap_msgs::GetNodeData::Request& req, rtabmap_msgs::GetNodeData::Response& res) { - NODELET_INFO("rtabmap: Getting node data (%d node(s), images=%s scan=%s grid=%s user_data=%s)...", + NODELET_INFO("rtabmap: Getting node data (%d node(s), images=%s scan=%s grid=%s user_data=%s )...", (int)req.ids.size(), req.images?"true":"false", req.scan?"true":"false", req.grid?"true":"false", req.user_data?"true":"false"); - if(req.ids.empty() && rtabmap_.getMemory() && rtabmap_.getMemory()->getLastWorkingSignature()) + if(req.ids.empty() && rtabmap_.getMemory() && rtabmap_.getMemory()->getLastWorkingSignature(true)) { - req.ids.push_back(rtabmap_.getMemory()->getLastWorkingSignature()->id()); + req.ids.push_back(rtabmap_.getMemory()->getLastWorkingSignature(true)->id()); } for(size_t i=0; i signatures; std::map poses; std::multimap constraints; From f81536f50856898b4b7d6a53395330df6f328f89 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Mon, 20 Apr 2026 01:22:12 +0000 Subject: [PATCH 46/56] typo --- rtabmap_slam/src/CoreWrapper.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/rtabmap_slam/src/CoreWrapper.cpp b/rtabmap_slam/src/CoreWrapper.cpp index 82ae6fc6..a6831327 100644 --- a/rtabmap_slam/src/CoreWrapper.cpp +++ b/rtabmap_slam/src/CoreWrapper.cpp @@ -2196,8 +2196,8 @@ void CoreWrapper::process( if(rtabmap_.getMemory() == 0 || filteredPoses.size() == 0 || rtabmap_.getMemory()->getLastSignatureId() != filteredPoses.rbegin()->first || - rtabmap_.getMemory()->getLastWorkingSignature(true) == 0 || - rtabmap_.getMemory()->getLastWorkingSignature(true)->sensorData().gridCellSize() == 0 || + rtabmap_.getMemory()->getLastWorkingSignature(false) == 0 || + rtabmap_.getMemory()->getLastWorkingSignature(false)->sensorData().gridCellSize() == 0 || (!mapsManager_.getLocalMapMaker()->isGridFromDepth() && data.laserScanRaw().is2d())) // 2d laser scan would fill empty space for latest data { SensorData tmpData = data; From 45375bb3c05555a4dae58d6cbcb1628d8e8f763f Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sun, 19 Apr 2026 18:23:34 -0700 Subject: [PATCH 47/56] Demo multi-session: updated Kp/BadSignRatio --- rtabmap_demos/launch/multisession_mapping_demo.launch.py | 1 + 1 file changed, 1 insertion(+) diff --git a/rtabmap_demos/launch/multisession_mapping_demo.launch.py b/rtabmap_demos/launch/multisession_mapping_demo.launch.py index b5841039..77211c39 100644 --- a/rtabmap_demos/launch/multisession_mapping_demo.launch.py +++ b/rtabmap_demos/launch/multisession_mapping_demo.launch.py @@ -60,6 +60,7 @@ def generate_launch_description(): 'Kp/DetectorStrategy': '1', # Referred paper used SURF (0), here use SIFT as it is available with opencv binaries 'Vis/FeatureType': '1', # Referred paper used SURF (0), here use SIFT as it is available with opencv binaries 'Kp/MaxFeatures': '400', + 'Kp/BadSignRatio': '0.25', # Kp/BadSignRatio behaves differently than before if Kp/MaxFeatures is not 0, that is now a ratio of Kp/MaxFeatures directly. 'Reg/Force3DoF': 'true', 'RGBD/OptimizeMaxError': '10', 'Optimizer/Strategy': '2', # Referred paper used TORO (0), latest version recommends GTSAM (2) From 8871f934b598e316a2bbf6684b2e8747eadce427 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Tue, 5 May 2026 22:23:02 -0700 Subject: [PATCH 48/56] Fixing turtlebot3 demos on Jazzy (#1422) * Fixing turtlebot3 demos on Jazzy * Updated humble * Working turtlebot3 demos on humble and jazzy --- .devcontainer/jazzy/devcontainer.json | 16 +- .../turtlebot3/turtlebot3_rgbd.launch.py | 39 +- .../turtlebot3/turtlebot3_rgbd_scan.launch.py | 40 +- .../turtlebot3_sim_rgbd_demo.launch.py | 66 ++- ...rtlebot3_sim_rgbd_fake_scan_demo.launch.py | 59 ++- .../turtlebot3_sim_rgbd_scan_demo.launch.py | 66 ++- .../turtlebot3_sim_scan_demo.launch.py | 161 ++++--- .../humble/turtlebot3_rgbd_nav2_params.yaml | 287 ++++++++++++ .../turtlebot3_rgbd_scan_nav2_params.yaml | 301 +++++++++++++ .../humble/turtlebot3_scan_nav2_params.yaml | 295 ++++++++++++ .../params/turtlebot3_nav2_params.yaml | 421 ++++++++++++++++++ .../params/turtlebot3_rgbd_nav2_params.yaml | 381 ++++++++++------ .../turtlebot3_rgbd_scan_nav2_params.yaml | 371 ++++++++++----- .../params/turtlebot3_scan_nav2_params.yaml | 373 +++++++++++----- 14 files changed, 2379 insertions(+), 497 deletions(-) create mode 100644 rtabmap_demos/params/humble/turtlebot3_rgbd_nav2_params.yaml create mode 100644 rtabmap_demos/params/humble/turtlebot3_rgbd_scan_nav2_params.yaml create mode 100644 rtabmap_demos/params/humble/turtlebot3_scan_nav2_params.yaml create mode 100644 rtabmap_demos/params/turtlebot3_nav2_params.yaml diff --git a/.devcontainer/jazzy/devcontainer.json b/.devcontainer/jazzy/devcontainer.json index 5e768e9a..3cff6f58 100644 --- a/.devcontainer/jazzy/devcontainer.json +++ b/.devcontainer/jazzy/devcontainer.json @@ -22,6 +22,18 @@ }, "workspaceMount": "source=${localWorkspaceFolder},target=/home/vscode/ros2_ws/src/rtabmap_ros,type=bind", "workspaceFolder": "/home/vscode/ros2_ws", - "postCreateCommand": "echo 'Initialize: export MAKEFLAGS=\"-j6\" && colcon build --cmake-args -DRTABMAP_SYNC_MULTI_RGBD=ON -DCMAKE_BUILD_TYPE=Release --parallel-workers 2'" - //"runArgs": ["--privileged", "--runtime=nvidia", "--network=host"] + "postCreateCommand": "echo 'Initialize: export MAKEFLAGS=\"-j6\" && colcon build --cmake-args -DRTABMAP_SYNC_MULTI_RGBD=ON -DCMAKE_BUILD_TYPE=Release --parallel-workers 2'", + "hostRequirements": { + "gpu": "optional" + }, + "runArgs": ["--privileged", + "--network=host", + "--gpus=all", + //"--runtime=nvidia", // uncommment this if rtabmap doesn't show up in nvidia-smi on the host computer + "--env=DISPLAY", + "--env=QT_X11_NO_MITSHM=1", + "--volume=/tmp/.X11-unix:/tmp/.X11-unix"], + "containerEnv": { + "NVIDIA_VISIBLE_DEVICES": "all" + } } diff --git a/rtabmap_demos/launch/turtlebot3/turtlebot3_rgbd.launch.py b/rtabmap_demos/launch/turtlebot3/turtlebot3_rgbd.launch.py index b9076449..37d59c89 100644 --- a/rtabmap_demos/launch/turtlebot3/turtlebot3_rgbd.launch.py +++ b/rtabmap_demos/launch/turtlebot3/turtlebot3_rgbd.launch.py @@ -20,11 +20,13 @@ from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable from launch.substitutions import LaunchConfiguration from launch.conditions import IfCondition, UnlessCondition from launch_ros.actions import Node +from launch.actions import OpaqueFunction -def generate_launch_description(): +def launch_setup(context, *args, **kwargs): use_sim_time = LaunchConfiguration('use_sim_time') localization = LaunchConfiguration('localization') + max_ground_height = LaunchConfiguration('max_ground_height').perform(context) parameters={ 'frame_id':'base_footprint', @@ -36,7 +38,7 @@ def generate_launch_description(): 'Grid/3D':'false', # Use 2D occupancy 'Grid/RangeMax':'3', 'Grid/NormalsSegmentation':'false', # Use passthrough filter to detect obstacles - 'Grid/MaxGroundHeight':'0.05', # All points above 5 cm are obstacles + 'Grid/MaxGroundHeight': str(max_ground_height), # All points above 5 cm are obstacles 'Grid/MaxObstacleHeight':'0.4', # All points over 1 meter are ignored 'Optimizer/GravitySigma':'0' # Disable imu constraints (we are already in 2D) } @@ -46,17 +48,7 @@ def generate_launch_description(): ('rgb/camera_info', '/camera/camera_info'), ('depth/image', '/camera/depth/image_raw')] - return LaunchDescription([ - - # Launch arguments - DeclareLaunchArgument( - 'use_sim_time', default_value='true', - description='Use simulation (Gazebo) clock if true'), - - DeclareLaunchArgument( - 'localization', default_value='false', - description='Launch in localization mode.'), - + return [ # Nodes to launch # SLAM mode: @@ -98,4 +90,23 @@ def generate_launch_description(): remappings=[('cloud', '/camera/cloud'), ('obstacles', '/camera/obstacles'), ('ground', '/camera/ground')]), - ]) + ] + +def generate_launch_description(): + return LaunchDescription([ + + # Launch arguments + DeclareLaunchArgument( + 'use_sim_time', default_value='true', + description='Use simulation (Gazebo) clock if true'), + + DeclareLaunchArgument( + 'localization', default_value='false', + description='Launch in localization mode.'), + + DeclareLaunchArgument( + 'max_ground_height', default_value='0.05', + description='Maximum ground height, everything above is obstacle'), + + OpaqueFunction(function=launch_setup) + ]) \ No newline at end of file diff --git a/rtabmap_demos/launch/turtlebot3/turtlebot3_rgbd_scan.launch.py b/rtabmap_demos/launch/turtlebot3/turtlebot3_rgbd_scan.launch.py index 733330fc..030a6741 100644 --- a/rtabmap_demos/launch/turtlebot3/turtlebot3_rgbd_scan.launch.py +++ b/rtabmap_demos/launch/turtlebot3/turtlebot3_rgbd_scan.launch.py @@ -20,12 +20,13 @@ from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable from launch.substitutions import LaunchConfiguration from launch.conditions import IfCondition, UnlessCondition from launch_ros.actions import Node +from launch.actions import OpaqueFunction - -def generate_launch_description(): +def launch_setup(context, *args, **kwargs): use_sim_time = LaunchConfiguration('use_sim_time') localization = LaunchConfiguration('localization') + max_ground_height = LaunchConfiguration('max_ground_height').perform(context) parameters={ 'frame_id':'base_footprint', @@ -42,7 +43,7 @@ def generate_launch_description(): 'Grid/RangeMax':'3', 'Grid/NormalsSegmentation':'false', # Use passthrough filter to detect obstacles 'Grid/Sensor':'2', # Use both laser scan and camera for obstacle detection in global map - 'Grid/MaxGroundHeight':'0.05', # All points above 5 cm are obstacles + 'Grid/MaxGroundHeight': str(max_ground_height), # All points above are obstacles 'Grid/MaxObstacleHeight':'0.4', # All points over 1 meter are ignored 'Grid/RangeMin':'0.2', # ignore laser scan points on the robot itself 'Optimizer/GravitySigma':'0' # Disable imu constraints (we are already in 2D) @@ -53,17 +54,7 @@ def generate_launch_description(): ('rgb/camera_info', '/camera/camera_info'), ('depth/image', '/camera/depth/image_raw')] - return LaunchDescription([ - - # Launch arguments - DeclareLaunchArgument( - 'use_sim_time', default_value='false', - description='Use simulation (Gazebo) clock if true'), - - DeclareLaunchArgument( - 'localization', default_value='false', - description='Launch in localization mode.'), - + return [ # Nodes to launch Node( package='rtabmap_sync', executable='rgbd_sync', output='screen', @@ -109,4 +100,23 @@ def generate_launch_description(): remappings=[('cloud', '/camera/cloud'), ('obstacles', '/camera/obstacles'), ('ground', '/camera/ground')]), - ]) + ] + +def generate_launch_description(): + return LaunchDescription([ + + # Launch arguments + DeclareLaunchArgument( + 'use_sim_time', default_value='true', + description='Use simulation (Gazebo) clock if true'), + + DeclareLaunchArgument( + 'localization', default_value='false', + description='Launch in localization mode.'), + + DeclareLaunchArgument( + 'max_ground_height', default_value='0.05', + description='Maximum ground height, everything above is obstacle'), + + OpaqueFunction(function=launch_setup) + ]) \ No newline at end of file diff --git a/rtabmap_demos/launch/turtlebot3/turtlebot3_sim_rgbd_demo.launch.py b/rtabmap_demos/launch/turtlebot3/turtlebot3_sim_rgbd_demo.launch.py index ad14ca21..a4155c6a 100644 --- a/rtabmap_demos/launch/turtlebot3/turtlebot3_sim_rgbd_demo.launch.py +++ b/rtabmap_demos/launch/turtlebot3/turtlebot3_sim_rgbd_demo.launch.py @@ -13,8 +13,30 @@ # # 3) Rename to # 4) Add -# 5) Change to -# 6) Change image width/height from 1920x1080 to 640x480 +# 5) Change image width/height from 1920x1080 to 640x480 +# 6) [ROS2 HUMBLE] Change to +# 6) [ROS2 JAZZY] Change camera_rgb_frame to camera_rgb_optical_frame +# 7) [ROS2 JAZZY] Add the following just after section +# +# true +# true +# 30 +# camera/depth/image_raw +# camera_rgb_optical_frame +# +# camera/depth/camera_info +# 1.02974 +# +# 640 +# 480 +# R8G8B8 +# +# +# 0.02 +# 300 +# +# +# # Example: # $ ros2 launch rtabmap_demos turtlebot3_sim_rgbd_demo.launch.py # @@ -28,9 +50,12 @@ from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, Opaq from launch.substitutions import LaunchConfiguration, PathJoinSubstitution from launch.launch_description_sources import PythonLaunchDescriptionSource from launch_ros.substitutions import FindPackageShare +from launch_ros.actions import Node import os +ROS_DISTRO = os.environ.get('ROS_DISTRO') + def launch_setup(context, *args, **kwargs): if not 'TURTLEBOT3_MODEL' in os.environ: os.environ['TURTLEBOT3_MODEL'] = 'waffle' @@ -45,9 +70,14 @@ def launch_setup(context, *args, **kwargs): world = LaunchConfiguration('world').perform(context) - nav2_params_file = PathJoinSubstitution( - [FindPackageShare('rtabmap_demos'), 'params', 'turtlebot3_rgbd_nav2_params.yaml'] - ) + if ROS_DISTRO == 'humble': + nav2_params_file = PathJoinSubstitution( + [FindPackageShare('rtabmap_demos'), 'params', 'humble', 'turtlebot3_rgbd_nav2_params.yaml'] + ) + else: + nav2_params_file = PathJoinSubstitution( + [FindPackageShare('rtabmap_demos'), 'params', 'turtlebot3_rgbd_nav2_params.yaml'] + ) # Paths gazebo_launch = PathJoinSubstitution( @@ -60,13 +90,22 @@ def launch_setup(context, *args, **kwargs): [pkg_rtabmap_demos, 'launch', 'turtlebot3', 'turtlebot3_rgbd.launch.py']) # Includes - gazebo = IncludeLaunchDescription( + gazebo = [IncludeLaunchDescription( PythonLaunchDescriptionSource([gazebo_launch]), launch_arguments=[ ('x_pose', LaunchConfiguration('x_pose')), ('y_pose', LaunchConfiguration('y_pose')) ] - ) + )] + if ROS_DISTRO != 'humble': + start_gazebo_ros_depth_image_bridge_cmd = Node( + package='ros_gz_image', + executable='image_bridge', + arguments=['/camera/depth/image_raw'], + output='screen', + ) + gazebo.append(start_gazebo_ros_depth_image_bridge_cmd) + nav2 = IncludeLaunchDescription( PythonLaunchDescriptionSource([nav2_launch]), launch_arguments=[ @@ -77,20 +116,25 @@ def launch_setup(context, *args, **kwargs): rviz = IncludeLaunchDescription( PythonLaunchDescriptionSource([rviz_launch]) ) + + max_ground_height = '0.05' + if ROS_DISTRO == 'jazzy': + max_ground_height = '0.02' # for the demo, on new gazebo the depth is more accurate + rtabmap = IncludeLaunchDescription( PythonLaunchDescriptionSource([rtabmap_launch]), launch_arguments=[ ('localization', LaunchConfiguration('localization')), - ('use_sim_time', 'true') + ('use_sim_time', 'true'), + ('max_ground_height', max_ground_height) ] ) return [ # Nodes to launch nav2, rviz, - rtabmap, - gazebo - ] + rtabmap + ] + gazebo def generate_launch_description(): return LaunchDescription([ diff --git a/rtabmap_demos/launch/turtlebot3/turtlebot3_sim_rgbd_fake_scan_demo.launch.py b/rtabmap_demos/launch/turtlebot3/turtlebot3_sim_rgbd_fake_scan_demo.launch.py index 314ad3eb..311edd68 100644 --- a/rtabmap_demos/launch/turtlebot3/turtlebot3_sim_rgbd_fake_scan_demo.launch.py +++ b/rtabmap_demos/launch/turtlebot3/turtlebot3_sim_rgbd_fake_scan_demo.launch.py @@ -13,8 +13,30 @@ # # 3) Rename to # 4) Add -# 5) Change to -# 6) Change image width/height from 1920x1080 to 640x480 +# 5) Change image width/height from 1920x1080 to 640x480 +# 6) [ROS2 HUMBLE] Change to +# 6) [ROS2 JAZZY] Change camera_rgb_frame to camera_rgb_optical_frame +# 7) [ROS2 JAZZY] Add the following just after section (under same link) +# +# true +# true +# 30 +# camera/depth/image_raw +# camera_rgb_optical_frame +# +# camera/depth/camera_info +# 1.02974 +# +# 640 +# 480 +# R8G8B8 +# +# +# 0.02 +# 300 +# +# +# # Example: # $ ros2 launch rtabmap_demos turtlebot3_sim_rgbd_fake_scan_demo.launch.py # @@ -28,9 +50,12 @@ from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, Opaq from launch.substitutions import LaunchConfiguration, PathJoinSubstitution from launch.launch_description_sources import PythonLaunchDescriptionSource from launch_ros.substitutions import FindPackageShare +from launch_ros.actions import Node import os +ROS_DISTRO = os.environ.get('ROS_DISTRO') + def launch_setup(context, *args, **kwargs): if not 'TURTLEBOT3_MODEL' in os.environ: os.environ['TURTLEBOT3_MODEL'] = 'waffle' @@ -45,9 +70,14 @@ def launch_setup(context, *args, **kwargs): world = LaunchConfiguration('world').perform(context) - nav2_params_file = PathJoinSubstitution( - [FindPackageShare('rtabmap_demos'), 'params', 'turtlebot3_rgbd_nav2_params.yaml'] - ) + if ROS_DISTRO == 'humble': + nav2_params_file = PathJoinSubstitution( + [FindPackageShare('rtabmap_demos'), 'params', 'humble', 'turtlebot3_rgbd_nav2_params.yaml'] + ) + else: + nav2_params_file = PathJoinSubstitution( + [FindPackageShare('rtabmap_demos'), 'params', 'turtlebot3_rgbd_nav2_params.yaml'] + ) # Paths gazebo_launch = PathJoinSubstitution( @@ -60,13 +90,22 @@ def launch_setup(context, *args, **kwargs): [pkg_rtabmap_demos, 'launch', 'turtlebot3', 'turtlebot3_rgbd_fake_scan.launch.py']) # Includes - gazebo = IncludeLaunchDescription( + gazebo = [IncludeLaunchDescription( PythonLaunchDescriptionSource([gazebo_launch]), launch_arguments=[ ('x_pose', LaunchConfiguration('x_pose')), ('y_pose', LaunchConfiguration('y_pose')) ] - ) + )] + if ROS_DISTRO != 'humble': + start_gazebo_ros_depth_image_bridge_cmd = Node( + package='ros_gz_image', + executable='image_bridge', + arguments=['/camera/depth/image_raw'], + output='screen', + ) + gazebo.append(start_gazebo_ros_depth_image_bridge_cmd) + nav2 = IncludeLaunchDescription( PythonLaunchDescriptionSource([nav2_launch]), launch_arguments=[ @@ -77,6 +116,7 @@ def launch_setup(context, *args, **kwargs): rviz = IncludeLaunchDescription( PythonLaunchDescriptionSource([rviz_launch]) ) + rtabmap = IncludeLaunchDescription( PythonLaunchDescriptionSource([rtabmap_launch]), launch_arguments=[ @@ -88,9 +128,8 @@ def launch_setup(context, *args, **kwargs): # Nodes to launch nav2, rviz, - rtabmap, - gazebo - ] + rtabmap + ] + gazebo def generate_launch_description(): return LaunchDescription([ diff --git a/rtabmap_demos/launch/turtlebot3/turtlebot3_sim_rgbd_scan_demo.launch.py b/rtabmap_demos/launch/turtlebot3/turtlebot3_sim_rgbd_scan_demo.launch.py index 5a826cb6..d269639c 100644 --- a/rtabmap_demos/launch/turtlebot3/turtlebot3_sim_rgbd_scan_demo.launch.py +++ b/rtabmap_demos/launch/turtlebot3/turtlebot3_sim_rgbd_scan_demo.launch.py @@ -13,8 +13,30 @@ # # 3) Rename to # 4) Add -# 5) Change to -# 6) Change image width/height from 1920x1080 to 640x480 +# 5) Change image width/height from 1920x1080 to 640x480 +# 6) [ROS2 HUMBLE] Change to +# 6) [ROS2 JAZZY] Change camera_rgb_frame to camera_rgb_optical_frame +# 7) [ROS2 JAZZY] Add the following just after section (under same link) +# +# true +# true +# 30 +# camera/depth/image_raw +# camera_rgb_optical_frame +# +# camera/depth/camera_info +# 1.02974 +# +# 640 +# 480 +# R8G8B8 +# +# +# 0.02 +# 300 +# +# +# # 7) Note that we can increase min scan range from 0.12 to 0.2 to avoid having scans # hitting the robot itself # Example: @@ -30,9 +52,12 @@ from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, Opaq from launch.substitutions import LaunchConfiguration, PathJoinSubstitution from launch.launch_description_sources import PythonLaunchDescriptionSource from launch_ros.substitutions import FindPackageShare +from launch_ros.actions import Node import os +ROS_DISTRO = os.environ.get('ROS_DISTRO') + def launch_setup(context, *args, **kwargs): if not 'TURTLEBOT3_MODEL' in os.environ: os.environ['TURTLEBOT3_MODEL'] = 'waffle' @@ -47,9 +72,14 @@ def launch_setup(context, *args, **kwargs): world = LaunchConfiguration('world').perform(context) - nav2_params_file = PathJoinSubstitution( - [FindPackageShare('rtabmap_demos'), 'params', 'turtlebot3_rgbd_scan_nav2_params.yaml'] - ) + if ROS_DISTRO == 'humble': + nav2_params_file = PathJoinSubstitution( + [FindPackageShare('rtabmap_demos'), 'params', 'humble', 'turtlebot3_rgbd_scan_nav2_params.yaml'] + ) + else: + nav2_params_file = PathJoinSubstitution( + [FindPackageShare('rtabmap_demos'), 'params', 'turtlebot3_rgbd_scan_nav2_params.yaml'] + ) # Paths gazebo_launch = PathJoinSubstitution( @@ -62,13 +92,22 @@ def launch_setup(context, *args, **kwargs): [pkg_rtabmap_demos, 'launch', 'turtlebot3', 'turtlebot3_rgbd_scan.launch.py']) # Includes - gazebo = IncludeLaunchDescription( + gazebo = [IncludeLaunchDescription( PythonLaunchDescriptionSource([gazebo_launch]), launch_arguments=[ ('x_pose', LaunchConfiguration('x_pose')), ('y_pose', LaunchConfiguration('y_pose')) ] - ) + )] + if ROS_DISTRO != 'humble': + start_gazebo_ros_depth_image_bridge_cmd = Node( + package='ros_gz_image', + executable='image_bridge', + arguments=['/camera/depth/image_raw'], + output='screen', + ) + gazebo.append(start_gazebo_ros_depth_image_bridge_cmd) + nav2 = IncludeLaunchDescription( PythonLaunchDescriptionSource([nav2_launch]), launch_arguments=[ @@ -79,20 +118,25 @@ def launch_setup(context, *args, **kwargs): rviz = IncludeLaunchDescription( PythonLaunchDescriptionSource([rviz_launch]) ) + + max_ground_height = '0.05' + if ROS_DISTRO == 'jazzy': + max_ground_height = '0.02' # for the demo, on new gazebo the depth is more accurate + rtabmap = IncludeLaunchDescription( PythonLaunchDescriptionSource([rtabmap_launch]), launch_arguments=[ ('localization', LaunchConfiguration('localization')), - ('use_sim_time', 'true') + ('use_sim_time', 'true'), + ('max_ground_height', max_ground_height) ] ) return [ # Nodes to launch nav2, rviz, - rtabmap, - gazebo - ] + rtabmap + ] + gazebo def generate_launch_description(): return LaunchDescription([ diff --git a/rtabmap_demos/launch/turtlebot3/turtlebot3_sim_scan_demo.launch.py b/rtabmap_demos/launch/turtlebot3/turtlebot3_sim_scan_demo.launch.py index 284b5b68..3fecc353 100644 --- a/rtabmap_demos/launch/turtlebot3/turtlebot3_sim_scan_demo.launch.py +++ b/rtabmap_demos/launch/turtlebot3/turtlebot3_sim_scan_demo.launch.py @@ -14,18 +14,22 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, OpaqueFunction +from launch.actions import AppendEnvironmentVariable, DeclareLaunchArgument, IncludeLaunchDescription, OpaqueFunction from launch.substitutions import LaunchConfiguration, PathJoinSubstitution from launch.launch_description_sources import PythonLaunchDescriptionSource from launch_ros.substitutions import FindPackageShare import os +ROS_DISTRO = os.environ.get('ROS_DISTRO') + def launch_setup(context, *args, **kwargs): if not 'TURTLEBOT3_MODEL' in os.environ: os.environ['TURTLEBOT3_MODEL'] = 'waffle' # Directories + pkg_turtlebot3_gazebo = get_package_share_directory( + 'turtlebot3_gazebo') pkg_nav2_bringup = get_package_share_directory( 'nav2_bringup') pkg_rtabmap_demos = get_package_share_directory( @@ -37,14 +41,26 @@ def launch_setup(context, *args, **kwargs): icp_odometry = icp_odometry == 'True' or icp_odometry == 'true' if icp_odometry: # modified nav2 params to use icp_odom instead odom frame - nav2_params_file = PathJoinSubstitution( - [FindPackageShare('rtabmap_demos'), 'params', 'turtlebot3_scan_nav2_params.yaml'] - ) + if ROS_DISTRO == 'humble': + nav2_params_file = PathJoinSubstitution( + [FindPackageShare('rtabmap_demos'), 'params', 'humble', 'turtlebot3_scan_nav2_params.yaml'] + ) + else: + nav2_params_file = PathJoinSubstitution( + [FindPackageShare('rtabmap_demos'), 'params', 'turtlebot3_scan_nav2_params.yaml'] + ) else: - # original nav2 params - nav2_params_file = PathJoinSubstitution( - [FindPackageShare('nav2_bringup'), 'params', 'nav2_params.yaml'] - ) + if ROS_DISTRO == 'humble': + # original nav2 params + nav2_params_file = PathJoinSubstitution( + [FindPackageShare('nav2_bringup'), 'params', 'nav2_params.yaml'] + ) + else: + # original nav2 params but with "enable_stamped_cmd_vel: True" + nav2_params_file = PathJoinSubstitution( + [FindPackageShare('rtabmap_demos'), 'params', 'turtlebot3_nav2_params.yaml'] + ) + # Paths nav2_launch = PathJoinSubstitution( @@ -53,56 +69,6 @@ def launch_setup(context, *args, **kwargs): [pkg_nav2_bringup, 'launch', 'rviz_launch.py']) rtabmap_launch = PathJoinSubstitution( [pkg_rtabmap_demos, 'launch', 'turtlebot3', 'turtlebot3_scan.launch.py']) - - # To use ICP odometry, we should increase clock rate of gazebo, we copied content of - # turtlebot3_gazebo/launch/turtlebot3_world.launch here - launch_file_dir = os.path.join(get_package_share_directory('turtlebot3_gazebo'), 'launch') - pkg_gazebo_ros = get_package_share_directory('gazebo_ros') - - world = os.path.join( - get_package_share_directory('turtlebot3_gazebo'), - 'worlds', - f'turtlebot3_{world_name}.world' - ) - - import tempfile - with tempfile.NamedTemporaryFile(mode='w+t', delete=False) as clock_override_file: - clock_override_file.write("---\n"+ - "gazebo:\n"+ - " ros__parameters:\n"+ - " publish_rate: 100.0") - - gzserver_cmd = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(pkg_gazebo_ros, 'launch', 'gzserver.launch.py') - ), - launch_arguments={ - 'world': world, - 'params_file': clock_override_file.name}.items() - ) - - gzclient_cmd = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(pkg_gazebo_ros, 'launch', 'gzclient.launch.py') - ) - ) - - robot_state_publisher_cmd = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(launch_file_dir, 'robot_state_publisher.launch.py') - ), - launch_arguments={'use_sim_time': 'true'}.items() - ) - - spawn_turtlebot_cmd = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(launch_file_dir, 'spawn_turtlebot3.launch.py') - ), - launch_arguments={ - 'x_pose': LaunchConfiguration('x_pose'), - 'y_pose': LaunchConfiguration('y_pose') - }.items() - ) nav2 = IncludeLaunchDescription( PythonLaunchDescriptionSource([nav2_launch]), @@ -121,16 +87,83 @@ def launch_setup(context, *args, **kwargs): ('use_sim_time', 'true') ] ) + + # To use ICP odometry, we should increase clock rate of gazebo (humble), we copied content of + # turtlebot3_gazebo/launch/turtlebot3_world.launch here. + turtlebot3_nodes = [] + if ROS_DISTRO == 'humble': + pkg_gazebo_ros = get_package_share_directory('gazebo_ros') + + import tempfile + with tempfile.NamedTemporaryFile(mode='w+t', delete=False) as clock_override_file: + clock_override_file.write("---\n"+ + "gazebo:\n"+ + " ros__parameters:\n"+ + " publish_rate: 100.0") + + world = os.path.join( + pkg_turtlebot3_gazebo, + 'worlds', + f'turtlebot3_{world_name}.world' + ) + + gzserver_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(pkg_gazebo_ros, 'launch', 'gzserver.launch.py') + ), + launch_arguments={ + 'world': world, + 'params_file': clock_override_file.name}.items() + ) + + gzclient_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(pkg_gazebo_ros, 'launch', 'gzclient.launch.py') + ) + ) + + robot_state_publisher_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(pkg_turtlebot3_gazebo, 'launch', 'robot_state_publisher.launch.py') + ), + launch_arguments={'use_sim_time': 'true'}.items() + ) + spawn_turtlebot_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(pkg_turtlebot3_gazebo, 'launch', 'spawn_turtlebot3.launch.py') + ), + launch_arguments={ + 'x_pose': LaunchConfiguration('x_pose'), + 'y_pose': LaunchConfiguration('y_pose') + }.items() + ) + + set_env_vars_resources = AppendEnvironmentVariable( + 'GZ_SIM_RESOURCE_PATH', + os.path.join(pkg_turtlebot3_gazebo, 'models')) + turtlebot3_nodes = [ + gzserver_cmd, + gzclient_cmd, + robot_state_publisher_cmd, + spawn_turtlebot_cmd, + set_env_vars_resources + ] + else: + gazebo_launch = PathJoinSubstitution([pkg_turtlebot3_gazebo, 'launch', f'turtlebot3_{world_name}.launch.py']) + gazebo = IncludeLaunchDescription( + PythonLaunchDescriptionSource([gazebo_launch]), + launch_arguments=[ + ('x_pose', LaunchConfiguration('x_pose')), + ('y_pose', LaunchConfiguration('y_pose')) + ] + ) + turtlebot3_nodes = [gazebo] + return [ # Nodes to launch nav2, rviz, - rtabmap, - gzserver_cmd, - gzclient_cmd, - robot_state_publisher_cmd, - spawn_turtlebot_cmd - ] + rtabmap] + turtlebot3_nodes def generate_launch_description(): return LaunchDescription([ diff --git a/rtabmap_demos/params/humble/turtlebot3_rgbd_nav2_params.yaml b/rtabmap_demos/params/humble/turtlebot3_rgbd_nav2_params.yaml new file mode 100644 index 00000000..838c80b3 --- /dev/null +++ b/rtabmap_demos/params/humble/turtlebot3_rgbd_nav2_params.yaml @@ -0,0 +1,287 @@ +# rtabmap_demos: We add segmented ground and obstacles to voxel_layer of the local costmap and removed scan source. +bt_navigator: + ros__parameters: + use_sim_time: True + global_frame: map + robot_base_frame: base_link + odom_topic: /odom + bt_loop_duration: 10 + default_server_timeout: 20 + wait_for_service_timeout: 1000 + # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults: + # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml + # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml + # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2. + plugin_lib_names: + - nav2_compute_path_to_pose_action_bt_node + - nav2_compute_path_through_poses_action_bt_node + - nav2_smooth_path_action_bt_node + - nav2_follow_path_action_bt_node + - nav2_spin_action_bt_node + - nav2_wait_action_bt_node + - nav2_assisted_teleop_action_bt_node + - nav2_back_up_action_bt_node + - nav2_drive_on_heading_bt_node + - nav2_clear_costmap_service_bt_node + - nav2_is_stuck_condition_bt_node + - nav2_goal_reached_condition_bt_node + - nav2_goal_updated_condition_bt_node + - nav2_globally_updated_goal_condition_bt_node + - nav2_is_path_valid_condition_bt_node + - nav2_initial_pose_received_condition_bt_node + - nav2_reinitialize_global_localization_service_bt_node + - nav2_rate_controller_bt_node + - nav2_distance_controller_bt_node + - nav2_speed_controller_bt_node + - nav2_truncate_path_action_bt_node + - nav2_truncate_path_local_action_bt_node + - nav2_goal_updater_node_bt_node + - nav2_recovery_node_bt_node + - nav2_pipeline_sequence_bt_node + - nav2_round_robin_node_bt_node + - nav2_transform_available_condition_bt_node + - nav2_time_expired_condition_bt_node + - nav2_path_expiring_timer_condition + - nav2_distance_traveled_condition_bt_node + - nav2_single_trigger_bt_node + - nav2_goal_updated_controller_bt_node + - nav2_is_battery_low_condition_bt_node + - nav2_navigate_through_poses_action_bt_node + - nav2_navigate_to_pose_action_bt_node + - nav2_remove_passed_goals_action_bt_node + - nav2_planner_selector_bt_node + - nav2_controller_selector_bt_node + - nav2_goal_checker_selector_bt_node + - nav2_controller_cancel_bt_node + - nav2_path_longer_on_approach_bt_node + - nav2_wait_cancel_bt_node + - nav2_spin_cancel_bt_node + - nav2_back_up_cancel_bt_node + - nav2_assisted_teleop_cancel_bt_node + - nav2_drive_on_heading_cancel_bt_node + - nav2_is_battery_charging_condition_bt_node + +bt_navigator_navigate_through_poses_rclcpp_node: + ros__parameters: + use_sim_time: True + +bt_navigator_navigate_to_pose_rclcpp_node: + ros__parameters: + use_sim_time: True + +controller_server: + ros__parameters: + use_sim_time: True + controller_frequency: 20.0 + min_x_velocity_threshold: 0.001 + min_y_velocity_threshold: 0.5 + min_theta_velocity_threshold: 0.001 + failure_tolerance: 0.3 + progress_checker_plugin: "progress_checker" + goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker" + controller_plugins: ["FollowPath"] + + # Progress checker parameters + progress_checker: + plugin: "nav2_controller::SimpleProgressChecker" + required_movement_radius: 0.5 + movement_time_allowance: 10.0 + # Goal checker parameters + #precise_goal_checker: + # plugin: "nav2_controller::SimpleGoalChecker" + # xy_goal_tolerance: 0.25 + # yaw_goal_tolerance: 0.25 + # stateful: True + general_goal_checker: + stateful: True + plugin: "nav2_controller::SimpleGoalChecker" + xy_goal_tolerance: 0.25 + yaw_goal_tolerance: 0.25 + # DWB parameters + FollowPath: + plugin: "dwb_core::DWBLocalPlanner" + debug_trajectory_details: True + min_vel_x: 0.0 + min_vel_y: 0.0 + max_vel_x: 0.26 + max_vel_y: 0.0 + max_vel_theta: 1.0 + min_speed_xy: 0.0 + max_speed_xy: 0.26 + min_speed_theta: 0.0 + # Add high threshold velocity for turtlebot 3 issue. + # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75 + acc_lim_x: 2.5 + acc_lim_y: 0.0 + acc_lim_theta: 3.2 + decel_lim_x: -2.5 + decel_lim_y: 0.0 + decel_lim_theta: -3.2 + vx_samples: 20 + vy_samples: 5 + vtheta_samples: 20 + sim_time: 1.7 + linear_granularity: 0.05 + angular_granularity: 0.025 + transform_tolerance: 0.2 + xy_goal_tolerance: 0.25 + trans_stopped_velocity: 0.25 + short_circuit_trajectory_evaluation: True + stateful: True + critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"] + BaseObstacle.scale: 0.02 + PathAlign.scale: 32.0 + PathAlign.forward_point_distance: 0.1 + GoalAlign.scale: 24.0 + GoalAlign.forward_point_distance: 0.1 + PathDist.scale: 32.0 + GoalDist.scale: 24.0 + RotateToGoal.scale: 32.0 + RotateToGoal.slowing_factor: 5.0 + RotateToGoal.lookahead_time: -1.0 + +local_costmap: + local_costmap: + ros__parameters: + update_frequency: 5.0 + publish_frequency: 2.0 + global_frame: odom + robot_base_frame: base_link + use_sim_time: True + rolling_window: true + width: 3 + height: 3 + resolution: 0.05 + robot_radius: 0.22 + plugins: ["voxel_layer", "inflation_layer"] + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.55 + voxel_layer: + plugin: "nav2_costmap_2d::VoxelLayer" + enabled: True + publish_voxel_map: True + origin_z: 0.0 + z_resolution: 0.05 + z_voxels: 16 + max_obstacle_height: 2.0 + mark_threshold: 0 + observation_sources: ground obstacles + ground: + topic: /camera/ground + max_obstacle_height: 0.4 + clearing: True + marking: False + data_type: "PointCloud2" + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + obstacles: + topic: /camera/obstacles + max_obstacle_height: 0.4 + clearing: True + marking: True + data_type: "PointCloud2" + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + static_layer: + plugin: "nav2_costmap_2d::StaticLayer" + map_subscribe_transient_local: True + always_send_full_costmap: True + +global_costmap: + global_costmap: + ros__parameters: + update_frequency: 1.0 + publish_frequency: 1.0 + global_frame: map + robot_base_frame: base_link + use_sim_time: True + robot_radius: 0.22 + resolution: 0.05 + track_unknown_space: true + plugins: ["static_layer", "inflation_layer"] + static_layer: + plugin: "nav2_costmap_2d::StaticLayer" + map_subscribe_transient_local: True + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.55 + always_send_full_costmap: True + +map_server: + ros__parameters: + use_sim_time: True + # Overridden in launch by the "map" launch configuration or provided default value. + # To use in yaml, remove the default "map" value in the tb3_simulation_launch.py file & provide full path to map below. + yaml_filename: "" + +smoother_server: + ros__parameters: + use_sim_time: True + smoother_plugins: ["simple_smoother"] + simple_smoother: + plugin: "nav2_smoother::SimpleSmoother" + tolerance: 1.0e-10 + max_its: 1000 + do_refinement: True + +behavior_server: + ros__parameters: + costmap_topic: local_costmap/costmap_raw + footprint_topic: local_costmap/published_footprint + cycle_frequency: 10.0 + behavior_plugins: ["spin", "backup", "drive_on_heading", "assisted_teleop", "wait"] + spin: + plugin: "nav2_behaviors/Spin" + backup: + plugin: "nav2_behaviors/BackUp" + drive_on_heading: + plugin: "nav2_behaviors/DriveOnHeading" + wait: + plugin: "nav2_behaviors/Wait" + assisted_teleop: + plugin: "nav2_behaviors/AssistedTeleop" + global_frame: odom + robot_base_frame: base_link + transform_tolerance: 0.1 + use_sim_time: true + simulate_ahead_time: 2.0 + max_rotational_vel: 1.0 + min_rotational_vel: 0.4 + rotational_acc_lim: 3.2 + +robot_state_publisher: + ros__parameters: + use_sim_time: True + +waypoint_follower: + ros__parameters: + use_sim_time: True + loop_rate: 20 + stop_on_failure: false + waypoint_task_executor_plugin: "wait_at_waypoint" + wait_at_waypoint: + plugin: "nav2_waypoint_follower::WaitAtWaypoint" + enabled: True + waypoint_pause_duration: 200 + +velocity_smoother: + ros__parameters: + use_sim_time: True + smoothing_frequency: 20.0 + scale_velocities: False + feedback: "OPEN_LOOP" + max_velocity: [0.26, 0.0, 1.0] + min_velocity: [-0.26, 0.0, -1.0] + max_accel: [2.5, 0.0, 3.2] + max_decel: [-2.5, 0.0, -3.2] + odom_topic: "odom" + odom_duration: 0.1 + deadband_velocity: [0.0, 0.0, 0.0] + velocity_timeout: 1.0 diff --git a/rtabmap_demos/params/humble/turtlebot3_rgbd_scan_nav2_params.yaml b/rtabmap_demos/params/humble/turtlebot3_rgbd_scan_nav2_params.yaml new file mode 100644 index 00000000..43dce5ba --- /dev/null +++ b/rtabmap_demos/params/humble/turtlebot3_rgbd_scan_nav2_params.yaml @@ -0,0 +1,301 @@ +# rtabmap_demos: We add segmented ground and obstacles to voxel_layer of the local costmap. +bt_navigator: + ros__parameters: + use_sim_time: True + global_frame: map + robot_base_frame: base_link + odom_topic: /odom + bt_loop_duration: 10 + default_server_timeout: 20 + wait_for_service_timeout: 1000 + # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults: + # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml + # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml + # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2. + plugin_lib_names: + - nav2_compute_path_to_pose_action_bt_node + - nav2_compute_path_through_poses_action_bt_node + - nav2_smooth_path_action_bt_node + - nav2_follow_path_action_bt_node + - nav2_spin_action_bt_node + - nav2_wait_action_bt_node + - nav2_assisted_teleop_action_bt_node + - nav2_back_up_action_bt_node + - nav2_drive_on_heading_bt_node + - nav2_clear_costmap_service_bt_node + - nav2_is_stuck_condition_bt_node + - nav2_goal_reached_condition_bt_node + - nav2_goal_updated_condition_bt_node + - nav2_globally_updated_goal_condition_bt_node + - nav2_is_path_valid_condition_bt_node + - nav2_initial_pose_received_condition_bt_node + - nav2_reinitialize_global_localization_service_bt_node + - nav2_rate_controller_bt_node + - nav2_distance_controller_bt_node + - nav2_speed_controller_bt_node + - nav2_truncate_path_action_bt_node + - nav2_truncate_path_local_action_bt_node + - nav2_goal_updater_node_bt_node + - nav2_recovery_node_bt_node + - nav2_pipeline_sequence_bt_node + - nav2_round_robin_node_bt_node + - nav2_transform_available_condition_bt_node + - nav2_time_expired_condition_bt_node + - nav2_path_expiring_timer_condition + - nav2_distance_traveled_condition_bt_node + - nav2_single_trigger_bt_node + - nav2_goal_updated_controller_bt_node + - nav2_is_battery_low_condition_bt_node + - nav2_navigate_through_poses_action_bt_node + - nav2_navigate_to_pose_action_bt_node + - nav2_remove_passed_goals_action_bt_node + - nav2_planner_selector_bt_node + - nav2_controller_selector_bt_node + - nav2_goal_checker_selector_bt_node + - nav2_controller_cancel_bt_node + - nav2_path_longer_on_approach_bt_node + - nav2_wait_cancel_bt_node + - nav2_spin_cancel_bt_node + - nav2_back_up_cancel_bt_node + - nav2_assisted_teleop_cancel_bt_node + - nav2_drive_on_heading_cancel_bt_node + - nav2_is_battery_charging_condition_bt_node + +bt_navigator_navigate_through_poses_rclcpp_node: + ros__parameters: + use_sim_time: True + +bt_navigator_navigate_to_pose_rclcpp_node: + ros__parameters: + use_sim_time: True + +controller_server: + ros__parameters: + use_sim_time: True + controller_frequency: 20.0 + min_x_velocity_threshold: 0.001 + min_y_velocity_threshold: 0.5 + min_theta_velocity_threshold: 0.001 + failure_tolerance: 0.3 + progress_checker_plugin: "progress_checker" + goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker" + controller_plugins: ["FollowPath"] + + # Progress checker parameters + progress_checker: + plugin: "nav2_controller::SimpleProgressChecker" + required_movement_radius: 0.5 + movement_time_allowance: 10.0 + # Goal checker parameters + #precise_goal_checker: + # plugin: "nav2_controller::SimpleGoalChecker" + # xy_goal_tolerance: 0.25 + # yaw_goal_tolerance: 0.25 + # stateful: True + general_goal_checker: + stateful: True + plugin: "nav2_controller::SimpleGoalChecker" + xy_goal_tolerance: 0.25 + yaw_goal_tolerance: 0.25 + # DWB parameters + FollowPath: + plugin: "dwb_core::DWBLocalPlanner" + debug_trajectory_details: True + min_vel_x: 0.0 + min_vel_y: 0.0 + max_vel_x: 0.26 + max_vel_y: 0.0 + max_vel_theta: 1.0 + min_speed_xy: 0.0 + max_speed_xy: 0.26 + min_speed_theta: 0.0 + # Add high threshold velocity for turtlebot 3 issue. + # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75 + acc_lim_x: 2.5 + acc_lim_y: 0.0 + acc_lim_theta: 3.2 + decel_lim_x: -2.5 + decel_lim_y: 0.0 + decel_lim_theta: -3.2 + vx_samples: 20 + vy_samples: 5 + vtheta_samples: 20 + sim_time: 1.7 + linear_granularity: 0.05 + angular_granularity: 0.025 + transform_tolerance: 0.2 + xy_goal_tolerance: 0.25 + trans_stopped_velocity: 0.25 + short_circuit_trajectory_evaluation: True + stateful: True + critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"] + BaseObstacle.scale: 0.02 + PathAlign.scale: 32.0 + PathAlign.forward_point_distance: 0.1 + GoalAlign.scale: 24.0 + GoalAlign.forward_point_distance: 0.1 + PathDist.scale: 32.0 + GoalDist.scale: 24.0 + RotateToGoal.scale: 32.0 + RotateToGoal.slowing_factor: 5.0 + RotateToGoal.lookahead_time: -1.0 + +local_costmap: + local_costmap: + ros__parameters: + update_frequency: 5.0 + publish_frequency: 2.0 + global_frame: odom + robot_base_frame: base_link + use_sim_time: True + rolling_window: true + width: 3 + height: 3 + resolution: 0.05 + robot_radius: 0.22 + plugins: ["voxel_layer", "inflation_layer"] + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.55 + voxel_layer: + plugin: "nav2_costmap_2d::VoxelLayer" + enabled: True + publish_voxel_map: True + origin_z: 0.0 + z_resolution: 0.05 + z_voxels: 16 + max_obstacle_height: 2.0 + mark_threshold: 0 + observation_sources: scan ground obstacles + scan: + topic: /scan + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "LaserScan" + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + ground: + topic: /camera/ground + max_obstacle_height: 0.4 + clearing: True + marking: False + data_type: "PointCloud2" + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + obstacles: + topic: /camera/obstacles + max_obstacle_height: 0.4 + clearing: True + marking: True + data_type: "PointCloud2" + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + static_layer: + plugin: "nav2_costmap_2d::StaticLayer" + map_subscribe_transient_local: True + always_send_full_costmap: True + +global_costmap: + global_costmap: + ros__parameters: + update_frequency: 1.0 + publish_frequency: 1.0 + global_frame: map + robot_base_frame: base_link + use_sim_time: True + robot_radius: 0.22 + resolution: 0.05 + track_unknown_space: true + plugins: ["static_layer", "inflation_layer"] + static_layer: + plugin: "nav2_costmap_2d::StaticLayer" + map_subscribe_transient_local: True + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.55 + always_send_full_costmap: True + +planner_server: + ros__parameters: + expected_planner_frequency: 20.0 + use_sim_time: True + planner_plugins: ["GridBased"] + GridBased: + plugin: "nav2_navfn_planner/NavfnPlanner" + tolerance: 0.5 + use_astar: false + allow_unknown: true + +smoother_server: + ros__parameters: + use_sim_time: True + smoother_plugins: ["simple_smoother"] + simple_smoother: + plugin: "nav2_smoother::SimpleSmoother" + tolerance: 1.0e-10 + max_its: 1000 + do_refinement: True + +behavior_server: + ros__parameters: + costmap_topic: local_costmap/costmap_raw + footprint_topic: local_costmap/published_footprint + cycle_frequency: 10.0 + behavior_plugins: ["spin", "backup", "drive_on_heading", "assisted_teleop", "wait"] + spin: + plugin: "nav2_behaviors/Spin" + backup: + plugin: "nav2_behaviors/BackUp" + drive_on_heading: + plugin: "nav2_behaviors/DriveOnHeading" + wait: + plugin: "nav2_behaviors/Wait" + assisted_teleop: + plugin: "nav2_behaviors/AssistedTeleop" + global_frame: odom + robot_base_frame: base_link + transform_tolerance: 0.1 + use_sim_time: true + simulate_ahead_time: 2.0 + max_rotational_vel: 1.0 + min_rotational_vel: 0.4 + rotational_acc_lim: 3.2 + +robot_state_publisher: + ros__parameters: + use_sim_time: True + +waypoint_follower: + ros__parameters: + use_sim_time: True + loop_rate: 20 + stop_on_failure: false + waypoint_task_executor_plugin: "wait_at_waypoint" + wait_at_waypoint: + plugin: "nav2_waypoint_follower::WaitAtWaypoint" + enabled: True + waypoint_pause_duration: 200 + +velocity_smoother: + ros__parameters: + use_sim_time: True + smoothing_frequency: 20.0 + scale_velocities: False + feedback: "OPEN_LOOP" + max_velocity: [0.26, 0.0, 1.0] + min_velocity: [-0.26, 0.0, -1.0] + max_accel: [2.5, 0.0, 3.2] + max_decel: [-2.5, 0.0, -3.2] + odom_topic: "odom" + odom_duration: 0.1 + deadband_velocity: [0.0, 0.0, 0.0] + velocity_timeout: 1.0 diff --git a/rtabmap_demos/params/humble/turtlebot3_scan_nav2_params.yaml b/rtabmap_demos/params/humble/turtlebot3_scan_nav2_params.yaml new file mode 100644 index 00000000..9c33bdb2 --- /dev/null +++ b/rtabmap_demos/params/humble/turtlebot3_scan_nav2_params.yaml @@ -0,0 +1,295 @@ +# Modified to use icp_odom frame +bt_navigator: + ros__parameters: + use_sim_time: True + global_frame: map + robot_base_frame: base_link + odom_topic: /odom + bt_loop_duration: 10 + default_server_timeout: 20 + wait_for_service_timeout: 1000 + # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults: + # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml + # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml + # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2. + plugin_lib_names: + - nav2_compute_path_to_pose_action_bt_node + - nav2_compute_path_through_poses_action_bt_node + - nav2_smooth_path_action_bt_node + - nav2_follow_path_action_bt_node + - nav2_spin_action_bt_node + - nav2_wait_action_bt_node + - nav2_assisted_teleop_action_bt_node + - nav2_back_up_action_bt_node + - nav2_drive_on_heading_bt_node + - nav2_clear_costmap_service_bt_node + - nav2_is_stuck_condition_bt_node + - nav2_goal_reached_condition_bt_node + - nav2_goal_updated_condition_bt_node + - nav2_globally_updated_goal_condition_bt_node + - nav2_is_path_valid_condition_bt_node + - nav2_initial_pose_received_condition_bt_node + - nav2_reinitialize_global_localization_service_bt_node + - nav2_rate_controller_bt_node + - nav2_distance_controller_bt_node + - nav2_speed_controller_bt_node + - nav2_truncate_path_action_bt_node + - nav2_truncate_path_local_action_bt_node + - nav2_goal_updater_node_bt_node + - nav2_recovery_node_bt_node + - nav2_pipeline_sequence_bt_node + - nav2_round_robin_node_bt_node + - nav2_transform_available_condition_bt_node + - nav2_time_expired_condition_bt_node + - nav2_path_expiring_timer_condition + - nav2_distance_traveled_condition_bt_node + - nav2_single_trigger_bt_node + - nav2_goal_updated_controller_bt_node + - nav2_is_battery_low_condition_bt_node + - nav2_navigate_through_poses_action_bt_node + - nav2_navigate_to_pose_action_bt_node + - nav2_remove_passed_goals_action_bt_node + - nav2_planner_selector_bt_node + - nav2_controller_selector_bt_node + - nav2_goal_checker_selector_bt_node + - nav2_controller_cancel_bt_node + - nav2_path_longer_on_approach_bt_node + - nav2_wait_cancel_bt_node + - nav2_spin_cancel_bt_node + - nav2_back_up_cancel_bt_node + - nav2_assisted_teleop_cancel_bt_node + - nav2_drive_on_heading_cancel_bt_node + - nav2_is_battery_charging_condition_bt_node + +bt_navigator_navigate_through_poses_rclcpp_node: + ros__parameters: + use_sim_time: True + +bt_navigator_navigate_to_pose_rclcpp_node: + ros__parameters: + use_sim_time: True + +controller_server: + ros__parameters: + use_sim_time: True + controller_frequency: 20.0 + min_x_velocity_threshold: 0.001 + min_y_velocity_threshold: 0.5 + min_theta_velocity_threshold: 0.001 + failure_tolerance: 0.3 + progress_checker_plugin: "progress_checker" + goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker" + controller_plugins: ["FollowPath"] + + # Progress checker parameters + progress_checker: + plugin: "nav2_controller::SimpleProgressChecker" + required_movement_radius: 0.5 + movement_time_allowance: 10.0 + # Goal checker parameters + #precise_goal_checker: + # plugin: "nav2_controller::SimpleGoalChecker" + # xy_goal_tolerance: 0.25 + # yaw_goal_tolerance: 0.25 + # stateful: True + general_goal_checker: + stateful: True + plugin: "nav2_controller::SimpleGoalChecker" + xy_goal_tolerance: 0.25 + yaw_goal_tolerance: 0.25 + # DWB parameters + FollowPath: + plugin: "dwb_core::DWBLocalPlanner" + debug_trajectory_details: True + min_vel_x: 0.0 + min_vel_y: 0.0 + max_vel_x: 0.26 + max_vel_y: 0.0 + max_vel_theta: 1.0 + min_speed_xy: 0.0 + max_speed_xy: 0.26 + min_speed_theta: 0.0 + # Add high threshold velocity for turtlebot 3 issue. + # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75 + acc_lim_x: 2.5 + acc_lim_y: 0.0 + acc_lim_theta: 3.2 + decel_lim_x: -2.5 + decel_lim_y: 0.0 + decel_lim_theta: -3.2 + vx_samples: 20 + vy_samples: 5 + vtheta_samples: 20 + sim_time: 1.7 + linear_granularity: 0.05 + angular_granularity: 0.025 + transform_tolerance: 0.2 + xy_goal_tolerance: 0.25 + trans_stopped_velocity: 0.25 + short_circuit_trajectory_evaluation: True + stateful: True + critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"] + BaseObstacle.scale: 0.02 + PathAlign.scale: 32.0 + PathAlign.forward_point_distance: 0.1 + GoalAlign.scale: 24.0 + GoalAlign.forward_point_distance: 0.1 + PathDist.scale: 32.0 + GoalDist.scale: 24.0 + RotateToGoal.scale: 32.0 + RotateToGoal.slowing_factor: 5.0 + RotateToGoal.lookahead_time: -1.0 + +local_costmap: + local_costmap: + ros__parameters: + update_frequency: 5.0 + publish_frequency: 2.0 + global_frame: icp_odom + robot_base_frame: base_link + use_sim_time: True + rolling_window: true + width: 3 + height: 3 + resolution: 0.05 + robot_radius: 0.22 + plugins: ["voxel_layer", "inflation_layer"] + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.55 + voxel_layer: + plugin: "nav2_costmap_2d::VoxelLayer" + enabled: True + publish_voxel_map: True + origin_z: 0.0 + z_resolution: 0.05 + z_voxels: 16 + max_obstacle_height: 2.0 + mark_threshold: 0 + observation_sources: scan + scan: + topic: /scan + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "LaserScan" + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + static_layer: + plugin: "nav2_costmap_2d::StaticLayer" + map_subscribe_transient_local: True + always_send_full_costmap: True + +global_costmap: + global_costmap: + ros__parameters: + update_frequency: 1.0 + publish_frequency: 1.0 + global_frame: map + robot_base_frame: base_link + use_sim_time: True + robot_radius: 0.22 + resolution: 0.05 + track_unknown_space: true + plugins: ["static_layer", "obstacle_layer", "inflation_layer"] + obstacle_layer: + plugin: "nav2_costmap_2d::ObstacleLayer" + enabled: True + observation_sources: scan + scan: + topic: /scan + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "LaserScan" + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + static_layer: + plugin: "nav2_costmap_2d::StaticLayer" + map_subscribe_transient_local: True + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.55 + always_send_full_costmap: True + +planner_server: + ros__parameters: + expected_planner_frequency: 20.0 + use_sim_time: True + planner_plugins: ["GridBased"] + GridBased: + plugin: "nav2_navfn_planner/NavfnPlanner" + tolerance: 0.5 + use_astar: false + allow_unknown: true + +smoother_server: + ros__parameters: + use_sim_time: True + smoother_plugins: ["simple_smoother"] + simple_smoother: + plugin: "nav2_smoother::SimpleSmoother" + tolerance: 1.0e-10 + max_its: 1000 + do_refinement: True + +behavior_server: + ros__parameters: + costmap_topic: local_costmap/costmap_raw + footprint_topic: local_costmap/published_footprint + cycle_frequency: 10.0 + behavior_plugins: ["spin", "backup", "drive_on_heading", "assisted_teleop", "wait"] + spin: + plugin: "nav2_behaviors/Spin" + backup: + plugin: "nav2_behaviors/BackUp" + drive_on_heading: + plugin: "nav2_behaviors/DriveOnHeading" + wait: + plugin: "nav2_behaviors/Wait" + assisted_teleop: + plugin: "nav2_behaviors/AssistedTeleop" + global_frame: icp_odom + robot_base_frame: base_link + transform_tolerance: 0.1 + use_sim_time: true + simulate_ahead_time: 2.0 + max_rotational_vel: 1.0 + min_rotational_vel: 0.4 + rotational_acc_lim: 3.2 + +robot_state_publisher: + ros__parameters: + use_sim_time: True + +waypoint_follower: + ros__parameters: + use_sim_time: True + loop_rate: 20 + stop_on_failure: false + waypoint_task_executor_plugin: "wait_at_waypoint" + wait_at_waypoint: + plugin: "nav2_waypoint_follower::WaitAtWaypoint" + enabled: True + waypoint_pause_duration: 200 + +velocity_smoother: + ros__parameters: + use_sim_time: True + smoothing_frequency: 20.0 + scale_velocities: False + feedback: "OPEN_LOOP" + max_velocity: [0.26, 0.0, 1.0] + min_velocity: [-0.26, 0.0, -1.0] + max_accel: [2.5, 0.0, 3.2] + max_decel: [-2.5, 0.0, -3.2] + odom_topic: "odom" + odom_duration: 0.1 + deadband_velocity: [0.0, 0.0, 0.0] + velocity_timeout: 1.0 diff --git a/rtabmap_demos/params/turtlebot3_nav2_params.yaml b/rtabmap_demos/params/turtlebot3_nav2_params.yaml new file mode 100644 index 00000000..0a861836 --- /dev/null +++ b/rtabmap_demos/params/turtlebot3_nav2_params.yaml @@ -0,0 +1,421 @@ +bt_navigator: + ros__parameters: + global_frame: map + robot_base_frame: base_link + odom_topic: /odom + bt_loop_duration: 10 + default_server_timeout: 20 + wait_for_service_timeout: 1000 + action_server_result_timeout: 900.0 + navigators: ["navigate_to_pose", "navigate_through_poses"] + navigate_to_pose: + plugin: "nav2_bt_navigator::NavigateToPoseNavigator" + navigate_through_poses: + plugin: "nav2_bt_navigator::NavigateThroughPosesNavigator" + # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults: + # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml + # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml + # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2. + + # plugin_lib_names is used to add custom BT plugins to the executor (vector of strings). + # Built-in plugins are added automatically + # plugin_lib_names: [] + + error_code_names: + - compute_path_error_code + - follow_path_error_code + +controller_server: + ros__parameters: + enable_stamped_cmd_vel: True + controller_frequency: 20.0 + costmap_update_timeout: 0.30 + min_x_velocity_threshold: 0.001 + min_y_velocity_threshold: 0.5 + min_theta_velocity_threshold: 0.001 + failure_tolerance: 0.3 + progress_checker_plugins: ["progress_checker"] + goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker" + controller_plugins: ["FollowPath"] + use_realtime_priority: false + + # Progress checker parameters + progress_checker: + plugin: "nav2_controller::SimpleProgressChecker" + required_movement_radius: 0.5 + movement_time_allowance: 10.0 + # Goal checker parameters + #precise_goal_checker: + # plugin: "nav2_controller::SimpleGoalChecker" + # xy_goal_tolerance: 0.25 + # yaw_goal_tolerance: 0.25 + # stateful: True + general_goal_checker: + stateful: True + plugin: "nav2_controller::SimpleGoalChecker" + xy_goal_tolerance: 0.25 + yaw_goal_tolerance: 0.25 + FollowPath: + plugin: "nav2_mppi_controller::MPPIController" + time_steps: 56 + model_dt: 0.05 + batch_size: 2000 + ax_max: 3.0 + ax_min: -3.0 + ay_max: 3.0 + ay_min: -3.0 + az_max: 3.5 + vx_std: 0.2 + vy_std: 0.2 + wz_std: 0.4 + vx_max: 0.5 + vx_min: -0.35 + vy_max: 0.5 + wz_max: 1.9 + iteration_count: 1 + prune_distance: 1.7 + transform_tolerance: 0.1 + temperature: 0.3 + gamma: 0.015 + motion_model: "DiffDrive" + visualize: true + regenerate_noises: true + TrajectoryVisualizer: + trajectory_step: 5 + time_step: 3 + AckermannConstraints: + min_turning_r: 0.2 + critics: [ + "ConstraintCritic", "CostCritic", "GoalCritic", + "GoalAngleCritic", "PathAlignCritic", "PathFollowCritic", + "PathAngleCritic", "PreferForwardCritic"] + ConstraintCritic: + enabled: true + cost_power: 1 + cost_weight: 4.0 + GoalCritic: + enabled: true + cost_power: 1 + cost_weight: 5.0 + threshold_to_consider: 1.4 + GoalAngleCritic: + enabled: true + cost_power: 1 + cost_weight: 3.0 + threshold_to_consider: 0.5 + PreferForwardCritic: + enabled: true + cost_power: 1 + cost_weight: 5.0 + threshold_to_consider: 0.5 + CostCritic: + enabled: true + cost_power: 1 + cost_weight: 3.81 + near_collision_cost: 253 + critical_cost: 300.0 + consider_footprint: false + collision_cost: 1000000.0 + near_goal_distance: 1.0 + trajectory_point_step: 2 + PathAlignCritic: + enabled: true + cost_power: 1 + cost_weight: 14.0 + max_path_occupancy_ratio: 0.05 + trajectory_point_step: 4 + threshold_to_consider: 0.5 + offset_from_furthest: 20 + use_path_orientations: false + PathFollowCritic: + enabled: true + cost_power: 1 + cost_weight: 5.0 + offset_from_furthest: 5 + threshold_to_consider: 1.4 + PathAngleCritic: + enabled: true + cost_power: 1 + cost_weight: 2.0 + offset_from_furthest: 4 + threshold_to_consider: 0.5 + max_angle_to_furthest: 1.0 + mode: 0 + # TwirlingCritic: + # enabled: true + # twirling_cost_power: 1 + # twirling_cost_weight: 10.0 + +local_costmap: + local_costmap: + ros__parameters: + update_frequency: 5.0 + publish_frequency: 2.0 + global_frame: odom + robot_base_frame: base_link + rolling_window: true + width: 3 + height: 3 + resolution: 0.05 + robot_radius: 0.22 + plugins: ["voxel_layer", "inflation_layer"] + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.70 + voxel_layer: + plugin: "nav2_costmap_2d::VoxelLayer" + enabled: True + publish_voxel_map: True + origin_z: 0.0 + z_resolution: 0.05 + z_voxels: 16 + max_obstacle_height: 2.0 + mark_threshold: 0 + observation_sources: scan + scan: + topic: /scan + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "LaserScan" + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + static_layer: + plugin: "nav2_costmap_2d::StaticLayer" + map_subscribe_transient_local: True + always_send_full_costmap: True + +global_costmap: + global_costmap: + ros__parameters: + update_frequency: 1.0 + publish_frequency: 1.0 + global_frame: map + robot_base_frame: base_link + robot_radius: 0.22 + resolution: 0.05 + track_unknown_space: true + plugins: ["static_layer", "obstacle_layer", "inflation_layer"] + obstacle_layer: + plugin: "nav2_costmap_2d::ObstacleLayer" + enabled: True + observation_sources: scan + scan: + topic: /scan + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "LaserScan" + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + static_layer: + plugin: "nav2_costmap_2d::StaticLayer" + map_subscribe_transient_local: True + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.7 + always_send_full_costmap: True + +planner_server: + ros__parameters: + expected_planner_frequency: 20.0 + planner_plugins: ["GridBased"] + costmap_update_timeout: 1.0 + GridBased: + plugin: "nav2_navfn_planner::NavfnPlanner" + tolerance: 0.5 + use_astar: false + allow_unknown: true + +smoother_server: + ros__parameters: + smoother_plugins: ["simple_smoother"] + simple_smoother: + plugin: "nav2_smoother::SimpleSmoother" + tolerance: 1.0e-10 + max_its: 1000 + do_refinement: True + +behavior_server: + ros__parameters: + enable_stamped_cmd_vel: True + local_costmap_topic: local_costmap/costmap_raw + global_costmap_topic: global_costmap/costmap_raw + local_footprint_topic: local_costmap/published_footprint + global_footprint_topic: global_costmap/published_footprint + cycle_frequency: 10.0 + behavior_plugins: ["spin", "backup", "drive_on_heading", "assisted_teleop", "wait"] + spin: + plugin: "nav2_behaviors::Spin" + backup: + plugin: "nav2_behaviors::BackUp" + drive_on_heading: + plugin: "nav2_behaviors::DriveOnHeading" + wait: + plugin: "nav2_behaviors::Wait" + assisted_teleop: + plugin: "nav2_behaviors::AssistedTeleop" + local_frame: odom + global_frame: map + robot_base_frame: base_link + transform_tolerance: 0.1 + simulate_ahead_time: 2.0 + max_rotational_vel: 1.0 + min_rotational_vel: 0.4 + rotational_acc_lim: 3.2 + +waypoint_follower: + ros__parameters: + loop_rate: 20 + stop_on_failure: false + action_server_result_timeout: 900.0 + waypoint_task_executor_plugin: "wait_at_waypoint" + wait_at_waypoint: + plugin: "nav2_waypoint_follower::WaitAtWaypoint" + enabled: True + waypoint_pause_duration: 200 + +route_server: + ros__parameters: + # The graph_filepath does not need to be specified since it going to be set by defaults in launch. + # If you'd rather set it in the yaml, remove the default "graph" value in the launch file(s). + # file & provide full path to map below. If graph config or launch default is provided, it is used + # graph_filepath: $(find-pkg-share nav2_route)/graphs/aws_graph.geojson + boundary_radius_to_achieve_node: 1.0 + radius_to_achieve_node: 2.0 + smooth_corners: true + operations: ["AdjustSpeedLimit", "ReroutingService", "CollisionMonitor"] + ReroutingService: + plugin: "nav2_route::ReroutingService" + AdjustSpeedLimit: + plugin: "nav2_route::AdjustSpeedLimit" + CollisionMonitor: + plugin: "nav2_route::CollisionMonitor" + max_collision_dist: 3.0 + edge_cost_functions: ["DistanceScorer", "CostmapScorer"] + DistanceScorer: + plugin: "nav2_route::DistanceScorer" + CostmapScorer: + plugin: "nav2_route::CostmapScorer" + +velocity_smoother: + ros__parameters: + enable_stamped_cmd_vel: True + smoothing_frequency: 20.0 + stamp_smoothed_velocity_with_smoothing_time: False + scale_velocities: False + feedback: "OPEN_LOOP" + max_velocity: [0.5, 0.0, 2.0] + min_velocity: [-0.5, 0.0, -2.0] + max_accel: [2.5, 0.0, 3.2] + max_decel: [-2.5, 0.0, -3.2] + odom_topic: "odom" + odom_duration: 0.1 + deadband_velocity: [0.0, 0.0, 0.0] + velocity_timeout: 1.0 + +collision_monitor: + ros__parameters: + enable_stamped_cmd_vel: True + base_frame_id: "base_footprint" + odom_frame_id: "odom" + cmd_vel_in_topic: "cmd_vel_smoothed" + cmd_vel_out_topic: "cmd_vel" + state_topic: "collision_monitor_state" + transform_tolerance: 0.2 + source_timeout: 1.0 + base_shift_correction: True + stop_pub_timeout: 2.0 + # Polygons represent zone around the robot for "stop", "slowdown" and "limit" action types, + # and robot footprint for "approach" action type. + polygons: ["FootprintApproach"] + FootprintApproach: + type: "polygon" + action_type: "approach" + footprint_topic: "/local_costmap/published_footprint" + time_before_collision: 1.2 + simulation_time_step: 0.1 + min_points: 6 + visualize: False + enabled: True + observation_sources: ["scan"] + scan: + type: "scan" + topic: "scan" + min_height: 0.15 + max_height: 2.0 + enabled: True + +docking_server: + ros__parameters: + enable_stamped_cmd_vel: True + controller_frequency: 50.0 + initial_perception_timeout: 5.0 + wait_charge_timeout: 5.0 + dock_approach_timeout: 30.0 + undock_linear_tolerance: 0.05 + undock_angular_tolerance: 0.1 + max_retries: 3 + base_frame: "base_link" + fixed_frame: "odom" + dock_backwards: false + dock_prestaging_tolerance: 0.5 + + # Types of docks + dock_plugins: ['simple_charging_dock'] + simple_charging_dock: + plugin: 'opennav_docking::SimpleChargingDock' + docking_threshold: 0.05 + staging_x_offset: -0.7 + use_external_detection_pose: true + use_battery_status: false # true + use_stall_detection: false # true + + external_detection_timeout: 1.0 + external_detection_translation_x: -0.18 + external_detection_translation_y: 0.0 + external_detection_rotation_roll: -1.57 + external_detection_rotation_pitch: -1.57 + external_detection_rotation_yaw: 0.0 + filter_coef: 0.1 + + # Dock instances + # The following example illustrates configuring dock instances. + # docks: ['home_dock'] # Input your docks here + # home_dock: + # type: 'simple_charging_dock' + # frame: map + # pose: [0.0, 0.0, 0.0] + + controller: + k_phi: 3.0 + k_delta: 2.0 + v_linear_min: 0.15 + v_linear_max: 0.15 + use_collision_detection: true + costmap_topic: "local_costmap/costmap_raw" + footprint_topic: "local_costmap/published_footprint" + transform_tolerance: 0.1 + projection_time: 5.0 + simulation_step: 0.1 + dock_collision_threshold: 0.3 + +loopback_simulator: + ros__parameters: + base_frame_id: "base_footprint" + odom_frame_id: "odom" + map_frame_id: "map" + scan_frame_id: "base_scan" # tb4_loopback_simulator.launch.py remaps to 'rplidar_link' + update_duration: 0.02 + scan_range_min: 0.05 + scan_range_max: 30.0 + scan_angle_min: -3.1415 + scan_angle_max: 3.1415 + scan_angle_increment: 0.02617 + scan_use_inf: true diff --git a/rtabmap_demos/params/turtlebot3_rgbd_nav2_params.yaml b/rtabmap_demos/params/turtlebot3_rgbd_nav2_params.yaml index 838c80b3..5d32f405 100644 --- a/rtabmap_demos/params/turtlebot3_rgbd_nav2_params.yaml +++ b/rtabmap_demos/params/turtlebot3_rgbd_nav2_params.yaml @@ -1,85 +1,44 @@ # rtabmap_demos: We add segmented ground and obstacles to voxel_layer of the local costmap and removed scan source. bt_navigator: ros__parameters: - use_sim_time: True global_frame: map robot_base_frame: base_link odom_topic: /odom bt_loop_duration: 10 default_server_timeout: 20 wait_for_service_timeout: 1000 + action_server_result_timeout: 900.0 + navigators: ["navigate_to_pose", "navigate_through_poses"] + navigate_to_pose: + plugin: "nav2_bt_navigator::NavigateToPoseNavigator" + navigate_through_poses: + plugin: "nav2_bt_navigator::NavigateThroughPosesNavigator" # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults: # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2. - plugin_lib_names: - - nav2_compute_path_to_pose_action_bt_node - - nav2_compute_path_through_poses_action_bt_node - - nav2_smooth_path_action_bt_node - - nav2_follow_path_action_bt_node - - nav2_spin_action_bt_node - - nav2_wait_action_bt_node - - nav2_assisted_teleop_action_bt_node - - nav2_back_up_action_bt_node - - nav2_drive_on_heading_bt_node - - nav2_clear_costmap_service_bt_node - - nav2_is_stuck_condition_bt_node - - nav2_goal_reached_condition_bt_node - - nav2_goal_updated_condition_bt_node - - nav2_globally_updated_goal_condition_bt_node - - nav2_is_path_valid_condition_bt_node - - nav2_initial_pose_received_condition_bt_node - - nav2_reinitialize_global_localization_service_bt_node - - nav2_rate_controller_bt_node - - nav2_distance_controller_bt_node - - nav2_speed_controller_bt_node - - nav2_truncate_path_action_bt_node - - nav2_truncate_path_local_action_bt_node - - nav2_goal_updater_node_bt_node - - nav2_recovery_node_bt_node - - nav2_pipeline_sequence_bt_node - - nav2_round_robin_node_bt_node - - nav2_transform_available_condition_bt_node - - nav2_time_expired_condition_bt_node - - nav2_path_expiring_timer_condition - - nav2_distance_traveled_condition_bt_node - - nav2_single_trigger_bt_node - - nav2_goal_updated_controller_bt_node - - nav2_is_battery_low_condition_bt_node - - nav2_navigate_through_poses_action_bt_node - - nav2_navigate_to_pose_action_bt_node - - nav2_remove_passed_goals_action_bt_node - - nav2_planner_selector_bt_node - - nav2_controller_selector_bt_node - - nav2_goal_checker_selector_bt_node - - nav2_controller_cancel_bt_node - - nav2_path_longer_on_approach_bt_node - - nav2_wait_cancel_bt_node - - nav2_spin_cancel_bt_node - - nav2_back_up_cancel_bt_node - - nav2_assisted_teleop_cancel_bt_node - - nav2_drive_on_heading_cancel_bt_node - - nav2_is_battery_charging_condition_bt_node -bt_navigator_navigate_through_poses_rclcpp_node: - ros__parameters: - use_sim_time: True + # plugin_lib_names is used to add custom BT plugins to the executor (vector of strings). + # Built-in plugins are added automatically + # plugin_lib_names: [] -bt_navigator_navigate_to_pose_rclcpp_node: - ros__parameters: - use_sim_time: True + error_code_names: + - compute_path_error_code + - follow_path_error_code controller_server: ros__parameters: - use_sim_time: True + enable_stamped_cmd_vel: True controller_frequency: 20.0 + costmap_update_timeout: 0.30 min_x_velocity_threshold: 0.001 min_y_velocity_threshold: 0.5 min_theta_velocity_threshold: 0.001 failure_tolerance: 0.3 - progress_checker_plugin: "progress_checker" + progress_checker_plugins: ["progress_checker"] goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker" controller_plugins: ["FollowPath"] + use_realtime_priority: false # Progress checker parameters progress_checker: @@ -97,48 +56,96 @@ controller_server: plugin: "nav2_controller::SimpleGoalChecker" xy_goal_tolerance: 0.25 yaw_goal_tolerance: 0.25 - # DWB parameters FollowPath: - plugin: "dwb_core::DWBLocalPlanner" - debug_trajectory_details: True - min_vel_x: 0.0 - min_vel_y: 0.0 - max_vel_x: 0.26 - max_vel_y: 0.0 - max_vel_theta: 1.0 - min_speed_xy: 0.0 - max_speed_xy: 0.26 - min_speed_theta: 0.0 - # Add high threshold velocity for turtlebot 3 issue. - # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75 - acc_lim_x: 2.5 - acc_lim_y: 0.0 - acc_lim_theta: 3.2 - decel_lim_x: -2.5 - decel_lim_y: 0.0 - decel_lim_theta: -3.2 - vx_samples: 20 - vy_samples: 5 - vtheta_samples: 20 - sim_time: 1.7 - linear_granularity: 0.05 - angular_granularity: 0.025 - transform_tolerance: 0.2 - xy_goal_tolerance: 0.25 - trans_stopped_velocity: 0.25 - short_circuit_trajectory_evaluation: True - stateful: True - critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"] - BaseObstacle.scale: 0.02 - PathAlign.scale: 32.0 - PathAlign.forward_point_distance: 0.1 - GoalAlign.scale: 24.0 - GoalAlign.forward_point_distance: 0.1 - PathDist.scale: 32.0 - GoalDist.scale: 24.0 - RotateToGoal.scale: 32.0 - RotateToGoal.slowing_factor: 5.0 - RotateToGoal.lookahead_time: -1.0 + plugin: "nav2_mppi_controller::MPPIController" + time_steps: 56 + model_dt: 0.05 + batch_size: 2000 + ax_max: 3.0 + ax_min: -3.0 + ay_max: 3.0 + ay_min: -3.0 + az_max: 3.5 + vx_std: 0.2 + vy_std: 0.2 + wz_std: 0.4 + vx_max: 0.5 + vx_min: -0.35 + vy_max: 0.5 + wz_max: 1.9 + iteration_count: 1 + prune_distance: 1.7 + transform_tolerance: 0.1 + temperature: 0.3 + gamma: 0.015 + motion_model: "DiffDrive" + visualize: true + regenerate_noises: true + TrajectoryVisualizer: + trajectory_step: 5 + time_step: 3 + AckermannConstraints: + min_turning_r: 0.2 + critics: [ + "ConstraintCritic", "CostCritic", "GoalCritic", + "GoalAngleCritic", "PathAlignCritic", "PathFollowCritic", + "PathAngleCritic", "PreferForwardCritic"] + ConstraintCritic: + enabled: true + cost_power: 1 + cost_weight: 4.0 + GoalCritic: + enabled: true + cost_power: 1 + cost_weight: 5.0 + threshold_to_consider: 1.4 + GoalAngleCritic: + enabled: true + cost_power: 1 + cost_weight: 3.0 + threshold_to_consider: 0.5 + PreferForwardCritic: + enabled: true + cost_power: 1 + cost_weight: 5.0 + threshold_to_consider: 0.5 + CostCritic: + enabled: true + cost_power: 1 + cost_weight: 3.81 + near_collision_cost: 253 + critical_cost: 300.0 + consider_footprint: false + collision_cost: 1000000.0 + near_goal_distance: 1.0 + trajectory_point_step: 2 + PathAlignCritic: + enabled: true + cost_power: 1 + cost_weight: 14.0 + max_path_occupancy_ratio: 0.05 + trajectory_point_step: 4 + threshold_to_consider: 0.5 + offset_from_furthest: 20 + use_path_orientations: false + PathFollowCritic: + enabled: true + cost_power: 1 + cost_weight: 5.0 + offset_from_furthest: 5 + threshold_to_consider: 1.4 + PathAngleCritic: + enabled: true + cost_power: 1 + cost_weight: 2.0 + offset_from_furthest: 4 + threshold_to_consider: 0.5 + max_angle_to_furthest: 1.0 + mode: 0 + # TwirlingCritic: + # enabled: true + # twirling_cost_power: 1 + # twirling_cost_weight: 10.0 local_costmap: local_costmap: @@ -147,7 +154,6 @@ local_costmap: publish_frequency: 2.0 global_frame: odom robot_base_frame: base_link - use_sim_time: True rolling_window: true width: 3 height: 3 @@ -157,7 +163,7 @@ local_costmap: inflation_layer: plugin: "nav2_costmap_2d::InflationLayer" cost_scaling_factor: 3.0 - inflation_radius: 0.55 + inflation_radius: 0.70 voxel_layer: plugin: "nav2_costmap_2d::VoxelLayer" enabled: True @@ -200,7 +206,6 @@ global_costmap: publish_frequency: 1.0 global_frame: map robot_base_frame: base_link - use_sim_time: True robot_radius: 0.22 resolution: 0.05 track_unknown_space: true @@ -211,19 +216,22 @@ global_costmap: inflation_layer: plugin: "nav2_costmap_2d::InflationLayer" cost_scaling_factor: 3.0 - inflation_radius: 0.55 + inflation_radius: 0.7 always_send_full_costmap: True -map_server: +planner_server: ros__parameters: - use_sim_time: True - # Overridden in launch by the "map" launch configuration or provided default value. - # To use in yaml, remove the default "map" value in the tb3_simulation_launch.py file & provide full path to map below. - yaml_filename: "" + expected_planner_frequency: 20.0 + planner_plugins: ["GridBased"] + costmap_update_timeout: 1.0 + GridBased: + plugin: "nav2_navfn_planner::NavfnPlanner" + tolerance: 0.5 + use_astar: false + allow_unknown: true smoother_server: ros__parameters: - use_sim_time: True smoother_plugins: ["simple_smoother"] simple_smoother: plugin: "nav2_smoother::SimpleSmoother" @@ -233,55 +241,178 @@ smoother_server: behavior_server: ros__parameters: - costmap_topic: local_costmap/costmap_raw - footprint_topic: local_costmap/published_footprint + enable_stamped_cmd_vel: True + local_costmap_topic: local_costmap/costmap_raw + global_costmap_topic: global_costmap/costmap_raw + local_footprint_topic: local_costmap/published_footprint + global_footprint_topic: global_costmap/published_footprint cycle_frequency: 10.0 behavior_plugins: ["spin", "backup", "drive_on_heading", "assisted_teleop", "wait"] spin: - plugin: "nav2_behaviors/Spin" + plugin: "nav2_behaviors::Spin" backup: - plugin: "nav2_behaviors/BackUp" + plugin: "nav2_behaviors::BackUp" drive_on_heading: - plugin: "nav2_behaviors/DriveOnHeading" + plugin: "nav2_behaviors::DriveOnHeading" wait: - plugin: "nav2_behaviors/Wait" + plugin: "nav2_behaviors::Wait" assisted_teleop: - plugin: "nav2_behaviors/AssistedTeleop" - global_frame: odom + plugin: "nav2_behaviors::AssistedTeleop" + local_frame: odom + global_frame: map robot_base_frame: base_link transform_tolerance: 0.1 - use_sim_time: true simulate_ahead_time: 2.0 max_rotational_vel: 1.0 min_rotational_vel: 0.4 rotational_acc_lim: 3.2 -robot_state_publisher: - ros__parameters: - use_sim_time: True - waypoint_follower: ros__parameters: - use_sim_time: True loop_rate: 20 stop_on_failure: false + action_server_result_timeout: 900.0 waypoint_task_executor_plugin: "wait_at_waypoint" wait_at_waypoint: plugin: "nav2_waypoint_follower::WaitAtWaypoint" enabled: True waypoint_pause_duration: 200 +route_server: + ros__parameters: + # The graph_filepath does not need to be specified since it going to be set by defaults in launch. + # If you'd rather set it in the yaml, remove the default "graph" value in the launch file(s). + # file & provide full path to map below. If graph config or launch default is provided, it is used + # graph_filepath: $(find-pkg-share nav2_route)/graphs/aws_graph.geojson + boundary_radius_to_achieve_node: 1.0 + radius_to_achieve_node: 2.0 + smooth_corners: true + operations: ["AdjustSpeedLimit", "ReroutingService", "CollisionMonitor"] + ReroutingService: + plugin: "nav2_route::ReroutingService" + AdjustSpeedLimit: + plugin: "nav2_route::AdjustSpeedLimit" + CollisionMonitor: + plugin: "nav2_route::CollisionMonitor" + max_collision_dist: 3.0 + edge_cost_functions: ["DistanceScorer", "CostmapScorer"] + DistanceScorer: + plugin: "nav2_route::DistanceScorer" + CostmapScorer: + plugin: "nav2_route::CostmapScorer" + velocity_smoother: ros__parameters: - use_sim_time: True + enable_stamped_cmd_vel: True smoothing_frequency: 20.0 + stamp_smoothed_velocity_with_smoothing_time: False scale_velocities: False feedback: "OPEN_LOOP" - max_velocity: [0.26, 0.0, 1.0] - min_velocity: [-0.26, 0.0, -1.0] + max_velocity: [0.5, 0.0, 2.0] + min_velocity: [-0.5, 0.0, -2.0] max_accel: [2.5, 0.0, 3.2] max_decel: [-2.5, 0.0, -3.2] odom_topic: "odom" odom_duration: 0.1 deadband_velocity: [0.0, 0.0, 0.0] velocity_timeout: 1.0 + +collision_monitor: + ros__parameters: + enable_stamped_cmd_vel: True + base_frame_id: "base_footprint" + odom_frame_id: "odom" + cmd_vel_in_topic: "cmd_vel_smoothed" + cmd_vel_out_topic: "cmd_vel" + state_topic: "collision_monitor_state" + transform_tolerance: 0.2 + source_timeout: 1.0 + base_shift_correction: True + stop_pub_timeout: 2.0 + # Polygons represent zone around the robot for "stop", "slowdown" and "limit" action types, + # and robot footprint for "approach" action type. + polygons: ["FootprintApproach"] + FootprintApproach: + type: "polygon" + action_type: "approach" + footprint_topic: "/local_costmap/published_footprint" + time_before_collision: 1.2 + simulation_time_step: 0.1 + min_points: 6 + visualize: False + enabled: True + observation_sources: ["scan"] + scan: + type: "scan" + topic: "scan" + min_height: 0.15 + max_height: 2.0 + enabled: True + +docking_server: + ros__parameters: + enable_stamped_cmd_vel: True + controller_frequency: 50.0 + initial_perception_timeout: 5.0 + wait_charge_timeout: 5.0 + dock_approach_timeout: 30.0 + undock_linear_tolerance: 0.05 + undock_angular_tolerance: 0.1 + max_retries: 3 + base_frame: "base_link" + fixed_frame: "odom" + dock_backwards: false + dock_prestaging_tolerance: 0.5 + + # Types of docks + dock_plugins: ['simple_charging_dock'] + simple_charging_dock: + plugin: 'opennav_docking::SimpleChargingDock' + docking_threshold: 0.05 + staging_x_offset: -0.7 + use_external_detection_pose: true + use_battery_status: false # true + use_stall_detection: false # true + + external_detection_timeout: 1.0 + external_detection_translation_x: -0.18 + external_detection_translation_y: 0.0 + external_detection_rotation_roll: -1.57 + external_detection_rotation_pitch: -1.57 + external_detection_rotation_yaw: 0.0 + filter_coef: 0.1 + + # Dock instances + # The following example illustrates configuring dock instances. + # docks: ['home_dock'] # Input your docks here + # home_dock: + # type: 'simple_charging_dock' + # frame: map + # pose: [0.0, 0.0, 0.0] + + controller: + k_phi: 3.0 + k_delta: 2.0 + v_linear_min: 0.15 + v_linear_max: 0.15 + use_collision_detection: true + costmap_topic: "local_costmap/costmap_raw" + footprint_topic: "local_costmap/published_footprint" + transform_tolerance: 0.1 + projection_time: 5.0 + simulation_step: 0.1 + dock_collision_threshold: 0.3 + +loopback_simulator: + ros__parameters: + base_frame_id: "base_footprint" + odom_frame_id: "odom" + map_frame_id: "map" + scan_frame_id: "base_scan" # tb4_loopback_simulator.launch.py remaps to 'rplidar_link' + update_duration: 0.02 + scan_range_min: 0.05 + scan_range_max: 30.0 + scan_angle_min: -3.1415 + scan_angle_max: 3.1415 + scan_angle_increment: 0.02617 + scan_use_inf: true diff --git a/rtabmap_demos/params/turtlebot3_rgbd_scan_nav2_params.yaml b/rtabmap_demos/params/turtlebot3_rgbd_scan_nav2_params.yaml index 43dce5ba..cb833425 100644 --- a/rtabmap_demos/params/turtlebot3_rgbd_scan_nav2_params.yaml +++ b/rtabmap_demos/params/turtlebot3_rgbd_scan_nav2_params.yaml @@ -1,85 +1,44 @@ # rtabmap_demos: We add segmented ground and obstacles to voxel_layer of the local costmap. bt_navigator: ros__parameters: - use_sim_time: True global_frame: map robot_base_frame: base_link odom_topic: /odom bt_loop_duration: 10 default_server_timeout: 20 wait_for_service_timeout: 1000 + action_server_result_timeout: 900.0 + navigators: ["navigate_to_pose", "navigate_through_poses"] + navigate_to_pose: + plugin: "nav2_bt_navigator::NavigateToPoseNavigator" + navigate_through_poses: + plugin: "nav2_bt_navigator::NavigateThroughPosesNavigator" # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults: # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2. - plugin_lib_names: - - nav2_compute_path_to_pose_action_bt_node - - nav2_compute_path_through_poses_action_bt_node - - nav2_smooth_path_action_bt_node - - nav2_follow_path_action_bt_node - - nav2_spin_action_bt_node - - nav2_wait_action_bt_node - - nav2_assisted_teleop_action_bt_node - - nav2_back_up_action_bt_node - - nav2_drive_on_heading_bt_node - - nav2_clear_costmap_service_bt_node - - nav2_is_stuck_condition_bt_node - - nav2_goal_reached_condition_bt_node - - nav2_goal_updated_condition_bt_node - - nav2_globally_updated_goal_condition_bt_node - - nav2_is_path_valid_condition_bt_node - - nav2_initial_pose_received_condition_bt_node - - nav2_reinitialize_global_localization_service_bt_node - - nav2_rate_controller_bt_node - - nav2_distance_controller_bt_node - - nav2_speed_controller_bt_node - - nav2_truncate_path_action_bt_node - - nav2_truncate_path_local_action_bt_node - - nav2_goal_updater_node_bt_node - - nav2_recovery_node_bt_node - - nav2_pipeline_sequence_bt_node - - nav2_round_robin_node_bt_node - - nav2_transform_available_condition_bt_node - - nav2_time_expired_condition_bt_node - - nav2_path_expiring_timer_condition - - nav2_distance_traveled_condition_bt_node - - nav2_single_trigger_bt_node - - nav2_goal_updated_controller_bt_node - - nav2_is_battery_low_condition_bt_node - - nav2_navigate_through_poses_action_bt_node - - nav2_navigate_to_pose_action_bt_node - - nav2_remove_passed_goals_action_bt_node - - nav2_planner_selector_bt_node - - nav2_controller_selector_bt_node - - nav2_goal_checker_selector_bt_node - - nav2_controller_cancel_bt_node - - nav2_path_longer_on_approach_bt_node - - nav2_wait_cancel_bt_node - - nav2_spin_cancel_bt_node - - nav2_back_up_cancel_bt_node - - nav2_assisted_teleop_cancel_bt_node - - nav2_drive_on_heading_cancel_bt_node - - nav2_is_battery_charging_condition_bt_node -bt_navigator_navigate_through_poses_rclcpp_node: - ros__parameters: - use_sim_time: True + # plugin_lib_names is used to add custom BT plugins to the executor (vector of strings). + # Built-in plugins are added automatically + # plugin_lib_names: [] -bt_navigator_navigate_to_pose_rclcpp_node: - ros__parameters: - use_sim_time: True + error_code_names: + - compute_path_error_code + - follow_path_error_code controller_server: ros__parameters: - use_sim_time: True + enable_stamped_cmd_vel: True controller_frequency: 20.0 + costmap_update_timeout: 0.30 min_x_velocity_threshold: 0.001 min_y_velocity_threshold: 0.5 min_theta_velocity_threshold: 0.001 failure_tolerance: 0.3 - progress_checker_plugin: "progress_checker" + progress_checker_plugins: ["progress_checker"] goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker" controller_plugins: ["FollowPath"] + use_realtime_priority: false # Progress checker parameters progress_checker: @@ -97,48 +56,96 @@ controller_server: plugin: "nav2_controller::SimpleGoalChecker" xy_goal_tolerance: 0.25 yaw_goal_tolerance: 0.25 - # DWB parameters FollowPath: - plugin: "dwb_core::DWBLocalPlanner" - debug_trajectory_details: True - min_vel_x: 0.0 - min_vel_y: 0.0 - max_vel_x: 0.26 - max_vel_y: 0.0 - max_vel_theta: 1.0 - min_speed_xy: 0.0 - max_speed_xy: 0.26 - min_speed_theta: 0.0 - # Add high threshold velocity for turtlebot 3 issue. - # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75 - acc_lim_x: 2.5 - acc_lim_y: 0.0 - acc_lim_theta: 3.2 - decel_lim_x: -2.5 - decel_lim_y: 0.0 - decel_lim_theta: -3.2 - vx_samples: 20 - vy_samples: 5 - vtheta_samples: 20 - sim_time: 1.7 - linear_granularity: 0.05 - angular_granularity: 0.025 - transform_tolerance: 0.2 - xy_goal_tolerance: 0.25 - trans_stopped_velocity: 0.25 - short_circuit_trajectory_evaluation: True - stateful: True - critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"] - BaseObstacle.scale: 0.02 - PathAlign.scale: 32.0 - PathAlign.forward_point_distance: 0.1 - GoalAlign.scale: 24.0 - GoalAlign.forward_point_distance: 0.1 - PathDist.scale: 32.0 - GoalDist.scale: 24.0 - RotateToGoal.scale: 32.0 - RotateToGoal.slowing_factor: 5.0 - RotateToGoal.lookahead_time: -1.0 + plugin: "nav2_mppi_controller::MPPIController" + time_steps: 56 + model_dt: 0.05 + batch_size: 2000 + ax_max: 3.0 + ax_min: -3.0 + ay_max: 3.0 + ay_min: -3.0 + az_max: 3.5 + vx_std: 0.2 + vy_std: 0.2 + wz_std: 0.4 + vx_max: 0.5 + vx_min: -0.35 + vy_max: 0.5 + wz_max: 1.9 + iteration_count: 1 + prune_distance: 1.7 + transform_tolerance: 0.1 + temperature: 0.3 + gamma: 0.015 + motion_model: "DiffDrive" + visualize: true + regenerate_noises: true + TrajectoryVisualizer: + trajectory_step: 5 + time_step: 3 + AckermannConstraints: + min_turning_r: 0.2 + critics: [ + "ConstraintCritic", "CostCritic", "GoalCritic", + "GoalAngleCritic", "PathAlignCritic", "PathFollowCritic", + "PathAngleCritic", "PreferForwardCritic"] + ConstraintCritic: + enabled: true + cost_power: 1 + cost_weight: 4.0 + GoalCritic: + enabled: true + cost_power: 1 + cost_weight: 5.0 + threshold_to_consider: 1.4 + GoalAngleCritic: + enabled: true + cost_power: 1 + cost_weight: 3.0 + threshold_to_consider: 0.5 + PreferForwardCritic: + enabled: true + cost_power: 1 + cost_weight: 5.0 + threshold_to_consider: 0.5 + CostCritic: + enabled: true + cost_power: 1 + cost_weight: 3.81 + near_collision_cost: 253 + critical_cost: 300.0 + consider_footprint: false + collision_cost: 1000000.0 + near_goal_distance: 1.0 + trajectory_point_step: 2 + PathAlignCritic: + enabled: true + cost_power: 1 + cost_weight: 14.0 + max_path_occupancy_ratio: 0.05 + trajectory_point_step: 4 + threshold_to_consider: 0.5 + offset_from_furthest: 20 + use_path_orientations: false + PathFollowCritic: + enabled: true + cost_power: 1 + cost_weight: 5.0 + offset_from_furthest: 5 + threshold_to_consider: 1.4 + PathAngleCritic: + enabled: true + cost_power: 1 + cost_weight: 2.0 + offset_from_furthest: 4 + threshold_to_consider: 0.5 + max_angle_to_furthest: 1.0 + mode: 0 + # TwirlingCritic: + # enabled: true + # twirling_cost_power: 1 + # twirling_cost_weight: 10.0 local_costmap: local_costmap: @@ -147,7 +154,6 @@ local_costmap: publish_frequency: 2.0 global_frame: odom robot_base_frame: base_link - use_sim_time: True rolling_window: true width: 3 height: 3 @@ -157,7 +163,7 @@ local_costmap: inflation_layer: plugin: "nav2_costmap_2d::InflationLayer" cost_scaling_factor: 3.0 - inflation_radius: 0.55 + inflation_radius: 0.70 voxel_layer: plugin: "nav2_costmap_2d::VoxelLayer" enabled: True @@ -210,7 +216,6 @@ global_costmap: publish_frequency: 1.0 global_frame: map robot_base_frame: base_link - use_sim_time: True robot_radius: 0.22 resolution: 0.05 track_unknown_space: true @@ -221,23 +226,22 @@ global_costmap: inflation_layer: plugin: "nav2_costmap_2d::InflationLayer" cost_scaling_factor: 3.0 - inflation_radius: 0.55 + inflation_radius: 0.7 always_send_full_costmap: True planner_server: ros__parameters: expected_planner_frequency: 20.0 - use_sim_time: True planner_plugins: ["GridBased"] + costmap_update_timeout: 1.0 GridBased: - plugin: "nav2_navfn_planner/NavfnPlanner" + plugin: "nav2_navfn_planner::NavfnPlanner" tolerance: 0.5 use_astar: false allow_unknown: true smoother_server: ros__parameters: - use_sim_time: True smoother_plugins: ["simple_smoother"] simple_smoother: plugin: "nav2_smoother::SimpleSmoother" @@ -247,55 +251,178 @@ smoother_server: behavior_server: ros__parameters: - costmap_topic: local_costmap/costmap_raw - footprint_topic: local_costmap/published_footprint + enable_stamped_cmd_vel: True + local_costmap_topic: local_costmap/costmap_raw + global_costmap_topic: global_costmap/costmap_raw + local_footprint_topic: local_costmap/published_footprint + global_footprint_topic: global_costmap/published_footprint cycle_frequency: 10.0 behavior_plugins: ["spin", "backup", "drive_on_heading", "assisted_teleop", "wait"] spin: - plugin: "nav2_behaviors/Spin" + plugin: "nav2_behaviors::Spin" backup: - plugin: "nav2_behaviors/BackUp" + plugin: "nav2_behaviors::BackUp" drive_on_heading: - plugin: "nav2_behaviors/DriveOnHeading" + plugin: "nav2_behaviors::DriveOnHeading" wait: - plugin: "nav2_behaviors/Wait" + plugin: "nav2_behaviors::Wait" assisted_teleop: - plugin: "nav2_behaviors/AssistedTeleop" - global_frame: odom + plugin: "nav2_behaviors::AssistedTeleop" + local_frame: odom + global_frame: map robot_base_frame: base_link transform_tolerance: 0.1 - use_sim_time: true simulate_ahead_time: 2.0 max_rotational_vel: 1.0 min_rotational_vel: 0.4 rotational_acc_lim: 3.2 -robot_state_publisher: - ros__parameters: - use_sim_time: True - waypoint_follower: ros__parameters: - use_sim_time: True loop_rate: 20 stop_on_failure: false + action_server_result_timeout: 900.0 waypoint_task_executor_plugin: "wait_at_waypoint" wait_at_waypoint: plugin: "nav2_waypoint_follower::WaitAtWaypoint" enabled: True waypoint_pause_duration: 200 +route_server: + ros__parameters: + # The graph_filepath does not need to be specified since it going to be set by defaults in launch. + # If you'd rather set it in the yaml, remove the default "graph" value in the launch file(s). + # file & provide full path to map below. If graph config or launch default is provided, it is used + # graph_filepath: $(find-pkg-share nav2_route)/graphs/aws_graph.geojson + boundary_radius_to_achieve_node: 1.0 + radius_to_achieve_node: 2.0 + smooth_corners: true + operations: ["AdjustSpeedLimit", "ReroutingService", "CollisionMonitor"] + ReroutingService: + plugin: "nav2_route::ReroutingService" + AdjustSpeedLimit: + plugin: "nav2_route::AdjustSpeedLimit" + CollisionMonitor: + plugin: "nav2_route::CollisionMonitor" + max_collision_dist: 3.0 + edge_cost_functions: ["DistanceScorer", "CostmapScorer"] + DistanceScorer: + plugin: "nav2_route::DistanceScorer" + CostmapScorer: + plugin: "nav2_route::CostmapScorer" + velocity_smoother: ros__parameters: - use_sim_time: True + enable_stamped_cmd_vel: True smoothing_frequency: 20.0 + stamp_smoothed_velocity_with_smoothing_time: False scale_velocities: False feedback: "OPEN_LOOP" - max_velocity: [0.26, 0.0, 1.0] - min_velocity: [-0.26, 0.0, -1.0] + max_velocity: [0.5, 0.0, 2.0] + min_velocity: [-0.5, 0.0, -2.0] max_accel: [2.5, 0.0, 3.2] max_decel: [-2.5, 0.0, -3.2] odom_topic: "odom" odom_duration: 0.1 deadband_velocity: [0.0, 0.0, 0.0] velocity_timeout: 1.0 + +collision_monitor: + ros__parameters: + enable_stamped_cmd_vel: True + base_frame_id: "base_footprint" + odom_frame_id: "odom" + cmd_vel_in_topic: "cmd_vel_smoothed" + cmd_vel_out_topic: "cmd_vel" + state_topic: "collision_monitor_state" + transform_tolerance: 0.2 + source_timeout: 1.0 + base_shift_correction: True + stop_pub_timeout: 2.0 + # Polygons represent zone around the robot for "stop", "slowdown" and "limit" action types, + # and robot footprint for "approach" action type. + polygons: ["FootprintApproach"] + FootprintApproach: + type: "polygon" + action_type: "approach" + footprint_topic: "/local_costmap/published_footprint" + time_before_collision: 1.2 + simulation_time_step: 0.1 + min_points: 6 + visualize: False + enabled: True + observation_sources: ["scan"] + scan: + type: "scan" + topic: "scan" + min_height: 0.15 + max_height: 2.0 + enabled: True + +docking_server: + ros__parameters: + enable_stamped_cmd_vel: True + controller_frequency: 50.0 + initial_perception_timeout: 5.0 + wait_charge_timeout: 5.0 + dock_approach_timeout: 30.0 + undock_linear_tolerance: 0.05 + undock_angular_tolerance: 0.1 + max_retries: 3 + base_frame: "base_link" + fixed_frame: "odom" + dock_backwards: false + dock_prestaging_tolerance: 0.5 + + # Types of docks + dock_plugins: ['simple_charging_dock'] + simple_charging_dock: + plugin: 'opennav_docking::SimpleChargingDock' + docking_threshold: 0.05 + staging_x_offset: -0.7 + use_external_detection_pose: true + use_battery_status: false # true + use_stall_detection: false # true + + external_detection_timeout: 1.0 + external_detection_translation_x: -0.18 + external_detection_translation_y: 0.0 + external_detection_rotation_roll: -1.57 + external_detection_rotation_pitch: -1.57 + external_detection_rotation_yaw: 0.0 + filter_coef: 0.1 + + # Dock instances + # The following example illustrates configuring dock instances. + # docks: ['home_dock'] # Input your docks here + # home_dock: + # type: 'simple_charging_dock' + # frame: map + # pose: [0.0, 0.0, 0.0] + + controller: + k_phi: 3.0 + k_delta: 2.0 + v_linear_min: 0.15 + v_linear_max: 0.15 + use_collision_detection: true + costmap_topic: "local_costmap/costmap_raw" + footprint_topic: "local_costmap/published_footprint" + transform_tolerance: 0.1 + projection_time: 5.0 + simulation_step: 0.1 + dock_collision_threshold: 0.3 + +loopback_simulator: + ros__parameters: + base_frame_id: "base_footprint" + odom_frame_id: "odom" + map_frame_id: "map" + scan_frame_id: "base_scan" # tb4_loopback_simulator.launch.py remaps to 'rplidar_link' + update_duration: 0.02 + scan_range_min: 0.05 + scan_range_max: 30.0 + scan_angle_min: -3.1415 + scan_angle_max: 3.1415 + scan_angle_increment: 0.02617 + scan_use_inf: true \ No newline at end of file diff --git a/rtabmap_demos/params/turtlebot3_scan_nav2_params.yaml b/rtabmap_demos/params/turtlebot3_scan_nav2_params.yaml index 9c33bdb2..1acddc5b 100644 --- a/rtabmap_demos/params/turtlebot3_scan_nav2_params.yaml +++ b/rtabmap_demos/params/turtlebot3_scan_nav2_params.yaml @@ -1,85 +1,44 @@ -# Modified to use icp_odom frame +# Using icp_odom TF instead of odom bt_navigator: ros__parameters: - use_sim_time: True global_frame: map robot_base_frame: base_link odom_topic: /odom bt_loop_duration: 10 default_server_timeout: 20 wait_for_service_timeout: 1000 + action_server_result_timeout: 900.0 + navigators: ["navigate_to_pose", "navigate_through_poses"] + navigate_to_pose: + plugin: "nav2_bt_navigator::NavigateToPoseNavigator" + navigate_through_poses: + plugin: "nav2_bt_navigator::NavigateThroughPosesNavigator" # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults: # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2. - plugin_lib_names: - - nav2_compute_path_to_pose_action_bt_node - - nav2_compute_path_through_poses_action_bt_node - - nav2_smooth_path_action_bt_node - - nav2_follow_path_action_bt_node - - nav2_spin_action_bt_node - - nav2_wait_action_bt_node - - nav2_assisted_teleop_action_bt_node - - nav2_back_up_action_bt_node - - nav2_drive_on_heading_bt_node - - nav2_clear_costmap_service_bt_node - - nav2_is_stuck_condition_bt_node - - nav2_goal_reached_condition_bt_node - - nav2_goal_updated_condition_bt_node - - nav2_globally_updated_goal_condition_bt_node - - nav2_is_path_valid_condition_bt_node - - nav2_initial_pose_received_condition_bt_node - - nav2_reinitialize_global_localization_service_bt_node - - nav2_rate_controller_bt_node - - nav2_distance_controller_bt_node - - nav2_speed_controller_bt_node - - nav2_truncate_path_action_bt_node - - nav2_truncate_path_local_action_bt_node - - nav2_goal_updater_node_bt_node - - nav2_recovery_node_bt_node - - nav2_pipeline_sequence_bt_node - - nav2_round_robin_node_bt_node - - nav2_transform_available_condition_bt_node - - nav2_time_expired_condition_bt_node - - nav2_path_expiring_timer_condition - - nav2_distance_traveled_condition_bt_node - - nav2_single_trigger_bt_node - - nav2_goal_updated_controller_bt_node - - nav2_is_battery_low_condition_bt_node - - nav2_navigate_through_poses_action_bt_node - - nav2_navigate_to_pose_action_bt_node - - nav2_remove_passed_goals_action_bt_node - - nav2_planner_selector_bt_node - - nav2_controller_selector_bt_node - - nav2_goal_checker_selector_bt_node - - nav2_controller_cancel_bt_node - - nav2_path_longer_on_approach_bt_node - - nav2_wait_cancel_bt_node - - nav2_spin_cancel_bt_node - - nav2_back_up_cancel_bt_node - - nav2_assisted_teleop_cancel_bt_node - - nav2_drive_on_heading_cancel_bt_node - - nav2_is_battery_charging_condition_bt_node -bt_navigator_navigate_through_poses_rclcpp_node: - ros__parameters: - use_sim_time: True + # plugin_lib_names is used to add custom BT plugins to the executor (vector of strings). + # Built-in plugins are added automatically + # plugin_lib_names: [] -bt_navigator_navigate_to_pose_rclcpp_node: - ros__parameters: - use_sim_time: True + error_code_names: + - compute_path_error_code + - follow_path_error_code controller_server: ros__parameters: - use_sim_time: True + enable_stamped_cmd_vel: True controller_frequency: 20.0 + costmap_update_timeout: 0.30 min_x_velocity_threshold: 0.001 min_y_velocity_threshold: 0.5 min_theta_velocity_threshold: 0.001 failure_tolerance: 0.3 - progress_checker_plugin: "progress_checker" + progress_checker_plugins: ["progress_checker"] goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker" controller_plugins: ["FollowPath"] + use_realtime_priority: false # Progress checker parameters progress_checker: @@ -97,48 +56,96 @@ controller_server: plugin: "nav2_controller::SimpleGoalChecker" xy_goal_tolerance: 0.25 yaw_goal_tolerance: 0.25 - # DWB parameters FollowPath: - plugin: "dwb_core::DWBLocalPlanner" - debug_trajectory_details: True - min_vel_x: 0.0 - min_vel_y: 0.0 - max_vel_x: 0.26 - max_vel_y: 0.0 - max_vel_theta: 1.0 - min_speed_xy: 0.0 - max_speed_xy: 0.26 - min_speed_theta: 0.0 - # Add high threshold velocity for turtlebot 3 issue. - # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75 - acc_lim_x: 2.5 - acc_lim_y: 0.0 - acc_lim_theta: 3.2 - decel_lim_x: -2.5 - decel_lim_y: 0.0 - decel_lim_theta: -3.2 - vx_samples: 20 - vy_samples: 5 - vtheta_samples: 20 - sim_time: 1.7 - linear_granularity: 0.05 - angular_granularity: 0.025 - transform_tolerance: 0.2 - xy_goal_tolerance: 0.25 - trans_stopped_velocity: 0.25 - short_circuit_trajectory_evaluation: True - stateful: True - critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"] - BaseObstacle.scale: 0.02 - PathAlign.scale: 32.0 - PathAlign.forward_point_distance: 0.1 - GoalAlign.scale: 24.0 - GoalAlign.forward_point_distance: 0.1 - PathDist.scale: 32.0 - GoalDist.scale: 24.0 - RotateToGoal.scale: 32.0 - RotateToGoal.slowing_factor: 5.0 - RotateToGoal.lookahead_time: -1.0 + plugin: "nav2_mppi_controller::MPPIController" + time_steps: 56 + model_dt: 0.05 + batch_size: 2000 + ax_max: 3.0 + ax_min: -3.0 + ay_max: 3.0 + ay_min: -3.0 + az_max: 3.5 + vx_std: 0.2 + vy_std: 0.2 + wz_std: 0.4 + vx_max: 0.5 + vx_min: -0.35 + vy_max: 0.5 + wz_max: 1.9 + iteration_count: 1 + prune_distance: 1.7 + transform_tolerance: 0.1 + temperature: 0.3 + gamma: 0.015 + motion_model: "DiffDrive" + visualize: true + regenerate_noises: true + TrajectoryVisualizer: + trajectory_step: 5 + time_step: 3 + AckermannConstraints: + min_turning_r: 0.2 + critics: [ + "ConstraintCritic", "CostCritic", "GoalCritic", + "GoalAngleCritic", "PathAlignCritic", "PathFollowCritic", + "PathAngleCritic", "PreferForwardCritic"] + ConstraintCritic: + enabled: true + cost_power: 1 + cost_weight: 4.0 + GoalCritic: + enabled: true + cost_power: 1 + cost_weight: 5.0 + threshold_to_consider: 1.4 + GoalAngleCritic: + enabled: true + cost_power: 1 + cost_weight: 3.0 + threshold_to_consider: 0.5 + PreferForwardCritic: + enabled: true + cost_power: 1 + cost_weight: 5.0 + threshold_to_consider: 0.5 + CostCritic: + enabled: true + cost_power: 1 + cost_weight: 3.81 + near_collision_cost: 253 + critical_cost: 300.0 + consider_footprint: false + collision_cost: 1000000.0 + near_goal_distance: 1.0 + trajectory_point_step: 2 + PathAlignCritic: + enabled: true + cost_power: 1 + cost_weight: 14.0 + max_path_occupancy_ratio: 0.05 + trajectory_point_step: 4 + threshold_to_consider: 0.5 + offset_from_furthest: 20 + use_path_orientations: false + PathFollowCritic: + enabled: true + cost_power: 1 + cost_weight: 5.0 + offset_from_furthest: 5 + threshold_to_consider: 1.4 + PathAngleCritic: + enabled: true + cost_power: 1 + cost_weight: 2.0 + offset_from_furthest: 4 + threshold_to_consider: 0.5 + max_angle_to_furthest: 1.0 + mode: 0 + # TwirlingCritic: + # enabled: true + # twirling_cost_power: 1 + # twirling_cost_weight: 10.0 local_costmap: local_costmap: @@ -147,7 +154,6 @@ local_costmap: publish_frequency: 2.0 global_frame: icp_odom robot_base_frame: base_link - use_sim_time: True rolling_window: true width: 3 height: 3 @@ -157,7 +163,7 @@ local_costmap: inflation_layer: plugin: "nav2_costmap_2d::InflationLayer" cost_scaling_factor: 3.0 - inflation_radius: 0.55 + inflation_radius: 0.70 voxel_layer: plugin: "nav2_costmap_2d::VoxelLayer" enabled: True @@ -190,7 +196,6 @@ global_costmap: publish_frequency: 1.0 global_frame: map robot_base_frame: base_link - use_sim_time: True robot_radius: 0.22 resolution: 0.05 track_unknown_space: true @@ -215,23 +220,22 @@ global_costmap: inflation_layer: plugin: "nav2_costmap_2d::InflationLayer" cost_scaling_factor: 3.0 - inflation_radius: 0.55 + inflation_radius: 0.7 always_send_full_costmap: True planner_server: ros__parameters: expected_planner_frequency: 20.0 - use_sim_time: True planner_plugins: ["GridBased"] + costmap_update_timeout: 1.0 GridBased: - plugin: "nav2_navfn_planner/NavfnPlanner" + plugin: "nav2_navfn_planner::NavfnPlanner" tolerance: 0.5 use_astar: false allow_unknown: true smoother_server: ros__parameters: - use_sim_time: True smoother_plugins: ["simple_smoother"] simple_smoother: plugin: "nav2_smoother::SimpleSmoother" @@ -241,55 +245,178 @@ smoother_server: behavior_server: ros__parameters: - costmap_topic: local_costmap/costmap_raw - footprint_topic: local_costmap/published_footprint + enable_stamped_cmd_vel: True + local_costmap_topic: local_costmap/costmap_raw + global_costmap_topic: global_costmap/costmap_raw + local_footprint_topic: local_costmap/published_footprint + global_footprint_topic: global_costmap/published_footprint cycle_frequency: 10.0 behavior_plugins: ["spin", "backup", "drive_on_heading", "assisted_teleop", "wait"] spin: - plugin: "nav2_behaviors/Spin" + plugin: "nav2_behaviors::Spin" backup: - plugin: "nav2_behaviors/BackUp" + plugin: "nav2_behaviors::BackUp" drive_on_heading: - plugin: "nav2_behaviors/DriveOnHeading" + plugin: "nav2_behaviors::DriveOnHeading" wait: - plugin: "nav2_behaviors/Wait" + plugin: "nav2_behaviors::Wait" assisted_teleop: - plugin: "nav2_behaviors/AssistedTeleop" - global_frame: icp_odom + plugin: "nav2_behaviors::AssistedTeleop" + local_frame: icp_odom + global_frame: map robot_base_frame: base_link transform_tolerance: 0.1 - use_sim_time: true simulate_ahead_time: 2.0 max_rotational_vel: 1.0 min_rotational_vel: 0.4 rotational_acc_lim: 3.2 -robot_state_publisher: - ros__parameters: - use_sim_time: True - waypoint_follower: ros__parameters: - use_sim_time: True loop_rate: 20 stop_on_failure: false + action_server_result_timeout: 900.0 waypoint_task_executor_plugin: "wait_at_waypoint" wait_at_waypoint: plugin: "nav2_waypoint_follower::WaitAtWaypoint" enabled: True waypoint_pause_duration: 200 +route_server: + ros__parameters: + # The graph_filepath does not need to be specified since it going to be set by defaults in launch. + # If you'd rather set it in the yaml, remove the default "graph" value in the launch file(s). + # file & provide full path to map below. If graph config or launch default is provided, it is used + # graph_filepath: $(find-pkg-share nav2_route)/graphs/aws_graph.geojson + boundary_radius_to_achieve_node: 1.0 + radius_to_achieve_node: 2.0 + smooth_corners: true + operations: ["AdjustSpeedLimit", "ReroutingService", "CollisionMonitor"] + ReroutingService: + plugin: "nav2_route::ReroutingService" + AdjustSpeedLimit: + plugin: "nav2_route::AdjustSpeedLimit" + CollisionMonitor: + plugin: "nav2_route::CollisionMonitor" + max_collision_dist: 3.0 + edge_cost_functions: ["DistanceScorer", "CostmapScorer"] + DistanceScorer: + plugin: "nav2_route::DistanceScorer" + CostmapScorer: + plugin: "nav2_route::CostmapScorer" + velocity_smoother: ros__parameters: - use_sim_time: True + enable_stamped_cmd_vel: True smoothing_frequency: 20.0 + stamp_smoothed_velocity_with_smoothing_time: False scale_velocities: False feedback: "OPEN_LOOP" - max_velocity: [0.26, 0.0, 1.0] - min_velocity: [-0.26, 0.0, -1.0] + max_velocity: [0.5, 0.0, 2.0] + min_velocity: [-0.5, 0.0, -2.0] max_accel: [2.5, 0.0, 3.2] max_decel: [-2.5, 0.0, -3.2] odom_topic: "odom" odom_duration: 0.1 deadband_velocity: [0.0, 0.0, 0.0] velocity_timeout: 1.0 + +collision_monitor: + ros__parameters: + enable_stamped_cmd_vel: True + base_frame_id: "base_footprint" + odom_frame_id: "icp_odom" + cmd_vel_in_topic: "cmd_vel_smoothed" + cmd_vel_out_topic: "cmd_vel" + state_topic: "collision_monitor_state" + transform_tolerance: 0.2 + source_timeout: 1.0 + base_shift_correction: True + stop_pub_timeout: 2.0 + # Polygons represent zone around the robot for "stop", "slowdown" and "limit" action types, + # and robot footprint for "approach" action type. + polygons: ["FootprintApproach"] + FootprintApproach: + type: "polygon" + action_type: "approach" + footprint_topic: "/local_costmap/published_footprint" + time_before_collision: 1.2 + simulation_time_step: 0.1 + min_points: 6 + visualize: False + enabled: True + observation_sources: ["scan"] + scan: + type: "scan" + topic: "scan" + min_height: 0.15 + max_height: 2.0 + enabled: True + +docking_server: + ros__parameters: + enable_stamped_cmd_vel: True + controller_frequency: 50.0 + initial_perception_timeout: 5.0 + wait_charge_timeout: 5.0 + dock_approach_timeout: 30.0 + undock_linear_tolerance: 0.05 + undock_angular_tolerance: 0.1 + max_retries: 3 + base_frame: "base_link" + fixed_frame: "icp_odom" + dock_backwards: false + dock_prestaging_tolerance: 0.5 + + # Types of docks + dock_plugins: ['simple_charging_dock'] + simple_charging_dock: + plugin: 'opennav_docking::SimpleChargingDock' + docking_threshold: 0.05 + staging_x_offset: -0.7 + use_external_detection_pose: true + use_battery_status: false # true + use_stall_detection: false # true + + external_detection_timeout: 1.0 + external_detection_translation_x: -0.18 + external_detection_translation_y: 0.0 + external_detection_rotation_roll: -1.57 + external_detection_rotation_pitch: -1.57 + external_detection_rotation_yaw: 0.0 + filter_coef: 0.1 + + # Dock instances + # The following example illustrates configuring dock instances. + # docks: ['home_dock'] # Input your docks here + # home_dock: + # type: 'simple_charging_dock' + # frame: map + # pose: [0.0, 0.0, 0.0] + + controller: + k_phi: 3.0 + k_delta: 2.0 + v_linear_min: 0.15 + v_linear_max: 0.15 + use_collision_detection: true + costmap_topic: "local_costmap/costmap_raw" + footprint_topic: "local_costmap/published_footprint" + transform_tolerance: 0.1 + projection_time: 5.0 + simulation_step: 0.1 + dock_collision_threshold: 0.3 + +loopback_simulator: + ros__parameters: + base_frame_id: "base_footprint" + odom_frame_id: "icp_odom" + map_frame_id: "map" + scan_frame_id: "base_scan" # tb4_loopback_simulator.launch.py remaps to 'rplidar_link' + update_duration: 0.02 + scan_range_min: 0.05 + scan_range_max: 30.0 + scan_angle_min: -3.1415 + scan_angle_max: 3.1415 + scan_angle_increment: 0.02617 + scan_use_inf: true From d9cd332c530782e89d4404c8d3138e0aa5e44875 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Wed, 6 May 2026 22:56:41 -0700 Subject: [PATCH 49/56] Removing ament_target_dependencies (#1424) * Removing ament_target_dependencies * always update ros * fixing rtabmap_costmap_plugins * rtabmap rviz plugins Qt5 * fixing qt5 private, nav2 costmap 2d layers * backward compatibility humble * backward comaptibility pre lyrical * fixing rolling build * should work on rolling now * fixed odom * auto cancel previous jobs --- .devcontainer/humble/devcontainer.json | 16 +++- .devcontainer/jazzy/devcontainer.json | 2 +- .devcontainer/kilted/devcontainer.json | 16 +++- .github/workflows/ros2.yml | 8 ++ rtabmap_conversions/CMakeLists.txt | 32 +++++-- rtabmap_conversions/package.xml | 1 + rtabmap_costmap_plugins/CMakeLists.txt | 26 ++++-- rtabmap_odom/CMakeLists.txt | 41 ++++++--- rtabmap_rviz_plugins/CMakeLists.txt | 24 ++++- rtabmap_rviz_plugins/package.xml | 1 + rtabmap_slam/CMakeLists.txt | 51 ++++++++++- rtabmap_sync/CMakeLists.txt | 40 +++++--- rtabmap_util/CMakeLists.txt | 122 +++++++++++++++---------- rtabmap_util/package.xml | 1 + rtabmap_viz/CMakeLists.txt | 25 ++++- 15 files changed, 305 insertions(+), 101 deletions(-) diff --git a/.devcontainer/humble/devcontainer.json b/.devcontainer/humble/devcontainer.json index a1666586..96db65bc 100644 --- a/.devcontainer/humble/devcontainer.json +++ b/.devcontainer/humble/devcontainer.json @@ -22,6 +22,18 @@ }, "workspaceMount": "source=${localWorkspaceFolder},target=/home/vscode/ros2_ws/src/rtabmap_ros,type=bind", "workspaceFolder": "/home/vscode/ros2_ws", - "postCreateCommand": "echo 'Initialize: export MAKEFLAGS=\"-j6\" && colcon build --cmake-args -DRTABMAP_SYNC_MULTI_RGBD=ON -DCMAKE_BUILD_TYPE=Release --parallel-workers 2'" - //"runArgs": ["--privileged", "--runtime=nvidia", "--network=host"] + "postCreateCommand": "sudo rosdep init && rosdep update && sudo apt update && rosdep install --from-paths ~/ros2_ws/src/rtabmap_ros -i --skip-keys=rtabmap -y && echo 'Initialize: export MAKEFLAGS=\"-j6\" && colcon build --cmake-args -DRTABMAP_SYNC_MULTI_RGBD=ON -DCMAKE_BUILD_TYPE=Release --parallel-workers 2'", + "hostRequirements": { + "gpu": "optional" + }, + "runArgs": ["--privileged", + "--network=host", + "--gpus=all", + //"--runtime=nvidia", // uncommment this if rtabmap doesn't show up in nvidia-smi on the host computer + "--env=DISPLAY", + "--env=QT_X11_NO_MITSHM=1", + "--volume=/tmp/.X11-unix:/tmp/.X11-unix"], + "containerEnv": { + "NVIDIA_VISIBLE_DEVICES": "all" + } } diff --git a/.devcontainer/jazzy/devcontainer.json b/.devcontainer/jazzy/devcontainer.json index 3cff6f58..447b63a9 100644 --- a/.devcontainer/jazzy/devcontainer.json +++ b/.devcontainer/jazzy/devcontainer.json @@ -22,7 +22,7 @@ }, "workspaceMount": "source=${localWorkspaceFolder},target=/home/vscode/ros2_ws/src/rtabmap_ros,type=bind", "workspaceFolder": "/home/vscode/ros2_ws", - "postCreateCommand": "echo 'Initialize: export MAKEFLAGS=\"-j6\" && colcon build --cmake-args -DRTABMAP_SYNC_MULTI_RGBD=ON -DCMAKE_BUILD_TYPE=Release --parallel-workers 2'", + "postCreateCommand": "sudo rosdep init && rosdep update && sudo apt update && rosdep install --from-paths ~/ros2_ws/src/rtabmap_ros -i --skip-keys=rtabmap -y && echo 'Initialize: export MAKEFLAGS=\"-j6\" && colcon build --cmake-args -DRTABMAP_SYNC_MULTI_RGBD=ON -DCMAKE_BUILD_TYPE=Release --parallel-workers 2'", "hostRequirements": { "gpu": "optional" }, diff --git a/.devcontainer/kilted/devcontainer.json b/.devcontainer/kilted/devcontainer.json index 6176ee77..bc93b422 100644 --- a/.devcontainer/kilted/devcontainer.json +++ b/.devcontainer/kilted/devcontainer.json @@ -22,6 +22,18 @@ }, "workspaceMount": "source=${localWorkspaceFolder},target=/home/vscode/ros2_ws/src/rtabmap_ros,type=bind", "workspaceFolder": "/home/vscode/ros2_ws", - "postCreateCommand": "echo 'Initialize: export MAKEFLAGS=\"-j6\" && colcon build --cmake-args -DRTABMAP_SYNC_MULTI_RGBD=ON -DCMAKE_BUILD_TYPE=Release --parallel-workers 2'" - //"runArgs": ["--privileged", "--runtime=nvidia", "--network=host"] + "postCreateCommand": "sudo rosdep init && rosdep update && sudo apt update && rosdep install --from-paths ~/ros2_ws/src/rtabmap_ros -i --skip-keys=rtabmap -y && echo 'Initialize: export MAKEFLAGS=\"-j6\" && colcon build --cmake-args -DRTABMAP_SYNC_MULTI_RGBD=ON -DCMAKE_BUILD_TYPE=Release --parallel-workers 2'", + "hostRequirements": { + "gpu": "optional" + }, + "runArgs": ["--privileged", + "--network=host", + "--gpus=all", + //"--runtime=nvidia", // uncommment this if rtabmap doesn't show up in nvidia-smi on the host computer + "--env=DISPLAY", + "--env=QT_X11_NO_MITSHM=1", + "--volume=/tmp/.X11-unix:/tmp/.X11-unix"], + "containerEnv": { + "NVIDIA_VISIBLE_DEVICES": "all" + } } diff --git a/.github/workflows/ros2.yml b/.github/workflows/ros2.yml index 715f5f3f..538949a2 100644 --- a/.github/workflows/ros2.yml +++ b/.github/workflows/ros2.yml @@ -10,6 +10,10 @@ on: env: BUILD_TYPE: Release +concurrency: + group: ${{ github.workflow }}-${{ github.event.pull_request.number || github.ref }} + cancel-in-progress: ${{ github.event_name == 'pull_request' }} + jobs: build: name: Build ros2 ${{ matrix.ros_distro }} @@ -39,6 +43,10 @@ jobs: - uses: ros-tooling/setup-ros@v0.7 with: required-ros-distributions: ${{ matrix.ros_distro }} + - run: | + DEBIAN_FRONTEND=noninteractive + sudo apt update + sudo apt upgrade -y - run: | echo "repositories:\n introlab/rtabmap:\n type: git\n url: https://github.com/introlab/rtabmap.git\n version: master\n" > /tmp/deps.repos &&\ cat /tmp/deps.repos diff --git a/rtabmap_conversions/CMakeLists.txt b/rtabmap_conversions/CMakeLists.txt index 11b82f62..4dc41907 100644 --- a/rtabmap_conversions/CMakeLists.txt +++ b/rtabmap_conversions/CMakeLists.txt @@ -28,15 +28,30 @@ find_package(std_msgs REQUIRED) find_package(tf2 REQUIRED) find_package(tf2_eigen REQUIRED) find_package(tf2_geometry_msgs REQUIRED) +find_package(tf2_ros REQUIRED) find_package(RTABMap 0.23.5 REQUIRED) -include_directories( - ${CMAKE_CURRENT_SOURCE_DIR}/include -) - # libraries SET(Libraries + image_geometry::image_geometry + laser_geometry::laser_geometry + pcl_conversions::pcl_conversions + std_msgs::std_msgs + tf2::tf2 + tf2_eigen::tf2_eigen + tf2_geometry_msgs::tf2_geometry_msgs +) +SET(PublicLibraries + sensor_msgs::sensor_msgs + geometry_msgs::geometry_msgs + rtabmap_msgs::rtabmap_msgs + rclcpp::rclcpp + rtabmap::core + cv_bridge::cv_bridge + tf2_ros::tf2_ros +) +SET(AmentLibraries cv_bridge geometry_msgs image_geometry @@ -74,7 +89,11 @@ target_include_directories(rtabmap_conversions $ $ ) -ament_target_dependencies(rtabmap_conversions ${Libraries}) +if("$ENV{ROS_DISTRO}" STRLESS "lyrical") + ament_target_dependencies(rtabmap_conversions ${AmentLibraries}) +ELSE() + target_link_libraries(rtabmap_conversions PRIVATE ${Libraries} PUBLIC ${PublicLibraries}) +ENDIF() IF("$ENV{ROS_DISTRO}" STRLESS "iron") target_compile_definitions(rtabmap_conversions PUBLIC -DPRE_ROS_IRON -DPRE_ROS_KILTED) @@ -85,8 +104,7 @@ ENDIF() ############# ## Install ## ############# - -ament_export_dependencies(${Libraries}) +ament_export_dependencies(${AmentLibraries}) ament_export_include_directories(include) ament_export_targets(${PROJECT_NAME}) # To include downstream with targets ament_export_libraries(rtabmap_conversions) # To include downstream without targets diff --git a/rtabmap_conversions/package.xml b/rtabmap_conversions/package.xml index a0c94b20..33c3fb7a 100644 --- a/rtabmap_conversions/package.xml +++ b/rtabmap_conversions/package.xml @@ -28,6 +28,7 @@ tf2 tf2_eigen tf2_geometry_msgs + tf2_ros ament_cmake diff --git a/rtabmap_costmap_plugins/CMakeLists.txt b/rtabmap_costmap_plugins/CMakeLists.txt index 390fa065..d592a057 100644 --- a/rtabmap_costmap_plugins/CMakeLists.txt +++ b/rtabmap_costmap_plugins/CMakeLists.txt @@ -16,6 +16,12 @@ include_directories( ) SET(Libraries + pluginlib::pluginlib + rclcpp::rclcpp + nav2_costmap_2d::layers + visualization_msgs::visualization_msgs +) +SET(AmentLibraries pluginlib rclcpp nav2_costmap_2d @@ -35,11 +41,16 @@ target_include_directories(rtabmap_costmap_plugins $ ) -IF("$ENV{ROS_DISTRO}" STRLESS "jazzy") - target_compile_definitions(rtabmap_costmap_plugins PRIVATE -DPRE_ROS_JAZZY) +IF("$ENV{ROS_DISTRO}" STRLESS "lyrical") + IF("$ENV{ROS_DISTRO}" STRLESS "jazzy") + target_compile_definitions(rtabmap_costmap_plugins PRIVATE -DPRE_ROS_JAZZY) + ENDIF() + ament_target_dependencies(rtabmap_costmap_plugins ${AmentLibraries}) +ELSE() + target_link_libraries(rtabmap_costmap_plugins PRIVATE ${Libraries}) ENDIF() -ament_target_dependencies(rtabmap_costmap_plugins ${Libraries}) + # Causes the visibility macros to use dllexport rather than dllimport, # which is appropriate when building the dll but not consuming it. @@ -51,14 +62,17 @@ target_compile_definitions(rtabmap_costmap_plugins PUBLIC "PLUGINLIB__DISABLE_BO pluginlib_export_plugin_description_file(nav2_costmap_2d costmap_plugins.xml) add_executable(rtabmap_costmap_voxel_marker src/voxel_marker.cpp) -ament_target_dependencies(rtabmap_costmap_voxel_marker ${Libraries}) +IF("$ENV{ROS_DISTRO}" STRLESS "lyrical") + ament_target_dependencies(rtabmap_costmap_voxel_marker ${AmentLibraries}) +ELSE() + target_link_libraries(rtabmap_costmap_voxel_marker PRIVATE ${Libraries}) +ENDIF() set_target_properties(rtabmap_costmap_voxel_marker PROPERTIES OUTPUT_NAME "voxel_marker") ############# ## Install ## ############# - -ament_export_dependencies(${Libraries}) +ament_export_dependencies(${AmentLibraries}) ament_export_include_directories(include) ament_export_targets(${PROJECT_NAME}) # To include downstream with targets ament_export_libraries(rtabmap_costmap_plugins) # To include downstream without targets diff --git a/rtabmap_odom/CMakeLists.txt b/rtabmap_odom/CMakeLists.txt index 96130502..129810c1 100644 --- a/rtabmap_odom/CMakeLists.txt +++ b/rtabmap_odom/CMakeLists.txt @@ -41,7 +41,25 @@ include_directories( ) SET(Libraries + cv_bridge::cv_bridge + rclcpp_components::component + image_geometry::image_geometry + laser_geometry::laser_geometry + message_filters::message_filters + pcl_conversions::pcl_conversions +) +SET(PublicLibraries + rclcpp::rclcpp + sensor_msgs::sensor_msgs + nav_msgs::nav_msgs + rtabmap_conversions::rtabmap_conversions + rtabmap_msgs::rtabmap_msgs + rtabmap_util::rtabmap_util + rtabmap_sync::rtabmap_sync +) +SET(AmentLibraries cv_bridge + rclcpp_components image_geometry laser_geometry message_filters @@ -81,10 +99,13 @@ target_include_directories(rtabmap_odom ) add_library(rtabmap_odom_plugins SHARED ${rtabmap_odom_plugins_lib_src}) -ament_target_dependencies(rtabmap_odom ${Libraries}) -ament_target_dependencies(rtabmap_odom_plugins ${Libraries}) - -target_link_libraries(rtabmap_odom_plugins rtabmap_odom) +if("$ENV{ROS_DISTRO}" STRLESS "lyrical") + ament_target_dependencies(rtabmap_odom ${AmentLibraries}) +else() + target_link_libraries(rtabmap_odom PRIVATE ${Libraries} PUBLIC ${PublicLibraries}) + target_link_libraries(rtabmap_odom_plugins PUBLIC ${Libraries}) +endif() +target_link_libraries(rtabmap_odom_plugins PUBLIC rtabmap_odom) rclcpp_components_register_nodes(rtabmap_odom_plugins "rtabmap_odom::RGBDOdometry") rclcpp_components_register_nodes(rtabmap_odom_plugins "rtabmap_odom::StereoOdometry") @@ -92,25 +113,21 @@ rclcpp_components_register_nodes(rtabmap_odom_plugins "rtabmap_odom::ICPOdometry add_executable(rtabmap_rgbd_odometry src/RGBDOdometryNode.cpp) -ament_target_dependencies(rtabmap_rgbd_odometry ${Libraries}) -target_link_libraries(rtabmap_rgbd_odometry rtabmap_odom_plugins) +target_link_libraries(rtabmap_rgbd_odometry PRIVATE rtabmap_odom_plugins) set_target_properties(rtabmap_rgbd_odometry PROPERTIES OUTPUT_NAME "rgbd_odometry") add_executable(rtabmap_stereo_odometry src/StereoOdometryNode.cpp) -ament_target_dependencies(rtabmap_stereo_odometry ${Libraries}) -target_link_libraries(rtabmap_stereo_odometry rtabmap_odom_plugins) +target_link_libraries(rtabmap_stereo_odometry PRIVATE rtabmap_odom_plugins) set_target_properties(rtabmap_stereo_odometry PROPERTIES OUTPUT_NAME "stereo_odometry") add_executable(rtabmap_icp_odometry src/ICPOdometryNode.cpp) -ament_target_dependencies(rtabmap_icp_odometry ${Libraries}) -target_link_libraries(rtabmap_icp_odometry rtabmap_odom_plugins) +target_link_libraries(rtabmap_icp_odometry PRIVATE rtabmap_odom_plugins) set_target_properties(rtabmap_icp_odometry PROPERTIES OUTPUT_NAME "icp_odometry") ############# ## Install ## ############# - -ament_export_dependencies(${Libraries}) +ament_export_dependencies(${AmentLibraries}) ament_export_include_directories(include) ament_export_targets(${PROJECT_NAME}) # To include downstream with targets ament_export_libraries(rtabmap_odom rtabmap_odom_plugins) # To include downstream without targets diff --git a/rtabmap_rviz_plugins/CMakeLists.txt b/rtabmap_rviz_plugins/CMakeLists.txt index c26c7e1a..4e47a684 100644 --- a/rtabmap_rviz_plugins/CMakeLists.txt +++ b/rtabmap_rviz_plugins/CMakeLists.txt @@ -14,11 +14,13 @@ if(CMAKE_SYSTEM_PROCESSOR STREQUAL "aarch64") ) endif() +# Make sure it is first to prevent Qt6 from claiming the generic versionless targets first +find_package(Qt5 QUIET COMPONENTS Widgets) + find_package(ament_cmake_ros REQUIRED) find_package(pcl_conversions REQUIRED) find_package(pluginlib REQUIRED) find_package(rclcpp REQUIRED) -find_package(QT NAMES Qt5 QUIET COMPONENTS Widgets) find_package(rviz_common REQUIRED) find_package(rviz_rendering REQUIRED) find_package(rviz_default_plugins REQUIRED) @@ -33,6 +35,19 @@ include_directories( ) SET(Libraries + pcl_conversions::pcl_conversions + pluginlib::pluginlib + rclcpp::rclcpp + rviz_common::rviz_common + rviz_rendering::rviz_rendering + rviz_default_plugins::rviz_default_plugins + sensor_msgs::sensor_msgs + std_msgs::std_msgs + tf2::tf2 + rtabmap_conversions::rtabmap_conversions + rtabmap_msgs::rtabmap_msgs +) +SET(AmentLibraries pcl_conversions pluginlib rclcpp @@ -73,8 +88,11 @@ target_include_directories(rtabmap_rviz_plugins $ $ ) - -ament_target_dependencies(rtabmap_rviz_plugins ${Libraries}) +if("$ENV{ROS_DISTRO}" STRLESS "lyrical") + ament_target_dependencies(rtabmap_rviz_plugins ${AmentLibraries}) +ELSE() + target_link_libraries(rtabmap_rviz_plugins PRIVATE ${Libraries}) +ENDIF() # Causes the visibility macros to use dllexport rather than dllimport, # which is appropriate when building the dll but not consuming it. diff --git a/rtabmap_rviz_plugins/package.xml b/rtabmap_rviz_plugins/package.xml index 326ab8f2..6ab1a040 100644 --- a/rtabmap_rviz_plugins/package.xml +++ b/rtabmap_rviz_plugins/package.xml @@ -13,6 +13,7 @@ ament_cmake_ros ros_environment + qtbase5-private-dev pcl_conversions pluginlib diff --git a/rtabmap_slam/CMakeLists.txt b/rtabmap_slam/CMakeLists.txt index f53b7aaa..aa7df4b4 100644 --- a/rtabmap_slam/CMakeLists.txt +++ b/rtabmap_slam/CMakeLists.txt @@ -54,6 +54,22 @@ include_directories( # libraries SET(Libraries + cv_bridge::cv_bridge + geometry_msgs::geometry_msgs + nav_msgs::nav_msgs + rclcpp::rclcpp + rclcpp_components::component + sensor_msgs::sensor_msgs + std_msgs::std_msgs + std_srvs::std_srvs + tf2::tf2 + tf2_ros::tf2_ros + visualization_msgs::visualization_msgs + rtabmap_msgs::rtabmap_msgs + rtabmap_util::rtabmap_util + rtabmap_sync::rtabmap_sync +) +SET(AmentLibraries cv_bridge geometry_msgs nav_msgs @@ -88,6 +104,10 @@ MESSAGE(STATUS "WITH apriltag_msgs") ADD_DEFINITIONS("-DWITH_APRILTAG_MSGS") SET(Libraries ${Libraries} + apriltag_msgs::apriltag_msgs +) +SET(AmentLibraries + ${AmentLibraries} apriltag_msgs ) ENDIF(apriltag_msgs_FOUND) @@ -98,6 +118,10 @@ MESSAGE(STATUS "WITH aruco_msgs") ADD_DEFINITIONS("-DWITH_ARUCO_MSGS") SET(Libraries ${Libraries} + aruco_msgs::aruco_msgs +) +SET(AmentLibraries + ${AmentLibraries} aruco_msgs ) ENDIF(aruco_msgs_FOUND) @@ -108,6 +132,10 @@ MESSAGE(STATUS "WITH aruco_opencv_msgs") ADD_DEFINITIONS("-DWITH_ARUCO_OPENCV_MSGS") SET(Libraries ${Libraries} + aruco_opencv_msgs::aruco_opencv_msgs +) +SET(AmentLibraries + ${AmentLibraries} aruco_opencv_msgs ) ENDIF(aruco_opencv_msgs_FOUND) @@ -118,6 +146,10 @@ MESSAGE(STATUS "WITH aruco_markers_msgs") ADD_DEFINITIONS("-DWITH_ARUCO_MARKERS_MSGS") SET(Libraries ${Libraries} + aruco_markers_msgs::aruco_markers_msgs +) +SET(AmentLibraries + ${AmentLibraries} aruco_markers_msgs ) ENDIF(aruco_markers_msgs_FOUND) @@ -128,6 +160,10 @@ MESSAGE(STATUS "WITH ros2_aruco_interfaces") ADD_DEFINITIONS("-DWITH_ROS2_ARUCO_INTERFACES") SET(Libraries ${Libraries} + ros2_aruco_interfaces::ros2_aruco_interfaces +) +SET(AmentLibraries + ${AmentLibraries} ros2_aruco_interfaces ) ENDIF(ros2_aruco_interfaces_FOUND) @@ -138,6 +174,10 @@ MESSAGE(STATUS "WITH nav2_msgs") ADD_DEFINITIONS("-DWITH_NAV2_MSGS") SET(Libraries ${Libraries} + nav2_msgs::nav2_msgs +) +SET(AmentLibraries + ${AmentLibraries} nav2_msgs ) IF(${nav2_msgs_VERSION_MAJOR} EQUAL 0) @@ -157,20 +197,23 @@ target_include_directories(rtabmap_slam_plugins $ ) -ament_target_dependencies(rtabmap_slam_plugins ${Libraries}) +if("$ENV{ROS_DISTRO}" STRLESS "lyrical") + ament_target_dependencies(rtabmap_slam_plugins ${AmentLibraries}) +else() + target_link_libraries(rtabmap_slam_plugins PUBLIC ${Libraries}) +endif() rclcpp_components_register_nodes(rtabmap_slam_plugins "rtabmap_slam::CoreWrapper") add_executable(rtabmap_node src/CoreNode.cpp) -ament_target_dependencies(rtabmap_node ${Libraries}) -target_link_libraries(rtabmap_node rtabmap_slam_plugins) +target_link_libraries(rtabmap_node PRIVATE rtabmap_slam_plugins) set_target_properties(rtabmap_node PROPERTIES OUTPUT_NAME "rtabmap") ############# ## Install ## ############# -ament_export_dependencies(${Libraries}) +ament_export_dependencies(${AmentLibraries}) ament_export_include_directories(include) ament_export_targets(${PROJECT_NAME}) # To include downstream with targets ament_export_libraries(rtabmap_slam_plugins) # To include downstream without targets diff --git a/rtabmap_sync/CMakeLists.txt b/rtabmap_sync/CMakeLists.txt index 5bbf39fd..710997b5 100644 --- a/rtabmap_sync/CMakeLists.txt +++ b/rtabmap_sync/CMakeLists.txt @@ -55,6 +55,21 @@ include_directories( # libraries SET(Libraries + rclcpp_components::component +) +SET(PublicLibraries + cv_bridge::cv_bridge + rclcpp::rclcpp + message_filters::message_filters + image_transport::image_transport + sensor_msgs::sensor_msgs + nav_msgs::nav_msgs + rtabmap_msgs::rtabmap_msgs + rtabmap_conversions::rtabmap_conversions + diagnostic_updater::diagnostic_updater +) + +SET(AmentLibraries cv_bridge image_transport message_filters @@ -117,8 +132,14 @@ target_include_directories(rtabmap_sync $ ) -ament_target_dependencies(rtabmap_sync ${Libraries}) -ament_target_dependencies(rtabmap_sync_plugins ${Libraries}) + +if("$ENV{ROS_DISTRO}" STRLESS "lyrical") + ament_target_dependencies(rtabmap_sync ${AmentLibraries}) +ELSE() + target_link_libraries(rtabmap_sync PRIVATE ${Libraries} PUBLIC ${PublicLibraries}) + target_link_libraries(rtabmap_sync_plugins PUBLIC ${Libraries}) +ENDIF() +target_link_libraries(rtabmap_sync_plugins PUBLIC rtabmap_sync) rclcpp_components_register_nodes(rtabmap_sync_plugins "rtabmap_sync::RGBDSync") rclcpp_components_register_nodes(rtabmap_sync_plugins "rtabmap_sync::StereoSync") @@ -126,30 +147,25 @@ rclcpp_components_register_nodes(rtabmap_sync_plugins "rtabmap_sync::RGBSync") rclcpp_components_register_nodes(rtabmap_sync_plugins "rtabmap_sync::RGBDXSync") add_executable(rtabmap_rgbd_sync src/RGBDSyncNode.cpp) -ament_target_dependencies(rtabmap_rgbd_sync ${Libraries}) -target_link_libraries(rtabmap_rgbd_sync rtabmap_sync_plugins) +target_link_libraries(rtabmap_rgbd_sync PRIVATE rtabmap_sync_plugins) set_target_properties(rtabmap_rgbd_sync PROPERTIES OUTPUT_NAME "rgbd_sync") add_executable(rtabmap_rgbdx_sync src/RGBDXSyncNode.cpp) -ament_target_dependencies(rtabmap_rgbdx_sync ${Libraries}) -target_link_libraries(rtabmap_rgbdx_sync rtabmap_sync_plugins) +target_link_libraries(rtabmap_rgbdx_sync PRIVATE rtabmap_sync_plugins) set_target_properties(rtabmap_rgbdx_sync PROPERTIES OUTPUT_NAME "rgbdx_sync") add_executable(rtabmap_stereo_sync src/StereoSyncNode.cpp) -ament_target_dependencies(rtabmap_stereo_sync ${Libraries}) -target_link_libraries(rtabmap_stereo_sync rtabmap_sync_plugins) +target_link_libraries(rtabmap_stereo_sync PRIVATE rtabmap_sync_plugins) set_target_properties(rtabmap_stereo_sync PROPERTIES OUTPUT_NAME "stereo_sync") add_executable(rtabmap_rgb_sync src/RGBSyncNode.cpp) -ament_target_dependencies(rtabmap_rgb_sync ${Libraries}) -target_link_libraries(rtabmap_rgb_sync rtabmap_sync_plugins) +target_link_libraries(rtabmap_rgb_sync PRIVATE rtabmap_sync_plugins) set_target_properties(rtabmap_rgb_sync PROPERTIES OUTPUT_NAME "rgb_sync") ############# ## Install ## ############# - -ament_export_dependencies(${Libraries}) +ament_export_dependencies(${AmentLibraries}) ament_export_include_directories(include) ament_export_targets(${PROJECT_NAME}) # To include downstream with targets ament_export_libraries(rtabmap_sync rtabmap_sync_plugins) # To include downstream without targets diff --git a/rtabmap_util/CMakeLists.txt b/rtabmap_util/CMakeLists.txt index e9edf324..7b08cfb9 100644 --- a/rtabmap_util/CMakeLists.txt +++ b/rtabmap_util/CMakeLists.txt @@ -28,6 +28,8 @@ find_package(rtabmap_msgs REQUIRED) find_package(rtabmap_conversions REQUIRED) find_package(rtabmap_sync REQUIRED) +find_package(RTABMap COMPONENTS gui REQUIRED) + # Optional components find_package(octomap_msgs) find_package(grid_map_ros) @@ -38,6 +40,29 @@ include_directories( # libraries SET(Libraries + cv_bridge::cv_bridge + image_transport::image_transport + rclcpp_components::component + stereo_msgs::stereo_msgs + std_msgs::std_msgs + tf2::tf2 + tf2_geometry_msgs::tf2_geometry_msgs + tf2_ros::tf2_ros + laser_geometry::laser_geometry + image_geometry::image_geometry + message_filters::message_filters + rtabmap_msgs::rtabmap_msgs + rtabmap_sync::rtabmap_sync +) +SET(PublicLibraries + rclcpp::rclcpp + nav_msgs::nav_msgs + pcl_conversions::pcl_conversions + sensor_msgs::sensor_msgs + rtabmap_conversions::rtabmap_conversions +) + +SET(AmentLibraries cv_bridge image_transport rclcpp @@ -65,9 +90,10 @@ endif() ########### ## Build ## ########### - -SET(rtabmap_util_plugins_lib_src - src/MapsManager.cpp +SET(rtabmap_util_lib_src + src/MapsManager.cpp +) +SET(rtabmap_util_plugins_src src/nodelets/point_cloud_xyzrgb.cpp src/nodelets/point_cloud_xyz.cpp src/nodelets/disparity_to_depth.cpp @@ -83,16 +109,16 @@ SET(rtabmap_util_plugins_lib_src src/nodelets/map_assembler.cpp ) - # If octomap is found, add dependency IF(octomap_msgs_FOUND) MESSAGE(STATUS "WITH octomap_msgs") -include_directories( - ${octomap_msgs_INCLUDE_DIRS} +SET(PublicLibraries + octomap_msgs::octomap_msgs + ${PublicLibraries} ) -SET(Libraries +SET(AmentLibraries octomap_msgs - ${Libraries} + ${AmentLibraries} ) ADD_DEFINITIONS("-DWITH_OCTOMAP_MSGS") ENDIF(octomap_msgs_FOUND) @@ -100,36 +126,46 @@ ENDIF(octomap_msgs_FOUND) # If grid_map is found, add dependency IF(grid_map_ros_FOUND) MESSAGE(STATUS "WITH grid_map_ros") -include_directories( - ${grid_map_ros_INCLUDE_DIRS} +SET(PublicLibraries + grid_map_ros::grid_map_ros + ${PublicLibraries} ) -SET(Libraries +SET(AmentLibraries grid_map_ros - ${Libraries} + ${AmentLibraries} ) ENDIF(grid_map_ros_FOUND) ############################ ## Declare a cpp library ############################ -add_library(rtabmap_util_plugins SHARED - ${rtabmap_util_plugins_lib_src} +add_library(rtabmap_util SHARED + ${rtabmap_util_lib_src} ) -target_include_directories(rtabmap_util_plugins +add_library(rtabmap_util_plugins SHARED + ${rtabmap_util_plugins_src} +) +target_include_directories(rtabmap_util PUBLIC $ $ ) IF(octomap_msgs_FOUND) - target_compile_definitions(rtabmap_util_plugins PUBLIC -DWITH_OCTOMAP_MSGS) + target_compile_definitions(rtabmap_util PUBLIC -DWITH_OCTOMAP_MSGS) ENDIF(octomap_msgs_FOUND) IF(grid_map_ros_FOUND) - target_compile_definitions(rtabmap_util_plugins PUBLIC -DWITH_GRID_MAP_ROS) + target_compile_definitions(rtabmap_util PUBLIC -DWITH_GRID_MAP_ROS) ENDIF(grid_map_ros_FOUND) -ament_target_dependencies(rtabmap_util_plugins ${Libraries}) +if("$ENV{ROS_DISTRO}" STRLESS "lyrical") + ament_target_dependencies(rtabmap_util ${AmentLibraries}) +ELSE() + target_link_libraries(rtabmap_util PRIVATE ${Libraries} PUBLIC ${PublicLibraries}) + target_link_libraries(rtabmap_util_plugins PUBLIC ${Libraries}) +ENDIF() +target_link_libraries(rtabmap_util_plugins PUBLIC rtabmap_util) rclcpp_components_register_nodes(rtabmap_util_plugins "rtabmap_util::RGBDRelay") rclcpp_components_register_nodes(rtabmap_util_plugins "rtabmap_util::RGBDSplit") @@ -146,88 +182,73 @@ rclcpp_components_register_nodes(rtabmap_util_plugins "rtabmap_util::PointCloudA rclcpp_components_register_nodes(rtabmap_util_plugins "rtabmap_util::MapAssembler") add_executable(rtabmap_rgbd_relay src/RGBDRelayNode.cpp) -ament_target_dependencies(rtabmap_rgbd_relay ${Libraries}) -target_link_libraries(rtabmap_rgbd_relay rtabmap_util_plugins) +target_link_libraries(rtabmap_rgbd_relay PRIVATE rtabmap_util_plugins) set_target_properties(rtabmap_rgbd_relay PROPERTIES OUTPUT_NAME "rgbd_relay") add_executable(rtabmap_rgbd_split src/RGBDSplitNode.cpp) -ament_target_dependencies(rtabmap_rgbd_split ${Libraries}) -target_link_libraries(rtabmap_rgbd_split rtabmap_util_plugins) +target_link_libraries(rtabmap_rgbd_split PRIVATE rtabmap_util_plugins) set_target_properties(rtabmap_rgbd_split PROPERTIES OUTPUT_NAME "rgbd_split") #add_executable(rtabmap_map_optimizer src/MapOptimizerNode.cpp) -#ament_target_dependencies(rtabmap_map_optimizer ${Libraries}) #target_link_libraries(rtabmap_map_optimizer rtabmap_util_plugins) #set_target_properties(rtabmap_map_optimizer PROPERTIES OUTPUT_NAME "map_optimizer") add_executable(rtabmap_map_assembler src/MapAssemblerNode.cpp) -ament_target_dependencies(rtabmap_map_assembler ${Libraries}) -target_link_libraries(rtabmap_map_assembler rtabmap_util_plugins) +target_link_libraries(rtabmap_map_assembler PRIVATE 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) +target_link_libraries(rtabmap_imu_to_tf PRIVATE 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) +target_link_libraries(rtabmap_disparity_to_depth PRIVATE 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}) -target_link_libraries(rtabmap_lidar_deskewing rtabmap_util_plugins) +target_link_libraries(rtabmap_lidar_deskewing PRIVATE rtabmap_util_plugins) set_target_properties(rtabmap_lidar_deskewing PROPERTIES OUTPUT_NAME "lidar_deskewing") 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_util_plugins) +target_link_libraries(rtabmap_point_cloud_xyz PRIVATE rtabmap_util_plugins) set_target_properties(rtabmap_point_cloud_xyz PROPERTIES OUTPUT_NAME "point_cloud_xyz") add_executable(rtabmap_point_cloud_xyzrgb src/PointCloudXYZRGBNode.cpp) -ament_target_dependencies(rtabmap_point_cloud_xyzrgb ${Libraries}) -target_link_libraries(rtabmap_point_cloud_xyzrgb rtabmap_util_plugins) +target_link_libraries(rtabmap_point_cloud_xyzrgb PRIVATE rtabmap_util_plugins) set_target_properties(rtabmap_point_cloud_xyzrgb PROPERTIES OUTPUT_NAME "point_cloud_xyzrgb") add_executable(rtabmap_data_player src/DbPlayerNode.cpp) -ament_target_dependencies(rtabmap_data_player ${Libraries}) -target_link_libraries(rtabmap_data_player rtabmap_util_plugins) +target_link_libraries(rtabmap_data_player PRIVATE rtabmap_util_plugins) set_target_properties(rtabmap_data_player PROPERTIES OUTPUT_NAME "data_player") #add_executable(rtabmap_odom_msg_to_tf src/OdomMsgToTFNode.cpp) -#ament_target_dependencies(rtabmap_odom_msg_to_tf ${Libraries}) -#target_link_libraries(rtabmap_odom_msg_to_tf rtabmap_util_plugins) +#target_link_libraries(rtabmap_odom_msg_to_tf PRIVATE ${Libraries}) +#target_link_libraries(rtabmap_odom_msg_to_tf PRIVATE rtabmap_util_plugins) #set_target_properties(rtabmap_odom_msg_to_tf PROPERTIES OUTPUT_NAME "odom_msg_to_tf") add_executable(rtabmap_pointcloud_to_depthimage src/PointCloudToDepthImageNode.cpp) -ament_target_dependencies(rtabmap_pointcloud_to_depthimage ${Libraries}) -target_link_libraries(rtabmap_pointcloud_to_depthimage rtabmap_util_plugins) +target_link_libraries(rtabmap_pointcloud_to_depthimage PRIVATE rtabmap_util_plugins) set_target_properties(rtabmap_pointcloud_to_depthimage PROPERTIES OUTPUT_NAME "pointcloud_to_depthimage") add_executable(rtabmap_obstacles_detection src/ObstaclesDetectionNode.cpp) -ament_target_dependencies(rtabmap_obstacles_detection ${Libraries}) -target_link_libraries(rtabmap_obstacles_detection rtabmap_util_plugins) +target_link_libraries(rtabmap_obstacles_detection PRIVATE rtabmap_util_plugins) set_target_properties(rtabmap_obstacles_detection PROPERTIES OUTPUT_NAME "obstacles_detection") add_executable(rtabmap_point_cloud_aggregator src/PointCloudAggregatorNode.cpp) -ament_target_dependencies(rtabmap_point_cloud_aggregator ${Libraries}) -target_link_libraries(rtabmap_point_cloud_aggregator rtabmap_util_plugins) +target_link_libraries(rtabmap_point_cloud_aggregator PRIVATE rtabmap_util_plugins) set_target_properties(rtabmap_point_cloud_aggregator PROPERTIES OUTPUT_NAME "point_cloud_aggregator") add_executable(rtabmap_point_cloud_assembler src/PointCloudAssemblerNode.cpp) -ament_target_dependencies(rtabmap_point_cloud_assembler ${Libraries}) -target_link_libraries(rtabmap_point_cloud_assembler rtabmap_util_plugins) +target_link_libraries(rtabmap_point_cloud_assembler PRIVATE rtabmap_util_plugins) set_target_properties(rtabmap_point_cloud_assembler PROPERTIES OUTPUT_NAME "point_cloud_assembler") ############# ## Install ## ############# - -ament_export_dependencies(${Libraries}) +ament_export_dependencies(${AmentLibraries}) ament_export_include_directories(include) ament_export_targets(${PROJECT_NAME}) # To include downstream with targets -ament_export_libraries(rtabmap_util_plugins) # To include downstream without targets +ament_export_libraries(rtabmap_util rtabmap_util_plugins) # To include downstream without targets # Install Python executables @@ -244,6 +265,7 @@ install(PROGRAMS ) install(TARGETS + rtabmap_util rtabmap_util_plugins EXPORT ${PROJECT_NAME} ARCHIVE DESTINATION lib diff --git a/rtabmap_util/package.xml b/rtabmap_util/package.xml index 21d5df95..d356f8c9 100644 --- a/rtabmap_util/package.xml +++ b/rtabmap_util/package.xml @@ -27,6 +27,7 @@ tf2_geometry_msgs tf2_ros laser_geometry + image_geometry pcl_conversions pcl_ros message_filters diff --git a/rtabmap_viz/CMakeLists.txt b/rtabmap_viz/CMakeLists.txt index 60dcfd7c..add21b02 100644 --- a/rtabmap_viz/CMakeLists.txt +++ b/rtabmap_viz/CMakeLists.txt @@ -43,6 +43,18 @@ include_directories( # libraries SET(Libraries + cv_bridge::cv_bridge + geometry_msgs::geometry_msgs + rclcpp::rclcpp + std_msgs::std_msgs + std_srvs::std_srvs + nav_msgs::nav_msgs + rtabmap_msgs::rtabmap_msgs + rtabmap_sync::rtabmap_sync + tf2::tf2 + rtabmap::gui +) +SET(AmentLibraries cv_bridge geometry_msgs rclcpp @@ -52,6 +64,7 @@ SET(Libraries rtabmap_msgs rtabmap_sync tf2 + RTABMap ) ########### @@ -59,7 +72,11 @@ SET(Libraries ########### add_executable(rtabmap_viz src/GuiNode.cpp src/GuiWrapper.cpp src/PreferencesDialogROS.cpp include/${PROJECT_NAME}/PreferencesDialogROS.h) -ament_target_dependencies(rtabmap_viz ${Libraries}) +if("$ENV{ROS_DISTRO}" STRLESS "lyrical") + ament_target_dependencies(rtabmap_viz ${AmentLibraries}) +else() + target_link_libraries(rtabmap_viz PRIVATE ${Libraries}) +endif() SET_TARGET_PROPERTIES( rtabmap_viz PROPERTIES @@ -69,7 +86,11 @@ SET_TARGET_PROPERTIES( ) add_executable(rgbd_image_viewer src/RGBDImageViewerNode.cpp src/rgbd_image_viewer.cpp include/${PROJECT_NAME}/rgbd_image_viewer.hpp) -ament_target_dependencies(rgbd_image_viewer ${Libraries}) +if("$ENV{ROS_DISTRO}" STRLESS "lyrical") + ament_target_dependencies(rgbd_image_viewer ${AmentLibraries}) +else() + target_link_libraries(rgbd_image_viewer PRIVATE ${Libraries}) +endif() SET_TARGET_PROPERTIES( rgbd_image_viewer PROPERTIES From aec4f91f6150e82572bae1a5378fd9e5be924ca0 Mon Sep 17 00:00:00 2001 From: Manan Kharwar <44316521+manankharwar@users.noreply.github.com> Date: Thu, 14 May 2026 01:26:25 -0400 Subject: [PATCH 50/56] demo(turtlebot3): FusionCore + icp_odometry feedback loop (issue #1418 Option A) (#1419) * demo(turtlebot3): add FusionCore + icp_odometry feedback loop demo Adds a TurtleBot3 Gazebo Harmonic demo (ref issue #1418 Option A) where FusionCore (wheel + IMU UKF) and icp_odometry tighten each other in a feedback loop: FusionCore's stable odom frame serves as the scan-match initial guess via guess_frame_id, and the ICP result feeds back into FusionCore as a second velocity source. rtabmap SLAM consumes the ICP output for mapping/loop-closure. Nav2 uses /fusion/odom. Files added: launch/turtlebot3/fusioncore/turtlebot3_fusioncore_icp.launch.py launch/turtlebot3/fusioncore/turtlebot3_sim_fusioncore_icp_demo.launch.py launch/turtlebot3/fusioncore/README.md params/fusioncore_tb3.yaml params/fusioncore_tb3_bridge.yaml (TF-conflict-free bridge) params/turtlebot3_fusioncore_icp_nav2_params.yaml * demo(turtlebot3): add FusionCore demo entry to rtabmap_demos README * fix(fusioncore-demo): remove Odom/ResetCountdown from rtabmap, strip GPS params * docs(demos): add FusionCore icp_odometry demo GIF to README * fix(fusioncore-demo): add imu.frame_id override and docking_server params * fix(fusioncore-demo): remove docking_server from nav2 params * Fixing turtlebot3 demos on Jazzy * Updated humble * Working turtlebot3 demos on humble and jazzy * Fixed Twist->TwistStamped. Fixed rviz2 crashing because missing docking server * demo(turtlebot3): add FusionCore + icp_odometry feedback loop demo Adds a TurtleBot3 Gazebo Harmonic demo (ref issue #1418 Option A) where FusionCore (wheel + IMU UKF) and icp_odometry tighten each other in a feedback loop: FusionCore's stable odom frame serves as the scan-match initial guess via guess_frame_id, and the ICP result feeds back into FusionCore as a second velocity source. rtabmap SLAM consumes the ICP output for mapping/loop-closure. Nav2 uses /fusion/odom. Files added: launch/turtlebot3/fusioncore/turtlebot3_fusioncore_icp.launch.py launch/turtlebot3/fusioncore/turtlebot3_sim_fusioncore_icp_demo.launch.py launch/turtlebot3/fusioncore/README.md params/fusioncore_tb3.yaml params/fusioncore_tb3_bridge.yaml (TF-conflict-free bridge) params/turtlebot3_fusioncore_icp_nav2_params.yaml * demo(turtlebot3): add FusionCore demo entry to rtabmap_demos README * fix(fusioncore-demo): remove Odom/ResetCountdown from rtabmap, strip GPS params * docs(demos): add FusionCore icp_odometry demo GIF to README * fix(fusioncore-demo): add imu.frame_id override and docking_server params * fix(fusioncore-demo): remove docking_server from nav2 params * Fixing turtlebot3 demos on Jazzy * Updated humble * Fixed Twist->TwistStamped. Fixed rviz2 crashing because missing docking server * fix(fusioncore-demo): tune IMU noise for Gazebo sim, add real-hardware comments * docs(fusioncore-demo): add sim vs real hardware note to README --------- Co-authored-by: manankharwar Co-authored-by: matlabbe --- rtabmap_demos/README.md | 7 + .../launch/turtlebot3/fusioncore/README.md | 91 ++++++ .../turtlebot3_fusioncore_icp.launch.py | 165 +++++++++++ ...rtlebot3_sim_fusioncore_icp_demo.launch.py | 181 +++++++++++ rtabmap_demos/params/fusioncore_tb3.yaml | 63 ++++ .../params/fusioncore_tb3_bridge.yaml | 47 +++ ...turtlebot3_fusioncore_icp_nav2_params.yaml | 280 ++++++++++++++++++ 7 files changed, 834 insertions(+) create mode 100644 rtabmap_demos/launch/turtlebot3/fusioncore/README.md create mode 100644 rtabmap_demos/launch/turtlebot3/fusioncore/turtlebot3_fusioncore_icp.launch.py create mode 100644 rtabmap_demos/launch/turtlebot3/fusioncore/turtlebot3_sim_fusioncore_icp_demo.launch.py create mode 100644 rtabmap_demos/params/fusioncore_tb3.yaml create mode 100644 rtabmap_demos/params/fusioncore_tb3_bridge.yaml create mode 100644 rtabmap_demos/params/turtlebot3_fusioncore_icp_nav2_params.yaml diff --git a/rtabmap_demos/README.md b/rtabmap_demos/README.md index 4f37769c..dfedb2a8 100644 --- a/rtabmap_demos/README.md +++ b/rtabmap_demos/README.md @@ -8,6 +8,7 @@ + [Turtlebot3 Nav2 and RGB-D SLAM](#turtlebot3-nav2-and-rgb-d-slam) + [Turtlebot3 Nav2, 2D LiDAR and RGB-D SLAM](#turtlebot3-nav2-2d-lidar-and-rgb-d-slam) + [Turtlebot3 Nav2, Fake 2D LiDAR and RGB-D SLAM](#turtlebot3-nav2-fake-2d-lidar-and-rgb-d-slam) ++ [Turtlebot3 Nav2, 2D LiDAR SLAM with FusionCore (IMU + wheel UKF)](#turtlebot3-nav2-2d-lidar-slam-with-fusioncore-imu--wheel-ukf) + [Champ Quadruped Nav2, Elevation Map and VSLAM](#champ-quadruped-nav2-elevation-map-and-vslam) + [Clearpath Husky Nav2, 2D LiDAR and RGB-D SLAM](#clearpath-husky-nav2-2d-lidar-and-rgb-d-slam) + [Clearpath Husky Nav2, 3D LiDAR and RGB-D SLAM](#clearpath-husky-nav2-3d-lidar-and-rgb-d-slam) @@ -59,6 +60,12 @@ * Yellow: The map. ![Peek 2025-07-04 16-57](https://github.com/user-attachments/assets/8869cf57-35a1-4236-bdab-151b88ae2ea1) +### Turtlebot3 Nav2, 2D LiDAR SLAM with FusionCore (IMU + wheel UKF) +[turtlebot3_sim_fusioncore_icp_demo.launch.py](https://github.com/introlab/rtabmap_ros/blob/ros2/rtabmap_demos/launch/turtlebot3/fusioncore/turtlebot3_sim_fusioncore_icp_demo.launch.py) (Jazzy + Gazebo Harmonic) + +FusionCore (wheel + IMU UKF) and `icp_odometry` run in a feedback loop: FusionCore's stable `odom` frame seeds scan matching via `guess_frame_id`, and the ICP result feeds back into FusionCore as a second velocity source. See [README](launch/turtlebot3/fusioncore/README.md) for architecture details. + +![FusionCore icp_odometry demo](https://github.com/user-attachments/assets/e1e07cfb-74e0-48b9-9bfd-32b68ee5a6ef) ### Champ Quadruped Nav2, Elevation Map and VSLAM [champ_sim_vslam.launch.py](https://github.com/introlab/rtabmap_ros/blob/ros2/rtabmap_demos/launch/champ/champ_sim_vslam.launch.py) diff --git a/rtabmap_demos/launch/turtlebot3/fusioncore/README.md b/rtabmap_demos/launch/turtlebot3/fusioncore/README.md new file mode 100644 index 00000000..ec5f0a57 --- /dev/null +++ b/rtabmap_demos/launch/turtlebot3/fusioncore/README.md @@ -0,0 +1,91 @@ +# FusionCore + icp_odometry: TurtleBot3 Gazebo Demo + +This demo shows a feedback loop between [FusionCore](https://github.com/manankharwar/fusioncore) and rtabmap's `icp_odometry` where each node tightens the other. + +## Architecture + +``` +/imu ──────────────────────┐ +/odom (wheel) ──────────────┤──→ FusionCore (UKF) +/rtabmap/icp_odometry ──────┘ │ + ↑ │ publishes: odom → base_footprint TF + │ │ /fusion/odom + │ guess_frame_id: odom│ + └────── icp_odometry ←───────┘ + │ (publish_tf: false) + │ + └──→ /rtabmap/icp_odometry ──→ rtabmap SLAM ──→ map → odom TF +``` + +**What each node contributes:** + +| Node | Input | Provides | +|---|---|---| +| FusionCore | wheels + IMU | stable `odom` frame, continuous state at 100 Hz | +| icp_odometry | `/scan` + FusionCore's `odom` as initial guess | scan-level pose corrections | +| FusionCore encoder2 | icp_odometry output | tighter velocity corrections from ICP | +| rtabmap SLAM | icp_odometry output | global map, loop closures | + +FusionCore gives `icp_odometry` a stable initial guess via `guess_frame_id: odom`. +Better initial guesses mean scan matching succeeds more often and with lower error. +The ICP result feeds back into FusionCore as a second velocity source (`encoder2`), +tightening the state estimate further. `Odom/ResetCountdown: 1` lets the system +auto-recover if ICP loses tracking. + +## Simulation vs real hardware + +In Gazebo, the DiffDrive plugin produces near-perfect wheel velocities with no slip or +encoder noise, while the simulated MPU9250 injects Gaussian noise. FusionCore fusing +both means the noisy IMU slightly degrades what is already a perfect odometry source, +so the `map → odom` correction on each scan update will be slightly larger than in the +standard wheel-odometry-only demo. On real hardware this completely inverts: wheel +encoders accumulate slip, terrain variation, and mechanical error that dwarfs IMU noise, +and fusion pays off measurably. The sim-tuned IMU noise values in `fusioncore_tb3.yaml` +(`gyro_noise: 0.002`, `accel_noise: 0.02`) reduce unnecessary filter uncertainty in +simulation; real MPU9250 users should use the hardware spec values noted in that file. + +## Quick start + +```bash +export TURTLEBOT3_MODEL=waffle + +ros2 launch rtabmap_demos turtlebot3_sim_fusioncore_icp_demo.launch.py +``` + +Optional arguments: + +```bash +# Localization mode (requires saved map from a previous mapping run) +ros2 launch rtabmap_demos turtlebot3_sim_fusioncore_icp_demo.launch.py localization:=true + +# Different Gazebo world +ros2 launch rtabmap_demos turtlebot3_sim_fusioncore_icp_demo.launch.py world:=house +``` + +## Prerequisites + +```bash +sudo apt install ros-jazzy-fusioncore-ros ros-jazzy-turtlebot3-gazebo ros-jazzy-rtabmap-ros ros-jazzy-nav2-bringup +export TURTLEBOT3_MODEL=waffle +``` + +## Files + +| File | Purpose | +|---|---| +| `turtlebot3_sim_fusioncore_icp_demo.launch.py` | Complete demo: Gazebo + FusionCore + rtabmap + Nav2 | +| `turtlebot3_fusioncore_icp.launch.py` | Core only: FusionCore + icp_odometry + rtabmap (no Gazebo) | +| `../../params/fusioncore_tb3.yaml` | FusionCore config for TB3 Waffle | +| `../../params/turtlebot3_fusioncore_icp_nav2_params.yaml` | Nav2 config using `/fusion/odom` | + +## Topic and TF summary + +| Topic / TF | Publisher | Subscribers | +|---|---|---| +| `/imu` | Gazebo | FusionCore | +| `/odom` | Gazebo (wheel) | FusionCore | +| `/scan` | Gazebo (lidar) | icp_odometry, rtabmap | +| `/rtabmap/icp_odometry` | icp_odometry | FusionCore (encoder2), rtabmap | +| `/fusion/odom` | FusionCore | Nav2 | +| TF `odom → base_footprint` | FusionCore | icp_odometry (guess), Nav2 | +| TF `map → odom` | rtabmap | Nav2 | diff --git a/rtabmap_demos/launch/turtlebot3/fusioncore/turtlebot3_fusioncore_icp.launch.py b/rtabmap_demos/launch/turtlebot3/fusioncore/turtlebot3_fusioncore_icp.launch.py new file mode 100644 index 00000000..ae70a95c --- /dev/null +++ b/rtabmap_demos/launch/turtlebot3/fusioncore/turtlebot3_fusioncore_icp.launch.py @@ -0,0 +1,165 @@ +""" +FusionCore + icp_odometry feedback loop for TurtleBot3. + +Architecture (Option A from rtabmap_ros issue #1418): + + FusionCore (wheels + IMU) + |-- publishes: odom -> base_footprint TF, /fusion/odom + |-- provides initial pose guess to icp_odometry via guess_frame_id + + icp_odometry (/scan) + |-- guess_frame_id: odom (uses FusionCore's stable odom as scan match seed) + |-- publish_tf: false (FusionCore owns the odom TF) + |-- publishes: /rtabmap/icp_odometry + + FusionCore encoder2 + |-- topic: /rtabmap/icp_odometry + |-- ICP corrections fed back as a second velocity source + + rtabmap SLAM + |-- subscribes to /rtabmap/icp_odometry for mapping + |-- Odom/ResetCountdown: 1 for auto-recovery if ICP loses tracking + |-- publishes: map -> odom TF +""" + +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import (DeclareLaunchArgument, EmitEvent, + OpaqueFunction, RegisterEventHandler, TimerAction) +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import LifecycleNode, Node +from launch_ros.event_handlers import OnStateTransition +from launch_ros.events.lifecycle import ChangeState +from lifecycle_msgs.msg import Transition + + +def launch_setup(context, *args, **kwargs): + use_sim_time = LaunchConfiguration('use_sim_time') + localization = LaunchConfiguration('localization').perform(context) + localization = localization in ('True', 'true') + + pkg_demos = get_package_share_directory('rtabmap_demos') + fusioncore_config = os.path.join(pkg_demos, 'params', 'fusioncore_tb3.yaml') + + # ── FusionCore lifecycle node ───────────────────────────────────────────── + fc = LifecycleNode( + package='fusioncore_ros', + executable='fusioncore_node', + name='fusioncore', + namespace='', + output='screen', + parameters=[fusioncore_config, {'use_sim_time': use_sim_time}], + remappings=[ + ('/imu/data', '/imu'), # TB3 Gazebo IMU topic + ('/odom/wheels', '/odom'), # TB3 Gazebo wheel odometry topic + ], + ) + + # Wait 2 s for node to spin up, then configure + configure = TimerAction( + period=2.0, + actions=[EmitEvent(event=ChangeState( + lifecycle_node_matcher=lambda a: a is fc, + transition_id=Transition.TRANSITION_CONFIGURE, + ))], + ) + + # As soon as configuring -> inactive, activate + activate = RegisterEventHandler(OnStateTransition( + target_lifecycle_node=fc, + start_state='configuring', + goal_state='inactive', + entities=[EmitEvent(event=ChangeState( + lifecycle_node_matcher=lambda a: a is fc, + transition_id=Transition.TRANSITION_ACTIVATE, + ))], + )) + + # ── icp_odometry ────────────────────────────────────────────────────────── + icp_parameters = { + 'frame_id': 'base_footprint', + 'odom_frame_id': 'odom', + 'guess_frame_id': 'odom', + 'publish_tf': False, + 'publish_null_when_lost': False, + 'use_sim_time': use_sim_time, + 'Reg/Strategy': '1', + 'Reg/Force3DoF': 'true', + 'Odom/ResetCountdown': '1', + 'RGBD/NeighborLinkRefining': 'True', + 'Grid/RangeMin': '0.2', + } + + icp_odometry_node = Node( + package='rtabmap_odom', + executable='icp_odometry', + output='screen', + parameters=[icp_parameters], + remappings=[ + ('scan', '/scan'), + ('odom', '/rtabmap/icp_odometry'), + ], + ) + + # ── rtabmap SLAM ────────────────────────────────────────────────────────── + slam_parameters = { + 'frame_id': 'base_footprint', + 'odom_frame_id': 'odom', + 'use_sim_time': use_sim_time, + 'subscribe_depth': False, + 'subscribe_rgb': False, + 'subscribe_scan': True, + 'approx_sync': True, + 'use_action_for_goal': True, + 'Reg/Strategy': '1', + 'Reg/Force3DoF': 'true', + 'RGBD/NeighborLinkRefining': 'True', + 'Grid/RangeMin': '0.2', + 'Optimizer/GravitySigma': '0', + } + + if localization: + slam_parameters['Mem/IncrementalMemory'] = 'False' + slam_parameters['Mem/InitWMWithAllNodes'] = 'True' + + rtabmap_args = [] if localization else ['-d'] + + rtabmap_node = Node( + package='rtabmap_slam', + executable='rtabmap', + output='screen', + parameters=[slam_parameters], + remappings=[ + ('scan', '/scan'), + ('odom', '/rtabmap/icp_odometry'), + ], + arguments=rtabmap_args, + ) + + rtabmap_viz_node = Node( + package='rtabmap_viz', + executable='rtabmap_viz', + output='screen', + parameters=[slam_parameters, {'odometry_node_name': 'icp_odometry'}], + remappings=[ + ('scan', '/scan'), + ('odom', '/rtabmap/icp_odometry'), + ], + ) + + return [fc, configure, activate, icp_odometry_node, rtabmap_node, rtabmap_viz_node] + + +def generate_launch_description(): + return LaunchDescription([ + DeclareLaunchArgument( + 'use_sim_time', default_value='true', + description='Use simulation (Gazebo) clock if true'), + + DeclareLaunchArgument( + 'localization', default_value='false', + description='Launch in localization mode (requires existing map)'), + + OpaqueFunction(function=launch_setup), + ]) diff --git a/rtabmap_demos/launch/turtlebot3/fusioncore/turtlebot3_sim_fusioncore_icp_demo.launch.py b/rtabmap_demos/launch/turtlebot3/fusioncore/turtlebot3_sim_fusioncore_icp_demo.launch.py new file mode 100644 index 00000000..064531b5 --- /dev/null +++ b/rtabmap_demos/launch/turtlebot3/fusioncore/turtlebot3_sim_fusioncore_icp_demo.launch.py @@ -0,0 +1,181 @@ +""" +Complete TurtleBot3 demo: Gazebo (new) + FusionCore + icp_odometry + Nav2. + +Launches in order: + 1. Gazebo Harmonic (via ros_gz_sim) with turtlebot3_world + 2. Robot state publisher + spawn TurtleBot3 + 3. Custom ros_gz_bridge WITHOUT the odom TF (FusionCore owns odom->base_footprint) + 4. FusionCore lifecycle node (configure -> activate automatically) + 5. icp_odometry using FusionCore's odom frame as scan-match initial guess + 6. rtabmap SLAM subscribing to icp_odometry output + 7. Nav2 using /fusion/odom + +Usage: + export TURTLEBOT3_MODEL=waffle + ros2 launch rtabmap_demos turtlebot3_sim_fusioncore_icp_demo.launch.py + + # Localization mode (requires existing map): + ros2 launch rtabmap_demos turtlebot3_sim_fusioncore_icp_demo.launch.py localization:=true + + # Different world: + ros2 launch rtabmap_demos turtlebot3_sim_fusioncore_icp_demo.launch.py world:=house + +Note on TF ownership: + The standard turtlebot3_gazebo bridge forwards the DiffDrive TF to ROS, which + conflicts with FusionCore's odom->base_footprint. This demo uses a custom bridge + config (fusioncore_tb3_bridge.yaml) that suppresses the Gazebo TF entry. + FusionCore is the sole publisher of odom->base_footprint. +""" + +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import (AppendEnvironmentVariable, DeclareLaunchArgument, + IncludeLaunchDescription, OpaqueFunction) +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + + +def launch_setup(context, *args, **kwargs): + if 'TURTLEBOT3_MODEL' not in os.environ: + os.environ['TURTLEBOT3_MODEL'] = 'waffle' + + tb3_model = os.environ['TURTLEBOT3_MODEL'] + pkg_tb3_gz = get_package_share_directory('turtlebot3_gazebo') + pkg_ros_gz = get_package_share_directory('ros_gz_sim') + pkg_nav2 = get_package_share_directory('nav2_bringup') + pkg_demos = get_package_share_directory('rtabmap_demos') + + world_name = LaunchConfiguration('world').perform(context) + world_file = os.path.join(pkg_tb3_gz, 'worlds', f'turtlebot3_{world_name}.world') + + # ── Gazebo server + client ──────────────────────────────────────────────── + gz_server = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(pkg_ros_gz, 'launch', 'gz_sim.launch.py')), + launch_arguments={ + 'gz_args': f'-r -s -v2 {world_file}', + 'on_exit_shutdown': 'true', + }.items(), + ) + gz_client = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(pkg_ros_gz, 'launch', 'gz_sim.launch.py')), + launch_arguments={'gz_args': '-g -v2', 'on_exit_shutdown': 'true'}.items(), + ) + + # ── Robot state publisher ───────────────────────────────────────────────── + robot_state_publisher = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(pkg_tb3_gz, 'launch', 'robot_state_publisher.launch.py')), + launch_arguments={'use_sim_time': 'true'}.items(), + ) + + # ── Spawn TurtleBot3 (entity only, no bridge) ───────────────────────────── + urdf_path = os.path.join(pkg_tb3_gz, 'models', + f'turtlebot3_{tb3_model}', 'model.sdf') + spawn_robot = Node( + package='ros_gz_sim', + executable='create', + arguments=[ + '-name', tb3_model, + '-file', urdf_path, + '-x', LaunchConfiguration('x_pose'), + '-y', LaunchConfiguration('y_pose'), + '-z', '0.01', + ], + output='screen', + ) + + # ── Custom bridge: all topics EXCEPT odom TF ────────────────────────────── + # FusionCore publishes odom->base_footprint; suppress the Gazebo DiffDrive TF. + bridge_config = os.path.join(pkg_demos, 'params', 'fusioncore_tb3_bridge.yaml') + bridge = Node( + package='ros_gz_bridge', + executable='parameter_bridge', + arguments=['--ros-args', '-p', f'config_file:={bridge_config}'], + output='screen', + ) + + # Camera image bridge (waffle only) + image_bridge = Node( + package='ros_gz_image', + executable='image_bridge', + arguments=['/camera/image_raw'], + output='screen', + ) + + # ── FusionCore + icp_odometry + rtabmap ─────────────────────────────────── + fusioncore_icp = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(pkg_demos, 'launch', 'turtlebot3', 'fusioncore', + 'turtlebot3_fusioncore_icp.launch.py')), + launch_arguments=[ + ('localization', LaunchConfiguration('localization')), + ('use_sim_time', 'true'), + ], + ) + + # ── Nav2 ────────────────────────────────────────────────────────────────── + nav2 = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(pkg_nav2, 'launch', 'navigation_launch.py')), + launch_arguments=[ + ('use_sim_time', 'true'), + ('params_file', PathJoinSubstitution( + [FindPackageShare('rtabmap_demos'), 'params', + 'turtlebot3_fusioncore_icp_nav2_params.yaml'])), + ], + ) + + rviz = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(pkg_nav2, 'launch', 'rviz_launch.py')), + ) + + set_gz_resource_path = AppendEnvironmentVariable( + 'GZ_SIM_RESOURCE_PATH', + os.path.join(pkg_tb3_gz, 'models'), + ) + + nodes = [ + set_gz_resource_path, + gz_server, + gz_client, + robot_state_publisher, + spawn_robot, + bridge, + fusioncore_icp, + nav2, + rviz, + ] + if tb3_model == 'waffle': + nodes.insert(6, image_bridge) + + return nodes + + +def generate_launch_description(): + return LaunchDescription([ + DeclareLaunchArgument( + 'localization', default_value='false', + description='Launch in localization mode.'), + + DeclareLaunchArgument( + 'world', default_value='world', + choices=['world', 'house', 'dqn_stage1', 'dqn_stage2', + 'dqn_stage3', 'dqn_stage4'], + description='Turtlebot3 Gazebo world.'), + + DeclareLaunchArgument( + 'x_pose', default_value='-2.0', + description='Initial X position in Gazebo.'), + + DeclareLaunchArgument( + 'y_pose', default_value='-0.5', + description='Initial Y position in Gazebo.'), + + OpaqueFunction(function=launch_setup), + ]) diff --git a/rtabmap_demos/params/fusioncore_tb3.yaml b/rtabmap_demos/params/fusioncore_tb3.yaml new file mode 100644 index 00000000..7f8f476a --- /dev/null +++ b/rtabmap_demos/params/fusioncore_tb3.yaml @@ -0,0 +1,63 @@ +# FusionCore config for TurtleBot3 Waffle (Gazebo Harmonic) +# +# Platform: TurtleBot3 Waffle (simulated) +# IMU: MPU9250 (9-axis, magnetometer unreliable in sim) +# GPS: None (indoor / simulation) +# LiDAR: HLS-LFCD2 via /scan (used by icp_odometry, not fused here directly) +# encoder2: rtabmap icp_odometry output on /rtabmap/icp_odometry +# +# Architecture: FusionCore (wheels + IMU) provides the odom frame. +# icp_odometry uses that odom frame as its initial scan-matching guess +# (guess_frame_id: odom). The ICP output feeds back into FusionCore +# as a second velocity source (encoder2). Each tightens the other. + +fusioncore: + ros__parameters: + base_frame: base_footprint + odom_frame: odom + publish_rate: 100.0 + publish.force_2d: true + + # Gazebo Harmonic prefixes sensor frames with the model name (waffle/imu_link/tb3_imu). + # Override to the TF frame that robot_state_publisher actually publishes. + imu.frame_id: "imu_link" + + # MPU9250: magnetometer disabled (unreliable in sim / near motors) + imu.has_magnetometer: false + # Simulation-tuned noise values for Gazebo Harmonic MPU9250 plugin. + # Real MPU9250 hardware: gyro ~0.005 rad/s, accel ~0.1 m/s2. + # Gazebo injects lower noise than the real sensor, so tighter values + # reduce unnecessary filter uncertainty in sim without affecting real-hardware users + # (who should revert to the hardware spec values above). + imu.gyro_noise: 0.002 # rad/s (Gazebo sim-tuned; real MPU9250: 0.005) + imu.accel_noise: 0.02 # m/s2 (Gazebo sim-tuned; real MPU9250: 0.1) + imu.remove_gravitational_acceleration: false + + # Wheel odometry noise (TB3 differential drive, simulated) + encoder.vel_noise: 0.05 # m/s + encoder.yaw_noise: 0.02 # rad/s + + # ICP odometry as second velocity source + encoder2.topic: "/rtabmap/icp_odometry" + + outlier_rejection: true + outlier_threshold_imu: 15.09 + outlier_threshold_enc: 11.34 + + adaptive.imu: true + adaptive.encoder: true + adaptive.window: 50 + adaptive.alpha: 0.01 + + zupt.enabled: true + zupt.velocity_threshold: 0.08 # m/s: slightly loose for ICP jitter + zupt.angular_threshold: 0.05 # rad/s + zupt.noise_sigma: 0.01 + + ukf.q_position: 0.01 + ukf.q_orientation: 1.0e-9 + ukf.q_velocity: 0.1 + ukf.q_angular_vel: 0.1 + ukf.q_acceleration: 1.0 + ukf.q_gyro_bias: 1.0e-5 + ukf.q_accel_bias: 1.0e-5 diff --git a/rtabmap_demos/params/fusioncore_tb3_bridge.yaml b/rtabmap_demos/params/fusioncore_tb3_bridge.yaml new file mode 100644 index 00000000..fd3130df --- /dev/null +++ b/rtabmap_demos/params/fusioncore_tb3_bridge.yaml @@ -0,0 +1,47 @@ +# ros_gz_bridge config for the FusionCore + icp_odometry demo. +# +# Identical to turtlebot3_waffle_bridge.yaml EXCEPT the 'tf' entry is removed. +# FusionCore publishes odom -> base_footprint TF directly, so the DiffDrive +# plugin's TF must not be forwarded to avoid a competing transform. + +- ros_topic_name: "clock" + gz_topic_name: "clock" + ros_type_name: "rosgraph_msgs/msg/Clock" + gz_type_name: "gz.msgs.Clock" + direction: GZ_TO_ROS + +- ros_topic_name: "joint_states" + gz_topic_name: "joint_states" + ros_type_name: "sensor_msgs/msg/JointState" + gz_type_name: "gz.msgs.Model" + direction: GZ_TO_ROS + +- ros_topic_name: "odom" + gz_topic_name: "odom" + ros_type_name: "nav_msgs/msg/Odometry" + gz_type_name: "gz.msgs.Odometry" + direction: GZ_TO_ROS + +- ros_topic_name: "cmd_vel" + gz_topic_name: "cmd_vel" + ros_type_name: "geometry_msgs/msg/TwistStamped" + gz_type_name: "gz.msgs.Twist" + direction: ROS_TO_GZ + +- ros_topic_name: "imu" + gz_topic_name: "imu" + ros_type_name: "sensor_msgs/msg/Imu" + gz_type_name: "gz.msgs.IMU" + direction: GZ_TO_ROS + +- ros_topic_name: "scan" + gz_topic_name: "scan" + ros_type_name: "sensor_msgs/msg/LaserScan" + gz_type_name: "gz.msgs.LaserScan" + direction: GZ_TO_ROS + +- ros_topic_name: "camera/camera_info" + gz_topic_name: "camera/camera_info" + ros_type_name: "sensor_msgs/msg/CameraInfo" + gz_type_name: "gz.msgs.CameraInfo" + direction: GZ_TO_ROS diff --git a/rtabmap_demos/params/turtlebot3_fusioncore_icp_nav2_params.yaml b/rtabmap_demos/params/turtlebot3_fusioncore_icp_nav2_params.yaml new file mode 100644 index 00000000..22ddc1c5 --- /dev/null +++ b/rtabmap_demos/params/turtlebot3_fusioncore_icp_nav2_params.yaml @@ -0,0 +1,280 @@ +# Nav2 parameters for FusionCore + icp_odometry TurtleBot3 demo. +# +# Key difference from stock nav2 params: +# - odom_topic: /fusion/odom (FusionCore output, not /odom or /odometry/filtered) +# - No AMCL: rtabmap handles the map -> odom transform via its SLAM output. + +bt_navigator: + ros__parameters: + use_sim_time: true + global_frame: map + robot_base_frame: base_footprint + odom_topic: /fusion/odom + bt_loop_duration: 10 + default_server_timeout: 20 + wait_for_service_timeout: 1000 + action_server_result_timeout: 900.0 + navigators: ['navigate_to_pose', 'navigate_through_poses'] + navigate_to_pose: + plugin: 'nav2_bt_navigator::NavigateToPoseNavigator' + navigate_through_poses: + plugin: 'nav2_bt_navigator::NavigateThroughPosesNavigator' + +controller_server: + ros__parameters: + use_sim_time: true + enable_stamped_cmd_vel: True + controller_frequency: 20.0 + min_x_velocity_threshold: 0.001 + min_y_velocity_threshold: 0.5 + min_theta_velocity_threshold: 0.001 + failure_tolerance: 0.3 + odom_topic: /fusion/odom + progress_checker_plugins: ['progress_checker'] + goal_checker_plugins: ['general_goal_checker'] + controller_plugins: ['FollowPath'] + progress_checker: + plugin: 'nav2_controller::SimpleProgressChecker' + required_movement_radius: 0.5 + movement_time_allowance: 10.0 + general_goal_checker: + stateful: true + plugin: 'nav2_controller::SimpleGoalChecker' + xy_goal_tolerance: 0.25 + yaw_goal_tolerance: 0.25 + FollowPath: + plugin: 'nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController' + desired_linear_vel: 0.2 + lookahead_dist: 0.6 + min_lookahead_dist: 0.3 + max_lookahead_dist: 0.9 + lookahead_time: 1.5 + rotate_to_heading_angular_vel: 1.8 + transform_tolerance: 0.1 + use_velocity_scaled_lookahead_dist: false + min_approach_linear_velocity: 0.05 + approach_velocity_scaling_dist: 0.6 + use_collision_detection: true + max_allowed_time_to_collision_up_to_goal: 1.0 + use_regulated_linear_velocity_scaling: true + use_fixed_curvature_lookahead: false + curvature_feedforward_gain: 1.0 + use_cost_regulated_linear_velocity_scaling: false + regulated_linear_scaling_min_radius: 0.9 + regulated_linear_scaling_min_speed: 0.25 + use_rotate_to_heading: true + allow_reversing: false + rotate_to_heading_min_angle: 0.785 + max_angular_accel: 3.2 + max_robot_pose_search_dist: 10.0 + +local_costmap: + local_costmap: + ros__parameters: + use_sim_time: true + update_frequency: 5.0 + publish_frequency: 2.0 + global_frame: odom + robot_base_frame: base_footprint + rolling_window: true + width: 3 + height: 3 + resolution: 0.05 + robot_radius: 0.22 + plugins: ['obstacle_layer', 'inflation_layer'] + obstacle_layer: + plugin: 'nav2_costmap_2d::ObstacleLayer' + enabled: true + observation_sources: scan + scan: + topic: /scan + max_obstacle_height: 2.0 + clearing: true + marking: true + data_type: 'LaserScan' + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + inflation_layer: + plugin: 'nav2_costmap_2d::InflationLayer' + cost_scaling_factor: 3.0 + inflation_radius: 0.55 + always_send_full_costmap: true + +global_costmap: + global_costmap: + ros__parameters: + use_sim_time: true + update_frequency: 1.0 + publish_frequency: 1.0 + global_frame: map + robot_base_frame: base_footprint + robot_radius: 0.22 + resolution: 0.05 + track_unknown_space: true + plugins: ['static_layer', 'obstacle_layer', 'inflation_layer'] + static_layer: + plugin: 'nav2_costmap_2d::StaticLayer' + map_subscribe_transient_local: true + obstacle_layer: + plugin: 'nav2_costmap_2d::ObstacleLayer' + enabled: true + observation_sources: scan + scan: + topic: /scan + max_obstacle_height: 2.0 + clearing: true + marking: true + data_type: 'LaserScan' + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + inflation_layer: + plugin: 'nav2_costmap_2d::InflationLayer' + cost_scaling_factor: 3.0 + inflation_radius: 0.55 + always_send_full_costmap: true + +planner_server: + ros__parameters: + use_sim_time: true + expected_planner_frequency: 20.0 + planner_plugins: ['GridBased'] + GridBased: + plugin: 'nav2_navfn_planner::NavfnPlanner' + tolerance: 0.5 + use_astar: false + allow_unknown: true + +smoother_server: + ros__parameters: + use_sim_time: true + enable_stamped_cmd_vel: True + smoother_plugins: ['simple_smoother'] + simple_smoother: + plugin: 'nav2_smoother::SimpleSmoother' + tolerance: 1.0e-10 + max_its: 1000 + do_refinement: true + +behavior_server: + ros__parameters: + use_sim_time: true + enable_stamped_cmd_vel: True + local_costmap_topic: local_costmap/costmap_raw + global_costmap_topic: global_costmap/costmap_raw + local_footprint_topic: local_costmap/published_footprint + global_footprint_topic: global_costmap/published_footprint + cycle_frequency: 10.0 + behavior_plugins: ['spin', 'backup', 'drive_on_heading', 'assisted_teleop', 'wait'] + spin: + plugin: 'nav2_behaviors::Spin' + backup: + plugin: 'nav2_behaviors::BackUp' + drive_on_heading: + plugin: 'nav2_behaviors::DriveOnHeading' + wait: + plugin: 'nav2_behaviors::Wait' + assisted_teleop: + plugin: 'nav2_behaviors::AssistedTeleop' + local_frame: odom + global_frame: map + robot_base_frame: base_footprint + transform_tolerance: 0.1 + simulate_ahead_time: 2.0 + max_rotational_vel: 1.0 + min_rotational_vel: 0.4 + rotational_acc_lim: 3.2 + +velocity_smoother: + ros__parameters: + use_sim_time: true + enable_stamped_cmd_vel: True + smoothing_frequency: 20.0 + scale_velocities: false + feedback: 'OPEN_LOOP' + max_velocity: [0.26, 0.0, 1.0] + min_velocity: [-0.26, 0.0, -1.0] + max_accel: [2.5, 0.0, 3.2] + max_decel: [-2.5, 0.0, -3.2] + odom_topic: /fusion/odom + odom_duration: 0.1 + deadband_velocity: [0.0, 0.0, 0.0] + velocity_timeout: 1.0 + +collision_monitor: + ros__parameters: + use_sim_time: true + enable_stamped_cmd_vel: True + base_frame_id: base_footprint + odom_frame_id: odom + cmd_vel_in_topic: cmd_vel_smoothed + cmd_vel_out_topic: cmd_vel + state_topic: collision_monitor_state + transform_error_pub_topic: transform_error + polygons: ['FootprintApproach'] + FootprintApproach: + type: polygon + action_type: approach + footprint_topic: local_costmap/published_footprint + time_before_collision: 1.2 + simulation_time_step: 0.1 + min_points: 6 + visualize: false + enabled: true + observation_sources: ['scan'] + scan: + type: scan + topic: /scan + min_height: 0.15 + max_height: 2.0 + enabled: true + + +docking_server: + ros__parameters: + enable_stamped_cmd_vel: True + controller_frequency: 50.0 + initial_perception_timeout: 5.0 + wait_charge_timeout: 5.0 + dock_approach_timeout: 30.0 + undock_linear_tolerance: 0.05 + undock_angular_tolerance: 0.1 + max_retries: 3 + base_frame: "base_footprint" + fixed_frame: "odom" + dock_backwards: false + dock_prestaging_tolerance: 0.5 + + # Types of docks + dock_plugins: ['simple_charging_dock'] + simple_charging_dock: + plugin: 'opennav_docking::SimpleChargingDock' + docking_threshold: 0.05 + staging_x_offset: -0.7 + use_external_detection_pose: true + use_battery_status: false # true + use_stall_detection: false # true + + external_detection_timeout: 1.0 + external_detection_translation_x: -0.18 + external_detection_translation_y: 0.0 + external_detection_rotation_roll: -1.57 + external_detection_rotation_pitch: -1.57 + external_detection_rotation_yaw: 0.0 + filter_coef: 0.1 + + controller: + k_phi: 3.0 + k_delta: 2.0 + v_linear_min: 0.15 + v_linear_max: 0.15 + use_collision_detection: true + costmap_topic: "local_costmap/costmap_raw" + footprint_topic: "local_costmap/published_footprint" + transform_tolerance: 0.1 + projection_time: 5.0 + simulation_step: 0.1 + dock_collision_threshold: 0.3 \ No newline at end of file From a6921845b614855840f7f4052d7600fdec3a5488 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sat, 23 May 2026 15:08:21 -0700 Subject: [PATCH 51/56] ros1: Migrating tf to tf2 (#1425) * Migrating tf to tf2 * Added ci action to test PR on ros1 * updated dev container with nvidia working * backward compatibility with topics having frame_id with leading slash not allowed with tf2 * backward compatibility of leading slash for other tf2 buffers * updated comment --- .devcontainer/devcontainer.json | 11 +- .github/workflows/noetic-pr.yml | 41 ++++++ rtabmap_conversions/CMakeLists.txt | 4 +- .../rtabmap_conversions/MsgConversion.h | 21 +-- rtabmap_conversions/package.xml | 1 + rtabmap_conversions/src/MsgConversion.cpp | 135 ++++++++---------- rtabmap_costmap_plugins/src/voxel_layer.cpp | 5 +- .../launch/demo_turtlebot3_navigation.launch | 6 +- rtabmap_demos/src/SaveObjectsExample.cpp | 30 ++-- .../src/nodelets/obstacles_detection_old.cpp | 29 ++-- .../include/rtabmap_odom/OdometryROS.h | 8 +- rtabmap_odom/src/OdometryROS.cpp | 11 +- rtabmap_odom/src/nodelets/icp_odometry.cpp | 16 +-- rtabmap_odom/src/nodelets/rgbd_odometry.cpp | 2 +- .../src/nodelets/rgbdicp_odometry.cpp | 8 +- rtabmap_odom/src/nodelets/stereo_odometry.cpp | 6 +- .../include/rtabmap_slam/CoreWrapper.h | 6 +- rtabmap_slam/src/CoreWrapper.cpp | 41 +++--- rtabmap_util/CMakeLists.txt | 4 +- rtabmap_util/package.xml | 1 + rtabmap_util/src/nodelets/imu_to_tf.cpp | 28 ++-- rtabmap_util/src/nodelets/lidar_deskewing.cpp | 17 ++- .../src/nodelets/obstacles_detection.cpp | 49 +++---- .../src/nodelets/point_cloud_aggregator.cpp | 15 +- .../src/nodelets/point_cloud_assembler.cpp | 13 +- .../src/nodelets/pointcloud_to_depthimage.cpp | 12 +- rtabmap_viz/CMakeLists.txt | 4 +- rtabmap_viz/include/rtabmap_viz/GuiWrapper.h | 6 +- rtabmap_viz/package.xml | 1 + rtabmap_viz/src/GuiWrapper.cpp | 27 ++-- 30 files changed, 311 insertions(+), 247 deletions(-) create mode 100644 .github/workflows/noetic-pr.yml diff --git a/.devcontainer/devcontainer.json b/.devcontainer/devcontainer.json index 091c51e4..43f6becb 100644 --- a/.devcontainer/devcontainer.json +++ b/.devcontainer/devcontainer.json @@ -23,9 +23,16 @@ "workspaceMount": "source=${localWorkspaceFolder},target=/home/vscode/catkin_ws/src/rtabmap_ros,type=bind", "workspaceFolder": "/home/vscode/catkin_ws", "postCreateCommand": "cd /home/vscode/catkin_ws/src && catkin_init_workspace", + "hostRequirements": { + "gpu": "optional" + }, "runArgs": ["--privileged", - "--runtime=nvidia", + "--gpus=all", + //"--runtime=nvidia", // uncommment this if rtabmap doesn't show up in nvidia-smi on the host computer "--env=DISPLAY", "--env=QT_X11_NO_MITSHM=1", - "--volume=/tmp/.X11-unix:/tmp/.X11-unix"] + "--volume=/tmp/.X11-unix:/tmp/.X11-unix"], + "containerEnv": { + "NVIDIA_VISIBLE_DEVICES": "all" + } } diff --git a/.github/workflows/noetic-pr.yml b/.github/workflows/noetic-pr.yml new file mode 100644 index 00000000..b984137f --- /dev/null +++ b/.github/workflows/noetic-pr.yml @@ -0,0 +1,41 @@ +name: noetic-pr + +on: + pull_request: + branches: + - 'master' + +concurrency: + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: true + +jobs: + docker: + runs-on: ubuntu-latest + + strategy: + matrix: + docker_tag: [rtabmap_ros_noetic_pr] + include: + - docker_tag: rtabmap_ros_noetic_pr + docker_path: 'noetic/latest' + docker_platforms: | + linux/amd64 + + steps: + - + name: Checkout + uses: actions/checkout@v2 + - + name: Set up Docker Buildx + uses: docker/setup-buildx-action@v1 + - + name: Build and push + uses: docker/build-push-action@v2 + with: + context: . + push: false + platforms: ${{ matrix.docker_platforms }} + file: ./docker/${{ matrix.docker_path }}/Dockerfile + tags: ${{ matrix.docker_tag }} + diff --git a/rtabmap_conversions/CMakeLists.txt b/rtabmap_conversions/CMakeLists.txt index 17e44954..01882399 100644 --- a/rtabmap_conversions/CMakeLists.txt +++ b/rtabmap_conversions/CMakeLists.txt @@ -3,7 +3,7 @@ project(rtabmap_conversions) find_package(catkin REQUIRED COMPONENTS cv_bridge roscpp sensor_msgs std_msgs geometry_msgs - tf tf_conversions eigen_conversions laser_geometry pcl_conversions + tf tf2_ros tf_conversions eigen_conversions laser_geometry pcl_conversions image_geometry rtabmap_msgs ) @@ -13,7 +13,7 @@ catkin_package( INCLUDE_DIRS include LIBRARIES rtabmap_conversions CATKIN_DEPENDS cv_bridge roscpp sensor_msgs std_msgs geometry_msgs - tf tf_conversions eigen_conversions laser_geometry pcl_conversions + tf tf2_ros tf_conversions eigen_conversions laser_geometry pcl_conversions image_geometry rtabmap_msgs DEPENDS RTABMap ) diff --git a/rtabmap_conversions/include/rtabmap_conversions/MsgConversion.h b/rtabmap_conversions/include/rtabmap_conversions/MsgConversion.h index 4c3b60bd..ac2777a8 100644 --- a/rtabmap_conversions/include/rtabmap_conversions/MsgConversion.h +++ b/rtabmap_conversions/include/rtabmap_conversions/MsgConversion.h @@ -29,7 +29,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #define MSGCONVERSION_H_ #include -#include +#include +#include #include #include #include @@ -135,7 +136,7 @@ rtabmap::StereoCameraModel stereoCameraModelFromROS( const sensor_msgs::CameraInfo & leftCamInfo, const sensor_msgs::CameraInfo & rightCamInfo, const std::string & frameId, - tf::TransformListener & listener, + tf2_ros::Buffer & tfBuffer, double waitForTransform); void mapDataFromROS( @@ -190,7 +191,7 @@ rtabmap::Landmarks landmarksFromROS( const std::string & frameId, const std::string & odomFrameId, const ros::Time & odomStamp, - tf::TransformListener & listener, + tf2_ros::Buffer & tfBuffer, double waitForTransform, double defaultLinVariance, double defaultAngVariance); @@ -202,7 +203,7 @@ rtabmap::Transform getTransform( const std::string & fromFrameId, const std::string & toFrameId, const ros::Time & stamp, - tf::TransformListener & listener, + tf2_ros::Buffer & tfBuffer, double waitForTransform); @@ -213,7 +214,7 @@ rtabmap::Transform getMovingTransform( const std::string & fixedFrame, const ros::Time & stampFrom, const ros::Time & stampTo, - tf::TransformListener & listener, + tf2_ros::Buffer & tfBuffer, double waitForTransform); bool convertRGBDMsgs( @@ -228,7 +229,7 @@ bool convertRGBDMsgs( cv::Mat & depth, std::vector & cameraModels, std::vector & stereoCameraModels, - tf::TransformListener & listener, + tf2_ros::Buffer & tfBuffer, double waitForTransform, bool alreadRectifiedImages, const std::vector > & localKeyPointsMsgs = std::vector >(), @@ -249,7 +250,7 @@ bool convertStereoMsg( cv::Mat & left, cv::Mat & right, rtabmap::StereoCameraModel & stereoModel, - tf::TransformListener & listener, + tf2_ros::Buffer & tfBuffer, double waitForTransform, bool alreadyRectified); @@ -259,7 +260,7 @@ bool convertScanMsg( const std::string & odomFrameId, const ros::Time & odomStamp, rtabmap::LaserScan & scan, - tf::TransformListener & listener, + tf2_ros::Buffer & tfBuffer, double waitForTransform, bool outputInFrameId = false); @@ -269,7 +270,7 @@ bool convertScan3dMsg( const std::string & odomFrameId, const ros::Time & odomStamp, rtabmap::LaserScan & scan, - tf::TransformListener & listener, + tf2_ros::Buffer & tfBuffer, double waitForTransform, int maxPoints = 0, float maxRange = 0.0f, @@ -279,7 +280,7 @@ bool deskew( const sensor_msgs::PointCloud2 & input, sensor_msgs::PointCloud2 & output, const std::string & fixedFrameId, - tf::TransformListener & listener, + tf2_ros::Buffer & tfBuffer, double waitForTransform, bool slerp = false); diff --git a/rtabmap_conversions/package.xml b/rtabmap_conversions/package.xml index ebd35cff..93515c62 100644 --- a/rtabmap_conversions/package.xml +++ b/rtabmap_conversions/package.xml @@ -23,6 +23,7 @@ sensor_msgs std_msgs tf + tf2_ros tf_conversions diff --git a/rtabmap_conversions/src/MsgConversion.cpp b/rtabmap_conversions/src/MsgConversion.cpp index ff155b4f..512384f1 100644 --- a/rtabmap_conversions/src/MsgConversion.cpp +++ b/rtabmap_conversions/src/MsgConversion.cpp @@ -928,14 +928,14 @@ rtabmap::StereoCameraModel stereoCameraModelFromROS( const sensor_msgs::CameraInfo & leftCamInfo, const sensor_msgs::CameraInfo & rightCamInfo, const std::string & frameId, - tf::TransformListener & listener, + tf2_ros::Buffer & tfBuffer, double waitForTransform) { rtabmap::Transform localTransform = getTransform( frameId, leftCamInfo.header.frame_id, leftCamInfo.header.stamp, - listener, + tfBuffer, waitForTransform); if(localTransform.isNull()) { @@ -946,7 +946,7 @@ rtabmap::StereoCameraModel stereoCameraModelFromROS( leftCamInfo.header.frame_id, rightCamInfo.header.frame_id, leftCamInfo.header.stamp, - listener, + tfBuffer, waitForTransform); if(stereoTransform.isNull()) { @@ -1885,7 +1885,7 @@ rtabmap::Landmarks landmarksFromROS( const std::string & frameId, const std::string & odomFrameId, const ros::Time & odomStamp, - tf::TransformListener & listener, + tf2_ros::Buffer & tfBuffer, double waitForTransform, double defaultLinVariance, double defaultAngVariance) @@ -1903,7 +1903,7 @@ rtabmap::Landmarks landmarksFromROS( frameId, iter->second.first.header.frame_id, iter->second.first.header.stamp, - listener, + tfBuffer, waitForTransform); if(baseToCamera.isNull()) @@ -1923,7 +1923,7 @@ rtabmap::Landmarks landmarksFromROS( odomFrameId, odomStamp, iter->second.first.header.stamp, - listener, + tfBuffer, waitForTransform); if(!correction.isNull()) { @@ -1952,32 +1952,24 @@ rtabmap::Transform getTransform( const std::string & fromFrameId, const std::string & toFrameId, const ros::Time & stamp, - tf::TransformListener & listener, + tf2_ros::Buffer & tfBuffer, double waitForTransform) { // TF ready? rtabmap::Transform transform; try { - if(waitForTransform > 0.0 && !stamp.isZero()) - { - //if(!tfBuffer_.canTransform(fromFrameId, toFrameId, stamp, ros::Duration(1))) - std::string errorMsg; - if(!listener.waitForTransform(fromFrameId, toFrameId, stamp, ros::Duration(waitForTransform), ros::Duration(0.01), &errorMsg)) - { - ROS_WARN("Could not get transform from %s to %s after %f seconds (for stamp=%f)! Error=\"%s\".", - fromFrameId.c_str(), toFrameId.c_str(), waitForTransform, stamp.toSec(), errorMsg.c_str()); - return transform; - } - } - - tf::StampedTransform tmp; - listener.lookupTransform(fromFrameId, toFrameId, stamp, tmp); - transform = rtabmap_conversions::transformFromTF(tmp); + geometry_msgs::TransformStamped tmp; + tmp = tfBuffer.lookupTransform( + !fromFrameId.empty()&&fromFrameId.at(0)=='/'?fromFrameId.substr(1):fromFrameId, + !toFrameId.empty()&&toFrameId.at(0)=='/'?toFrameId.substr(1):toFrameId, + stamp, + ros::Duration(waitForTransform)); + transform = rtabmap_conversions::transformFromGeometryMsg(tmp.transform); } - catch(tf::TransformException & ex) + catch(tf2::TransformException & ex) { - ROS_WARN("(getting transform %s -> %s) %s", fromFrameId.c_str(), toFrameId.c_str(), ex.what()); + ROS_WARN("(getting transform %s -> %s) %s (wait_for_transform=%f)", fromFrameId.c_str(), toFrameId.c_str(), ex.what(), waitForTransform); } return transform; } @@ -1989,30 +1981,24 @@ rtabmap::Transform getMovingTransform( const std::string & fixedFrame, const ros::Time & stampFrom, const ros::Time & stampTo, - tf::TransformListener & listener, + tf2_ros::Buffer & tfBuffer, double waitForTransform) { // TF ready? rtabmap::Transform transform; try { - ros::Time stamp = stampTo>stampFrom?stampTo:stampFrom; - if(waitForTransform > 0.0 && !stamp.isZero()) - { - std::string errorMsg; - if(!listener.waitForTransform(movingFrame, fixedFrame, stamp, ros::Duration(waitForTransform), ros::Duration(0.01), &errorMsg)) - { - ROS_WARN("Could not get transform from %s to %s accordingly to %s after %f seconds (for stamps=%f -> %f)! Error=\"%s\".", - movingFrame.c_str(), movingFrame.c_str(), fixedFrame.c_str(), waitForTransform, stampTo.toSec(), stampFrom.toSec(), errorMsg.c_str()); - return transform; - } - } - - tf::StampedTransform tmp; - listener.lookupTransform(movingFrame, stampFrom, movingFrame, stampTo, fixedFrame, tmp); - transform = rtabmap_conversions::transformFromTF(tmp); + geometry_msgs::TransformStamped tmp; + tmp = tfBuffer.lookupTransform( + !movingFrame.empty()&&movingFrame.at(0)=='/'?movingFrame.substr(1):movingFrame, + stampFrom, + !movingFrame.empty()&&movingFrame.at(0)=='/'?movingFrame.substr(1):movingFrame, + stampTo, + !fixedFrame.empty()&&fixedFrame.at(0)=='/'?fixedFrame.substr(1):fixedFrame, + ros::Duration(waitForTransform)); + transform = rtabmap_conversions::transformFromGeometryMsg(tmp.transform); } - catch(tf::TransformException & ex) + catch(tf2::TransformException & ex) { ROS_WARN("(getting transform movement of %s according to fixed %s) %s", movingFrame.c_str(), fixedFrame.c_str(), ex.what()); } @@ -2031,7 +2017,7 @@ bool convertRGBDMsgs( cv::Mat & depth, std::vector & cameraModels, std::vector & stereoCameraModels, - tf::TransformListener & listener, + tf2_ros::Buffer & tfBuffer, double waitForTransform, bool alreadRectifiedImages, const std::vector > & localKeyPointsMsgs, @@ -2157,7 +2143,7 @@ bool convertRGBDMsgs( } // use depth's stamp so that geometry is sync to odom, use rgb frame as we assume depth is registered (normally depth msg should have same frame than rgb) - rtabmap::Transform localTransform = rtabmap_conversions::getTransform(frameId, !imageMsgs.empty()?imageMsgs[i]->header.frame_id:cameraInfoMsgs[i].header.frame_id, stamp, listener, waitForTransform); + rtabmap::Transform localTransform = rtabmap_conversions::getTransform(frameId, !imageMsgs.empty()?imageMsgs[i]->header.frame_id:cameraInfoMsgs[i].header.frame_id, stamp, tfBuffer, waitForTransform); if(localTransform.isNull()) { ROS_ERROR("TF of received image %d at time %fs is not set!", i, stamp.toSec()); @@ -2171,7 +2157,7 @@ bool convertRGBDMsgs( odomFrameId, odomStamp, stamp, - listener, + tfBuffer, waitForTransform); if(sensorT.isNull()) { @@ -2310,7 +2296,7 @@ bool convertRGBDMsgs( depthCameraInfoMsgs[i].header.frame_id, cameraInfoMsgs[i].header.frame_id, cameraInfoMsgs[i].header.stamp, - listener, + tfBuffer, waitForTransform); if(stereoTransform.isNull()) { @@ -2355,7 +2341,7 @@ bool convertRGBDMsgs( cameraInfoMsgs[i].header.frame_id, depthCameraInfoMsgs[i].header.frame_id, cameraInfoMsgs[i].header.stamp, - listener, + tfBuffer, waitForTransform); } if(stereoTransform.isNull() || stereoTransform.x()<=0) @@ -2423,7 +2409,7 @@ bool convertStereoMsg( cv::Mat & left, cv::Mat & right, rtabmap::StereoCameraModel & stereoModel, - tf::TransformListener & listener, + tf2_ros::Buffer & tfBuffer, double waitForTransform, bool alreadyRectified) { @@ -2474,7 +2460,7 @@ bool convertStereoMsg( right = cv_bridge::cvtColor(rightImageMsg, "mono8")->image; } - rtabmap::Transform localTransform = getTransform(frameId, leftImageMsg->header.frame_id, leftImageMsg->header.stamp, listener, waitForTransform); + rtabmap::Transform localTransform = getTransform(frameId, leftImageMsg->header.frame_id, leftImageMsg->header.stamp, tfBuffer, waitForTransform); if(localTransform.isNull()) { return false; @@ -2487,7 +2473,7 @@ bool convertStereoMsg( odomFrameId, odomStamp, leftImageMsg->header.stamp, - listener, + tfBuffer, waitForTransform); if(sensorT.isNull()) { @@ -2507,7 +2493,7 @@ bool convertStereoMsg( rightCamInfoMsg.header.frame_id, leftCamInfoMsg.header.frame_id, leftCamInfoMsg.header.stamp, - listener, + tfBuffer, waitForTransform); if(stereoTransform.isNull()) { @@ -2537,7 +2523,7 @@ bool convertStereoMsg( leftCamInfoMsg.header.frame_id, rightCamInfoMsg.header.frame_id, leftCamInfoMsg.header.stamp, - listener, + tfBuffer, waitForTransform); if(stereoTransform.isNull() || stereoTransform.x()<=0) { @@ -2575,7 +2561,7 @@ bool convertScanMsg( const std::string & odomFrameId, const ros::Time & odomStamp, rtabmap::LaserScan & scan, - tf::TransformListener & listener, + tf2_ros::Buffer & tfBuffer, double waitForTransform, bool outputInFrameId) { @@ -2603,7 +2589,7 @@ bool convertScanMsg( odomFrameId.empty()?frameId:odomFrameId, scan2dMsg.header.stamp, scan2dMsg.header.stamp + ros::Duration().fromSec(scan2dMsg.ranges.size()*scan2dMsg.time_increment), - listener, + tfBuffer, waitForTransform); if(tmpT.isNull()) { @@ -2614,7 +2600,7 @@ bool convertScanMsg( frameId, scan2dMsg.header.frame_id, scan2dMsg.header.stamp, - listener, + tfBuffer, waitForTransform); if(scanLocalTransform.isNull()) { @@ -2624,14 +2610,14 @@ bool convertScanMsg( //transform in frameId_ frame sensor_msgs::PointCloud2 scanOut; laser_geometry::LaserProjection projection; - projection.transformLaserScanToPointCloud(odomFrameId.empty()?frameId:odomFrameId, scan2dMsg, scanOut, listener); + projection.transformLaserScanToPointCloud(odomFrameId.empty()?frameId:odomFrameId, scan2dMsg, scanOut, tfBuffer); //transform back in laser frame rtabmap::Transform laserToOdom = getTransform( scan2dMsg.header.frame_id, odomFrameId.empty()?frameId:odomFrameId, scan2dMsg.header.stamp, - listener, + tfBuffer, waitForTransform); if(laserToOdom.isNull()) { @@ -2646,7 +2632,7 @@ bool convertScanMsg( odomFrameId, odomStamp, scan2dMsg.header.stamp, - listener, + tfBuffer, waitForTransform); if(sensorT.isNull()) { @@ -2734,7 +2720,7 @@ bool convertScan3dMsg( const std::string & odomFrameId, const ros::Time & odomStamp, rtabmap::LaserScan & scan, - tf::TransformListener & listener, + tf2_ros::Buffer & tfBuffer, double waitForTransform, int maxPoints, float maxRange, @@ -2743,7 +2729,7 @@ bool convertScan3dMsg( 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()); - rtabmap::Transform scanLocalTransform = getTransform(frameId, scan3dMsg.header.frame_id, scan3dMsg.header.stamp, listener, waitForTransform); + rtabmap::Transform scanLocalTransform = getTransform(frameId, scan3dMsg.header.frame_id, scan3dMsg.header.stamp, tfBuffer, waitForTransform); if(scanLocalTransform.isNull()) { ROS_ERROR("TF of received scan cloud at time %fs is not set, aborting rtabmap update.", scan3dMsg.header.stamp.toSec()); @@ -2758,7 +2744,7 @@ bool convertScan3dMsg( odomFrameId, odomStamp, scan3dMsg.header.stamp, - listener, + tfBuffer, waitForTransform); if(sensorT.isNull()) { @@ -2779,13 +2765,13 @@ bool deskew_impl( const sensor_msgs::PointCloud2 & input, sensor_msgs::PointCloud2 & output, const std::string & fixedFrameId, - tf::TransformListener * listener, + tf2_ros::Buffer * tfBuffer, double waitForTransform, bool slerp, const rtabmap::Transform & velocity, double previousStamp) { - if(listener != 0) + if(tfBuffer != 0) { if(input.header.frame_id.empty()) { @@ -3088,16 +3074,15 @@ bool deskew_impl( } std::string errorMsg; - if(listener != 0 && + if(tfBuffer != 0 && waitForTransform>0.0 && - !listener->waitForTransform( - input.header.frame_id, + !tfBuffer->canTransform( + !input.header.frame_id.empty()&&input.header.frame_id.at(0)=='/'?input.header.frame_id.substr(1):input.header.frame_id, firstStamp, - input.header.frame_id, + !input.header.frame_id.empty()&&input.header.frame_id.at(0)=='/'?input.header.frame_id.substr(1):input.header.frame_id, lastStamp, - fixedFrameId, + !fixedFrameId.empty()&&fixedFrameId.at(0)=='/'?fixedFrameId.substr(1):fixedFrameId, ros::Duration(waitForTransform), - ros::Duration(0.01), &errorMsg)) { ROS_ERROR("Could not estimate motion of %s accordingly to fixed frame %s between stamps %f and %f! (%s)", @@ -3114,21 +3099,21 @@ bool deskew_impl( double scanTime = 0; if(slerp) { - if(listener != 0) + if(tfBuffer != 0) { firstPose = rtabmap_conversions::getMovingTransform( input.header.frame_id, fixedFrameId, input.header.stamp, firstStamp, - *listener, + *tfBuffer, 0); lastPose = rtabmap_conversions::getMovingTransform( input.header.frame_id, fixedFrameId, input.header.stamp, lastStamp, - *listener, + *tfBuffer, 0); } else @@ -3233,7 +3218,7 @@ bool deskew_impl( fixedFrameId, output.header.stamp, stamp, - *listener, + *tfBuffer, 0); if(transform.isNull()) { @@ -3327,7 +3312,7 @@ bool deskew_impl( fixedFrameId, output.header.stamp, stamp, - *listener, + *tfBuffer, 0); if(transform.isNull()) { @@ -3376,11 +3361,11 @@ bool deskew( const sensor_msgs::PointCloud2 & input, sensor_msgs::PointCloud2 & output, const std::string & fixedFrameId, - tf::TransformListener & listener, + tf2_ros::Buffer & tfBuffer, double waitForTransform, bool slerp) { - return deskew_impl(input, output, fixedFrameId, &listener, waitForTransform, slerp, rtabmap::Transform(), 0); + return deskew_impl(input, output, fixedFrameId, &tfBuffer, waitForTransform, slerp, rtabmap::Transform(), 0); } bool deskew( diff --git a/rtabmap_costmap_plugins/src/voxel_layer.cpp b/rtabmap_costmap_plugins/src/voxel_layer.cpp index 35574349..495bb2ba 100644 --- a/rtabmap_costmap_plugins/src/voxel_layer.cpp +++ b/rtabmap_costmap_plugins/src/voxel_layer.cpp @@ -406,7 +406,10 @@ void VoxelLayer::updateOrigin(double new_origin_x, double new_origin_y) { geometry_msgs::TransformStamped transformStamped; #ifdef COSTMAP_2D_POINTCLOUD2 - transformStamped = tf_->lookupTransform(global_frame_, robot_base_frame_, ros::Time(0)); + transformStamped = tf_->lookupTransform( + !global_frame_.empty()&&global_frame_.at(0)=='/'?global_frame_.substr(1):global_frame_, + !robot_base_frame_.empty()&&robot_base_frame_.at(0)=='/'?robot_base_frame_.substr(1):robot_base_frame_, + ros::Time(0)); #else tf::StampedTransform stampedTransform; tf_->lookupTransform(global_frame_, robot_base_frame_, ros::Time(0), stampedTransform); diff --git a/rtabmap_demos/launch/demo_turtlebot3_navigation.launch b/rtabmap_demos/launch/demo_turtlebot3_navigation.launch index b5c9e1a1..7bb41425 100644 --- a/rtabmap_demos/launch/demo_turtlebot3_navigation.launch +++ b/rtabmap_demos/launch/demo_turtlebot3_navigation.launch @@ -1,10 +1,10 @@