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 053d03510..2818eaa1f 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 @@ -70,6 +70,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 @@ -188,7 +189,6 @@ void CameraDisplay::onInitialize() this->addChild(visibility_property_, 0); } - void CameraDisplay::setupSceneNodes() { background_scene_node_ = scene_node_->createChildSceneNode(); @@ -349,7 +349,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 = @@ -412,7 +412,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, @@ -459,7 +459,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 9c9df5e2f..ed6c17b4e 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 @@ -40,8 +40,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 index 0d9a1dce2..14d49e180 100644 --- 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 @@ -41,7 +41,8 @@ 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 != "pcl" && last_subtopic != "zstd" && + last_subtopic != "cloudini"; } std::string getPointCloud2TransportFromTopic(const std::string & topic)