diff --git a/.gitignore b/.gitignore index db54a93ea..d756dcf7e 100644 --- a/.gitignore +++ b/.gitignore @@ -4,3 +4,4 @@ bin build lib *.sublime-workspace +**/.idea/** diff --git a/README.md b/README.md index fd167b6c7..a35540908 100644 --- a/README.md +++ b/README.md @@ -50,7 +50,6 @@ These features have not been ported to `ros2/rviz` yet. | Effort | Other features: -- Image transport features - Stereo In case you wished to see those features in RViz for ROS 2, feel free to add a pull request. diff --git a/rviz_default_plugins/CMakeLists.txt b/rviz_default_plugins/CMakeLists.txt index d6b1e67f4..f278e8ffa 100644 --- a/rviz_default_plugins/CMakeLists.txt +++ b/rviz_default_plugins/CMakeLists.txt @@ -59,6 +59,7 @@ find_package(rviz_ogre_vendor REQUIRED) find_package(Qt5 REQUIRED COMPONENTS Widgets Test) find_package(geometry_msgs REQUIRED) +find_package(image_transport REQUIRED) find_package(interactive_markers REQUIRED) find_package(laser_geometry REQUIRED) find_package(map_msgs REQUIRED) @@ -79,7 +80,9 @@ set(rviz_default_plugins_headers_to_moc include/rviz_default_plugins/displays/grid/grid_display.hpp include/rviz_default_plugins/displays/grid_cells/grid_cells_display.hpp include/rviz_default_plugins/displays/illuminance/illuminance_display.hpp + include/rviz_default_plugins/displays/image/get_transport_from_topic.hpp include/rviz_default_plugins/displays/image/image_display.hpp + include/rviz_default_plugins/displays/image/image_transport_display.hpp include/rviz_default_plugins/displays/laser_scan/laser_scan_display.hpp include/rviz_default_plugins/displays/map/map_display.hpp include/rviz_default_plugins/displays/marker/marker_common.hpp @@ -132,6 +135,7 @@ set(rviz_default_plugins_source_files src/rviz_default_plugins/displays/grid_cells/grid_cells_display.cpp src/rviz_default_plugins/displays/fluid_pressure/fluid_pressure_display.cpp src/rviz_default_plugins/displays/illuminance/illuminance_display.cpp + src/rviz_default_plugins/displays/image/get_transport_from_topic.cpp src/rviz_default_plugins/displays/image/image_display.cpp src/rviz_default_plugins/displays/image/ros_image_texture.cpp src/rviz_default_plugins/displays/interactive_markers/integer_action.cpp @@ -252,6 +256,7 @@ ament_target_dependencies(rviz_default_plugins tf2_ros urdf visualization_msgs + image_transport ) ament_export_include_directories(include) @@ -362,6 +367,13 @@ if(BUILD_TESTING) ament_target_dependencies(frame_info_test ${TEST_TARGET_DEPENDENCIES}) endif() + ament_add_gmock(get_transport_from_topic_test + test/rviz_default_plugins/displays/image/get_transport_from_topic_test.cpp) + if(TARGET get_transport_from_topic_test) + target_include_directories(get_transport_from_topic_test PUBLIC ${TEST_INCLUDE_DIRS} ${rviz_common_INCLUDE_DIRS}) + target_link_libraries(get_transport_from_topic_test ${TEST_LINK_LIBRARIES} ${rviz_common}) + endif() + ament_add_gmock(grid_cells_display_test test/rviz_default_plugins/displays/grid_cells/grid_cells_display_test.cpp ${SKIP_DISPLAY_TESTS}) @@ -1014,3 +1026,4 @@ if(BUILD_TESTING) endif() ament_package() + diff --git a/rviz_default_plugins/include/rviz_default_plugins/displays/camera/camera_display.hpp b/rviz_default_plugins/include/rviz_default_plugins/displays/camera/camera_display.hpp index eb3c1407f..56181e04b 100644 --- a/rviz_default_plugins/include/rviz_default_plugins/displays/camera/camera_display.hpp +++ b/rviz_default_plugins/include/rviz_default_plugins/displays/camera/camera_display.hpp @@ -1,6 +1,7 @@ /* * Copyright (c) 2009, Willow Garage, Inc. * Copyright (c) 2018, Bosch Software Innovations GmbH. + * Copyright (c) 2020, TNG Technology Consulting GmbH. * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -47,10 +48,11 @@ # include "sensor_msgs/msg/camera_info.hpp" -# include "rviz_rendering/render_window.hpp" -# include "rviz_common/message_filter_display.hpp" +# include "rviz_default_plugins/displays/image/image_transport_display.hpp" # include "rviz_default_plugins/displays/image/ros_image_texture_iface.hpp" # include "rviz_default_plugins/visibility_control.hpp" +# include "rviz_rendering/render_window.hpp" + #endif namespace Ogre @@ -65,17 +67,16 @@ namespace rviz_common { class QueueSizeProperty; + class RenderPanel; namespace properties { - class EnumProperty; class FloatProperty; class IntProperty; class RosTopicProperty; class DisplayGroupVisibilityProperty; - } // namespace properties } // namespace rviz_common @@ -96,7 +97,7 @@ struct ImageDimensions * */ class RVIZ_DEFAULT_PLUGINS_PUBLIC CameraDisplay - : public rviz_common::MessageFilterDisplay, + : public rviz_default_plugins::displays::ImageTransportDisplay, public Ogre::RenderTargetListener { Q_OBJECT @@ -197,7 +198,9 @@ private Q_SLOTS: bool force_render_; uint32_t vis_bit_; + void setupSceneNodes(); + void setupRenderPanel(); }; diff --git a/rviz_default_plugins/include/rviz_default_plugins/displays/image/get_transport_from_topic.hpp b/rviz_default_plugins/include/rviz_default_plugins/displays/image/get_transport_from_topic.hpp new file mode 100644 index 000000000..7c06494d6 --- /dev/null +++ b/rviz_default_plugins/include/rviz_default_plugins/displays/image/get_transport_from_topic.hpp @@ -0,0 +1,52 @@ +/* + * Copyright (c) 2020, TNG Technology Consulting GmbH. + * 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 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. + */ + +#ifndef RVIZ_DEFAULT_PLUGINS__DISPLAYS__IMAGE__GET_TRANSPORT_FROM_TOPIC_HPP_ +#define RVIZ_DEFAULT_PLUGINS__DISPLAYS__IMAGE__GET_TRANSPORT_FROM_TOPIC_HPP_ + +#include + +#include "rviz_default_plugins/visibility_control.hpp" + +namespace rviz_default_plugins +{ +namespace displays +{ + +RVIZ_DEFAULT_PLUGINS_PUBLIC +std::string getTransportFromTopic(const std::string & topic); + +RVIZ_DEFAULT_PLUGINS_PUBLIC +std::string getBaseTopicFromTopic(const std::string & topic); + + +} // end namespace displays +} // end namespace rviz_default_plugins + +#endif // RVIZ_DEFAULT_PLUGINS__DISPLAYS__IMAGE__GET_TRANSPORT_FROM_TOPIC_HPP_ diff --git a/rviz_default_plugins/include/rviz_default_plugins/displays/image/image_display.hpp b/rviz_default_plugins/include/rviz_default_plugins/displays/image/image_display.hpp index d52d4403f..bec0b1a6a 100644 --- a/rviz_default_plugins/include/rviz_default_plugins/displays/image/image_display.hpp +++ b/rviz_default_plugins/include/rviz_default_plugins/displays/image/image_display.hpp @@ -1,6 +1,7 @@ /* * Copyright (c) 2012, Willow Garage, Inc. * Copyright (c) 2017, Bosch Software Innovations GmbH. + * Copyright (c) 2020, TNG Technology Consulting GmbH. * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -49,6 +50,7 @@ # include "rviz_default_plugins/displays/image/ros_image_texture_iface.hpp" # include "rviz_default_plugins/visibility_control.hpp" +#include "rviz_default_plugins/displays/image/image_transport_display.hpp" #endif @@ -68,7 +70,7 @@ namespace displays * */ class RVIZ_DEFAULT_PLUGINS_PUBLIC ImageDisplay : public - rviz_common::MessageFilterDisplay + rviz_default_plugins::displays::ImageTransportDisplay { Q_OBJECT diff --git a/rviz_default_plugins/include/rviz_default_plugins/displays/image/image_transport_display.hpp b/rviz_default_plugins/include/rviz_default_plugins/displays/image/image_transport_display.hpp new file mode 100644 index 000000000..ae97c4ddb --- /dev/null +++ b/rviz_default_plugins/include/rviz_default_plugins/displays/image/image_transport_display.hpp @@ -0,0 +1,223 @@ +/* +* Copyright (c) 2012, Willow Garage, Inc. +* Copyright (c) 2017, Bosch Software Innovations GmbH. +* Copyright (c) 2020, TNG Technology Consulting GmbH. +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted (subject to the limitations in the disclaimer +* below) 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. +* +* NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS +* LICENSE. 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 RVIZ_DEFAULT_PLUGINS__DISPLAYS__IMAGE__IMAGE_TRANSPORT_DISPLAY_HPP_ +#define RVIZ_DEFAULT_PLUGINS__DISPLAYS__IMAGE__IMAGE_TRANSPORT_DISPLAY_HPP_ + +#include + +#include "get_transport_from_topic.hpp" +#include "image_transport/image_transport.hpp" +#include "image_transport/subscriber_filter.hpp" +#include "tf2_ros/message_filter.h" +#include "rviz_common/ros_topic_display.hpp" + +namespace rviz_default_plugins +{ +namespace displays +{ + +template +class ImageTransportDisplay : public rviz_common::_RosTopicDisplay +{ +// No Q_OBJECT macro here, moc does not support Q_OBJECT in a templated class. + +public: +/// Convenience typedef so subclasses don't have to use +/// the long templated class name to refer to their super class. + typedef ImageTransportDisplay ITDClass; + + ImageTransportDisplay() + : tf_filter_(nullptr), + messages_received_(0) + { + // TODO(Martin-Idel-SI): We need a way to extract the MessageType from the template to set a + // correct string. Previously was: + // QString message_type = + // QString::fromStdString(message_filters::message_traits::datatype()); + QString message_type = QString::fromStdString(""); + topic_property_->setMessageType(message_type); + topic_property_->setDescription(message_type + " topic to subscribe to."); + } + +/** +* When overriding this method, the onInitialize() method of this superclass has to be called. +* Otherwise, the ros node will not be initialized. +*/ + void onInitialize() override + { + _RosTopicDisplay::onInitialize(); + } + + ~ImageTransportDisplay() override + { + unsubscribe(); + } + + void reset() override + { + Display::reset(); + if (tf_filter_) { + tf_filter_->clear(); + } + messages_received_ = 0; + } + + void setTopic(const QString & topic, const QString & datatype) override + { + (void) datatype; + topic_property_->setString(topic); + } + +protected: + void updateTopic() override + { + resetSubscription(); + } + + virtual void subscribe() + { + if (!isEnabled()) { + return; + } + + if (topic_property_->isEmpty()) { + setStatus( + rviz_common::properties::StatusProperty::Error, "Topic", + QString("Error subscribing: Empty topic name")); + return; + } + + try { + subscription_ = std::make_shared(); + subscription_->subscribe( + rviz_ros_node_.lock()->get_raw_node().get(), + getBaseTopicFromTopic(topic_property_->getTopicStd()), + getTransportFromTopic(topic_property_->getTopicStd()), + qos_profile.get_rmw_qos_profile()); + tf_filter_ = + std::make_shared>( + *context_->getFrameManager()->getTransformer(), + fixed_frame_.toStdString(), 10, rviz_ros_node_.lock()->get_raw_node()); + tf_filter_->connectInput(*subscription_); + tf_filter_->registerCallback( + std::bind( + &ImageTransportDisplay::incomingMessage, this, + std::placeholders::_1)); + setStatus(rviz_common::properties::StatusProperty::Ok, "Topic", "OK"); + } catch (rclcpp::exceptions::InvalidTopicNameError & e) { + setStatus( + rviz_common::properties::StatusProperty::Error, "Topic", + QString("Error subscribing: ") + e.what()); + } + } + + void transformerChangedCallback() override + { + resetSubscription(); + } + + void resetSubscription() + { + unsubscribe(); + reset(); + subscribe(); + context_->queueRender(); + } + + virtual void unsubscribe() + { + subscription_.reset(); + tf_filter_.reset(); + } + + void onEnable() override + { + subscribe(); + } + + void onDisable() override + { + unsubscribe(); + reset(); + } + + void fixedFrameChanged() override + { + if (tf_filter_) { + tf_filter_->setTargetFrame(fixed_frame_.toStdString()); + } + reset(); + } + +/// Incoming message callback. +/** +* Checks if the message pointer +* is valid, increments messages_received_, then calls +* processMessage(). +*/ + void incomingMessage(const typename MessageType::ConstSharedPtr msg) + { + if (!msg) { + return; + } + + ++messages_received_; + setStatus( + rviz_common::properties::StatusProperty::Ok, + "Topic", + QString::number(messages_received_) + " messages received"); + + processMessage(msg); + } + + +/// Implement this to process the contents of a message. +/** +* This is called by incomingMessage(). +*/ + virtual void processMessage(typename MessageType::ConstSharedPtr msg) = 0; + + std::shared_ptr> tf_filter_; + uint32_t messages_received_; + + std::shared_ptr subscription_; +}; + +} // end namespace displays +} // end namespace rviz_default_plugins + + +#endif // RVIZ_DEFAULT_PLUGINS__DISPLAYS__IMAGE__IMAGE_TRANSPORT_DISPLAY_HPP_ diff --git a/rviz_default_plugins/package.xml b/rviz_default_plugins/package.xml index 618d873aa..f43c2b213 100644 --- a/rviz_default_plugins/package.xml +++ b/rviz_default_plugins/package.xml @@ -35,6 +35,7 @@ rviz_ogre_vendor geometry_msgs + image_transport interactive_markers laser_geometry nav_msgs 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 e871d3edb..81a8c78e4 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 @@ -1,6 +1,7 @@ /* * Copyright (c) 2012, Willow Garage, Inc. * Copyright (c) 2018, Bosch Software Innovations GmbH. + * Copyright (c) 2020, TNG Technology Consulting GmbH. * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -45,6 +46,8 @@ #include #include +#include "image_transport/camera_common.hpp" + #include "rviz_rendering/material_manager.hpp" #include "rviz_rendering/objects/axes.hpp" #include "rviz_rendering/render_window.hpp" @@ -130,7 +133,7 @@ CameraDisplay::~CameraDisplay() void CameraDisplay::onInitialize() { - MFDClass::onInitialize(); + ITDClass::onInitialize(); setupSceneNodes(); setupRenderPanel(); @@ -265,7 +268,7 @@ void CameraDisplay::onDisable() void CameraDisplay::subscribe() { - MFDClass::subscribe(); + ITDClass::subscribe(); if ((!isEnabled()) || (topic_property_->getTopicStd().empty())) { return; @@ -281,9 +284,9 @@ void CameraDisplay::createCameraInfoSubscription() // The camera_info topic should be at the same level as the image topic // TODO(anyone) Store this in a member variable - auto camera_info_topic = topic_property_->getTopicStd(); - camera_info_topic = - camera_info_topic.substr(0, camera_info_topic.rfind("/") + 1) + "camera_info"; + + std::string camera_info_topic = image_transport::getCameraInfoTopic( + topic_property_->getTopicStd()); caminfo_sub_ = rviz_ros_node_.lock()->get_raw_node()-> template create_subscription( @@ -302,7 +305,7 @@ void CameraDisplay::createCameraInfoSubscription() void CameraDisplay::unsubscribe() { - MFDClass::unsubscribe(); + ITDClass::unsubscribe(); caminfo_sub_.reset(); } @@ -332,9 +335,8 @@ void CameraDisplay::clear() new_caminfo_ = false; current_caminfo_.reset(); - auto camera_info_topic = topic_property_->getTopicStd(); - camera_info_topic = - camera_info_topic.substr(0, camera_info_topic.rfind("/") + 1) + "camera_info"; + std::string camera_info_topic = + image_transport::getCameraInfoTopic(topic_property_->getTopicStd()); setStatus( StatusLevel::Warn, CAM_INFO_STATUS, @@ -375,9 +377,8 @@ bool CameraDisplay::updateCamera() } if (!info) { - auto camera_info_topic = topic_property_->getTopicStd(); - camera_info_topic = - camera_info_topic.substr(0, camera_info_topic.rfind("/") + 1) + "camera_info"; + std::string camera_info_topic = image_transport::getCameraInfoTopic( + topic_property_->getTopicStd()); setStatus( StatusLevel::Warn, CAM_INFO_STATUS, @@ -572,7 +573,7 @@ void CameraDisplay::processMessage(sensor_msgs::msg::Image::ConstSharedPtr msg) void CameraDisplay::reset() { - MFDClass::reset(); + ITDClass::reset(); clear(); } 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 new file mode 100644 index 000000000..c34602e23 --- /dev/null +++ b/rviz_default_plugins/src/rviz_default_plugins/displays/image/get_transport_from_topic.cpp @@ -0,0 +1,63 @@ +/* + * Copyright (c) 2020, TNG Technology Consulting GmbH. + * 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 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. + */ + +#include + +#include "rviz_default_plugins/displays/image/get_transport_from_topic.hpp" + +namespace rviz_default_plugins +{ +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"; +} + +std::string getTransportFromTopic(const std::string & topic) +{ + if (isRawTransport(topic)) { + return "raw"; + } + return topic.substr(topic.find_last_of('/') + 1); +} + +std::string getBaseTopicFromTopic(const std::string & topic) +{ + if (isRawTransport(topic)) { + return topic; + } + return topic.substr(0, topic.find_last_of('/')); +} + +} // end namespace displays +} // end namespace rviz_default_plugins diff --git a/rviz_default_plugins/src/rviz_default_plugins/displays/image/image_display.cpp b/rviz_default_plugins/src/rviz_default_plugins/displays/image/image_display.cpp index 8977d8c96..c283bd04b 100644 --- a/rviz_default_plugins/src/rviz_default_plugins/displays/image/image_display.cpp +++ b/rviz_default_plugins/src/rviz_default_plugins/displays/image/image_display.cpp @@ -102,7 +102,7 @@ ImageDisplay::ImageDisplay(std::unique_ptr texture) void ImageDisplay::onInitialize() { - MFDClass::onInitialize(); + ITDClass::onInitialize(); updateNormalizeOptions(); setupScreenRectangle(); @@ -119,12 +119,12 @@ ImageDisplay::~ImageDisplay() = default; void ImageDisplay::onEnable() { - MFDClass::subscribe(); + ITDClass::subscribe(); } void ImageDisplay::onDisable() { - MFDClass::unsubscribe(); + ITDClass::unsubscribe(); clear(); } @@ -187,7 +187,7 @@ void ImageDisplay::update(float wall_dt, float ros_dt) void ImageDisplay::reset() { - MFDClass::reset(); + ITDClass::reset(); clear(); } diff --git a/rviz_default_plugins/test/rviz_default_plugins/displays/image/get_transport_from_topic_test.cpp b/rviz_default_plugins/test/rviz_default_plugins/displays/image/get_transport_from_topic_test.cpp new file mode 100644 index 000000000..d2273048c --- /dev/null +++ b/rviz_default_plugins/test/rviz_default_plugins/displays/image/get_transport_from_topic_test.cpp @@ -0,0 +1,58 @@ +/* + * Copyright (c) 2020, TNG Technology Consulting GmbH. + * 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 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. + */ + +#include +#include + +#include "rviz_default_plugins/displays/image/get_transport_from_topic.hpp" + +using namespace rviz_default_plugins::displays; // NOLINT + +TEST(getTransportFromTopicTest, get_transport_from_topic_finds_right_transport) +{ + EXPECT_EQ(getTransportFromTopic("/image/compressed"), "compressed"); + EXPECT_EQ(getTransportFromTopic("/image_transport"), "raw"); + EXPECT_EQ(getTransportFromTopic("/image_compressed"), "raw"); + EXPECT_EQ(getTransportFromTopic("/image_transport/camera/compressedDepth"), "compressedDepth"); + EXPECT_EQ(getTransportFromTopic("/topic_name/publisher_name/compressed_and_theora"), "raw"); +} + +TEST(getBaseTopicFromTopicTest, get_transport_from_topic_finds_right_topic) +{ + EXPECT_EQ(getBaseTopicFromTopic("/image/compressed"), "/image"); + EXPECT_EQ(getBaseTopicFromTopic("/image_transport"), "/image_transport"); + EXPECT_EQ(getBaseTopicFromTopic("/image_compressed"), "/image_compressed"); + EXPECT_EQ( + getBaseTopicFromTopic( + "/image_transport/camera/compressedDepth"), "/image_transport/camera"); + EXPECT_EQ( + getBaseTopicFromTopic( + "/topic_name/publisher_name/compressed_and_theora"), + "/topic_name/publisher_name/compressed_and_theora"); +}