From 196d01dcbb28f1be308f71f5ab0106f55af36c03 Mon Sep 17 00:00:00 2001 From: Lennart Reiher Date: Fri, 29 Aug 2025 11:34:55 +0200 Subject: [PATCH 1/2] add support for ffmpeg_image_transport and point_cloud_transport (#1568) Co-authored-by: Emre Karincali (cherry picked from commit a33b5e9445547020092346c2d8b14ae4961d60cd) # Conflicts: # rviz_default_plugins/src/rviz_default_plugins/displays/pointcloud/get_transport_from_topic.cpp --- .../displays/camera/camera_display.cpp | 8 +-- .../image/get_transport_from_topic.cpp | 6 +- .../pointcloud/get_transport_from_topic.cpp | 65 +++++++++++++++++++ 3 files changed, 73 insertions(+), 6 deletions(-) create mode 100644 rviz_default_plugins/src/rviz_default_plugins/displays/pointcloud/get_transport_from_topic.cpp diff --git a/rviz_default_plugins/src/rviz_default_plugins/displays/camera/camera_display.cpp b/rviz_default_plugins/src/rviz_default_plugins/displays/camera/camera_display.cpp index a1c5c9564..0285bad9a 100644 --- a/rviz_default_plugins/src/rviz_default_plugins/displays/camera/camera_display.cpp +++ b/rviz_default_plugins/src/rviz_default_plugins/displays/camera/camera_display.cpp @@ -67,6 +67,7 @@ #include "rviz_common/display_context.hpp" #include "rviz_common/load_resource.hpp" +#include "rviz_default_plugins/displays/image/get_transport_from_topic.hpp" #include "rviz_default_plugins/displays/image/ros_image_texture.hpp" namespace rviz_default_plugins @@ -163,7 +164,6 @@ void CameraDisplay::onInitialize() this->addChild(visibility_property_, 0); } - void CameraDisplay::setupSceneNodes() { background_scene_node_ = scene_node_->createChildSceneNode(); @@ -323,7 +323,7 @@ void CameraDisplay::createCameraInfoSubscription() // TODO(anyone) Store this in a member variable std::string camera_info_topic = image_transport::getCameraInfoTopic( - topic_property_->getTopicStd()); + getBaseTopicFromTopic(topic_property_->getTopicStd())); rclcpp::SubscriptionOptions sub_opts; sub_opts.event_callbacks.message_lost_callback = @@ -386,7 +386,7 @@ void CameraDisplay::clear() current_caminfo_.reset(); std::string camera_info_topic = - image_transport::getCameraInfoTopic(topic_property_->getTopicStd()); + image_transport::getCameraInfoTopic(getBaseTopicFromTopic(topic_property_->getTopicStd())); setStatus( StatusLevel::Warn, CAM_INFO_STATUS, @@ -432,7 +432,7 @@ bool CameraDisplay::updateCamera() if (!info) { std::string camera_info_topic = image_transport::getCameraInfoTopic( - topic_property_->getTopicStd()); + getBaseTopicFromTopic(topic_property_->getTopicStd())); setStatus( StatusLevel::Warn, CAM_INFO_STATUS, diff --git a/rviz_default_plugins/src/rviz_default_plugins/displays/image/get_transport_from_topic.cpp b/rviz_default_plugins/src/rviz_default_plugins/displays/image/get_transport_from_topic.cpp index c34602e23..2289f03d1 100644 --- a/rviz_default_plugins/src/rviz_default_plugins/displays/image/get_transport_from_topic.cpp +++ b/rviz_default_plugins/src/rviz_default_plugins/displays/image/get_transport_from_topic.cpp @@ -39,8 +39,10 @@ namespace displays bool isRawTransport(const std::string & topic) { std::string last_subtopic = topic.substr(topic.find_last_of('/') + 1); - return last_subtopic != "compressed" && last_subtopic != "compressedDepth" && - last_subtopic != "theora"; + return last_subtopic != "compressed" && + last_subtopic != "compressedDepth" && + last_subtopic != "theora" && + last_subtopic != "ffmpeg"; } std::string getTransportFromTopic(const std::string & topic) diff --git a/rviz_default_plugins/src/rviz_default_plugins/displays/pointcloud/get_transport_from_topic.cpp b/rviz_default_plugins/src/rviz_default_plugins/displays/pointcloud/get_transport_from_topic.cpp new file mode 100644 index 000000000..14d49e180 --- /dev/null +++ b/rviz_default_plugins/src/rviz_default_plugins/displays/pointcloud/get_transport_from_topic.cpp @@ -0,0 +1,65 @@ +// Copyright (c) 2023, Open Source Robotics Foundation, 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 the copyright holder 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 "rviz_default_plugins/displays/pointcloud/get_transport_from_topic.hpp" + +namespace rviz_default_plugins +{ +namespace displays +{ + +bool isPointCloud2RawTransport(const std::string & topic) +{ + std::string last_subtopic = topic.substr(topic.find_last_of('/') + 1); + return last_subtopic != "draco" && last_subtopic != "zlib" && + last_subtopic != "pcl" && last_subtopic != "zstd" && + last_subtopic != "cloudini"; +} + +std::string getPointCloud2TransportFromTopic(const std::string & topic) +{ + if (isPointCloud2RawTransport(topic)) { + return "raw"; + } + return topic.substr(topic.find_last_of('/') + 1); +} + +std::string getPointCloud2BaseTopicFromTopic(const std::string & topic) +{ + if (isPointCloud2RawTransport(topic)) { + return topic; + } + return topic.substr(0, topic.find_last_of('/')); +} + +} // end namespace displays +} // end namespace rviz_default_plugins From e78bf83c255582f88b4e970ae9df540d98a423a3 Mon Sep 17 00:00:00 2001 From: Alejandro Hernandez Cordero Date: Fri, 29 Aug 2025 11:41:23 +0200 Subject: [PATCH 2/2] Fixed merge Signed-off-by: Alejandro Hernandez Cordero --- .../pointcloud/get_transport_from_topic.cpp | 65 ------------------- 1 file changed, 65 deletions(-) delete mode 100644 rviz_default_plugins/src/rviz_default_plugins/displays/pointcloud/get_transport_from_topic.cpp diff --git a/rviz_default_plugins/src/rviz_default_plugins/displays/pointcloud/get_transport_from_topic.cpp b/rviz_default_plugins/src/rviz_default_plugins/displays/pointcloud/get_transport_from_topic.cpp deleted file mode 100644 index 14d49e180..000000000 --- a/rviz_default_plugins/src/rviz_default_plugins/displays/pointcloud/get_transport_from_topic.cpp +++ /dev/null @@ -1,65 +0,0 @@ -// Copyright (c) 2023, Open Source Robotics Foundation, 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 the copyright holder 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 "rviz_default_plugins/displays/pointcloud/get_transport_from_topic.hpp" - -namespace rviz_default_plugins -{ -namespace displays -{ - -bool isPointCloud2RawTransport(const std::string & topic) -{ - std::string last_subtopic = topic.substr(topic.find_last_of('/') + 1); - return last_subtopic != "draco" && last_subtopic != "zlib" && - last_subtopic != "pcl" && last_subtopic != "zstd" && - last_subtopic != "cloudini"; -} - -std::string getPointCloud2TransportFromTopic(const std::string & topic) -{ - if (isPointCloud2RawTransport(topic)) { - return "raw"; - } - return topic.substr(topic.find_last_of('/') + 1); -} - -std::string getPointCloud2BaseTopicFromTopic(const std::string & topic) -{ - if (isPointCloud2RawTransport(topic)) { - return topic; - } - return topic.substr(0, topic.find_last_of('/')); -} - -} // end namespace displays -} // end namespace rviz_default_plugins