|
| 1 | +// Copyright (c) 2012, Willow Garage, Inc. |
| 2 | +// Copyright (c) 2017, Bosch Software Innovations GmbH. |
| 3 | +// All rights reserved. |
| 4 | +// |
| 5 | +// Redistribution and use in source and binary forms, with or without |
| 6 | +// modification, are permitted provided that the following conditions are met: |
| 7 | +// |
| 8 | +// * Redistributions of source code must retain the above copyright |
| 9 | +// notice, this list of conditions and the following disclaimer. |
| 10 | +// |
| 11 | +// * Redistributions in binary form must reproduce the above copyright |
| 12 | +// notice, this list of conditions and the following disclaimer in the |
| 13 | +// documentation and/or other materials provided with the distribution. |
| 14 | +// |
| 15 | +// * Neither the name of the copyright holder nor the names of its |
| 16 | +// contributors may be used to endorse or promote products derived from |
| 17 | +// this software without specific prior written permission. |
| 18 | +// |
| 19 | +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" |
| 20 | +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE |
| 21 | +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE |
| 22 | +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE |
| 23 | +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR |
| 24 | +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF |
| 25 | +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS |
| 26 | +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN |
| 27 | +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) |
| 28 | +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
| 29 | +// POSSIBILITY OF SUCH DAMAGE. |
| 30 | + |
| 31 | +#ifndef RVIZ_COMMON__ROS_ADAPTER_DISPLAY_HPP_ |
| 32 | +#define RVIZ_COMMON__ROS_ADAPTER_DISPLAY_HPP_ |
| 33 | + |
| 34 | +#include <cstdint> |
| 35 | +#include <functional> |
| 36 | +#include <memory> |
| 37 | +#include <sstream> |
| 38 | +#include <stdexcept> |
| 39 | +#include <string> |
| 40 | + |
| 41 | +#include <QString> // NOLINT: cpplint is unable to handle the include order here |
| 42 | + |
| 43 | +#include "rviz_common/ros_topic_display.hpp" |
| 44 | + |
| 45 | +#include "rclcpp/qos.hpp" |
| 46 | +#include "rclcpp/node.hpp" |
| 47 | +#include "rclcpp/subscription_options.hpp" |
| 48 | +#include "rclcpp/time.hpp" |
| 49 | +#include "rclcpp/type_adapter.hpp" |
| 50 | + |
| 51 | +namespace rviz_common |
| 52 | +{ |
| 53 | + |
| 54 | +/** @brief Display subclass using a rclcpp::subscription, |
| 55 | + * templated on the ROS message type and a custom type, to support REP 2007 |
| 56 | + * |
| 57 | + * This class handles subscribing and unsubscribing to a ROS node when the display is |
| 58 | + * enabled or disabled. */ |
| 59 | +template<class MessageType, class AdaptedType> |
| 60 | +class RosAdapterDisplay : public rviz_common::_RosTopicDisplay |
| 61 | +{ |
| 62 | +// No Q_OBJECT macro here, moc does not support Q_OBJECT in a templated class. |
| 63 | + |
| 64 | +public: |
| 65 | + /** @brief Convenience typedef so subclasses don't have to use |
| 66 | + * the long templated class name to refer to their super class. */ |
| 67 | + typedef RosAdapterDisplay<MessageType, AdaptedType> RADClass; |
| 68 | + using Adapter = typename rclcpp::adapt_type<AdaptedType>::as<MessageType>; |
| 69 | + |
| 70 | + RosAdapterDisplay() |
| 71 | + : messages_received_(0) |
| 72 | + { |
| 73 | + QString message_type = QString::fromStdString(rosidl_generator_traits::name<MessageType>()); |
| 74 | + topic_property_->setMessageType(message_type); |
| 75 | + topic_property_->setDescription(message_type + " topic to subscribe to."); |
| 76 | + } |
| 77 | + |
| 78 | + ~RosAdapterDisplay() override |
| 79 | + { |
| 80 | + unsubscribe(); |
| 81 | + } |
| 82 | + |
| 83 | + void reset() override |
| 84 | + { |
| 85 | + Display::reset(); |
| 86 | + messages_received_ = 0; |
| 87 | + } |
| 88 | + |
| 89 | + void setTopic(const QString & topic, const QString & datatype) override |
| 90 | + { |
| 91 | + (void) datatype; |
| 92 | + topic_property_->setString(topic); |
| 93 | + } |
| 94 | + |
| 95 | +protected: |
| 96 | + void updateTopic() override |
| 97 | + { |
| 98 | + unsubscribe(); |
| 99 | + reset(); |
| 100 | + subscribe(); |
| 101 | + context_->queueRender(); |
| 102 | + } |
| 103 | + |
| 104 | + virtual void subscribe() |
| 105 | + { |
| 106 | + if (!isEnabled() ) { |
| 107 | + return; |
| 108 | + } |
| 109 | + |
| 110 | + if (topic_property_->isEmpty()) { |
| 111 | + setStatus( |
| 112 | + properties::StatusProperty::Error, |
| 113 | + "Topic", |
| 114 | + QString("Error subscribing: Empty topic name")); |
| 115 | + return; |
| 116 | + } |
| 117 | + |
| 118 | + try { |
| 119 | + rclcpp::SubscriptionOptions sub_opts; |
| 120 | + sub_opts.event_callbacks.message_lost_callback = |
| 121 | + [&](rclcpp::QOSMessageLostInfo & info) |
| 122 | + { |
| 123 | + std::ostringstream sstm; |
| 124 | + sstm << "Some messages were lost:\n>\tNumber of new lost messages: " << |
| 125 | + info.total_count_change << " \n>\tTotal number of messages lost: " << |
| 126 | + info.total_count; |
| 127 | + setStatus(properties::StatusProperty::Warn, "Topic", QString(sstm.str().c_str())); |
| 128 | + }; |
| 129 | + |
| 130 | + rclcpp::Node::SharedPtr node = rviz_ros_node_.lock()->get_raw_node(); |
| 131 | + subscription_ = |
| 132 | + node-> template create_subscription<Adapter>( |
| 133 | + topic_property_->getTopicStd(), |
| 134 | + qos_profile, |
| 135 | + std::bind( |
| 136 | + &RosAdapterDisplay<MessageType, AdaptedType>::incomingMessage, this, |
| 137 | + std::placeholders::_1), |
| 138 | + sub_opts); |
| 139 | + subscription_start_time_ = node->now(); |
| 140 | + setStatus(rviz_common::properties::StatusProperty::Ok, "Topic", "OK"); |
| 141 | + } catch (rclcpp::exceptions::InvalidTopicNameError & e) { |
| 142 | + setStatus( |
| 143 | + rviz_common::properties::StatusProperty::Error, "Topic", |
| 144 | + QString("Error subscribing: ") + e.what()); |
| 145 | + } |
| 146 | + } |
| 147 | + |
| 148 | + virtual void unsubscribe() |
| 149 | + { |
| 150 | + subscription_.reset(); |
| 151 | + } |
| 152 | + |
| 153 | + void onEnable() override |
| 154 | + { |
| 155 | + subscribe(); |
| 156 | + } |
| 157 | + |
| 158 | + void onDisable() override |
| 159 | + { |
| 160 | + unsubscribe(); |
| 161 | + reset(); |
| 162 | + } |
| 163 | + |
| 164 | + void fixedFrameChanged() override |
| 165 | + { |
| 166 | + reset(); |
| 167 | + } |
| 168 | + |
| 169 | + /** @brief Incoming message callback. Checks if the message pointer |
| 170 | + * is valid, increments messages_received_, then calls |
| 171 | + * processMessage(). */ |
| 172 | + void incomingMessage(const typename std::shared_ptr<const AdaptedType> msg) |
| 173 | + { |
| 174 | + if (!msg) { |
| 175 | + return; |
| 176 | + } |
| 177 | + |
| 178 | + ++messages_received_; |
| 179 | + QString topic_str = QString::number(messages_received_) + " messages received"; |
| 180 | + rviz_common::properties::StatusProperty::Level topic_status_level = |
| 181 | + rviz_common::properties::StatusProperty::Ok; |
| 182 | + // Append topic subscription frequency if we can lock rviz_ros_node_. |
| 183 | + std::shared_ptr<ros_integration::RosNodeAbstractionIface> node_interface = |
| 184 | + rviz_ros_node_.lock(); |
| 185 | + if (node_interface != nullptr) { |
| 186 | + try { |
| 187 | + const double duration = |
| 188 | + (node_interface->get_raw_node()->now() - subscription_start_time_).seconds(); |
| 189 | + const double subscription_frequency = |
| 190 | + static_cast<double>(messages_received_) / duration; |
| 191 | + topic_str += " at " + QString::number(subscription_frequency, 'f', 1) + " hz."; |
| 192 | + } catch (const std::runtime_error & e) { |
| 193 | + if (std::string(e.what()).find("can't subtract times with different time sources") != |
| 194 | + std::string::npos) |
| 195 | + { |
| 196 | + topic_status_level = rviz_common::properties::StatusProperty::Warn; |
| 197 | + topic_str += ". "; |
| 198 | + topic_str += e.what(); |
| 199 | + } else { |
| 200 | + throw; |
| 201 | + } |
| 202 | + } |
| 203 | + } |
| 204 | + setStatus( |
| 205 | + topic_status_level, |
| 206 | + "Topic", |
| 207 | + topic_str); |
| 208 | + |
| 209 | + processMessage(msg); |
| 210 | + } |
| 211 | + |
| 212 | + /** @brief Implement this to process the contents of a message. |
| 213 | + * |
| 214 | + * This is called by incomingMessage(). */ |
| 215 | + virtual void processMessage(const typename std::shared_ptr<const AdaptedType> msg) = 0; |
| 216 | + |
| 217 | + typename rclcpp::Subscription<Adapter>::SharedPtr subscription_; |
| 218 | + rclcpp::Time subscription_start_time_; |
| 219 | + uint32_t messages_received_; |
| 220 | +}; |
| 221 | + |
| 222 | +} // end namespace rviz_common |
| 223 | + |
| 224 | +#endif // RVIZ_COMMON__ROS_ADAPTER_DISPLAY_HPP_ |
0 commit comments