From 9599dd488d543671121c40df9aec5533064e86fb Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Tue, 18 Jul 2023 17:10:24 +0200 Subject: [PATCH] Added TwistStamped and AccelStamped default plugins (#991) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero --- rviz_default_plugins/CMakeLists.txt | 36 +++ .../displays/accel/accel_display.hpp | 55 ++++ .../displays/screw/screw_display.hpp | 95 +++++++ .../displays/twist/twist_display.hpp | 55 ++++ rviz_default_plugins/plugins_description.xml | 21 ++ .../displays/accel/accel_display.cpp | 49 ++++ .../displays/screw/screw_display.cpp | 248 ++++++++++++++++++ .../displays/twist/twist_display.cpp | 49 ++++ .../accels_are_displayed_ref.png | Bin 0 -> 27916 bytes .../twists_are_displayed_ref.png | Bin 0 -> 27916 bytes .../accel/accel_display_visual_test.cpp | 61 +++++ .../twist/twist_display_visual_test.cpp | 61 +++++ .../accel_display_page_object.cpp | 79 ++++++ .../accel_display_page_object.hpp | 52 ++++ .../twist_display_page_object.cpp | 79 ++++++ .../twist_display_page_object.hpp | 51 ++++ .../publishers/accel_publisher.hpp | 92 +++++++ .../publishers/twist_publisher.hpp | 92 +++++++ rviz_rendering/CMakeLists.txt | 13 + .../rviz_rendering/objects/screw_visual.hpp | 110 ++++++++ .../rviz_rendering/objects/screw_visual.cpp | 169 ++++++++++++ .../objects/screw_visual_test.cpp | 139 ++++++++++ 22 files changed, 1606 insertions(+) create mode 100644 rviz_default_plugins/include/rviz_default_plugins/displays/accel/accel_display.hpp create mode 100644 rviz_default_plugins/include/rviz_default_plugins/displays/screw/screw_display.hpp create mode 100644 rviz_default_plugins/include/rviz_default_plugins/displays/twist/twist_display.hpp create mode 100644 rviz_default_plugins/src/rviz_default_plugins/displays/accel/accel_display.cpp create mode 100644 rviz_default_plugins/src/rviz_default_plugins/displays/screw/screw_display.cpp create mode 100644 rviz_default_plugins/src/rviz_default_plugins/displays/twist/twist_display.cpp create mode 100644 rviz_default_plugins/test/reference_images/accels_are_displayed_ref.png create mode 100644 rviz_default_plugins/test/reference_images/twists_are_displayed_ref.png create mode 100644 rviz_default_plugins/test/rviz_default_plugins/displays/accel/accel_display_visual_test.cpp create mode 100644 rviz_default_plugins/test/rviz_default_plugins/displays/twist/twist_display_visual_test.cpp create mode 100644 rviz_default_plugins/test/rviz_default_plugins/page_objects/accel_display_page_object.cpp create mode 100644 rviz_default_plugins/test/rviz_default_plugins/page_objects/accel_display_page_object.hpp create mode 100644 rviz_default_plugins/test/rviz_default_plugins/page_objects/twist_display_page_object.cpp create mode 100644 rviz_default_plugins/test/rviz_default_plugins/page_objects/twist_display_page_object.hpp create mode 100644 rviz_default_plugins/test/rviz_default_plugins/publishers/accel_publisher.hpp create mode 100644 rviz_default_plugins/test/rviz_default_plugins/publishers/twist_publisher.hpp create mode 100644 rviz_rendering/include/rviz_rendering/objects/screw_visual.hpp create mode 100644 rviz_rendering/src/rviz_rendering/objects/screw_visual.cpp create mode 100644 rviz_rendering/test/rviz_rendering/objects/screw_visual_test.cpp diff --git a/rviz_default_plugins/CMakeLists.txt b/rviz_default_plugins/CMakeLists.txt index e30919931..c42672023 100644 --- a/rviz_default_plugins/CMakeLists.txt +++ b/rviz_default_plugins/CMakeLists.txt @@ -80,6 +80,7 @@ find_package(urdf REQUIRED) find_package(visualization_msgs REQUIRED) set(rviz_default_plugins_headers_to_moc + include/rviz_default_plugins/displays/accel/accel_display.hpp include/rviz_default_plugins/displays/axes/axes_display.hpp include/rviz_default_plugins/displays/camera/camera_display.hpp include/rviz_default_plugins/displays/effort/effort_display.hpp @@ -111,9 +112,11 @@ set(rviz_default_plugins_headers_to_moc include/rviz_default_plugins/displays/range/range_display.hpp include/rviz_default_plugins/displays/relative_humidity/relative_humidity_display.hpp include/rviz_default_plugins/displays/robot_model/robot_model_display.hpp + include/rviz_default_plugins/displays/screw/screw_display.hpp include/rviz_default_plugins/displays/temperature/temperature_display.hpp include/rviz_default_plugins/displays/tf/frame_info.hpp include/rviz_default_plugins/displays/tf/tf_display.hpp + include/rviz_default_plugins/displays/twist/twist_display.hpp include/rviz_default_plugins/displays/wrench/wrench_display.hpp include/rviz_default_plugins/robot/robot.hpp include/rviz_default_plugins/robot/robot_joint.hpp @@ -134,6 +137,7 @@ foreach(header "${rviz_default_plugins_headers_to_moc}") endforeach() set(rviz_default_plugins_source_files + src/rviz_default_plugins/displays/accel/accel_display.cpp src/rviz_default_plugins/displays/axes/axes_display.cpp src/rviz_default_plugins/displays/camera/camera_display.cpp src/rviz_default_plugins/displays/effort/effort_display.cpp @@ -193,11 +197,13 @@ set(rviz_default_plugins_source_files src/rviz_default_plugins/displays/range/range_display.cpp src/rviz_default_plugins/displays/relative_humidity/relative_humidity_display.cpp src/rviz_default_plugins/displays/robot_model/robot_model_display.cpp + src/rviz_default_plugins/displays/screw/screw_display.cpp src/rviz_default_plugins/displays/temperature/temperature_display.cpp src/rviz_default_plugins/displays/tf/frame_info.cpp src/rviz_default_plugins/displays/tf/frame_selection_handler.cpp src/rviz_default_plugins/displays/tf/tf_display.cpp src/rviz_default_plugins/displays/wrench/wrench_display.cpp + src/rviz_default_plugins/displays/twist/twist_display.cpp src/rviz_default_plugins/robot/robot.cpp src/rviz_default_plugins/robot/robot_joint.cpp src/rviz_default_plugins/robot/robot_link.cpp @@ -717,6 +723,21 @@ if(BUILD_TESTING) ) endif() + ament_add_gtest(accel_display_visual_test + test/rviz_default_plugins/displays/accel/accel_display_visual_test.cpp + test/rviz_default_plugins/page_objects/accel_display_page_object.cpp + ${SKIP_VISUAL_TESTS} + TIMEOUT 180) + if(TARGET accel_display_visual_test) + target_include_directories(accel_display_visual_test PRIVATE test) + target_link_libraries(accel_display_visual_test + rviz_visual_testing_framework::rviz_visual_testing_framework + ${geometry_msgs_TARGETS} + ${std_msgs_TARGETS} + rclcpp::rclcpp + ) + endif() + ament_add_gtest(axes_display_visual_test test/rviz_default_plugins/displays/axes/axes_display_visual_test.cpp test/rviz_default_plugins/page_objects/axes_display_page_object.cpp @@ -1120,6 +1141,21 @@ if(BUILD_TESTING) ) endif() + ament_add_gtest(twist_display_visual_test + test/rviz_default_plugins/displays/twist/twist_display_visual_test.cpp + test/rviz_default_plugins/page_objects/twist_display_page_object.cpp + ${SKIP_VISUAL_TESTS} + TIMEOUT 180) + if(TARGET twist_display_visual_test) + target_include_directories(twist_display_visual_test PRIVATE test) + target_link_libraries(twist_display_visual_test + rviz_visual_testing_framework::rviz_visual_testing_framework + ${geometry_msgs_TARGETS} + ${std_msgs_TARGETS} + rclcpp::rclcpp + ) + endif() + ament_add_gtest(wrench_display_visual_test test/rviz_default_plugins/displays/wrench/wrench_stamped_display_visual_test.cpp test/rviz_default_plugins/page_objects/wrench_display_page_object.cpp diff --git a/rviz_default_plugins/include/rviz_default_plugins/displays/accel/accel_display.hpp b/rviz_default_plugins/include/rviz_default_plugins/displays/accel/accel_display.hpp new file mode 100644 index 000000000..51b0e37b4 --- /dev/null +++ b/rviz_default_plugins/include/rviz_default_plugins/displays/accel/accel_display.hpp @@ -0,0 +1,55 @@ +/* + * 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 Willow Garage, Inc. 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__ACCEL__ACCEL_DISPLAY_HPP_ +#define RVIZ_DEFAULT_PLUGINS__DISPLAYS__ACCEL__ACCEL_DISPLAY_HPP_ + +#include + +#include "rviz_default_plugins/displays/screw/screw_display.hpp" + +#include + +namespace rviz_default_plugins +{ +namespace displays +{ +class AccelStampedDisplay : public ScrewDisplay +{ + Q_OBJECT + + // Function to handle an incoming ROS message. + void processMessage(geometry_msgs::msg::AccelStamped::ConstSharedPtr msg) override; +}; + +} // namespace displays +} // namespace rviz_default_plugins + +#endif // RVIZ_DEFAULT_PLUGINS__DISPLAYS__ACCEL__ACCEL_DISPLAY_HPP_ diff --git a/rviz_default_plugins/include/rviz_default_plugins/displays/screw/screw_display.hpp b/rviz_default_plugins/include/rviz_default_plugins/displays/screw/screw_display.hpp new file mode 100644 index 000000000..ce8f33b44 --- /dev/null +++ b/rviz_default_plugins/include/rviz_default_plugins/displays/screw/screw_display.hpp @@ -0,0 +1,95 @@ +/* + * 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 Willow Garage, Inc. 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__SCREW__SCREW_DISPLAY_HPP_ +#define RVIZ_DEFAULT_PLUGINS__DISPLAYS__SCREW__SCREW_DISPLAY_HPP_ + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + + +#include "rviz_default_plugins/visibility_control.hpp" + +namespace rviz_default_plugins +{ +namespace displays +{ +template +class RVIZ_DEFAULT_PLUGINS_PUBLIC ScrewDisplay + : public rviz_common::MessageFilterDisplay +{ +public: + // Constructor. pluginlib::ClassLoader creates instances by calling + // the default constructor, so make sure you have one. + ScrewDisplay(); + ~ScrewDisplay() override = default; + +protected: + // Overrides of public virtual functions from the Display class. + void onInitialize() override; + void reset() override; + + // Helper function to properties for all visuals. + void updateProperties(); + void updateHistoryLength(); + + void processMessagePrivate( + const std_msgs::msg::Header & header, + const geometry_msgs::msg::Vector3 & linear, + const geometry_msgs::msg::Vector3 & angular); + + // Storage for the list of visuals par each joint intem + // Storage for the list of visuals. It is a circular buffer where + // data gets popped from the front (oldest) and pushed to the back (newest) + std::deque> visuals_; + + // Property objects for user-editable properties. + rviz_common::properties::ColorProperty * linear_color_property_; + rviz_common::properties::ColorProperty * angular_color_property_; + rviz_common::properties::FloatProperty * alpha_property_; + rviz_common::properties::FloatProperty * linear_scale_property_; + rviz_common::properties::FloatProperty * angular_scale_property_; + rviz_common::properties::FloatProperty * width_property_; + rviz_common::properties::IntProperty * history_length_property_; + rviz_common::properties::BoolProperty * hide_small_values_property_; +}; + +} // namespace displays +} // namespace rviz_default_plugins + +#endif // RVIZ_DEFAULT_PLUGINS__DISPLAYS__SCREW__SCREW_DISPLAY_HPP_ diff --git a/rviz_default_plugins/include/rviz_default_plugins/displays/twist/twist_display.hpp b/rviz_default_plugins/include/rviz_default_plugins/displays/twist/twist_display.hpp new file mode 100644 index 000000000..bb8be8488 --- /dev/null +++ b/rviz_default_plugins/include/rviz_default_plugins/displays/twist/twist_display.hpp @@ -0,0 +1,55 @@ +/* + * 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 Willow Garage, Inc. 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__TWIST__TWIST_DISPLAY_HPP_ +#define RVIZ_DEFAULT_PLUGINS__DISPLAYS__TWIST__TWIST_DISPLAY_HPP_ + +#include + +#include "rviz_default_plugins/displays/screw/screw_display.hpp" + +#include + +namespace rviz_default_plugins +{ +namespace displays +{ +class TwistStampedDisplay : public ScrewDisplay +{ + Q_OBJECT + + // Function to handle an incoming ROS message. + void processMessage(geometry_msgs::msg::TwistStamped::ConstSharedPtr msg) override; +}; + +} // namespace displays +} // namespace rviz_default_plugins + +#endif // RVIZ_DEFAULT_PLUGINS__DISPLAYS__TWIST__TWIST_DISPLAY_HPP_ diff --git a/rviz_default_plugins/plugins_description.xml b/rviz_default_plugins/plugins_description.xml index 79887f223..b03e0e951 100644 --- a/rviz_default_plugins/plugins_description.xml +++ b/rviz_default_plugins/plugins_description.xml @@ -1,6 +1,16 @@ + + + Displays from geometry_msgs/AccelStamped message + + geometry_msgs/msg/AccelStamped + tf2_msgs/msg/TFMessage + + + Displays from geometry_msgs/TwistStamped message + + geometry_msgs/msg/TwistStamped + + + +// Tell pluginlib about these classes. It is important to do this in +// global scope, outside our package's namespace. +#include +PLUGINLIB_EXPORT_CLASS( + rviz_default_plugins::displays::AccelStampedDisplay, + rviz_common::Display) + +namespace rviz_default_plugins +{ +namespace displays +{ +// Function to handle an incoming ROS message. +void AccelStampedDisplay::processMessage(geometry_msgs::msg::AccelStamped::ConstSharedPtr msg) +{ + processMessagePrivate(msg->header, msg->accel.linear, msg->accel.angular); +} +} // namespace displays +} // namespace rviz_default_plugins diff --git a/rviz_default_plugins/src/rviz_default_plugins/displays/screw/screw_display.cpp b/rviz_default_plugins/src/rviz_default_plugins/displays/screw/screw_display.cpp new file mode 100644 index 000000000..36d7ba508 --- /dev/null +++ b/rviz_default_plugins/src/rviz_default_plugins/displays/screw/screw_display.cpp @@ -0,0 +1,248 @@ +/* + * 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 Willow Garage, Inc. 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 "rviz_default_plugins/displays/screw/screw_display.hpp" + +#include +#include +#include + +#include +#include +#include +#include + +template +constexpr const char * linear() +{ + return "Linear"; +} +template +constexpr const char * angular() +{ + return "Angular"; +} + +namespace rviz_default_plugins +{ +namespace displays +{ +template +ScrewDisplay::ScrewDisplay() +{ + auto lin = linear(); + auto ang = angular(); + linear_color_property_ = new rviz_common::properties::ColorProperty( + QString("%1 Color").arg(lin), QColor(204, 51, 51), + QString("Color to draw the %1 arrows.").arg(QString(lin).toLower()), + this); + + QObject::connect( + linear_color_property_, &rviz_common::properties::Property::changed, + this, &ScrewDisplay::updateProperties); + + angular_color_property_ = new rviz_common::properties::ColorProperty( + QString("%1 Color").arg(ang), QColor(204, 204, 51), + QString("Color to draw the %1 arrows.").arg(QString(ang).toLower()), + this); + QObject::connect( + angular_color_property_, &rviz_common::properties::Property::changed, + this, &ScrewDisplay::updateProperties); + + alpha_property_ = new rviz_common::properties::FloatProperty( + "Alpha", 1.0, "0 is fully transparent, 1.0 is fully opaque.", + this); + QObject::connect( + alpha_property_, &rviz_common::properties::Property::changed, + this, &ScrewDisplay::updateProperties); + + linear_scale_property_ = new rviz_common::properties::FloatProperty( + QString("%1 Arrow Scale").arg(lin), 2.0, + QString("%1 arrow scale").arg(lin), this); + + QObject::connect( + linear_scale_property_, &rviz_common::properties::Property::changed, + this, &ScrewDisplay::updateProperties); + + angular_scale_property_ = new rviz_common::properties::FloatProperty( + QString("%1 Arrow Scale").arg(ang), 2.0, + QString("%1 arrow scale").arg(ang), this); + + QObject::connect( + angular_scale_property_, &rviz_common::properties::Property::changed, + this, &ScrewDisplay::updateProperties); + + width_property_ = new rviz_common::properties::FloatProperty( + "Arrow Width", 0.5, "arrow width", this); + + QObject::connect( + width_property_, &rviz_common::properties::Property::changed, + this, &ScrewDisplay::updateProperties); + + history_length_property_ = new rviz_common::properties::IntProperty( + "History Length", 1, "Number of prior measurements to display.", this); + + QObject::connect( + history_length_property_, &rviz_common::properties::Property::changed, + this, &ScrewDisplay::updateHistoryLength); + + hide_small_values_property_ = new rviz_common::properties::BoolProperty( + "Hide Small Values", true, "Hide small values", this); + + QObject::connect( + hide_small_values_property_, &rviz_common::properties::Property::changed, + this, &ScrewDisplay::updateProperties); + + history_length_property_->setMin(1); + history_length_property_->setMax(100000); +} + +template +void ScrewDisplay::onInitialize() +{ + rviz_common::MessageFilterDisplay::onInitialize(); + updateHistoryLength(); +} + +// Override rviz::Display's reset() function to add a call to clear(). +template +void ScrewDisplay::reset() +{ + rviz_common::MessageFilterDisplay::reset(); + visuals_.clear(); +} + +template +void ScrewDisplay::updateProperties() +{ + float alpha = alpha_property_->getFloat(); + float linear_scale = linear_scale_property_->getFloat(); + float angular_scale = angular_scale_property_->getFloat(); + float width = width_property_->getFloat(); + bool hide_small_values = hide_small_values_property_->getBool(); + Ogre::ColourValue linear_color = linear_color_property_->getOgreColor(); + Ogre::ColourValue angular_color = angular_color_property_->getOgreColor(); + + for (size_t i = 0; i < visuals_.size(); i++) { + visuals_[i]->setLinearColor(linear_color.r, linear_color.g, linear_color.b, alpha); + visuals_[i]->setAngularColor(angular_color.r, angular_color.g, angular_color.b, alpha); + visuals_[i]->setLinearScale(linear_scale); + visuals_[i]->setAngularScale(angular_scale); + visuals_[i]->setWidth(width); + visuals_[i]->setHideSmallValues(hide_small_values); + } +} + +// Set the number of past visuals to show. +template +void ScrewDisplay::updateHistoryLength() +{ + while (visuals_.size() > static_cast(history_length_property_->getInt())) { + visuals_.pop_front(); + } +} + +// This is our callback to handle an incoming message. +template +void ScrewDisplay::processMessagePrivate( + const std_msgs::msg::Header & header, + const geometry_msgs::msg::Vector3 & linear, + const geometry_msgs::msg::Vector3 & angular) +{ + if (!(rviz_common::validateFloats(linear) && rviz_common::validateFloats(angular))) { + rviz_common::MessageFilterDisplay::setStatus( + rviz_common::properties::StatusProperty::Error, "Topic", + "Message contained invalid floating point values (nans or infs)"); + return; + } + + // Here we call the rviz::FrameManager to get the transform from the + // fixed frame to the frame in the header of this Imu message. If + // it fails, we can't do anything else so we return. + Ogre::Quaternion orientation; + Ogre::Vector3 position; + if (!rviz_common::MessageFilterDisplay::context_->getFrameManager()->getTransform( + header.frame_id, header.stamp, position, + orientation)) + { + rviz_common::MessageFilterDisplay::setStatus( + rviz_common::properties::StatusProperty::Error, "Topic", + QString("Error transforming from frame '") + + QString::fromStdString(header.frame_id.c_str()) + + QString("' to frame '") + + qPrintable(rviz_common::MessageFilterDisplay::fixed_frame_) + + QString("'")); + return; + } + + // We are keeping a circular buffer of visual pointers. + // This gets the next one, or creates and stores it if the buffer is not full + std::shared_ptr visual; + if (visuals_.size() == static_cast(history_length_property_->getInt())) { + visual = visuals_.front(); + } else { + visual = std::make_shared( + rviz_common::MessageFilterDisplay::context_->getSceneManager(), + rviz_common::MessageFilterDisplay::scene_node_); + } + + if (visuals_.size() >= static_cast(history_length_property_->getInt())) { + visuals_.pop_front(); + } + + // Now set or update the contents of the chosen visual. + visual->setScrew( + Ogre::Vector3(linear.x, linear.y, linear.z), + Ogre::Vector3(angular.x, angular.y, angular.z)); + visual->setFramePosition(position); + visual->setFrameOrientation(orientation); + float alpha = alpha_property_->getFloat(); + float linear_scale = linear_scale_property_->getFloat(); + float angular_scale = angular_scale_property_->getFloat(); + float width = width_property_->getFloat(); + Ogre::ColourValue linear_color = linear_color_property_->getOgreColor(); + Ogre::ColourValue angular_color = angular_color_property_->getOgreColor(); + visual->setLinearColor(linear_color.r, linear_color.g, linear_color.b, alpha); + visual->setAngularColor(angular_color.r, angular_color.g, angular_color.b, alpha); + visual->setLinearScale(linear_scale); + visual->setAngularScale(angular_scale); + visual->setWidth(width); + visual->setScrew( + Ogre::Vector3(linear.x, linear.y, linear.z), + Ogre::Vector3(angular.x, angular.y, angular.z)); + + // And send it to the end of the circular buffer + visuals_.push_back(visual); +} + +template class ScrewDisplay; +template class ScrewDisplay; + +} // namespace displays +} // namespace rviz_default_plugins diff --git a/rviz_default_plugins/src/rviz_default_plugins/displays/twist/twist_display.cpp b/rviz_default_plugins/src/rviz_default_plugins/displays/twist/twist_display.cpp new file mode 100644 index 000000000..7eaa76d8b --- /dev/null +++ b/rviz_default_plugins/src/rviz_default_plugins/displays/twist/twist_display.cpp @@ -0,0 +1,49 @@ +/* + * 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 Willow Garage, Inc. 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 + +namespace rviz_default_plugins +{ +namespace displays +{ +// Function to handle an incoming ROS message. +void TwistStampedDisplay::processMessage(geometry_msgs::msg::TwistStamped::ConstSharedPtr msg) +{ + processMessagePrivate(msg->header, msg->twist.linear, msg->twist.angular); +} +} // namespace displays +} // namespace rviz_default_plugins + +// Tell pluginlib about these classes. It is important to do this in +// global scope, outside our package's namespace. +#include +PLUGINLIB_EXPORT_CLASS( + rviz_default_plugins::displays::TwistStampedDisplay, + rviz_common::Display) diff --git a/rviz_default_plugins/test/reference_images/accels_are_displayed_ref.png b/rviz_default_plugins/test/reference_images/accels_are_displayed_ref.png new file mode 100644 index 0000000000000000000000000000000000000000..a9eb9d33de5fc21394c7af35e819b781fe4dc3f8 GIT binary patch literal 27916 zcmeIbdpMNa`#=87!5GXisLYH*GelI*6JgXiG^muy?p($pMj;_dsu?sWhpAK|qOqm6 zj+72|#-T_~leUCvNTq`sskX}2cRkNz@7nwG{_FSG?~kr)Z&#ODv(~-tb zCJ1GH;kxly_<#K%V-VV{!Uei4jW@8u=g2G@_i*{10bf0z3Z2E0i%Y z2xCfqGUjjTOvsS*lQBOv=BFwB^fCWmoo}Nm#-PAbvUKi5XsNTrb9Gsg)A7JxuORX_ zS5q_I{~d_J+tW+Xs8pnn#bW(dt>P+&au1>LBvMT%(*#sF)3HXT_?(=Zkz1{a#B}bL zzoX4}I~9{Lyrk!CQwfs~%T}NIJFI;VW^Q(PL7gq9QcZ17-!uLEZFKJjB-qr=z;jO> z=W2fdQ7)m1n#49v%<}wvem1B6^MPV_?!K{fe^PPlt5=K*# z5F1Mf4knnHiC{8((dc^Il^4?#%5nyQiKvPhC6X_!HZpT?e9g)oMdGPw2@*w@j{qVH zDUgLK;am|Wp4wHYsY!Dd2eXFJi1vLciUvl0kB1S;TykYG=o*W_8h|P@Fw|28Gn$~? zUN>u2;VqVAVUox4ZwSLKs0d|XHvtA@;Q=kIbos|Q4w?l@7G#0}jU7*}UBSP#@0GfI!(3M~TKc+MEe-gr&Mfzx#(4=xTonXki&WdB{ zqSSsws4eLO)M_#X31A7zje z$mCv`c=hzyPqG^`JeI#lq~--@@aCtw7Qc){yL<|!HE<_~Ys7Nz|WO8aAkc8G04 zt)AzhQB3=vgcP1Faw*uBI7=dIcFfq2WR*J=-4$30-ql-VSJK{R_z!jo5^Xk7;6cbL zptEz0(P06|u#O=xESmxu*d_h(oA~KR#wh~sybE>xKjUC?4@H5il?aNwsq&gg?ma8r zR>QHo;EfCWkZwUX_#5qg$9ODPDEx(8f|xM)1>XPvmH%yuptn3DL`<0LigOCt@_~~p z%SN@o9%w(gg?~^V29`X*lVvI-wZ^iJ!FzI9kTv3lDqMLLF1EP=K#50Fpyyqr=l(Tf z7rOj;I4=?=r)4rKHVZ^-X|XK4Xb(~D@QVf@DX2*WwgG^Dtx+frv~vPc%2hFPi|Olj zUg=Zc%i1XDmD#oM-y>8|_o0wE0$p+3xrg$t?x-xi)iEPD?Z2*t&Jj zgzaM)Fj6pR4LS!*bxsOYv;3pgUsX}f?96ruy=bb8tNRG20zO4Ce)CNm%lt{bF@ZLIWlb!R?cz$`2| zh_KMb0LfyeInuFpY<#NUB$5ho)P~;pJiL%`!Mm- zN4Qa=hk%xs(LjjNL`tn?zmiX2!nBtLUgklB> z2{xCIturWCz(3(D?vgHDZwrKjNw%nP0j=D(L~=oXPYw`oqRO1t_#ng{?a*Q7$gt2T z#2$IhDWby4zSoJu83!!S=>7g%6sry8N-Y{~?!tV)@QH;(yvmI!d^tq-ayWLqMU@O zaG_TmW=WLqrJLlVqSHnc`TiWB$hLTeUpzEyTccJfJAycj=SPr1F1pB|dIvDb4&Rs) zT3aqtG0)+`&fekwY%hc;_24%;B@+xx`FHtppijVbo*zeMf^h(fY6?AowY$1ScB*8# z!L}8#o$lR&&c(!YVMo^o%M-du;jAT+{J6OLxMSPsu&WoKzVIJcFId6kNRr;QTcCnb zLO=`YAcd-5MDyMUrUbs=jT@iQhwZ(Vapi%EZ50=2Z?p_i7r{ir0T`qpVMb5KkN)m) zn~o?Y+ZItuBC2(>fY!0_K1itWQGd#IPL7vnBaR-=LdH>1f2R({QALpTsaGyl-j6M& zS8S$8GYWJhW}%2cSa^aJq6v4lARf;rPMvHO({90m_A%NVT@ts%&|3IhI^xc{P$iH8 z&WV_iUQtn`xz(D&!n0~zgCY<{g+0!GTJ`Io_J62f2$AC8 z>NY^9Hv=bBzF554Tn==mzJni&E?bH&`#Z9%Vq|*2rU}7@TYumh!t>S?AgZkDMa(D~xqCQX>f_c^BfsuBi>h}*;7HeZ;t{yp<<3exTa7t$SPr>aUyJy(q#cc)5_j>gH?Gd;~ z$se)!=KV8XmM4U;4kNA&*gOy-$c>iQBB~t96l_Z-M57qrAK_3t>cd}#1EpF^Z10QV zjaB+)nw&Gz{U?aS9{&ymou< zyghN;y6y0t9rnF;ReK8+xHj zV}IL;ET9g{QIhK5aIA@NNvwzcHcnPd(ntVeRWwgvzaN!j1rrKu_o?(w`nYcaPpnx5 z`NzZsvo`-n#h}kj#aH_~+RoF~?}E0|I=9t`jgmZ4Lb%J}q(%hgyCLb_9n@Yxu5fnC z^4iPk!7MLyA`$c#>K|W$-2%TyOJezw_Ev5&=Dx_|j{NcW954hPKu1K1MfdH>wkGPjOAAADXS3qxchMar>wgeSg4;k-SAvNN2uT-)wkn)eOlx46k@eFXL=;l$pX z;fiN!;oG+wY1~V{63iXBhX@8j(amR=Xm#O@?Z0fODvT*5JZKH)Nozo8S!*kyp$>}Y zCfLZrB7zcaCsrk>Y-;!PtdhR-2AFWly}P*7BNC5a9#_$RFArr+M|MmKLhY{_sLwcH zamTtU&qFUy*>9nK8ahM`Z%KcC5>Z2p29iFx1J7n;j2Cn)>F>7;+UV!=wy*63?d>BR zvvBFV$HNycW|W}g!I$I|-VrhN5zir46$FvuWa)$R?8*q?rLmq$c5SU+dsW7%^|g(} zU(~{ydaK(MZ)kN-s!1p23h~u@bOM46!e!~f6KezK1yZ+Z@Y)u5MtJVRqj9TNy)7-Z z@?2<3RMjFTZ+Sv}d~@UIcz$1lhUmn+rIT!Jak^ZI8lO9Y#>F3ipF1%!Qo)X|m)6%S zux#aDf4i%AH}$^0o*qg1$4%*TwPcN{1(+ydzk)zkyuvZQzG$;tw@$JYEdC!~WW`}h zTU$r|a;k86|K1ok7E$6?O--#<-c&K$Hg`7?UkPr+^tVxT?-cOWlWx$Qqxj)@krpZj z6nBs_#ydRYKSwb&8+E7|U|19w_8R{uySs@xz;T(oN@Jvqt^g6-7Y)mxY!A>W@P`OD z?D2x-i&r^k?MWs)E};Q{(85NDOq!0wjni9!)y5E|E64k3MNBHEdiO5c|I8Zh0au?+ zaA2ga=HkG}-Je0`1VRNYc}zEoxwZjyX(VF?X0xO|j3_Z-H`ACc7dTaV9Gc)Tkramd zDLGKuK??M)_qGhBe}*$+=vvlF(RxMDUh*KCDY#?P8;?kn)^f6 z%WxB<14P@Z4`ZK+V~u}eft47f3{5I+Cwa|g|I;^9XE8aqTf-Xx_sT#1k2**YkyCP9 zewQ!qVY!1!)$J#UwE;*kqEOdQV&}rxhlMW%9xvo|hKU5RgC9Tw2&6%8dPP&`+s0pc z%pXT3ISrL0jML~?Qt{hfgWpd^v1BO9y@I;xJ7A`lOwK@S__`Wa{QPJ9DJW7Qq2Gm- z1qkFpw)UEU#j6 z;mTDVqGvwDgl~)2%;7!0<_z;>0m>68GQXVCIbBH@&y_OlslJL2>|?um1#V z(n$*AIg8%hqXBd5|LThr@O($js`U=Aye6s(3JNtJ7-%v=Vmn3Px6~^rwNQUPQl>bF z=;IFv7v#^Au#HZY^LD!@s|N4%Gn(Vgq2Z1U7>R%ajUrmb`0K*$P_<11~-U~l>#`{8V=Z+rcnqR=mgL$ zM8U!*gs4K)%v!@Mz(%%5WSNv8mhe6Ip^j1*p-n0euSqsfRP<0vz!4=3rezd?2zt*w zCF7tbLKYT*s0Yk=5~#&Vm;z&b>a`PEr5VR)>)#DNjF-GbV;KjfL4nXfVRBvp0%o%i zQ$z_6sv?-S8*x+|70e}26%sxD+lG0FG!CKCFvNR4x|WeJsRhPO6UzCWl}UyG$Xs)# zNB_-bko&;7sXn4&g~`zA<%>b;hbYw&WDHIV*+4R46#^or39zN$tbK05f2Ak~6_VG< zRnd9Wkc@Lc3OCWOQ<10-=S)DOobe#3RM?TveKiw0T4$Z4k=s!Q{|EWS6Q$=%-H&i0 zI)GOYyR3D?Wm93poq#UiVK$TkfeLZGW>7CMC7|l6Xc0_|rjT(638qYFom-N_Gxs2> z3rLO$phkoEsP=jIRb1Sz34Kge5k9G=t-3Ionv>Yv>_?@>ae1Y?t*tR%zU;qfos3bz z6xfCork95ZNprTd;4Fks^sz@gkQtIHK*o5vQ>9S%LR!^Ht*#Q64gNUxzTmDckGY#~ zFe-WN_p~j{Jns^&U9;`(q_qIA$?-zu*3pLemw2LJd&C~5(K-<3iX%Q5xE5)Iq(Twv^KDRXBHmRUI#AB_UWCt*$Mj~1N* zO^NmO^;G%7T|0JY-sMwinvR0JA_mTu^Zw&z0xxjz-IG3_3+?4*Nwbx(DVqfRijB6p z&K_=p9MsJnLVVZ<%INUZyD!?dGsyJ0a|ye5mtqG9WFO+=5{&xeguhgm*@n?sxX0Np z`wz;FwD2wT$(TemW${Fm>q0WL!>O__&;dA(t}fAI?x~v2OrZy<-JempF(~~?7`Oa2 zsEth622vvj6|;fzkK$*MpqkU6!CtHJ`tYM9heGA>xD zCk0Yq@rK(Yj6i@JuyZ6IE+MU%=5j{14C`HsA679&PRzZG$o?1fUe0VikL+5A{Lt`~>Ids!I*Y#BGl1!n-F~$kw5vABf ztKKQ4#ERL4SlT$7vFEQh{ek zGe@+_G;EDaMV3L6eeN-jC=?5SLj(oD1MG4n{F@1wh^_QuP}=GwMdqeetx7qr>Lxjq zaQfaQn*JGDOPi4$qVZ{a5Ody{^!$(CoHlJ@n8_XeK!kOd`OMoVdJ}4rYqzo8Q1BiI zwLF-$GHW8TeMJ^~!&D5)lxQ@9oI4$;gR(L%a;4q6x^~C4SydmaadFjJ#^$>5SESr> z5XoVD@;z?80hX*YYsMF)C~NJ3`@BL&xq7&qK`sDF_3Dmx`;hXJ>feBUd~D1Kru&ky zmz0liQ#NtS|KzC&EyKb{wYBWF6U!g@OZF#vEdPqgvK0w~R_X1TCoyNHvO``U=%!PX z19hb>%Q^1c^573wb7gOw8BA!?Wyt*bbAk;g44l|8)-F(uInea(iPqOYJNos8VaR?T zIlB}%<0IEEP22q9Q$vSj%nke@6JEw~M=p)G;@D5Nk8HA(U(DVD1WaZ}R6s zm)9_!VK4k)IISK!evHtq%>Ia(Gmre{HIZmF_N@(KKj`~oW?FfO@!Q^_r7=kTU}S=g z5PR*r2bNk`&h#=lkPM%3)0kXssP< ze>xlj9jC$(j-iK@u79Z5KV5is%9JW&<$+I0C*~Es-a?p>;I*j*>amt~Hv4#~ib3J& zobhRI+&WKI^4OeQ^OsYwtJau%hU@sK5gtbhud3WzCEP+0RPY4L6kL*+9NGYB^o}Wn zFPbK=vERseO9x=3Wj5l$=eR*vWoe`NJxQ-j=?7O}G+xq<`8%l2&YgbR0BhReCf#_A zrzW4dp2L=N)WoivZ%Ef#HMZ!aGK$yYLhGT2;vyZM6QL1OAHwF@w4F%GwOMlFUW_ln zp%4a;v=Bs@Lx|#v=$$^Z`GTW*nzZwEfxA?^T5@H^v+Hcy0`>X+g7nKAHYxNH^pHd7 z6sft>-b18hvW8^^m*k_@l^`_obb4N=EUMhHLvVMa!24L+iI=qdlI4pfhmo2C)JlQ; zmVQ<;B_GG9%yivgnpCE}s*^brcdqF3*X_L8BaKOdlnyU*u~TvCaUp&EGTWFDcI!(f z%hILWLLpKs)=c`NCuhWi-z3kV4jc>S84(Wt&U|XkF_%9y<}DZXecYUOYKgJH{<_RL z+e4Q{k|3vP&=OF(i`I=GCR}b2;AcgaCFb;$r*xjA1-NQmx}4Wohx6!RB{@8JnrGk| zZGRxiqVYPzoH~l#P=*m+*2MX$gx!RuA7+pkDzt-`gHLXf!+R=UI6>FzTS?0;2=tiyb z7{aB9;4Tk`;c$9m>;>6SZ_soVt&$Y0A;y+=rSi{-*OXNJF1OV}PjZt_lXjGAt($U9 zA2%uC>k&*>?vB6W6|9X63w_|dRs{d8#dDtFkGgG8EDRFMM#7v8b+B2-bMuPr$dN}> zPwr;f^2nHjr&8J%34wL?$6e9U)czv9LiB3U;s26V351b!8O-mYdl2(!i{R`rOFzQe z<%CyvWdjJf1R*BQ6(rOjhIDTly%!iW#{dFwh?%=Kt8uT?C|nO4RbplB{k!ZVOHuq4 z+biefnHye7EsVTE?4YxqN{4T#+&rUGny&O`qf)n_-_>J$Hybg~tj)m}>^8z?a=8of zm?}&G+ZDG?J@v8}ZxExt+8?*t4R`HyUT4vwF;{ubp0k)g>Qo0e;@sQsu%x<^ zxjafuonBWe=1Q>Kn}*%$@;OX%{JEOXlpzu|F)gW)OKR6!H$*L*^tOalw_itk*+xse zesR*JH+s&iwp1+mTR4_}kNf&QW2=JIOFdUiIis~I>iv7e7*G`woTbS_Cvj91&&)~E zw|clF)w6X+b8J6wvkn?vdT2i_qWt9ieOSAVj6mB#(ehpGN)NFCu|7}#G#}2O7CPkY zPv{d#E(Z$wJta0t)n>TihCz@klq-qmoSw60k1su=sMI`WkGr_~5$w*x0lMkJUGgeL5gZ#n3%tod`QT3H(-ZcoUqU|8@}xW+z>t z-SdrUeM)-8MY73<@t}&i*7j<@+MF-g+>jdGd?Wu3ZGN0+l;!@br$w{B-ghtkTG$C;dupN&{~jS=-w z3$sKY=PTN_NyRj?GyC)Vy?w3vY0Ssj!sNGbBASLINE`-U43;|Fe_f`sdlK3GS=$Le z{#H8K8n9+~-*i03sXDlsF(<@6-QKwfoP}(~SawFHoPVX%bV>ZnNWb5JTIHk%Vp3-b zDKay3Ff70@M*@7gWG{L)Bm~_fgYMw)NBpZD%Xw{<;?=QZwjNI>?;%pI9nhU~#CFz_ zo}EQ!TR5Kq!(A5bFe32h9sq{5U>$ILX3a~F_5G18tcPpS743*sF{XorG&+yVbaG_7 zofX`weyP%xNjvBCxY~1p4WwzVWqO%z&8ft+4eTxZbnD$>Hkgb%Ftm_k5t$e|2lWeo zpnhQ~6FMFh|55df*5}snv0SDRt76XDteo-?uH;A~MSA&VXq@HgaYN(zEt6btGp$#h zogQ4Sb&1=b5+t}T+6I()2Hc_h4BhBbq+~ROt}{cDmseV_!P?$ABZ6Y2*LZ(ZJrr&j zb-=RMdo(4eJ@_b>yF3^U5*bL4py*+Bd-wsr?wgw4S(=ItNY-{^$W=;iXD@p6>wK;?(%s`kC?z$NHaCVgaS}JdR2@U9 zQpc37^wqj%k+Tm}*VNnEEiNY7Ig|f9>+d%{z6-Qo>e2v@EmU)D zq_3wLi|%_b%04v&gRhO?7y{&sbMO~7EKOUeJ7!9698v~VPXFNNMeJ;(?mq)yJAMOX zS(JS=MBe8wK0M!k-@!v~6?67;!JHY=p@mW*ZFK%-j;7+0pS-l07}mrIfY0sMxFZQM z1;ldW>AT6&Gnmh&^iBC^Q@$;@C0nIO##B(G_cC3UyW9Y-+R)H`y^)j390S|{R-;E` zb>X&Yu_mtCQ|n}KJ-}@>aN!0V!Nc`LVhS{{Z$YXfsJj@RI$pYMnN#1$BTLxsKFe@s1{cTSdTGumU#xs{`~QIII{rWNyGe>nw0+>ZNRD6;cqPo<^% z<7*jVhLL3Qu*B)=ahv#@$9$VxQ?c%s1Mx~OB1o4dopk5!!hD8tV7XRr0QFLNTCq~P<7ppUEAUSa0; zbrKt9J=%9g*h!?U#>_pftB?VzD#~G3!U_L?pfYs)F9j{t4ZT;tk z>-r5~UMKy{>-WG3&jz(YT-C{gLSafQ##u|fQa|)gcJ2_hF;%_$iV4WOB7b)bzOd|9 zeH`n^dnT=r31qhH-e2DV6@!Uq0KM#SmE8_2jRHil$$6B*#ZH8SjRISr`sSG~ zC5Hq|qMtfRuc+p7x~Nf5;=MdLW}e-{mkF@pd7_0FT^K~yZjbmj-olA&N;?P&r8@1T z@>ml8vgLSPpuphvw%sHj)tk%Hs$xk|{F{5)1l!hIjZ>xrK*%CL_+(r-DeR%8*r7YS z$m%q#O*6|8ZQAE5$dT|HZ+_u@7(M1Mc1&|_)>&dipsI?!jEPCSLdvzOqs1?2Vdtk9YY88tr)TNhdIHEK~RQVP3n5-=-^p zsl8?HV|K=#68s!TY*@$OAcg2=BMJht!yT}IBiIDbRG3ip$%FFNZ~z*zlP$+V;`}s( z;NSi0b|=Ek#-lmq?-`O=6fLb-4oIL$V`Hma52gdvGIO`zDA?p-Z@ttW^8;bvuyw}^ zyWM|D^LseA2J+9^1O1IGz13LUx8m}BE@R|~>r*sdBA3y%0ZK?_eQ}CqP zZ{76|u1m5wrTu;*YBF|rHm%TR+xLJdreJ}{BL7fiEWY{y_u4XUZ3?XNfvMw6PMDJ@ zr!KIxCljzZb+uj+`!rs|rH~8K-N84Oew-ny{1q)HyZ+f-j!CN>InA3^xOB#zaRudC zZwM=GxeSb>6Eox#E9;U}S=QR0VCPw47l|rEap@h9^@%-AyCCn1Dq!_tnyNcxNMdfKdu;wg8+s4-H zh2Ae^1)VsXa(638^?lRrvSUf-?w%X23`~7e=Kc}P|8iQ{p#>-A;X<;hR)jzWUVlMe zaD|y|@q&gQRAUVh3dz5@=bLMDYq+XgY)p^c*+VArPK4-o?4{PDGwf#`kYzGMPie6) z@D44|lG{y*oFfvZR2ZEp0>~bx{_U#atJ3Dd-ET0KUOyOHC=AKIj2#o^0PFRTiFX12 z^ufm&Ppk+`WwQw<)8${N{Y#`q!COh^yo@U*-}Iar*r?{r%i|^r`}StvR|^fDWs+{U zk+AU!9Epz{LG)uF1@hV+acEP^Ad$T7kjZ5Ak-cHpO?Dr!Jn%r1(z`VyWSL5Xs^1xW zetmfeC;46(Exd`;|32ym>4QSh`O^C97Syx-lD7}z*&%J2xAm-y(5nGwRJO>g=;@jz(9b5b8h2&4=RCV$ zmU)HvOR2!EWmT?-@(KcIC*}pqYB2x|@VVoP%!A>(=mqMgGAA3*(a7?2g9O9JfM!w=rhi zeP@iuw@2c*%xcNAi`Ei{9m%K&)R3?a#0bKApHt=V)cLrF;2^NmT?G9)L|tY-Q*)i3 z+d$@KpbEq5NDjGn0JKdsZx&5NIq0>!);S4Z3NSn-(-z-ud4qc+nRsaqp;Y%ll@Ksvf@x4LSC$i$j-_Wx3b!2@IhHlDKR@+|iPTF&t5Sje^o!`l_ zx;^DJimT7l9EYfmmYFnXyWk2bwG2Jh%tH)m(q3qI=TU|SIFhml;Nwi#3d84kcE3(T z;n20IU_8rA;}Wur34DrAiv$t}A4Kqa1N5ShF$FVAH| zfheVc)7c=;`UM2_Z<(7_O%87`HS_AdVV`>*@a>0oHagZ<4L}9-c#pq*etI+_S!gclPXCFcfG^TQ+L1R-p+G?8y zy1BsI%@jt|3%lN!WgzjornTm{|jC?_06fXn^P7MeBNF ziqB@Q-_$cx=EzJLirUu9zOfx?${O4ekdgj)xwVjt83X4?9xP7-^8Ac^&DJmX3zcfZy3y~u%f0Fsy&27{Xf;PkL}H)kZ4 zbYOpi%G;|Z0n5@n+>>hul`jXgSR7FyC{??rr;|9o$$@_;#`FYI^Q5JFpahmJxhu-rX9hSgh9Gut5P1sBVsQ1r5dz`4ycaBI%o zOkV_8VZ43A?_Uysn~l2I#Ng$$Ta(KV5ODh=4!Q}eZKFHOi@SuV++|&a9jq^*kgR24i*zT&iqsi}JWQr9X05F>hqL(wvw{5n+CI*$dAob9S@_J3^oSP*=G#lr?tgc*pAfj| zaV)+c1dBcO;NZeQnmbVy)RLMzyrxw0DvQ8fiR^|G5HT zULxBQckJxW;r_h^26caZm8?C%-EZ&;?a-KyB>m#4*s(FKC)K#^Zwwc4Zbs{Z|17*8 zm47VV_7&6gY=MmpiJx~^a87wcY(~h2wATHB7vB0}r|PU&&lUG1Z_&Mn@fCCJHJ%tB5Iq?R>`v~^uf8MQrrU7ZsP;e>r+w%r)Zz%+h@D3cr55)NqVk%s4+I7T_GTJf#Y+)qWX8eCAS{N zTi)yVrS&-T-p5ps?BEDF0Oj#W?TkyP6rNL`2n&2`>BZ8E$8=82weIRM3KG`Yp5Gw6 zN&)B5rIR8h9UZ5-p3YyOI!&D^T)9$JW!Hg2HyEa+QR8ESv2SGuQ~w(;K((=V5;#v_ zUMem)>+yL=dD7*;(yNU+-*_ae7{OEP#vy7~u~yrOd2DI0(XECjOLLlvK$ZCyZ>iaZ z^L{kJG|Kww#g{I&6$mRho>zmJMNU;!T2tpOps*=BSml`_=q(x`G7-yol?;vN_mq0b z5{jDoHc98#THeN>AQjWw`#I+u3LmyW-qeLojtcSSJb-gcs+-r?P2 zOvWxueRH$cEXn12{51DqB76a$80m25EQR|drh%Y8{K>2N_>PA9mA=XtB}|Q~|yA zC*0{e?cNX=Xqvv>z{I3I)+%6;OGLdmU}n3jN^mg4A$Kz9PXmxImw@;K!YsHM@Webx zPv*5~-SAL6!1j$`N1aM}dA6gUt=Ijo8ifIQb81+FG|%R$Yj)!;Yd3&Pm)yhH+4$z< z2+9}xjencoUgly@z|5vH!*xJ8QZoIeK#(s-|FoplMqaQeVy8~$xf3uYh4$K?Pqhn&(H=%TJWMan8+5JZ;6 zdV_2_4^)!&qqhDVB7qsnpirr@>NJL2TOO@D*9?C=3Y+naLpQ~1WC=~@#AjocBoX)* z%VyOjDR=F{hBb(1pJsVli1fVSYi4w8eKOMKmydvApXqo3POtt=Jz)(Ytevf87bGal zs&Rbd5?QWgy;uYg`May!JYUxv`+2NTiXM0C){3dAS?3h=XG`kY^u)Z&`~VKnd{J zUbx<1}c4y)$?WP7Y+`Vy*Oe~f_t%6F_)X~j9oLPr=A zUyH|sJ|}Q!I*~zM6m-YmcL3=d_( z;E^(qyex@vBLm&oGNgwRYz#VeeD}BKm;#-22iNE$4db)2aE)~K15KXuxO%J(=KV!$ zE!&XCH9IHw?@uQc=E~xJT|!K#xnUR=)Aot)SiBZd9_=1PdH!$}9R3AVG}Lpqwo!{) zFRiuaIp02778odyShDfTp^FxQ2LuRiO$HfzW$!nBk20bSL`XSw{I93}0bdfUqQB1i z&AN1U(i>e+&nec^b&C_5IIn-!d`!l2PXSHTn(W+W0_f<2U(UIO7~dw5^t|7US+gvu z)D0Yxl6o6EM4Y%r5WY-htMxQ9-Lop6zB0k>#}s9lg%5WQpyqNJH5Ui`_}BsHv67kT z=(7Uxm)GwSN!8WX&}T(b@`073Hb{ZWnr6WO=luaOV$mnBoiXzj{9)@j6gSh+e(tGQ zzv=}Ee>ExI>Qg?rBDe^Y#ES8JD4Ch)u|EqgM@>9iFwSHQ zzkRfappFy$>9>0f3SX|JS#qlROht2z$I#$DxvW1>fo^IxQe`3iK@pJd$+!QN|FGP{DCeIf1;EXJ-t6%uVgd-E zphb$GeV=>2h2ByJw_$^Ttc^AwQ`C{qU$3Xnw;DgvKS-;Q7^{J+F0j}J5JE(;L`t+t z3qMaZW$U6s7u)y?eSDeg6VxqUg(V{d4@cCm>%mt&3XSPcyDm$w&M>tOyv@H!Cu2k~ z8w}C=_MjUB)X!f<1O7IVbJIy|LT0OdMzBA)i3Yz(bOy=Pz)M6Bd|@x{WXLS7F#XH| zj_sPVA9~LL%mt+(?IlUmAEkqDKkoybq`@jGX|h73n+m4J02|g;fcc#hS=4N4W>(aa z=OM|frCbFGrGd8ZvgICbpw({>JiY{aiB3nrsWvYsSvpv+y~{8v#zDb9U_A%jzYQXE z_-Tm6Y#GzSPrhEAotqDa|EtFtd=5Q0nfW&EWN3`mI(5)<5XHI+vhaUpT%upVRl{Lc z{4BD0z%F=^P@8{sxYwEEQ)$zvzgHZrTGcS`34-@>|Fvr$FyeR|J-kCq?;iy>irNZj zEp_mvK~SSHL*HSd0;iEV{fx(q$b;(Nz)dJ{ENw~yS0|lQf>}za#ah4 z3mY*gZOwo-Pq%^58h##OVUnmlsAKTeF_~777&Q_zbj#l;jMLt>xePgz(+oxcz9iB> z^>yOS6L3e)kC=WX)RX*LcBe!(<^WzPfE&1PAzubhEHI*GVXhtw{y_2P<;Gb%K4h)2 zBb`PeVi%(P@IxMlS?&?>KsM>1LF9;<+xHg?fU@BCLSEsynmgqN)`z?$%5CXW^pZfG z=loo%_aP)u7|q0Y6*%MKb8_^%IkP#_Dt3&dsxnRHE2zL&VMB8U3JZGQ9~lSHk) zB2-YR<8tI+x)`Gpu*Oz8U=ttS!9Q(HM0FRvq{|H;Ql6AxM6JYYbq50EV#Hzye?{Y(%-xi?A^L)QQxv7U-z&-9H{=1?Pax z|Hwva43o1g4?G;vXaARb&}PEmcJnRoWRpNWGYRy#R)%Mpw4&?+Pi;6%I%u( z1sUT*6g&Qjzi%6J>V&{z&;72^*0!vitJBBnWk|O^j*9kVYa$)P$PY*b^WMT4dK+Zm zx^s_UWMh5S{0wkouKczS2%13iE5K6*2o0$esKAI6noIF}hMSF>tdTbIr{HbXm2ER4 zZ>BezPK9r{WiTfp_`7#b5e$@-u>MRmMGH~zY>$`ou!9@ zJ)Hw1SuX(qlcNx0)5OJAOn{qU^w6Lpj~M32X+14j@Re{clX+efxBW$X_l~)_K^|@p zdRg@}(kB4g89a%YO_xK(XOkwem0yPflAd4+!s0AL*tQX$9g<^SAmSNaTAoZ(AP9H4eE99uTKp|QQ5r- zy#h&o6?DGNIB=efv-j(uw3)we>Jg^YhJJox08o=mB_NC-f#&7k3tK zy=akXrLN6UR{Z7~a4WhG*=Y-^y!egPc#5q|u9$MENKeIT`*c#a27 z5r7h-$@5Rf{A3L39w**v`$skYg>8Ro%y$m_WX#V;WPiS{`(xJl9hUukcKBcE^iRfo z=fF?K{CvUt=hNo@yl3$Po)AenKN<6rG3W`-#Dl)ybF`m~`N^1{e^=t?AKZZP)6c&| zg4`KHtC~Mcn{ch>=U*cES&#WykNH`T`B{%a4-qFGl7t%z(b~iRpBOXX20;;bif?~e Uyl@5hD`$|`{Kf9)TzQHA52YdRX#fBK literal 0 HcmV?d00001 diff --git a/rviz_default_plugins/test/reference_images/twists_are_displayed_ref.png b/rviz_default_plugins/test/reference_images/twists_are_displayed_ref.png new file mode 100644 index 0000000000000000000000000000000000000000..a9eb9d33de5fc21394c7af35e819b781fe4dc3f8 GIT binary patch literal 27916 zcmeIbdpMNa`#=87!5GXisLYH*GelI*6JgXiG^muy?p($pMj;_dsu?sWhpAK|qOqm6 zj+72|#-T_~leUCvNTq`sskX}2cRkNz@7nwG{_FSG?~kr)Z&#ODv(~-tb zCJ1GH;kxly_<#K%V-VV{!Uei4jW@8u=g2G@_i*{10bf0z3Z2E0i%Y z2xCfqGUjjTOvsS*lQBOv=BFwB^fCWmoo}Nm#-PAbvUKi5XsNTrb9Gsg)A7JxuORX_ zS5q_I{~d_J+tW+Xs8pnn#bW(dt>P+&au1>LBvMT%(*#sF)3HXT_?(=Zkz1{a#B}bL zzoX4}I~9{Lyrk!CQwfs~%T}NIJFI;VW^Q(PL7gq9QcZ17-!uLEZFKJjB-qr=z;jO> z=W2fdQ7)m1n#49v%<}wvem1B6^MPV_?!K{fe^PPlt5=K*# z5F1Mf4knnHiC{8((dc^Il^4?#%5nyQiKvPhC6X_!HZpT?e9g)oMdGPw2@*w@j{qVH zDUgLK;am|Wp4wHYsY!Dd2eXFJi1vLciUvl0kB1S;TykYG=o*W_8h|P@Fw|28Gn$~? zUN>u2;VqVAVUox4ZwSLKs0d|XHvtA@;Q=kIbos|Q4w?l@7G#0}jU7*}UBSP#@0GfI!(3M~TKc+MEe-gr&Mfzx#(4=xTonXki&WdB{ zqSSsws4eLO)M_#X31A7zje z$mCv`c=hzyPqG^`JeI#lq~--@@aCtw7Qc){yL<|!HE<_~Ys7Nz|WO8aAkc8G04 zt)AzhQB3=vgcP1Faw*uBI7=dIcFfq2WR*J=-4$30-ql-VSJK{R_z!jo5^Xk7;6cbL zptEz0(P06|u#O=xESmxu*d_h(oA~KR#wh~sybE>xKjUC?4@H5il?aNwsq&gg?ma8r zR>QHo;EfCWkZwUX_#5qg$9ODPDEx(8f|xM)1>XPvmH%yuptn3DL`<0LigOCt@_~~p z%SN@o9%w(gg?~^V29`X*lVvI-wZ^iJ!FzI9kTv3lDqMLLF1EP=K#50Fpyyqr=l(Tf z7rOj;I4=?=r)4rKHVZ^-X|XK4Xb(~D@QVf@DX2*WwgG^Dtx+frv~vPc%2hFPi|Olj zUg=Zc%i1XDmD#oM-y>8|_o0wE0$p+3xrg$t?x-xi)iEPD?Z2*t&Jj zgzaM)Fj6pR4LS!*bxsOYv;3pgUsX}f?96ruy=bb8tNRG20zO4Ce)CNm%lt{bF@ZLIWlb!R?cz$`2| zh_KMb0LfyeInuFpY<#NUB$5ho)P~;pJiL%`!Mm- zN4Qa=hk%xs(LjjNL`tn?zmiX2!nBtLUgklB> z2{xCIturWCz(3(D?vgHDZwrKjNw%nP0j=D(L~=oXPYw`oqRO1t_#ng{?a*Q7$gt2T z#2$IhDWby4zSoJu83!!S=>7g%6sry8N-Y{~?!tV)@QH;(yvmI!d^tq-ayWLqMU@O zaG_TmW=WLqrJLlVqSHnc`TiWB$hLTeUpzEyTccJfJAycj=SPr1F1pB|dIvDb4&Rs) zT3aqtG0)+`&fekwY%hc;_24%;B@+xx`FHtppijVbo*zeMf^h(fY6?AowY$1ScB*8# z!L}8#o$lR&&c(!YVMo^o%M-du;jAT+{J6OLxMSPsu&WoKzVIJcFId6kNRr;QTcCnb zLO=`YAcd-5MDyMUrUbs=jT@iQhwZ(Vapi%EZ50=2Z?p_i7r{ir0T`qpVMb5KkN)m) zn~o?Y+ZItuBC2(>fY!0_K1itWQGd#IPL7vnBaR-=LdH>1f2R({QALpTsaGyl-j6M& zS8S$8GYWJhW}%2cSa^aJq6v4lARf;rPMvHO({90m_A%NVT@ts%&|3IhI^xc{P$iH8 z&WV_iUQtn`xz(D&!n0~zgCY<{g+0!GTJ`Io_J62f2$AC8 z>NY^9Hv=bBzF554Tn==mzJni&E?bH&`#Z9%Vq|*2rU}7@TYumh!t>S?AgZkDMa(D~xqCQX>f_c^BfsuBi>h}*;7HeZ;t{yp<<3exTa7t$SPr>aUyJy(q#cc)5_j>gH?Gd;~ z$se)!=KV8XmM4U;4kNA&*gOy-$c>iQBB~t96l_Z-M57qrAK_3t>cd}#1EpF^Z10QV zjaB+)nw&Gz{U?aS9{&ymou< zyghN;y6y0t9rnF;ReK8+xHj zV}IL;ET9g{QIhK5aIA@NNvwzcHcnPd(ntVeRWwgvzaN!j1rrKu_o?(w`nYcaPpnx5 z`NzZsvo`-n#h}kj#aH_~+RoF~?}E0|I=9t`jgmZ4Lb%J}q(%hgyCLb_9n@Yxu5fnC z^4iPk!7MLyA`$c#>K|W$-2%TyOJezw_Ev5&=Dx_|j{NcW954hPKu1K1MfdH>wkGPjOAAADXS3qxchMar>wgeSg4;k-SAvNN2uT-)wkn)eOlx46k@eFXL=;l$pX z;fiN!;oG+wY1~V{63iXBhX@8j(amR=Xm#O@?Z0fODvT*5JZKH)Nozo8S!*kyp$>}Y zCfLZrB7zcaCsrk>Y-;!PtdhR-2AFWly}P*7BNC5a9#_$RFArr+M|MmKLhY{_sLwcH zamTtU&qFUy*>9nK8ahM`Z%KcC5>Z2p29iFx1J7n;j2Cn)>F>7;+UV!=wy*63?d>BR zvvBFV$HNycW|W}g!I$I|-VrhN5zir46$FvuWa)$R?8*q?rLmq$c5SU+dsW7%^|g(} zU(~{ydaK(MZ)kN-s!1p23h~u@bOM46!e!~f6KezK1yZ+Z@Y)u5MtJVRqj9TNy)7-Z z@?2<3RMjFTZ+Sv}d~@UIcz$1lhUmn+rIT!Jak^ZI8lO9Y#>F3ipF1%!Qo)X|m)6%S zux#aDf4i%AH}$^0o*qg1$4%*TwPcN{1(+ydzk)zkyuvZQzG$;tw@$JYEdC!~WW`}h zTU$r|a;k86|K1ok7E$6?O--#<-c&K$Hg`7?UkPr+^tVxT?-cOWlWx$Qqxj)@krpZj z6nBs_#ydRYKSwb&8+E7|U|19w_8R{uySs@xz;T(oN@Jvqt^g6-7Y)mxY!A>W@P`OD z?D2x-i&r^k?MWs)E};Q{(85NDOq!0wjni9!)y5E|E64k3MNBHEdiO5c|I8Zh0au?+ zaA2ga=HkG}-Je0`1VRNYc}zEoxwZjyX(VF?X0xO|j3_Z-H`ACc7dTaV9Gc)Tkramd zDLGKuK??M)_qGhBe}*$+=vvlF(RxMDUh*KCDY#?P8;?kn)^f6 z%WxB<14P@Z4`ZK+V~u}eft47f3{5I+Cwa|g|I;^9XE8aqTf-Xx_sT#1k2**YkyCP9 zewQ!qVY!1!)$J#UwE;*kqEOdQV&}rxhlMW%9xvo|hKU5RgC9Tw2&6%8dPP&`+s0pc z%pXT3ISrL0jML~?Qt{hfgWpd^v1BO9y@I;xJ7A`lOwK@S__`Wa{QPJ9DJW7Qq2Gm- z1qkFpw)UEU#j6 z;mTDVqGvwDgl~)2%;7!0<_z;>0m>68GQXVCIbBH@&y_OlslJL2>|?um1#V z(n$*AIg8%hqXBd5|LThr@O($js`U=Aye6s(3JNtJ7-%v=Vmn3Px6~^rwNQUPQl>bF z=;IFv7v#^Au#HZY^LD!@s|N4%Gn(Vgq2Z1U7>R%ajUrmb`0K*$P_<11~-U~l>#`{8V=Z+rcnqR=mgL$ zM8U!*gs4K)%v!@Mz(%%5WSNv8mhe6Ip^j1*p-n0euSqsfRP<0vz!4=3rezd?2zt*w zCF7tbLKYT*s0Yk=5~#&Vm;z&b>a`PEr5VR)>)#DNjF-GbV;KjfL4nXfVRBvp0%o%i zQ$z_6sv?-S8*x+|70e}26%sxD+lG0FG!CKCFvNR4x|WeJsRhPO6UzCWl}UyG$Xs)# zNB_-bko&;7sXn4&g~`zA<%>b;hbYw&WDHIV*+4R46#^or39zN$tbK05f2Ak~6_VG< zRnd9Wkc@Lc3OCWOQ<10-=S)DOobe#3RM?TveKiw0T4$Z4k=s!Q{|EWS6Q$=%-H&i0 zI)GOYyR3D?Wm93poq#UiVK$TkfeLZGW>7CMC7|l6Xc0_|rjT(638qYFom-N_Gxs2> z3rLO$phkoEsP=jIRb1Sz34Kge5k9G=t-3Ionv>Yv>_?@>ae1Y?t*tR%zU;qfos3bz z6xfCork95ZNprTd;4Fks^sz@gkQtIHK*o5vQ>9S%LR!^Ht*#Q64gNUxzTmDckGY#~ zFe-WN_p~j{Jns^&U9;`(q_qIA$?-zu*3pLemw2LJd&C~5(K-<3iX%Q5xE5)Iq(Twv^KDRXBHmRUI#AB_UWCt*$Mj~1N* zO^NmO^;G%7T|0JY-sMwinvR0JA_mTu^Zw&z0xxjz-IG3_3+?4*Nwbx(DVqfRijB6p z&K_=p9MsJnLVVZ<%INUZyD!?dGsyJ0a|ye5mtqG9WFO+=5{&xeguhgm*@n?sxX0Np z`wz;FwD2wT$(TemW${Fm>q0WL!>O__&;dA(t}fAI?x~v2OrZy<-JempF(~~?7`Oa2 zsEth622vvj6|;fzkK$*MpqkU6!CtHJ`tYM9heGA>xD zCk0Yq@rK(Yj6i@JuyZ6IE+MU%=5j{14C`HsA679&PRzZG$o?1fUe0VikL+5A{Lt`~>Ids!I*Y#BGl1!n-F~$kw5vABf ztKKQ4#ERL4SlT$7vFEQh{ek zGe@+_G;EDaMV3L6eeN-jC=?5SLj(oD1MG4n{F@1wh^_QuP}=GwMdqeetx7qr>Lxjq zaQfaQn*JGDOPi4$qVZ{a5Ody{^!$(CoHlJ@n8_XeK!kOd`OMoVdJ}4rYqzo8Q1BiI zwLF-$GHW8TeMJ^~!&D5)lxQ@9oI4$;gR(L%a;4q6x^~C4SydmaadFjJ#^$>5SESr> z5XoVD@;z?80hX*YYsMF)C~NJ3`@BL&xq7&qK`sDF_3Dmx`;hXJ>feBUd~D1Kru&ky zmz0liQ#NtS|KzC&EyKb{wYBWF6U!g@OZF#vEdPqgvK0w~R_X1TCoyNHvO``U=%!PX z19hb>%Q^1c^573wb7gOw8BA!?Wyt*bbAk;g44l|8)-F(uInea(iPqOYJNos8VaR?T zIlB}%<0IEEP22q9Q$vSj%nke@6JEw~M=p)G;@D5Nk8HA(U(DVD1WaZ}R6s zm)9_!VK4k)IISK!evHtq%>Ia(Gmre{HIZmF_N@(KKj`~oW?FfO@!Q^_r7=kTU}S=g z5PR*r2bNk`&h#=lkPM%3)0kXssP< ze>xlj9jC$(j-iK@u79Z5KV5is%9JW&<$+I0C*~Es-a?p>;I*j*>amt~Hv4#~ib3J& zobhRI+&WKI^4OeQ^OsYwtJau%hU@sK5gtbhud3WzCEP+0RPY4L6kL*+9NGYB^o}Wn zFPbK=vERseO9x=3Wj5l$=eR*vWoe`NJxQ-j=?7O}G+xq<`8%l2&YgbR0BhReCf#_A zrzW4dp2L=N)WoivZ%Ef#HMZ!aGK$yYLhGT2;vyZM6QL1OAHwF@w4F%GwOMlFUW_ln zp%4a;v=Bs@Lx|#v=$$^Z`GTW*nzZwEfxA?^T5@H^v+Hcy0`>X+g7nKAHYxNH^pHd7 z6sft>-b18hvW8^^m*k_@l^`_obb4N=EUMhHLvVMa!24L+iI=qdlI4pfhmo2C)JlQ; zmVQ<;B_GG9%yivgnpCE}s*^brcdqF3*X_L8BaKOdlnyU*u~TvCaUp&EGTWFDcI!(f z%hILWLLpKs)=c`NCuhWi-z3kV4jc>S84(Wt&U|XkF_%9y<}DZXecYUOYKgJH{<_RL z+e4Q{k|3vP&=OF(i`I=GCR}b2;AcgaCFb;$r*xjA1-NQmx}4Wohx6!RB{@8JnrGk| zZGRxiqVYPzoH~l#P=*m+*2MX$gx!RuA7+pkDzt-`gHLXf!+R=UI6>FzTS?0;2=tiyb z7{aB9;4Tk`;c$9m>;>6SZ_soVt&$Y0A;y+=rSi{-*OXNJF1OV}PjZt_lXjGAt($U9 zA2%uC>k&*>?vB6W6|9X63w_|dRs{d8#dDtFkGgG8EDRFMM#7v8b+B2-bMuPr$dN}> zPwr;f^2nHjr&8J%34wL?$6e9U)czv9LiB3U;s26V351b!8O-mYdl2(!i{R`rOFzQe z<%CyvWdjJf1R*BQ6(rOjhIDTly%!iW#{dFwh?%=Kt8uT?C|nO4RbplB{k!ZVOHuq4 z+biefnHye7EsVTE?4YxqN{4T#+&rUGny&O`qf)n_-_>J$Hybg~tj)m}>^8z?a=8of zm?}&G+ZDG?J@v8}ZxExt+8?*t4R`HyUT4vwF;{ubp0k)g>Qo0e;@sQsu%x<^ zxjafuonBWe=1Q>Kn}*%$@;OX%{JEOXlpzu|F)gW)OKR6!H$*L*^tOalw_itk*+xse zesR*JH+s&iwp1+mTR4_}kNf&QW2=JIOFdUiIis~I>iv7e7*G`woTbS_Cvj91&&)~E zw|clF)w6X+b8J6wvkn?vdT2i_qWt9ieOSAVj6mB#(ehpGN)NFCu|7}#G#}2O7CPkY zPv{d#E(Z$wJta0t)n>TihCz@klq-qmoSw60k1su=sMI`WkGr_~5$w*x0lMkJUGgeL5gZ#n3%tod`QT3H(-ZcoUqU|8@}xW+z>t z-SdrUeM)-8MY73<@t}&i*7j<@+MF-g+>jdGd?Wu3ZGN0+l;!@br$w{B-ghtkTG$C;dupN&{~jS=-w z3$sKY=PTN_NyRj?GyC)Vy?w3vY0Ssj!sNGbBASLINE`-U43;|Fe_f`sdlK3GS=$Le z{#H8K8n9+~-*i03sXDlsF(<@6-QKwfoP}(~SawFHoPVX%bV>ZnNWb5JTIHk%Vp3-b zDKay3Ff70@M*@7gWG{L)Bm~_fgYMw)NBpZD%Xw{<;?=QZwjNI>?;%pI9nhU~#CFz_ zo}EQ!TR5Kq!(A5bFe32h9sq{5U>$ILX3a~F_5G18tcPpS743*sF{XorG&+yVbaG_7 zofX`weyP%xNjvBCxY~1p4WwzVWqO%z&8ft+4eTxZbnD$>Hkgb%Ftm_k5t$e|2lWeo zpnhQ~6FMFh|55df*5}snv0SDRt76XDteo-?uH;A~MSA&VXq@HgaYN(zEt6btGp$#h zogQ4Sb&1=b5+t}T+6I()2Hc_h4BhBbq+~ROt}{cDmseV_!P?$ABZ6Y2*LZ(ZJrr&j zb-=RMdo(4eJ@_b>yF3^U5*bL4py*+Bd-wsr?wgw4S(=ItNY-{^$W=;iXD@p6>wK;?(%s`kC?z$NHaCVgaS}JdR2@U9 zQpc37^wqj%k+Tm}*VNnEEiNY7Ig|f9>+d%{z6-Qo>e2v@EmU)D zq_3wLi|%_b%04v&gRhO?7y{&sbMO~7EKOUeJ7!9698v~VPXFNNMeJ;(?mq)yJAMOX zS(JS=MBe8wK0M!k-@!v~6?67;!JHY=p@mW*ZFK%-j;7+0pS-l07}mrIfY0sMxFZQM z1;ldW>AT6&Gnmh&^iBC^Q@$;@C0nIO##B(G_cC3UyW9Y-+R)H`y^)j390S|{R-;E` zb>X&Yu_mtCQ|n}KJ-}@>aN!0V!Nc`LVhS{{Z$YXfsJj@RI$pYMnN#1$BTLxsKFe@s1{cTSdTGumU#xs{`~QIII{rWNyGe>nw0+>ZNRD6;cqPo<^% z<7*jVhLL3Qu*B)=ahv#@$9$VxQ?c%s1Mx~OB1o4dopk5!!hD8tV7XRr0QFLNTCq~P<7ppUEAUSa0; zbrKt9J=%9g*h!?U#>_pftB?VzD#~G3!U_L?pfYs)F9j{t4ZT;tk z>-r5~UMKy{>-WG3&jz(YT-C{gLSafQ##u|fQa|)gcJ2_hF;%_$iV4WOB7b)bzOd|9 zeH`n^dnT=r31qhH-e2DV6@!Uq0KM#SmE8_2jRHil$$6B*#ZH8SjRISr`sSG~ zC5Hq|qMtfRuc+p7x~Nf5;=MdLW}e-{mkF@pd7_0FT^K~yZjbmj-olA&N;?P&r8@1T z@>ml8vgLSPpuphvw%sHj)tk%Hs$xk|{F{5)1l!hIjZ>xrK*%CL_+(r-DeR%8*r7YS z$m%q#O*6|8ZQAE5$dT|HZ+_u@7(M1Mc1&|_)>&dipsI?!jEPCSLdvzOqs1?2Vdtk9YY88tr)TNhdIHEK~RQVP3n5-=-^p zsl8?HV|K=#68s!TY*@$OAcg2=BMJht!yT}IBiIDbRG3ip$%FFNZ~z*zlP$+V;`}s( z;NSi0b|=Ek#-lmq?-`O=6fLb-4oIL$V`Hma52gdvGIO`zDA?p-Z@ttW^8;bvuyw}^ zyWM|D^LseA2J+9^1O1IGz13LUx8m}BE@R|~>r*sdBA3y%0ZK?_eQ}CqP zZ{76|u1m5wrTu;*YBF|rHm%TR+xLJdreJ}{BL7fiEWY{y_u4XUZ3?XNfvMw6PMDJ@ zr!KIxCljzZb+uj+`!rs|rH~8K-N84Oew-ny{1q)HyZ+f-j!CN>InA3^xOB#zaRudC zZwM=GxeSb>6Eox#E9;U}S=QR0VCPw47l|rEap@h9^@%-AyCCn1Dq!_tnyNcxNMdfKdu;wg8+s4-H zh2Ae^1)VsXa(638^?lRrvSUf-?w%X23`~7e=Kc}P|8iQ{p#>-A;X<;hR)jzWUVlMe zaD|y|@q&gQRAUVh3dz5@=bLMDYq+XgY)p^c*+VArPK4-o?4{PDGwf#`kYzGMPie6) z@D44|lG{y*oFfvZR2ZEp0>~bx{_U#atJ3Dd-ET0KUOyOHC=AKIj2#o^0PFRTiFX12 z^ufm&Ppk+`WwQw<)8${N{Y#`q!COh^yo@U*-}Iar*r?{r%i|^r`}StvR|^fDWs+{U zk+AU!9Epz{LG)uF1@hV+acEP^Ad$T7kjZ5Ak-cHpO?Dr!Jn%r1(z`VyWSL5Xs^1xW zetmfeC;46(Exd`;|32ym>4QSh`O^C97Syx-lD7}z*&%J2xAm-y(5nGwRJO>g=;@jz(9b5b8h2&4=RCV$ zmU)HvOR2!EWmT?-@(KcIC*}pqYB2x|@VVoP%!A>(=mqMgGAA3*(a7?2g9O9JfM!w=rhi zeP@iuw@2c*%xcNAi`Ei{9m%K&)R3?a#0bKApHt=V)cLrF;2^NmT?G9)L|tY-Q*)i3 z+d$@KpbEq5NDjGn0JKdsZx&5NIq0>!);S4Z3NSn-(-z-ud4qc+nRsaqp;Y%ll@Ksvf@x4LSC$i$j-_Wx3b!2@IhHlDKR@+|iPTF&t5Sje^o!`l_ zx;^DJimT7l9EYfmmYFnXyWk2bwG2Jh%tH)m(q3qI=TU|SIFhml;Nwi#3d84kcE3(T z;n20IU_8rA;}Wur34DrAiv$t}A4Kqa1N5ShF$FVAH| zfheVc)7c=;`UM2_Z<(7_O%87`HS_AdVV`>*@a>0oHagZ<4L}9-c#pq*etI+_S!gclPXCFcfG^TQ+L1R-p+G?8y zy1BsI%@jt|3%lN!WgzjornTm{|jC?_06fXn^P7MeBNF ziqB@Q-_$cx=EzJLirUu9zOfx?${O4ekdgj)xwVjt83X4?9xP7-^8Ac^&DJmX3zcfZy3y~u%f0Fsy&27{Xf;PkL}H)kZ4 zbYOpi%G;|Z0n5@n+>>hul`jXgSR7FyC{??rr;|9o$$@_;#`FYI^Q5JFpahmJxhu-rX9hSgh9Gut5P1sBVsQ1r5dz`4ycaBI%o zOkV_8VZ43A?_Uysn~l2I#Ng$$Ta(KV5ODh=4!Q}eZKFHOi@SuV++|&a9jq^*kgR24i*zT&iqsi}JWQr9X05F>hqL(wvw{5n+CI*$dAob9S@_J3^oSP*=G#lr?tgc*pAfj| zaV)+c1dBcO;NZeQnmbVy)RLMzyrxw0DvQ8fiR^|G5HT zULxBQckJxW;r_h^26caZm8?C%-EZ&;?a-KyB>m#4*s(FKC)K#^Zwwc4Zbs{Z|17*8 zm47VV_7&6gY=MmpiJx~^a87wcY(~h2wATHB7vB0}r|PU&&lUG1Z_&Mn@fCCJHJ%tB5Iq?R>`v~^uf8MQrrU7ZsP;e>r+w%r)Zz%+h@D3cr55)NqVk%s4+I7T_GTJf#Y+)qWX8eCAS{N zTi)yVrS&-T-p5ps?BEDF0Oj#W?TkyP6rNL`2n&2`>BZ8E$8=82weIRM3KG`Yp5Gw6 zN&)B5rIR8h9UZ5-p3YyOI!&D^T)9$JW!Hg2HyEa+QR8ESv2SGuQ~w(;K((=V5;#v_ zUMem)>+yL=dD7*;(yNU+-*_ae7{OEP#vy7~u~yrOd2DI0(XECjOLLlvK$ZCyZ>iaZ z^L{kJG|Kww#g{I&6$mRho>zmJMNU;!T2tpOps*=BSml`_=q(x`G7-yol?;vN_mq0b z5{jDoHc98#THeN>AQjWw`#I+u3LmyW-qeLojtcSSJb-gcs+-r?P2 zOvWxueRH$cEXn12{51DqB76a$80m25EQR|drh%Y8{K>2N_>PA9mA=XtB}|Q~|yA zC*0{e?cNX=Xqvv>z{I3I)+%6;OGLdmU}n3jN^mg4A$Kz9PXmxImw@;K!YsHM@Webx zPv*5~-SAL6!1j$`N1aM}dA6gUt=Ijo8ifIQb81+FG|%R$Yj)!;Yd3&Pm)yhH+4$z< z2+9}xjencoUgly@z|5vH!*xJ8QZoIeK#(s-|FoplMqaQeVy8~$xf3uYh4$K?Pqhn&(H=%TJWMan8+5JZ;6 zdV_2_4^)!&qqhDVB7qsnpirr@>NJL2TOO@D*9?C=3Y+naLpQ~1WC=~@#AjocBoX)* z%VyOjDR=F{hBb(1pJsVli1fVSYi4w8eKOMKmydvApXqo3POtt=Jz)(Ytevf87bGal zs&Rbd5?QWgy;uYg`May!JYUxv`+2NTiXM0C){3dAS?3h=XG`kY^u)Z&`~VKnd{J zUbx<1}c4y)$?WP7Y+`Vy*Oe~f_t%6F_)X~j9oLPr=A zUyH|sJ|}Q!I*~zM6m-YmcL3=d_( z;E^(qyex@vBLm&oGNgwRYz#VeeD}BKm;#-22iNE$4db)2aE)~K15KXuxO%J(=KV!$ zE!&XCH9IHw?@uQc=E~xJT|!K#xnUR=)Aot)SiBZd9_=1PdH!$}9R3AVG}Lpqwo!{) zFRiuaIp02778odyShDfTp^FxQ2LuRiO$HfzW$!nBk20bSL`XSw{I93}0bdfUqQB1i z&AN1U(i>e+&nec^b&C_5IIn-!d`!l2PXSHTn(W+W0_f<2U(UIO7~dw5^t|7US+gvu z)D0Yxl6o6EM4Y%r5WY-htMxQ9-Lop6zB0k>#}s9lg%5WQpyqNJH5Ui`_}BsHv67kT z=(7Uxm)GwSN!8WX&}T(b@`073Hb{ZWnr6WO=luaOV$mnBoiXzj{9)@j6gSh+e(tGQ zzv=}Ee>ExI>Qg?rBDe^Y#ES8JD4Ch)u|EqgM@>9iFwSHQ zzkRfappFy$>9>0f3SX|JS#qlROht2z$I#$DxvW1>fo^IxQe`3iK@pJd$+!QN|FGP{DCeIf1;EXJ-t6%uVgd-E zphb$GeV=>2h2ByJw_$^Ttc^AwQ`C{qU$3Xnw;DgvKS-;Q7^{J+F0j}J5JE(;L`t+t z3qMaZW$U6s7u)y?eSDeg6VxqUg(V{d4@cCm>%mt&3XSPcyDm$w&M>tOyv@H!Cu2k~ z8w}C=_MjUB)X!f<1O7IVbJIy|LT0OdMzBA)i3Yz(bOy=Pz)M6Bd|@x{WXLS7F#XH| zj_sPVA9~LL%mt+(?IlUmAEkqDKkoybq`@jGX|h73n+m4J02|g;fcc#hS=4N4W>(aa z=OM|frCbFGrGd8ZvgICbpw({>JiY{aiB3nrsWvYsSvpv+y~{8v#zDb9U_A%jzYQXE z_-Tm6Y#GzSPrhEAotqDa|EtFtd=5Q0nfW&EWN3`mI(5)<5XHI+vhaUpT%upVRl{Lc z{4BD0z%F=^P@8{sxYwEEQ)$zvzgHZrTGcS`34-@>|Fvr$FyeR|J-kCq?;iy>irNZj zEp_mvK~SSHL*HSd0;iEV{fx(q$b;(Nz)dJ{ENw~yS0|lQf>}za#ah4 z3mY*gZOwo-Pq%^58h##OVUnmlsAKTeF_~777&Q_zbj#l;jMLt>xePgz(+oxcz9iB> z^>yOS6L3e)kC=WX)RX*LcBe!(<^WzPfE&1PAzubhEHI*GVXhtw{y_2P<;Gb%K4h)2 zBb`PeVi%(P@IxMlS?&?>KsM>1LF9;<+xHg?fU@BCLSEsynmgqN)`z?$%5CXW^pZfG z=loo%_aP)u7|q0Y6*%MKb8_^%IkP#_Dt3&dsxnRHE2zL&VMB8U3JZGQ9~lSHk) zB2-YR<8tI+x)`Gpu*Oz8U=ttS!9Q(HM0FRvq{|H;Ql6AxM6JYYbq50EV#Hzye?{Y(%-xi?A^L)QQxv7U-z&-9H{=1?Pax z|Hwva43o1g4?G;vXaARb&}PEmcJnRoWRpNWGYRy#R)%Mpw4&?+Pi;6%I%u( z1sUT*6g&Qjzi%6J>V&{z&;72^*0!vitJBBnWk|O^j*9kVYa$)P$PY*b^WMT4dK+Zm zx^s_UWMh5S{0wkouKczS2%13iE5K6*2o0$esKAI6noIF}hMSF>tdTbIr{HbXm2ER4 zZ>BezPK9r{WiTfp_`7#b5e$@-u>MRmMGH~zY>$`ou!9@ zJ)Hw1SuX(qlcNx0)5OJAOn{qU^w6Lpj~M32X+14j@Re{clX+efxBW$X_l~)_K^|@p zdRg@}(kB4g89a%YO_xK(XOkwem0yPflAd4+!s0AL*tQX$9g<^SAmSNaTAoZ(AP9H4eE99uTKp|QQ5r- zy#h&o6?DGNIB=efv-j(uw3)we>Jg^YhJJox08o=mB_NC-f#&7k3tK zy=akXrLN6UR{Z7~a4WhG*=Y-^y!egPc#5q|u9$MENKeIT`*c#a27 z5r7h-$@5Rf{A3L39w**v`$skYg>8Ro%y$m_WX#V;WPiS{`(xJl9hUukcKBcE^iRfo z=fF?K{CvUt=hNo@yl3$Po)AenKN<6rG3W`-#Dl)ybF`m~`N^1{e^=t?AKZZP)6c&| zg4`KHtC~Mcn{ch>=U*cES&#WykNH`T`B{%a4-qFGl7t%z(b~iRpBOXX20;;bif?~e Uyl@5hD`$|`{Kf9)TzQHA52YdRX#fBK literal 0 HcmV?d00001 diff --git a/rviz_default_plugins/test/rviz_default_plugins/displays/accel/accel_display_visual_test.cpp b/rviz_default_plugins/test/rviz_default_plugins/displays/accel/accel_display_visual_test.cpp new file mode 100644 index 000000000..350c1983b --- /dev/null +++ b/rviz_default_plugins/test/rviz_default_plugins/displays/accel/accel_display_visual_test.cpp @@ -0,0 +1,61 @@ +/* + * 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 Willow Garage, Inc. 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_visual_testing_framework/visual_test_fixture.hpp" +#include "rviz_visual_testing_framework/visual_test_publisher.hpp" + +#include "../../page_objects/accel_display_page_object.hpp" +#include "../../publishers/accel_publisher.hpp" + +TEST_F(VisualTestFixture, accels_are_displayed) { + auto accel_publisher = std::make_shared(); + auto accel_visual_publisher = + std::make_unique(accel_publisher, "accel_frame"); + + setCamPose(Ogre::Vector3(10, 10, 16)); + setCamLookAt(Ogre::Vector3(0, 0, 0)); + + auto accel_display = addDisplay(); + accel_display->setTopic("/accel"); + accel_display->setLinearColor(255, 255, 0); + accel_display->setAngularColor(0, 255, 255); + accel_display->setLinearScale(2); + accel_display->setAngularScale(2); + accel_display->setWidth(4); + + captureMainWindow(); + + accel_display->setAlpha(0.0f); + captureMainWindow("empty_scene"); + + assertScreenShotsIdentity(); +} diff --git a/rviz_default_plugins/test/rviz_default_plugins/displays/twist/twist_display_visual_test.cpp b/rviz_default_plugins/test/rviz_default_plugins/displays/twist/twist_display_visual_test.cpp new file mode 100644 index 000000000..ca402855e --- /dev/null +++ b/rviz_default_plugins/test/rviz_default_plugins/displays/twist/twist_display_visual_test.cpp @@ -0,0 +1,61 @@ +/* + * 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 Willow Garage, Inc. 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_visual_testing_framework/visual_test_fixture.hpp" +#include "rviz_visual_testing_framework/visual_test_publisher.hpp" + +#include "../../page_objects/twist_display_page_object.hpp" +#include "../../publishers/twist_publisher.hpp" + +TEST_F(VisualTestFixture, twists_are_displayed) { + auto twist_publisher = std::make_shared(); + auto twist_visual_publisher = + std::make_unique(twist_publisher, "twist_frame"); + + setCamPose(Ogre::Vector3(10, 10, 16)); + setCamLookAt(Ogre::Vector3(0, 0, 0)); + + auto twist_display = addDisplay(); + twist_display->setTopic("/twist"); + twist_display->setLinearColor(255, 255, 0); + twist_display->setAngularColor(0, 255, 255); + twist_display->setLinearScale(2); + twist_display->setAngularScale(2); + twist_display->setWidth(4); + + captureMainWindow(); + + twist_display->setAlpha(0.0f); + captureMainWindow("empty_scene"); + + assertScreenShotsIdentity(); +} diff --git a/rviz_default_plugins/test/rviz_default_plugins/page_objects/accel_display_page_object.cpp b/rviz_default_plugins/test/rviz_default_plugins/page_objects/accel_display_page_object.cpp new file mode 100644 index 000000000..a5a6c2f4e --- /dev/null +++ b/rviz_default_plugins/test/rviz_default_plugins/page_objects/accel_display_page_object.cpp @@ -0,0 +1,79 @@ +/* + * 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 Willow Garage, Inc. 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 "accel_display_page_object.hpp" + +#include +#include +#include + +AccelDisplayPageObject::AccelDisplayPageObject() +: BasePageObject(0, "AccelStamped") +{} + +void AccelDisplayPageObject::setTopic(QString topic) +{ + setComboBox("Topic", topic); + waitForFirstMessage(); +} + +void AccelDisplayPageObject::setAlpha(float alpha) +{ + setFloat("Alpha", alpha); +} + +void AccelDisplayPageObject::setAngularColor(int r, int g, int b) +{ + setColorCode("Angular Color", r, g, b); +} + +void AccelDisplayPageObject::setLinearColor(int r, int g, int b) +{ + setColorCode("Linear Color", r, g, b); +} + +void AccelDisplayPageObject::setLinearScale(float scale) +{ + setFloat("Linear Arrow Scale", scale); +} + +void AccelDisplayPageObject::setAngularScale(float scale) +{ + setFloat("Angular Arrow Scale", scale); +} + +void AccelDisplayPageObject::setWidth(float width) +{ + setFloat("Arrow Width", width); +} + +void AccelDisplayPageObject::setHistoryLength(int history) +{ + setInt("History Length", history); +} diff --git a/rviz_default_plugins/test/rviz_default_plugins/page_objects/accel_display_page_object.hpp b/rviz_default_plugins/test/rviz_default_plugins/page_objects/accel_display_page_object.hpp new file mode 100644 index 000000000..f7c894407 --- /dev/null +++ b/rviz_default_plugins/test/rviz_default_plugins/page_objects/accel_display_page_object.hpp @@ -0,0 +1,52 @@ +/* + * 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 Willow Garage, Inc. 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__PAGE_OBJECTS__ACCEL_DISPLAY_PAGE_OBJECT_HPP_ +#define RVIZ_DEFAULT_PLUGINS__PAGE_OBJECTS__ACCEL_DISPLAY_PAGE_OBJECT_HPP_ + +#include "rviz_visual_testing_framework/page_objects/base_page_object.hpp" + +#include // NOLINT + +class AccelDisplayPageObject : public BasePageObject +{ +public: + AccelDisplayPageObject(); + + void setTopic(QString topic); + void setAlpha(float alpha); + void setAngularColor(int r, int g, int b); + void setLinearColor(int r, int g, int b); + void setAngularScale(float scale); + void setLinearScale(float scale); + void setWidth(float width); + void setHistoryLength(int history); +}; + +#endif // RVIZ_DEFAULT_PLUGINS__PAGE_OBJECTS__ACCEL_DISPLAY_PAGE_OBJECT_HPP_ diff --git a/rviz_default_plugins/test/rviz_default_plugins/page_objects/twist_display_page_object.cpp b/rviz_default_plugins/test/rviz_default_plugins/page_objects/twist_display_page_object.cpp new file mode 100644 index 000000000..17efbc594 --- /dev/null +++ b/rviz_default_plugins/test/rviz_default_plugins/page_objects/twist_display_page_object.cpp @@ -0,0 +1,79 @@ +/* + * 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 Willow Garage, Inc. 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 "twist_display_page_object.hpp" + +#include + +#include +#include + +TwistDisplayPageObject::TwistDisplayPageObject() +: BasePageObject(0, "TwistStamped") +{} + +void TwistDisplayPageObject::setTopic(QString topic) +{ + setComboBox("Topic", topic); + waitForFirstMessage(); +} + +void TwistDisplayPageObject::setAlpha(float alpha) +{ + setFloat("Alpha", alpha); +} + +void TwistDisplayPageObject::setAngularColor(int r, int g, int b) +{ + setColorCode("Angular Color", r, g, b); +} + +void TwistDisplayPageObject::setLinearColor(int r, int g, int b) +{ + setColorCode("Linear Color", r, g, b); +} + +void TwistDisplayPageObject::setLinearScale(float scale) +{ + setFloat("Linear Arrow Scale", scale); +} + +void TwistDisplayPageObject::setAngularScale(float scale) +{ + setFloat("Angular Arrow Scale", scale); +} + +void TwistDisplayPageObject::setWidth(float width) +{ + setFloat("Arrow Width", width); +} + +void TwistDisplayPageObject::setHistoryLength(int history) +{ + setInt("History Length", history); +} diff --git a/rviz_default_plugins/test/rviz_default_plugins/page_objects/twist_display_page_object.hpp b/rviz_default_plugins/test/rviz_default_plugins/page_objects/twist_display_page_object.hpp new file mode 100644 index 000000000..a82301ae4 --- /dev/null +++ b/rviz_default_plugins/test/rviz_default_plugins/page_objects/twist_display_page_object.hpp @@ -0,0 +1,51 @@ +/* + * 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 Willow Garage, Inc. 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__PAGE_OBJECTS__TWIST_DISPLAY_PAGE_OBJECT_HPP_ +#define RVIZ_DEFAULT_PLUGINS__PAGE_OBJECTS__TWIST_DISPLAY_PAGE_OBJECT_HPP_ + +#include "rviz_visual_testing_framework/page_objects/base_page_object.hpp" + +#include // NOLINT + +class TwistDisplayPageObject : public BasePageObject +{ +public: + TwistDisplayPageObject(); + + void setTopic(QString topic); + void setAlpha(float alpha); + void setAngularColor(int r, int g, int b); + void setLinearColor(int r, int g, int b); + void setAngularScale(float scale); + void setLinearScale(float scale); + void setWidth(float width); + void setHistoryLength(int history); +}; + +#endif // RVIZ_DEFAULT_PLUGINS__PAGE_OBJECTS__TWIST_DISPLAY_PAGE_OBJECT_HPP_ diff --git a/rviz_default_plugins/test/rviz_default_plugins/publishers/accel_publisher.hpp b/rviz_default_plugins/test/rviz_default_plugins/publishers/accel_publisher.hpp new file mode 100644 index 000000000..4a6cb3bc0 --- /dev/null +++ b/rviz_default_plugins/test/rviz_default_plugins/publishers/accel_publisher.hpp @@ -0,0 +1,92 @@ +/* + * Copyright (c) 2019, Martin Idel + * 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__PUBLISHERS__ACCEL_PUBLISHER_HPP_ +#define RVIZ_DEFAULT_PLUGINS__PUBLISHERS__ACCEL_PUBLISHER_HPP_ + +#include +#include + +#include "rclcpp/rclcpp.hpp" + +#include "geometry_msgs/msg/accel_stamped.hpp" +#include "std_msgs/msg/header.hpp" + +using namespace std::chrono_literals; // NOLINT + +namespace nodes +{ + +class AccelPublisher : public rclcpp::Node +{ +public: + AccelPublisher(); + +private: + geometry_msgs::msg::AccelStamped createAccelMessage(); + + rclcpp::TimerBase::SharedPtr timer_; + rclcpp::Publisher::SharedPtr publisher_; +}; + +AccelPublisher::AccelPublisher() +: Node("accel_publisher") +{ + publisher_ = this->create_publisher("accel", 10); + + auto timer_callback = + [this]() -> void { + auto message = createAccelMessage(); + this->publisher_->publish(message); + }; + timer_ = this->create_wall_timer(500ms, timer_callback); +} + +geometry_msgs::msg::AccelStamped AccelPublisher::createAccelMessage() +{ + geometry_msgs::msg::AccelStamped accelMessage; + + accelMessage.header = std_msgs::msg::Header(); + accelMessage.header.frame_id = "accel_frame"; + accelMessage.header.stamp = rclcpp::Clock().now(); + + accelMessage.accel.linear.x = 1; + accelMessage.accel.linear.y = 3; + accelMessage.accel.linear.z = 2; + + accelMessage.accel.angular.x = 3; + accelMessage.accel.angular.y = 2; + accelMessage.accel.angular.z = 1; + + return accelMessage; +} + +} // namespace nodes + +#endif // RVIZ_DEFAULT_PLUGINS__PUBLISHERS__ACCEL_PUBLISHER_HPP_ diff --git a/rviz_default_plugins/test/rviz_default_plugins/publishers/twist_publisher.hpp b/rviz_default_plugins/test/rviz_default_plugins/publishers/twist_publisher.hpp new file mode 100644 index 000000000..2591cb5f2 --- /dev/null +++ b/rviz_default_plugins/test/rviz_default_plugins/publishers/twist_publisher.hpp @@ -0,0 +1,92 @@ +/* + * Copyright (c) 2019, Martin Idel + * 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__PUBLISHERS__TWIST_PUBLISHER_HPP_ +#define RVIZ_DEFAULT_PLUGINS__PUBLISHERS__TWIST_PUBLISHER_HPP_ + +#include +#include + +#include "rclcpp/rclcpp.hpp" + +#include "geometry_msgs/msg/twist_stamped.hpp" +#include "std_msgs/msg/header.hpp" + +using namespace std::chrono_literals; // NOLINT + +namespace nodes +{ + +class TwistPublisher : public rclcpp::Node +{ +public: + TwistPublisher(); + +private: + geometry_msgs::msg::TwistStamped createTwistMessage(); + + rclcpp::TimerBase::SharedPtr timer_; + rclcpp::Publisher::SharedPtr publisher_; +}; + +TwistPublisher::TwistPublisher() +: Node("twist_publisher") +{ + publisher_ = this->create_publisher("twist", 10); + + auto timer_callback = + [this]() -> void { + auto message = createTwistMessage(); + this->publisher_->publish(message); + }; + timer_ = this->create_wall_timer(500ms, timer_callback); +} + +geometry_msgs::msg::TwistStamped TwistPublisher::createTwistMessage() +{ + geometry_msgs::msg::TwistStamped twistMessage; + + twistMessage.header = std_msgs::msg::Header(); + twistMessage.header.frame_id = "twist_frame"; + twistMessage.header.stamp = rclcpp::Clock().now(); + + twistMessage.twist.linear.x = 1; + twistMessage.twist.linear.y = 3; + twistMessage.twist.linear.z = 2; + + twistMessage.twist.angular.x = 3; + twistMessage.twist.angular.y = 2; + twistMessage.twist.angular.z = 1; + + return twistMessage; +} + +} // namespace nodes + +#endif // RVIZ_DEFAULT_PLUGINS__PUBLISHERS__TWIST_PUBLISHER_HPP_ diff --git a/rviz_rendering/CMakeLists.txt b/rviz_rendering/CMakeLists.txt index ca0278747..a053ad39e 100644 --- a/rviz_rendering/CMakeLists.txt +++ b/rviz_rendering/CMakeLists.txt @@ -92,6 +92,7 @@ add_library(rviz_rendering SHARED src/rviz_rendering/objects/object.cpp src/rviz_rendering/objects/point_cloud.cpp src/rviz_rendering/objects/point_cloud_renderable.cpp + src/rviz_rendering/objects/screw_visual.cpp src/rviz_rendering/objects/shape.cpp src/rviz_rendering/objects/wrench_visual.cpp ) @@ -276,6 +277,18 @@ if(BUILD_TESTING) ) endif() + ament_add_gmock(screw_visual_test_target + test/rviz_rendering/objects/screw_visual_test.cpp + ${SKIP_DISPLAY_TESTS}) + if(TARGET screw_visual_test_target) + target_link_libraries(screw_visual_test_target + rviz_ogre_vendor::OgreMain + rviz_rendering + rviz_rendering_test_utils + Qt5::Widgets # explicitly do this for include directories (not necessary for external use) + ) + endif() + ament_add_gmock(wrench_visual_test_target test/rviz_rendering/objects/wrench_visual_test.cpp ${SKIP_DISPLAY_TESTS}) diff --git a/rviz_rendering/include/rviz_rendering/objects/screw_visual.hpp b/rviz_rendering/include/rviz_rendering/objects/screw_visual.hpp new file mode 100644 index 000000000..aa6193184 --- /dev/null +++ b/rviz_rendering/include/rviz_rendering/objects/screw_visual.hpp @@ -0,0 +1,110 @@ +/* + * 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 Willow Garage, Inc. 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_RENDERING__OBJECTS__SCREW_VISUAL_HPP_ +#define RVIZ_RENDERING__OBJECTS__SCREW_VISUAL_HPP_ + +#include "rviz_rendering/objects/screw_visual.hpp" + +#include +#include + +#include + +#include "rviz_rendering/objects/arrow.hpp" +#include "rviz_rendering/objects/billboard_line.hpp" + +#include "rviz_rendering/visibility_control.hpp" + +namespace rviz_rendering +{ +// ScrewVisual visualizes a single screw, i.e. a wrench, twist, or acceleration +class ScrewVisual +{ +public: + // Constructor. + // Creates the visual stuff and puts it into the scene, but in an unconfigured state. + RVIZ_RENDERING_PUBLIC + ScrewVisual(Ogre::SceneManager * scene_manager, Ogre::SceneNode * parent_node); + + // Destructor. Removes the visual stuff from the scene. + virtual ~ScrewVisual(); + + // Configure the visual to show the given linear and angular vectors + RVIZ_RENDERING_PUBLIC + void setScrew(const Ogre::Vector3 & linear, const Ogre::Vector3 & angular); + + // Set the pose of the coordinate frame the message refers to. + // This could be done inside setMessage(), but that would require calls to FrameManager + // and error handling inside setMessage(), which doesn't seem as clean. + // This way ScrewVisual is only responsible for visualization. + RVIZ_RENDERING_PUBLIC + void setFramePosition(const Ogre::Vector3 & position); + RVIZ_RENDERING_PUBLIC + void setFrameOrientation(const Ogre::Quaternion & orientation); + + // Set the color and alpha of the visual, which are user-editable + // parameters and therefore don't come from the message. + RVIZ_RENDERING_PUBLIC + void setLinearColor(float r, float g, float b, float a); + RVIZ_RENDERING_PUBLIC + void setAngularColor(float r, float g, float b, float a); + RVIZ_RENDERING_PUBLIC + void setLinearScale(float s); + RVIZ_RENDERING_PUBLIC + void setAngularScale(float s); + RVIZ_RENDERING_PUBLIC + void setWidth(float w); + RVIZ_RENDERING_PUBLIC + void setHideSmallValues(bool h); + RVIZ_RENDERING_PUBLIC + void setVisible(bool visible); + +private: + // The object implementing the circle + std::unique_ptr arrow_linear_; + std::unique_ptr arrow_angular_; + std::unique_ptr circle_angular_; + std::unique_ptr circle_arrow_angular_; + float linear_scale_; + float angular_scale_; + float width_; + bool hide_small_values_; + + // A SceneNode whose pose is set to match the coordinate frame of the message header. + Ogre::SceneNode * frame_node_; + // allow showing/hiding of linear / angular arrows + Ogre::SceneNode * linear_node_; + Ogre::SceneNode * angular_node_; + + // The SceneManager, kept here only so the destructor can ask it to destroy the ``frame_node_``. + Ogre::SceneManager * scene_manager_; +}; +} // namespace rviz_rendering + +#endif // RVIZ_RENDERING__OBJECTS__SCREW_VISUAL_HPP_ diff --git a/rviz_rendering/src/rviz_rendering/objects/screw_visual.cpp b/rviz_rendering/src/rviz_rendering/objects/screw_visual.cpp new file mode 100644 index 000000000..a38cbe5c0 --- /dev/null +++ b/rviz_rendering/src/rviz_rendering/objects/screw_visual.cpp @@ -0,0 +1,169 @@ +/* + * 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 Willow Garage, Inc. 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. + */ + +#define _USE_MATH_DEFINES +#include +#include + +#include +#include + +#include "rviz_rendering/objects/arrow.hpp" +#include "rviz_rendering/objects/billboard_line.hpp" + +#include "rviz_rendering/objects/screw_visual.hpp" + +namespace rviz_rendering +{ +ScrewVisual::ScrewVisual(Ogre::SceneManager * scene_manager, Ogre::SceneNode * parent_node) +{ + scene_manager_ = scene_manager; + + // Ogre::SceneNode s form a tree, with each node storing the transform (position and orientation) + // of itself relative to its parent. Ogre does the math of combining those transforms + // for rendering. Here we create a node to store the pose of the screw's header + // frame relative to the RViz fixed frame. + frame_node_ = parent_node->createChildSceneNode(); + linear_node_ = frame_node_->createChildSceneNode(); + angular_node_ = frame_node_->createChildSceneNode(); + hide_small_values_ = true; + + // We create the arrow object within the frame node so that we can + // set its position and direction relative to its header frame. + arrow_linear_ = std::make_unique(scene_manager_, linear_node_); + arrow_angular_ = std::make_unique(scene_manager_, angular_node_); + circle_angular_ = std::make_unique(scene_manager_, angular_node_); + circle_arrow_angular_ = std::make_unique(scene_manager_, angular_node_); +} + +ScrewVisual::~ScrewVisual() +{ + // Destroy the frame node since we don't need it anymore. + scene_manager_->destroySceneNode(frame_node_); +} + +void ScrewVisual::setScrew(const Ogre::Vector3 & linear, const Ogre::Vector3 & angular) +{ + float linear_length = linear.length() * linear_scale_; + float angular_length = angular.length() * angular_scale_; + // hide markers if they get too short and hide_small_values_ is activated + // "too short" is defined as "linear_length > width_" + bool show_linear = (linear_length > width_) || !hide_small_values_; + bool show_angular = (angular_length > width_) || !hide_small_values_; + + if (show_linear) { + arrow_linear_->setScale(Ogre::Vector3(linear_length, width_, width_)); + arrow_linear_->setDirection(linear); + } + linear_node_->setVisible(show_linear); + + if (show_angular) { + arrow_angular_->setScale(Ogre::Vector3(angular_length, width_, width_)); + arrow_angular_->setDirection(angular); + Ogre::Vector3 axis_z(0, 0, 1); + Ogre::Quaternion orientation = axis_z.getRotationTo(angular); + if (std::isnan(orientation.x) || std::isnan(orientation.y) || std::isnan(orientation.z)) { + orientation = Ogre::Quaternion::IDENTITY; + } + // circle_arrow_angular_->setScale(Ogre::Vector3(width_, width_, 0.05)); + circle_arrow_angular_->set(0, width_ * 0.1f, width_ * 0.1f * 1.0f, width_ * 0.1f * 2.0f); + circle_arrow_angular_->setDirection(orientation * Ogre::Vector3(0, 1, 0)); + circle_arrow_angular_->setPosition( + orientation * + Ogre::Vector3(angular_length / 4.0f, 0, angular_length / 2.0f)); + circle_angular_->clear(); + circle_angular_->setLineWidth(width_ * 0.05f); + // create Torque Direction Circle + // The cirlce is divided in 32 parts because it plotted using lines. We are going to plot + // the cirle from the 4th portion to the 31. The first 4 parts are used to visualize the arrow + const int initialCiclePortion = 4; + const int endCirclePortion = 32; + for (int i = initialCiclePortion; i <= endCirclePortion; i++) { + Ogre::Vector3 point = + Ogre::Vector3( + static_cast((angular_length / 4.0f) * cos(i * 2.0f * M_PI / 32.0f)), + static_cast((angular_length / 4.0f) * sin(i * 2.0f * M_PI / 32.0f)), + static_cast(angular_length / 2.0f)); + circle_angular_->addPoint(orientation * point); + } + } + angular_node_->setVisible(show_angular); +} + +// Position and orientation are passed through to the SceneNode. +void ScrewVisual::setFramePosition(const Ogre::Vector3 & position) +{ + frame_node_->setPosition(position); +} + +void ScrewVisual::setFrameOrientation(const Ogre::Quaternion & orientation) +{ + frame_node_->setOrientation(orientation); +} + +// Color is passed through to the rviz object. +void ScrewVisual::setLinearColor(float r, float g, float b, float a) +{ + arrow_linear_->setColor(r, g, b, a); +} +// Color is passed through to the rviz object. +void ScrewVisual::setAngularColor(float r, float g, float b, float a) +{ + arrow_angular_->setColor(r, g, b, a); + circle_angular_->setColor(r, g, b, a); + circle_arrow_angular_->setColor(r, g, b, a); +} + +void ScrewVisual::setLinearScale(float s) +{ + linear_scale_ = s; +} + +void ScrewVisual::setAngularScale(float s) +{ + angular_scale_ = s; +} + +void ScrewVisual::setWidth(float w) +{ + width_ = w; +} + +void ScrewVisual::setHideSmallValues(bool h) +{ + hide_small_values_ = h; +} + + +void ScrewVisual::setVisible(bool visible) +{ + frame_node_->setVisible(visible); +} + +} // end namespace rviz_rendering diff --git a/rviz_rendering/test/rviz_rendering/objects/screw_visual_test.cpp b/rviz_rendering/test/rviz_rendering/objects/screw_visual_test.cpp new file mode 100644 index 000000000..60ee86c6c --- /dev/null +++ b/rviz_rendering/test/rviz_rendering/objects/screw_visual_test.cpp @@ -0,0 +1,139 @@ +/* + * Copyright (c) 2019, Martin Idel + * + * 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 Willow Garage, Inc. 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 +#include + +#include "../ogre_testing_environment.hpp" +#include "../scene_graph_introspection.hpp" +#include "rviz_rendering/objects/screw_visual.hpp" + +using namespace ::testing; // NOLINT + +MATCHER_P(Vector3Eq, expected, "") { + return Ogre::Math::Abs(expected.x - arg.x) < 0.0001f && + Ogre::Math::Abs(expected.y - arg.y) < 0.0001f && + Ogre::Math::Abs(expected.z - arg.z) < 0.0001f; +} + +MATCHER_P(ColorEq, expected, "") { + return Ogre::Math::Abs(expected.a - arg.a) < 0.0001f && + Ogre::Math::Abs(expected.r - arg.r) < 0.0001f && + Ogre::Math::Abs(expected.g - arg.g) < 0.0001f && + Ogre::Math::Abs(expected.b - arg.b) < 0.0001f; +} + +MATCHER_P(QuaterionEq, expected, "") { + return Ogre::Math::Abs(expected.x - arg.x) < 0.0001f && + Ogre::Math::Abs(expected.y - arg.y) < 0.0001f && + Ogre::Math::Abs(expected.z - arg.z) < 0.0001f && + Ogre::Math::Abs(expected.w - arg.w) < 0.0001f; +} + +class ScrewVisualTestFixture : public ::testing::Test +{ +protected: + void SetUp() + { + testing_environment_ = std::make_shared(); + testing_environment_->setUpOgreTestEnvironment(); + } + + std::shared_ptr testing_environment_; +}; + +Ogre::SceneNode * findLinearArrow(Ogre::SceneNode * scene_node) +{ + auto arrows = rviz_rendering::findAllArrows(scene_node); + auto billboard_line = rviz_rendering::findOneBillboardChain(scene_node); + for (const auto & arrow : arrows) { + if (billboard_line->getParentSceneNode()->getParent() != arrow->getParent()) { + return arrow; + } + } + return nullptr; +} + +Ogre::SceneNode * findAngularArrow(Ogre::SceneNode * scene_node) +{ + auto arrows = rviz_rendering::findAllArrows(scene_node); + auto billboard_line = rviz_rendering::findOneBillboardChain(scene_node); + for (const auto & arrow : arrows) { + if (billboard_line->getParentSceneNode()->getParent() == arrow->getParent()) { + return arrow; + } + } + return nullptr; +} + +TEST_F(ScrewVisualTestFixture, setScrew_sets_linear_arrow_correctly) { + auto scene_manager = Ogre::Root::getSingletonPtr()->createSceneManager(); + auto root_node = scene_manager->getRootSceneNode(); + + auto screw_visual = std::make_shared(scene_manager, root_node); + EXPECT_NE(nullptr, screw_visual); + + auto arrows = rviz_rendering::findAllArrows(root_node); + EXPECT_THAT(arrows, SizeIs(3u)); + auto linear_arrow = findLinearArrow(root_node); + auto angular_arrow = findAngularArrow(root_node); + EXPECT_THAT(linear_arrow->getScale(), Vector3Eq(Ogre::Vector3(1, 1, 1))); + EXPECT_THAT(angular_arrow->getScale(), Vector3Eq(Ogre::Vector3(1, 1, 1))); + EXPECT_THAT( + arrows[0]->convertWorldToLocalPosition(Ogre::Vector3(0, 0, 0)), + Vector3Eq(Ogre::Vector3(0, 0, 0))); + EXPECT_THAT( + arrows[1]->convertWorldToLocalPosition(Ogre::Vector3(0, 0, 0)), + Vector3Eq(Ogre::Vector3(0, 0, 0))); + EXPECT_THAT( + arrows[2]->convertWorldToLocalPosition(Ogre::Vector3(0, 0, 0)), + Vector3Eq(Ogre::Vector3(0, 0, 0))); + + EXPECT_THAT( + arrows[0]->convertWorldToLocalOrientation(Ogre::Quaternion()), + QuaterionEq(Ogre::Quaternion(0.707107f, 0.707107f, 0.0f, 0.0f))); + EXPECT_THAT( + arrows[1]->convertWorldToLocalOrientation(Ogre::Quaternion()), + QuaterionEq(Ogre::Quaternion(0.707107f, 0.707107f, 0.0f, 0.0f))); + EXPECT_THAT( + arrows[2]->convertWorldToLocalOrientation(Ogre::Quaternion()), + QuaterionEq(Ogre::Quaternion(0.707107f, 0.707107f, 0.0f, 0.0f))); + + screw_visual->setLinearScale(1); + screw_visual->setAngularScale(2); + screw_visual->setScrew(Ogre::Vector3(1, 1, 1), Ogre::Vector3(1, 1, 1)); + linear_arrow = findLinearArrow(root_node); + EXPECT_THAT(linear_arrow->getScale(), Vector3Eq(Ogre::Vector3(0.0f, 1.73205f, 0.0f))); + + angular_arrow = findAngularArrow(root_node); + EXPECT_THAT(angular_arrow->getScale(), Vector3Eq(Ogre::Vector3(0.0f, 3.4641f, 0.0f))); +}