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)); }