i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-04 16:57:46 +08:00
Code Issues Packages Projects Releases Wiki Activity

Resolving image_transport hints support (#1181) (#1348)

Browse Source 
* Resolving image_transport hints support (#1181)

* updated dev container to be able to install packages, updated comment
This commit is contained in:
matlabbe
2025-08-09 15:05:08 -07:00
committed by GitHub
parent e37f95d1f9
commit d23e3eef01
16 changed files with 111 additions and 49 deletions
Download Patch File Download Diff File Expand all files Collapse all files
.devcontainer/humble/Dockerfile
+3 -1
View File
@@ -8,7 +8,9 @@ ARG USER_GID=1000
RUN set -ex && \ RUN set -ex && \
groupadd --gid ${USER_GID} ${USERNAME} && \ groupadd --gid ${USER_GID} ${USERNAME} && \
useradd --uid ${USER_UID} --gid ${USER_GID} -m ${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 && \ RUN mkdir -p /home/${USERNAME}/ros2_ws/src && \
chown -R ${USERNAME}:${USERNAME} /home/${USERNAME}/ros2_ws chown -R ${USERNAME}:${USERNAME} /home/${USERNAME}/ros2_ws
.devcontainer/jazzy/Dockerfile
+3 -1
View File
@@ -11,7 +11,9 @@ ARG USER_GID=1000
RUN set -ex && \ RUN set -ex && \
groupadd --gid ${USER_GID} ${USERNAME} && \ groupadd --gid ${USER_GID} ${USERNAME} && \
useradd --uid ${USER_UID} --gid ${USER_GID} -m ${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 && \ RUN mkdir -p /home/${USERNAME}/ros2_ws/src && \
chown -R ${USERNAME}:${USERNAME} /home/${USERNAME}/ros2_ws chown -R ${USERNAME}:${USERNAME} /home/${USERNAME}/ros2_ws
.devcontainer/kilted/Dockerfile
+3 -1
View File
@@ -11,7 +11,9 @@ ARG USER_GID=1000
RUN set -ex && \ RUN set -ex && \
groupadd --gid ${USER_GID} ${USERNAME} && \ groupadd --gid ${USER_GID} ${USERNAME} && \
useradd --uid ${USER_UID} --gid ${USER_GID} -m ${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 && \ RUN mkdir -p /home/${USERNAME}/ros2_ws/src && \
chown -R ${USERNAME}:${USERNAME} /home/${USERNAME}/ros2_ws chown -R ${USERNAME}:${USERNAME} /home/${USERNAME}/ros2_ws
rtabmap_odom/src/nodelets/rgbd_odometry.cpp
+15 -13
View File
@@ -113,8 +113,16 @@ void RGBDOdometry::onOdomInit()
rgbdCameras = 0; rgbdCameras = 0;
} }
keepColor_ = this->declare_parameter("keep_color", keepColor_); 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")); std::string depthTransport = this->declare_parameter("depth_transport", std::string("raw"));
RCLCPP_INFO(this->get_logger(), "RGBDOdometry: approx_sync = %s", approxSync?"true":"false"); RCLCPP_INFO(this->get_logger(), "RGBDOdometry: approx_sync = %s", approxSync?"true":"false");
if(approxSync) 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: subscribe_rgbd = %s", subscribeRGBD?"true":"false");
RCLCPP_INFO(this->get_logger(), "RGBDOdometry: rgbd_cameras = %d", rgbdCameras); 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: 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_INFO(this->get_logger(), "RGBDOdometry: depth_transport = %s", depthTransport.c_str());
rclcpp::SubscriptionOptions options; rclcpp::SubscriptionOptions options;
@@ -354,18 +362,12 @@ void RGBDOdometry::onOdomInit()
} }
else 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"); image_transport::TransportHints depth_hints(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 rgb_topic = get_node_base_interface()->resolve_topic_or_service_name( 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
"rgb/image", false, false 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);
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);
info_sub_.subscribe(this, "rgb/camera_info", RCLCPP_QOS(topicQueueSize_, qosCamInfo), options); info_sub_.subscribe(this, "rgb/camera_info", RCLCPP_QOS(topicQueueSize_, qosCamInfo), options);
if(approxSync) if(approxSync)
rtabmap_odom/src/nodelets/stereo_odometry.cpp
+10 -5
View File
@@ -109,6 +109,7 @@ void StereoOdometry::onOdomInit()
subscribeRGBD = this->declare_parameter("subscribe_rgbd", subscribeRGBD); subscribeRGBD = this->declare_parameter("subscribe_rgbd", subscribeRGBD);
rgbdCameras = this->declare_parameter("rgbd_cameras", rgbdCameras); rgbdCameras = this->declare_parameter("rgbd_cameras", rgbdCameras);
keepColor_ = this->declare_parameter("keep_color", keepColor_); 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"); RCLCPP_INFO(this->get_logger(), "StereoOdometry: approx_sync = %s", approxSync?"true":"false");
if(approxSync) 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: qos_camera_info = %d", qosCamInfo);
RCLCPP_INFO(this->get_logger(), "StereoOdometry: subscribe_rgbd = %s", subscribeRGBD?"true":"false"); 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: keep_color = %s", keepColor_?"true":"false");
RCLCPP_INFO(this->get_logger(), "StereoOdometry: image_transport = %s", imageTransport.c_str());
rclcpp::SubscriptionOptions options; rclcpp::SubscriptionOptions options;
options.callback_group = dataCallbackGroup_; options.callback_group = dataCallbackGroup_;
@@ -347,9 +349,12 @@ void StereoOdometry::onOdomInit()
} }
else else
{ {
image_transport::TransportHints hints(this); image_transport::TransportHints hints(this); // using "image_transport" parameter
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); 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); cameraInfoLeft_.subscribe(this, "left/camera_info", RCLCPP_QOS(topicQueueSize_, qosCamInfo), options);
cameraInfoRight_.subscribe(this, "right/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():"", approxSync&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
topicQueueSize_, topicQueueSize_,
syncQueueSize_, syncQueueSize_,
imageRectLeft_.getTopic().c_str(), imageRectLeft_.getSubscriber().getTopic().c_str(),
imageRectRight_.getTopic().c_str(), imageRectRight_.getSubscriber().getTopic().c_str(),
cameraInfoLeft_.getSubscriber()->get_topic_name(), cameraInfoLeft_.getSubscriber()->get_topic_name(),
cameraInfoRight_.getSubscriber()->get_topic_name()); cameraInfoRight_.getSubscriber()->get_topic_name());
} }
rtabmap_slam/src/CoreWrapper.cpp
+3 -2
View File
@@ -768,8 +768,9 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) :
Parameters::kRGBDEnabled().c_str(), Parameters::kRGBDEnabled().c_str(),
Parameters::kRGBDEnabled().c_str()); Parameters::kRGBDEnabled().c_str());
} }
image_transport::TransportHints hints(this); image_transport::TransportHints hints(this); // using "image_transport" parameter
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); 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()); RCLCPP_INFO(this->get_logger(), "\n%s subscribed to:\n %s", get_name(), defaultSub_.getTopic().c_str());
rtabmap_sync/include/rtabmap_sync/CommonDataSubscriber.h
+2
View File
@@ -269,6 +269,8 @@ private:
std::string odomFrameId_; std::string odomFrameId_;
int rgbdCameras_; int rgbdCameras_;
std::string name_; std::string name_;
std::string imageTransport_;
std::string depthTransport_;
rclcpp::CallbackGroup::SharedPtr syncCallbackGroup_; rclcpp::CallbackGroup::SharedPtr syncCallbackGroup_;
rtabmap_sync/src/CommonDataSubscriber.cpp
+6
View File
@@ -47,6 +47,8 @@ CommonDataSubscriber::CommonDataSubscriber(rclcpp::Node & node, bool gui) :
subscribedToUserData_(false), subscribedToUserData_(false),
odomFrameId_(""), odomFrameId_(""),
rgbdCameras_(1), rgbdCameras_(1),
imageTransport_("raw"),
depthTransport_("raw"),
// RGB + Depth // RGB + Depth
SYNC_INIT(depth), SYNC_INIT(depth),
@@ -392,6 +394,8 @@ CommonDataSubscriber::CommonDataSubscriber(rclcpp::Node & node, bool gui) :
"\"sync_queue_size\".", syncQueueSize_); "\"sync_queue_size\".", syncQueueSize_);
} }
syncQueueSize_ = node.declare_parameter("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 qos = node.declare_parameter("qos", (int)RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT);
int qosOdom = node.declare_parameter("qos_odom", qos); 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_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: 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: 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; rclcpp::SubscriptionOptions callbackOptions;
callbackOptions.callback_group = syncCallbackGroup_; callbackOptions.callback_group = syncCallbackGroup_;
rtabmap_sync/src/impl/CommonDataSubscriberDepth.cpp
+6 -3
View File
@@ -503,9 +503,12 @@ void CommonDataSubscriber::setupDepthCallbacks(
{ {
RCLCPP_INFO(node.get_logger(), "Setup depth callback"); RCLCPP_INFO(node.get_logger(), "Setup depth callback");
image_transport::TransportHints hints(&node); image_transport::TransportHints rgbHints(&node); // using "image_transport" parameter
imageSub_.subscribe(&node, "rgb/image", hints.getTransport(), rclcpp::QoS(topicQueueSize_).reliability(qosImage_).get_rmw_qos_profile(), options); image_transport::TransportHints depthHints(&node, "raw", "depth_transport");
imageDepthSub_.subscribe(&node, "depth/image", hints.getTransport(), rclcpp::QoS(topicQueueSize_).reliability(qosImage_).get_rmw_qos_profile(), options); 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); cameraInfoSub_.subscribe(&node, "rgb/camera_info", RCLCPP_QOS(topicQueueSize_, qosCameraInfo_), options);
#ifdef RTABMAP_SYNC_USER_DATA #ifdef RTABMAP_SYNC_USER_DATA
rtabmap_sync/src/impl/CommonDataSubscriberRGB.cpp
+3 -2
View File
@@ -503,8 +503,9 @@ void CommonDataSubscriber::setupRGBCallbacks(
{ {
RCLCPP_INFO(node.get_logger(), "Setup rgb-only callback"); RCLCPP_INFO(node.get_logger(), "Setup rgb-only callback");
image_transport::TransportHints hints(&node); image_transport::TransportHints hints(&node); // using "image_transport" parameter
imageSub_.subscribe(&node, "rgb/image", hints.getTransport(), rclcpp::QoS(topicQueueSize_).reliability(qosImage_).get_rmw_qos_profile(), options); 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); cameraInfoSub_.subscribe(&node, "rgb/camera_info", RCLCPP_QOS(topicQueueSize_, qosCameraInfo_), options);
#ifdef RTABMAP_SYNC_USER_DATA #ifdef RTABMAP_SYNC_USER_DATA
rtabmap_sync/src/impl/CommonDataSubscriberStereo.cpp
+5 -3
View File
@@ -97,9 +97,11 @@ void CommonDataSubscriber::setupStereoCallbacks(
{ {
RCLCPP_INFO(node.get_logger(), "Setup stereo callback"); RCLCPP_INFO(node.get_logger(), "Setup stereo callback");
image_transport::TransportHints hints(&node); image_transport::TransportHints hints(&node); // using "image_transport" parameter
imageRectLeft_.subscribe(&node, "left/image_rect", hints.getTransport(), rclcpp::QoS(topicQueueSize_).reliability(qosImage_).get_rmw_qos_profile(), options); 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
imageRectRight_.subscribe(&node, "right/image_rect", hints.getTransport(), rclcpp::QoS(topicQueueSize_).reliability(qosImage_).get_rmw_qos_profile(), options); 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); cameraInfoLeft_.subscribe(&node, "left/camera_info", RCLCPP_QOS(topicQueueSize_, qosCameraInfo_), options);
cameraInfoRight_.subscribe(&node, "right/camera_info", RCLCPP_QOS(topicQueueSize_, qosCameraInfo_), options); cameraInfoRight_.subscribe(&node, "right/camera_info", RCLCPP_QOS(topicQueueSize_, qosCameraInfo_), options);
rtabmap_sync/src/nodelets/rgb_sync.cpp
+5 -2
View File
@@ -72,6 +72,7 @@ RGBSync::RGBSync(const rclcpp::NodeOptions & options) :
qos = this->declare_parameter("qos", qos); qos = this->declare_parameter("qos", qos);
int qosCaminfo = this->declare_parameter("qos_camera_info", qos); int qosCaminfo = this->declare_parameter("qos_camera_info", qos);
compressedRate_ = this->declare_parameter("compressed_rate", compressedRate_); 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 = %s", get_name(), approxSync?"true":"false");
if(approxSync) 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 = %d", get_name(), qos);
RCLCPP_INFO(this->get_logger(), "%s: qos_camera_info = %d", get_name(), qosCaminfo); 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: compressed_rate = %f", get_name(), compressedRate_);
RCLCPP_INFO(this->get_logger(), "%s: image_transport = %s", get_name(), imageTransport.c_str());
rgbdImagePub_ = this->create_publisher<rtabmap_msgs::msg::RGBDImage>("rgbd_image", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos)); rgbdImagePub_ = this->create_publisher<rtabmap_msgs::msg::RGBDImage>("rgbd_image", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos));
rgbdImageCompressedPub_ = this->create_publisher<rtabmap_msgs::msg::RGBDImage>("rgbd_image/compressed", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos)); rgbdImageCompressedPub_ = this->create_publisher<rtabmap_msgs::msg::RGBDImage>("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)); exactSync_->registerCallback(std::bind(&RGBSync::callback, this, std::placeholders::_1, std::placeholders::_2));
} }
image_transport::TransportHints hints(this); image_transport::TransportHints hints(this); // using "image_transport" parameter
imageSub_.subscribe(this, "rgb/image", hints.getTransport(), rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile()); 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)); 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", std::string subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s,\n %s",
rtabmap_sync/src/nodelets/rgbd_sync.cpp
+24 -6
View File
@@ -76,6 +76,22 @@ RGBDSync::RGBDSync(const rclcpp::NodeOptions & options) :
depthScale_ = this->declare_parameter("depth_scale", depthScale_); depthScale_ = this->declare_parameter("depth_scale", depthScale_);
decimation_ = this->declare_parameter("decimation", decimation_); decimation_ = this->declare_parameter("decimation", decimation_);
compressedRate_ = this->declare_parameter("compressed_rate", compressedRate_); compressedRate_ = this->declare_parameter("compressed_rate", compressedRate_);
std::string rgbImageTransport = this->declare_parameter<std::string>("rgb_image_transport", std::string("raw"));
std::string depthImageTransport = this->declare_parameter<std::string>("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) 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: depth_scale = %f", get_name(), depthScale_);
RCLCPP_INFO(this->get_logger(), "%s: decimation = %d", get_name(), decimation_); 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: 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<rtabmap_msgs::msg::RGBDImage>("rgbd_image", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos)); rgbdImagePub_ = this->create_publisher<rtabmap_msgs::msg::RGBDImage>("rgbd_image", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos));
rgbdImageCompressedPub_ = this->create_publisher<rtabmap_msgs::msg::RGBDImage>("rgbd_image/compressed", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos)); rgbdImageCompressedPub_ = this->create_publisher<rtabmap_msgs::msg::RGBDImage>("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)); exactSyncDepth_->registerCallback(std::bind(&RGBDSync::callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
} }
std::string rgbImageTransport = this->declare_parameter<std::string>("rgb_image_transport", "raw"); image_transport::TransportHints rgbHints(this); // using "image_transport" parameter
std::string depthImageTransport = this->declare_parameter<std::string>("depth_image_transport", "raw"); image_transport::TransportHints depthHints(this, "raw", "depth_transport");
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 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 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/Jazzy 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()); imageSub_.subscribe(this, rgbTopic, rgbHints.getTransport(), 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()); 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)); 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", std::string subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s,\n %s,\n %s",
rtabmap_sync/src/nodelets/stereo_sync.cpp
+7 -3
View File
@@ -71,6 +71,7 @@ StereoSync::StereoSync(const rclcpp::NodeOptions & options) :
qos = this->declare_parameter("qos", qos); qos = this->declare_parameter("qos", qos);
int qosCamInfo = this->declare_parameter("qos_camera_info", qos); int qosCamInfo = this->declare_parameter("qos_camera_info", qos);
compressedRate_ = this->declare_parameter("compressed_rate", compressedRate_); 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 = %s", get_name(), approxSync?"true":"false");
RCLCPP_INFO(this->get_logger(), "%s: approx_sync_max_interval = %f", get_name(), approxSyncMaxInterval); 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 = %d", get_name(), qos);
RCLCPP_INFO(this->get_logger(), "%s: qos_camera_info = %d", get_name(), qosCamInfo); 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: compressed_rate = %f", get_name(), compressedRate_);
RCLCPP_INFO(this->get_logger(), "%s: image_transport = %s", get_name(), imageTransport.c_str());
rgbdImagePub_ = create_publisher<rtabmap_msgs::msg::RGBDImage>("rgbd_image", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos)); rgbdImagePub_ = create_publisher<rtabmap_msgs::msg::RGBDImage>("rgbd_image", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos));
rgbdImageCompressedPub_ = create_publisher<rtabmap_msgs::msg::RGBDImage>("rgbd_image/compressed", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos)); rgbdImageCompressedPub_ = create_publisher<rtabmap_msgs::msg::RGBDImage>("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)); exactSync_->registerCallback(std::bind(&StereoSync::callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4));
} }
image_transport::TransportHints hints(this); image_transport::TransportHints hints(this); // using "image_transport" parameter
imageLeftSub_.subscribe(this, "left/image_rect", 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_rect"); // Humble/Jazzy don't resolve base topic, fixed by https://github.com/ros-perception/image_common/commit/ea7589ae8c1f7ecb83d6aab7b4c890c2d630d27a
imageRightSub_.subscribe(this, "right/image_rect", hints.getTransport(), rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile()); 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)); cameraInfoLeftSub_.subscribe(this, "left/camera_info", RCLCPP_QOS(topicQueueSize, qosCamInfo));
cameraInfoRightSub_.subscribe(this, "right/camera_info", RCLCPP_QOS(topicQueueSize, qosCamInfo)); cameraInfoRightSub_.subscribe(this, "right/camera_info", RCLCPP_QOS(topicQueueSize, qosCamInfo));
rtabmap_util/src/nodelets/point_cloud_xyz.cpp
+4 -2
View File
@@ -97,6 +97,7 @@ PointCloudXYZ::PointCloudXYZ(const rclcpp::NodeOptions & options) :
normalRadius_ = this->declare_parameter("normal_radius", normalRadius_); normalRadius_ = this->declare_parameter("normal_radius", normalRadius_);
filterNaNs_ = this->declare_parameter("filter_nans", filterNaNs_); filterNaNs_ = this->declare_parameter("filter_nans", filterNaNs_);
roiStr = this->declare_parameter("roi_ratios", roiStr); roiStr = this->declare_parameter("roi_ratios", roiStr);
this->declare_parameter("depth_transport", std::string("raw"));
//parse roi (region of interest) //parse roi (region of interest)
roiRatios_.resize(4, 0); roiRatios_.resize(4, 0);
@@ -156,8 +157,9 @@ PointCloudXYZ::PointCloudXYZ(const rclcpp::NodeOptions & options) :
cloudPub_ = create_publisher<sensor_msgs::msg::PointCloud2>("cloud", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos)); cloudPub_ = create_publisher<sensor_msgs::msg::PointCloud2>("cloud", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos));
image_transport::TransportHints hints(this); image_transport::TransportHints hints(this, "raw", "depth_transport");
imageDepthSub_.subscribe(this, "depth/image", hints.getTransport(), rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile()); 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)); cameraInfoSub_.subscribe(this, "depth/camera_info", RCLCPP_QOS(topicQueueSize, qosCamInfo));
disparitySub_.subscribe(this, "disparity/image", RCLCPP_QOS(topicQueueSize, qos)); disparitySub_.subscribe(this, "disparity/image", RCLCPP_QOS(topicQueueSize, qos));
rtabmap_util/src/nodelets/point_cloud_xyzrgb.cpp
+12 -5
View File
@@ -102,6 +102,8 @@ PointCloudXYZRGB::PointCloudXYZRGB(const rclcpp::NodeOptions & options) :
normalRadius_ = this->declare_parameter("normal_radius", normalRadius_); normalRadius_ = this->declare_parameter("normal_radius", normalRadius_);
filterNaNs_ = this->declare_parameter("filter_nans", filterNaNs_); filterNaNs_ = this->declare_parameter("filter_nans", filterNaNs_);
roiStr = this->declare_parameter("roi_ratios", roiStr); 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) //parse roi (region of interest)
roiRatios_.resize(4, 0); 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)); exactSyncStereo_->registerCallback(std::bind(&PointCloudXYZRGB::stereoCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4));
} }
image_transport::TransportHints hints(this); image_transport::TransportHints rgbHints(this); // using "image_transport" parameter
imageSub_.subscribe(this, "rgb/image", hints.getTransport(), rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile()); image_transport::TransportHints depthHints(this, "raw", "depth_transport");
imageDepthSub_.subscribe(this, "depth/image", hints.getTransport(), rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile()); 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)); cameraInfoSub_.subscribe(this, "rgb/camera_info", RCLCPP_QOS(topicQueueSize, qosCamInfo));
imageDisparitySub_.subscribe(this, "disparity", RCLCPP_QOS(topicQueueSize, qos)); 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()); 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
imageRight_.subscribe(this, "right/image", hints.getTransport(), rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile()); 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)); cameraInfoLeft_.subscribe(this, "left/camera_info", RCLCPP_QOS(topicQueueSize, qosCamInfo));
cameraInfoRight_.subscribe(this, "right/camera_info", RCLCPP_QOS(topicQueueSize, qosCamInfo)); cameraInfoRight_.subscribe(this, "right/camera_info", RCLCPP_QOS(topicQueueSize, qosCamInfo));
} }