diff --git a/README.md b/README.md index c8928b5fb..6568b9cb3 100644 --- a/README.md +++ b/README.md @@ -35,6 +35,7 @@ For some displays, the [documentation is updated](docs/FEATURES.md). | Robot Model | | Temperature | | TF | +| Wrench | ### Not yet ported @@ -48,7 +49,6 @@ These features have not been ported to `ros2/rviz` yet. | Interactive Marker | | Oculus | | Pose With Covariance | -| Wrench | Other features: - Filtering of Topic lists by topic type diff --git a/rviz/src/rviz/default_plugin/wrench_display.cpp b/rviz/src/rviz/default_plugin/wrench_display.cpp deleted file mode 100644 index 8767540ba..000000000 --- a/rviz/src/rviz/default_plugin/wrench_display.cpp +++ /dev/null @@ -1,176 +0,0 @@ -#include -#include - -#include -#include -#include -#include -#include -#include -#include - -#include - -#include "wrench_visual.h" - -#include "wrench_display.h" - -namespace rviz -{ - -WrenchStampedDisplay::WrenchStampedDisplay() -{ - force_color_property_ = - new rviz::ColorProperty( "Force Color", QColor( 204, 51, 51 ), - "Color to draw the force arrows.", - this, SLOT( updateColorAndAlpha() )); - - torque_color_property_ = - new rviz::ColorProperty( "Torque Color", QColor( 204, 204, 51), - "Color to draw the torque arrows.", - this, SLOT( updateColorAndAlpha() )); - - alpha_property_ = - new rviz::FloatProperty( "Alpha", 1.0, - "0 is fully transparent, 1.0 is fully opaque.", - this, SLOT( updateColorAndAlpha() )); - - force_scale_property_ = - new rviz::FloatProperty( "Force Arrow Scale", 2.0, - "force arrow scale", - this, SLOT( updateColorAndAlpha() )); - - torque_scale_property_ = - new rviz::FloatProperty( "Torque Arrow Scale", 2.0, - "torque arrow scale", - this, SLOT( updateColorAndAlpha() )); - - width_property_ = - new rviz::FloatProperty( "Arrow Width", 0.5, - "arrow width", - this, SLOT( updateColorAndAlpha() )); - - - history_length_property_ = - new rviz::IntProperty( "History Length", 1, - "Number of prior measurements to display.", - this, SLOT( updateHistoryLength() )); - - history_length_property_->setMin( 1 ); - history_length_property_->setMax( 100000 ); -} - -void WrenchStampedDisplay::onInitialize() -{ - MFDClass::onInitialize(); - updateHistoryLength( ); -} - -WrenchStampedDisplay::~WrenchStampedDisplay() -{ -} - -// Override rviz::Display's reset() function to add a call to clear(). -void WrenchStampedDisplay::reset() -{ - MFDClass::reset(); - visuals_.clear(); -} - -void WrenchStampedDisplay::updateColorAndAlpha() -{ - float alpha = alpha_property_->getFloat(); - float force_scale = force_scale_property_->getFloat(); - float torque_scale = torque_scale_property_->getFloat(); - float width = width_property_->getFloat(); - Ogre::ColourValue force_color = force_color_property_->getOgreColor(); - Ogre::ColourValue torque_color = torque_color_property_->getOgreColor(); - - for( size_t i = 0; i < visuals_.size(); i++ ) - { - visuals_[i]->setForceColor( force_color.r, force_color.g, force_color.b, alpha ); - visuals_[i]->setTorqueColor( torque_color.r, torque_color.g, torque_color.b, alpha ); - visuals_[i]->setForceScale( force_scale ); - visuals_[i]->setTorqueScale( torque_scale ); - visuals_[i]->setWidth( width ); - } -} - -// Set the number of past visuals to show. -void WrenchStampedDisplay::updateHistoryLength() -{ - visuals_.rset_capacity(history_length_property_->getInt()); -} - -bool validateFloats( const geometry_msgs::WrenchStamped& msg ) -{ - return rviz::validateFloats(msg.wrench.force) && rviz::validateFloats(msg.wrench.torque) ; -} - -// This is our callback to handle an incoming message. -void WrenchStampedDisplay::processMessage( const geometry_msgs::WrenchStamped::ConstPtr& msg ) -{ - if( !validateFloats( *msg )) - { - setStatus( rviz::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( !context_->getFrameManager()->getTransform( msg->header.frame_id, - msg->header.stamp, - position, orientation )) - { - ROS_DEBUG( "Error transforming from frame '%s' to frame '%s'", - msg->header.frame_id.c_str(), qPrintable( fixed_frame_ )); - return; - } - - if ( position.isNaN() ) - { - ROS_ERROR_THROTTLE(1.0, "Wrench position contains NaNs. Skipping render as long as the position is invalid"); - 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 - boost::shared_ptr visual; - if( visuals_.full() ) - { - visual = visuals_.front(); - } - else - { - visual.reset(new WrenchVisual( context_->getSceneManager(), scene_node_ )); - } - - // Now set or update the contents of the chosen visual. - visual->setWrench( msg->wrench ); - visual->setFramePosition( position ); - visual->setFrameOrientation( orientation ); - float alpha = alpha_property_->getFloat(); - float force_scale = force_scale_property_->getFloat(); - float torque_scale = torque_scale_property_->getFloat(); - float width = width_property_->getFloat(); - Ogre::ColourValue force_color = force_color_property_->getOgreColor(); - Ogre::ColourValue torque_color = torque_color_property_->getOgreColor(); - visual->setForceColor( force_color.r, force_color.g, force_color.b, alpha ); - visual->setTorqueColor( torque_color.r, torque_color.g, torque_color.b, alpha ); - visual->setForceScale( force_scale ); - visual->setTorqueScale( torque_scale ); - visual->setWidth( width ); - - // And send it to the end of the circular buffer - visuals_.push_back(visual); -} - -} // end namespace rviz - -// Tell pluginlib about this class. It is important to do this in -// global scope, outside our package's namespace. -#include -PLUGINLIB_EXPORT_CLASS( rviz::WrenchStampedDisplay, rviz::Display ) diff --git a/rviz/src/rviz/default_plugin/wrench_display.h b/rviz/src/rviz/default_plugin/wrench_display.h deleted file mode 100644 index 23514a8d5..000000000 --- a/rviz/src/rviz/default_plugin/wrench_display.h +++ /dev/null @@ -1,64 +0,0 @@ -#ifndef WRENCHSTAMPED_DISPLAY_H -#define WRENCHSTAMPED_DISPLAY_H - -#ifndef Q_MOC_RUN -#include -#endif - -#include -#include - -namespace Ogre -{ -class SceneNode; -} - -namespace rviz -{ -class ColorProperty; -class ROSTopicStringProperty; -class FloatProperty; -class IntProperty; -} - -namespace rviz -{ - -class WrenchVisual; - -class WrenchStampedDisplay: public rviz::MessageFilterDisplay -{ - Q_OBJECT -public: - // Constructor. pluginlib::ClassLoader creates instances by calling - // the default constructor, so make sure you have one. - WrenchStampedDisplay(); - virtual ~WrenchStampedDisplay(); - -protected: - // Overrides of public virtual functions from the Display class. - virtual void onInitialize(); - virtual void reset(); - -private Q_SLOTS: - // Helper function to apply color and alpha to all visuals. - void updateColorAndAlpha(); - void updateHistoryLength(); - -private: - // Function to handle an incoming ROS message. - void processMessage( const geometry_msgs::WrenchStamped::ConstPtr& msg ); - - // 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) - boost::circular_buffer > visuals_; - - // Property objects for user-editable properties. - rviz::ColorProperty *force_color_property_, *torque_color_property_; - rviz::FloatProperty *alpha_property_, *force_scale_property_, *torque_scale_property_, *width_property_; - rviz::IntProperty *history_length_property_; -}; -} // end namespace rviz_plugin_tutorials - -#endif // WRENCHSTAMPED_DISPLAY_H diff --git a/rviz/src/rviz/default_plugin/wrench_visual.cpp b/rviz/src/rviz/default_plugin/wrench_visual.cpp deleted file mode 100644 index 2590a5814..000000000 --- a/rviz/src/rviz/default_plugin/wrench_visual.cpp +++ /dev/null @@ -1,139 +0,0 @@ -#include -#include -#include - -#include -#include - -#include - -#include "wrench_visual.h" - -namespace rviz -{ - -WrenchVisual::WrenchVisual( 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 when it - // is time to render. - // - // Here we create a node to store the pose of the WrenchStamped's header frame - // relative to the RViz fixed frame. - frame_node_ = parent_node->createChildSceneNode(); - force_node_ = frame_node_->createChildSceneNode(); - torque_node_ = frame_node_->createChildSceneNode(); - - // We create the arrow object within the frame node so that we can - // set its position and direction relative to its header frame. - arrow_force_ = new rviz::Arrow( scene_manager_, force_node_ ); - arrow_torque_ = new rviz::Arrow( scene_manager_, torque_node_ ); - circle_torque_ = new rviz::BillboardLine( scene_manager_, torque_node_ ); - circle_arrow_torque_ = new rviz::Arrow( scene_manager_, torque_node_ ); -} - -WrenchVisual::~WrenchVisual() -{ - // Delete the arrow to make it disappear. - delete arrow_force_; - delete arrow_torque_; - delete circle_torque_; - delete circle_arrow_torque_; - - // Destroy the frame node since we don't need it anymore. - scene_manager_->destroySceneNode( frame_node_ ); -} - - -void WrenchVisual::setWrench( const geometry_msgs::Wrench& wrench ) -{ - Ogre::Vector3 force(wrench.force.x, wrench.force.y, wrench.force.z); - Ogre::Vector3 torque(wrench.torque.x, wrench.torque.y, wrench.torque.z); - setWrench(force, torque); -} - -void WrenchVisual::setWrench( const Ogre::Vector3 &force, const Ogre::Vector3 &torque ) -{ - double force_length = force.length() * force_scale_; - double torque_length = torque.length() * torque_scale_; - // hide markers if they get too short - bool show_force = (force_length > width_); - bool show_torque = (torque_length > width_); - if (show_force) { - arrow_force_->setScale(Ogre::Vector3(force_length, width_, width_)); - arrow_force_->setDirection(force); - } - force_node_->setVisible(show_force); - - if (show_torque) { - arrow_torque_->setScale(Ogre::Vector3(torque_length, width_, width_)); - arrow_torque_->setDirection(torque); - Ogre::Vector3 axis_z(0,0,1); - Ogre::Quaternion orientation = axis_z.getRotationTo(torque); - if ( std::isnan(orientation.x) || - std::isnan(orientation.y) || - std::isnan(orientation.z) ) orientation = Ogre::Quaternion::IDENTITY; - //circle_arrow_torque_->setScale(Ogre::Vector3(width_, width_, 0.05)); - circle_arrow_torque_->set(0, width_*0.1, width_*0.1*1.0, width_*0.1*2.0); - circle_arrow_torque_->setDirection(orientation * Ogre::Vector3(0,1,0)); - circle_arrow_torque_->setPosition(orientation * Ogre::Vector3(torque_length/4, 0, torque_length/2)); - circle_torque_->clear(); - circle_torque_->setLineWidth(width_*0.05); - for (int i = 4; i <= 32; i++) { - Ogre::Vector3 point = Ogre::Vector3((torque_length/4)*cos(i*2*M_PI/32), - (torque_length/4)*sin(i*2*M_PI/32), - torque_length/2); - circle_torque_->addPoint(orientation * point); - } - } - torque_node_->setVisible(show_torque); -} - -// Position and orientation are passed through to the SceneNode. -void WrenchVisual::setFramePosition( const Ogre::Vector3& position ) -{ - frame_node_->setPosition( position ); -} - -void WrenchVisual::setFrameOrientation( const Ogre::Quaternion& orientation ) -{ - frame_node_->setOrientation( orientation ); -} - -// Color is passed through to the rviz object. -void WrenchVisual::setForceColor( float r, float g, float b, float a ) -{ - arrow_force_->setColor( r, g, b, a ); -} -// Color is passed through to the rviz object. -void WrenchVisual::setTorqueColor( float r, float g, float b, float a ) -{ - arrow_torque_->setColor( r, g, b, a ); - circle_torque_->setColor( r, g, b, a ); - circle_arrow_torque_->setColor( r, g, b, a ); -} - -void WrenchVisual::setForceScale( float s ) -{ - force_scale_ = s; -} - -void WrenchVisual::setTorqueScale( float s ) -{ - torque_scale_ = s; -} - -void WrenchVisual::setWidth( float w ) -{ - width_ = w; -} - -void WrenchVisual::setVisible(bool visible) -{ - frame_node_->setVisible(visible); -} - -} // end namespace rviz diff --git a/rviz/src/rviz/default_plugin/wrench_visual.h b/rviz/src/rviz/default_plugin/wrench_visual.h deleted file mode 100644 index c6c5df6fc..000000000 --- a/rviz/src/rviz/default_plugin/wrench_visual.h +++ /dev/null @@ -1,81 +0,0 @@ -#ifndef WRENCHSTAMPED_VISUAL_H -#define WRENCHSTAMPED_VISUAL_H - -#include - -namespace Ogre -{ - class Vector3; - class Quaternion; -} - -namespace rviz -{ - class Arrow; - class BillboardLine; -} - -namespace rviz -{ - - -// Each instance of WrenchStampedVisual represents the visualization of a single -// sensor_msgs::WrenchStamped message. Currently it just shows an arrow with -// the direction and magnitude of the acceleration vector, but could -// easily be expanded to include more of the message data. -class WrenchVisual -{ -public: - // Constructor. Creates the visual stuff and puts it into the - // scene, but in an unconfigured state. - WrenchVisual( Ogre::SceneManager* scene_manager, Ogre::SceneNode* parent_node ); - - // Destructor. Removes the visual stuff from the scene. - virtual ~WrenchVisual(); - - // Configure the visual to show the given force and torque vectors - void setWrench( const Ogre::Vector3 &force, const Ogre::Vector3 &torque ); - // Configure the visual to show the data in the message. - void setWrench( const geometry_msgs::Wrench& wrench ); - - // Set the pose of the coordinate frame the message refers to. - // These 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 WrenchStampedVisual is only - // responsible for visualization. - void setFramePosition( const Ogre::Vector3& position ); - 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 WrenchStamped message. - void setForceColor( float r, float g, float b, float a ); - void setTorqueColor( float r, float g, float b, float a ); - void setForceScale( float s ); - void setTorqueScale( float s ); - void setWidth( float w ); - void setVisible( bool visible ); - -private: - // The object implementing the wrenchStamped circle - rviz::Arrow* arrow_force_; - rviz::Arrow* arrow_torque_; - rviz::BillboardLine* circle_torque_; - rviz::Arrow* circle_arrow_torque_; - float force_scale_, torque_scale_, width_; - - // A SceneNode whose pose is set to match the coordinate frame of - // the WrenchStamped message header. - Ogre::SceneNode* frame_node_; - // allow showing/hiding of force / torque arrows - Ogre::SceneNode* force_node_; - Ogre::SceneNode* torque_node_; - - // The SceneManager, kept here only so the destructor can ask it to - // destroy the ``frame_node_``. - Ogre::SceneManager* scene_manager_; - -}; - -} // end namespace rviz - -#endif // WRENCHSTAMPED_VISUAL_H diff --git a/rviz_default_plugins/CMakeLists.txt b/rviz_default_plugins/CMakeLists.txt index 5a1e444a1..352037dba 100644 --- a/rviz_default_plugins/CMakeLists.txt +++ b/rviz_default_plugins/CMakeLists.txt @@ -109,6 +109,7 @@ set(rviz_default_plugins_headers_to_moc 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/wrench/wrench_display.hpp include/rviz_default_plugins/robot/robot.hpp include/rviz_default_plugins/robot/robot_joint.hpp include/rviz_default_plugins/robot/robot_link.hpp @@ -179,6 +180,7 @@ set(rviz_default_plugins_source_files 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/robot/robot.cpp src/rviz_default_plugins/robot/robot_joint.cpp src/rviz_default_plugins/robot/robot_link.cpp @@ -942,6 +944,20 @@ if(BUILD_TESTING) ${TEST_LINK_LIBRARIES} rviz_visual_testing_framework::rviz_visual_testing_framework) 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 + ${SKIP_VISUAL_TESTS} + TIMEOUT 180) + if(TARGET wrench_display_visual_test) + target_include_directories(wrench_display_visual_test PUBLIC + ${TEST_INCLUDE_DIRS} + ${rviz_visual_testing_framework_INCLUDE_DIRS}) + target_link_libraries(wrench_display_visual_test + ${TEST_LINK_LIBRARIES} + rviz_visual_testing_framework::rviz_visual_testing_framework) + endif() endif() ament_package() diff --git a/rviz_default_plugins/include/rviz_default_plugins/displays/wrench/wrench_display.hpp b/rviz_default_plugins/include/rviz_default_plugins/displays/wrench/wrench_display.hpp new file mode 100644 index 000000000..9955fa490 --- /dev/null +++ b/rviz_default_plugins/include/rviz_default_plugins/displays/wrench/wrench_display.hpp @@ -0,0 +1,108 @@ +/* + * Copyright (c) 2019, Martin Idel and others + * 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__WRENCH__WRENCH_DISPLAY_HPP_ +#define RVIZ_DEFAULT_PLUGINS__DISPLAYS__WRENCH__WRENCH_DISPLAY_HPP_ + +#include +#include + +#include "geometry_msgs/msg/wrench_stamped.hpp" + +#include "rviz_common/ros_topic_display.hpp" +#include "rviz_default_plugins/visibility_control.hpp" + +namespace Ogre +{ +class SceneNode; +} + +namespace rviz_rendering +{ +class WrenchVisual; +} + +namespace rviz_common +{ +namespace properties +{ +class ColorProperty; +class FloatProperty; +class IntProperty; +} +} + +namespace rviz_default_plugins +{ +namespace displays +{ + + +class RVIZ_DEFAULT_PLUGINS_PUBLIC WrenchDisplay : public + rviz_common::RosTopicDisplay +{ + Q_OBJECT + +public: + WrenchDisplay(); + + ~WrenchDisplay() override; + + void onInitialize() override; + + void reset() override; + + void processMessage(geometry_msgs::msg::WrenchStamped::ConstSharedPtr msg) override; + +private + Q_SLOTS: + void updateWrenchVisuals(); + void updateHistoryLength(); + +private: + std::shared_ptr createWrenchVisual( + const geometry_msgs::msg::WrenchStamped::ConstSharedPtr & msg, + const Ogre::Quaternion & orientation, + const Ogre::Vector3 & position); + + std::deque> visuals_; + + rviz_common::properties::ColorProperty * force_color_property_; + rviz_common::properties::ColorProperty * torque_color_property_; + rviz_common::properties::FloatProperty * alpha_property_; + rviz_common::properties::FloatProperty * force_scale_property_; + rviz_common::properties::FloatProperty * torque_scale_property_; + rviz_common::properties::FloatProperty * width_property_; + rviz_common::properties::IntProperty * history_length_property_; +}; + +} // namespace displays +} // namespace rviz_default_plugins + +#endif // RVIZ_DEFAULT_PLUGINS__DISPLAYS__WRENCH__WRENCH_DISPLAY_HPP_ diff --git a/rviz_default_plugins/plugins_description.xml b/rviz_default_plugins/plugins_description.xml index a12fee459..9cef26970 100644 --- a/rviz_default_plugins/plugins_description.xml +++ b/rviz_default_plugins/plugins_description.xml @@ -253,6 +253,17 @@ geometry_msgs/msg/PointStamped + + + Displays from geometry_msgs/WrenchStamped message + + geometry_msgs/msg/WrenchStamped + + + +#include +#include + +#include "rviz_common/display_context.hpp" +#include "rviz_common/frame_manager_iface.hpp" +#include "rviz_common/properties/color_property.hpp" +#include "rviz_common/properties/float_property.hpp" +#include "rviz_common/properties/int_property.hpp" +#include "rviz_common/properties/parse_color.hpp" +#include "rviz_common/validate_floats.hpp" +#include "rviz_common/logging.hpp" +#include "rviz_rendering/objects/wrench_visual.hpp" + +#include "rviz_default_plugins/displays/wrench/wrench_display.hpp" + +namespace rviz_default_plugins +{ +namespace displays +{ + +WrenchDisplay::WrenchDisplay() +{ + force_color_property_ = new rviz_common::properties::ColorProperty( + "Force Color", QColor(204, 51, 51), "Color to draw the force arrows.", this, + SLOT(updateWrenchVisuals())); + + torque_color_property_ = new rviz_common::properties::ColorProperty( + "Torque Color", QColor(204, 204, 51), "Color to draw the torque arrows.", this, + SLOT(updateWrenchVisuals())); + + alpha_property_ = new rviz_common::properties::FloatProperty( + "Alpha", 1.0f, "0 is fully transparent, 1.0 is fully opaque.", this, + SLOT(updateWrenchVisuals())); + + force_scale_property_ = new rviz_common::properties::FloatProperty( + "Force Arrow Scale", 2.0f, "force arrow scale", this, SLOT(updateWrenchVisuals())); + + torque_scale_property_ = new rviz_common::properties::FloatProperty( + "Torque Arrow Scale", 2.0f, "torque arrow scale", this, SLOT(updateWrenchVisuals())); + + width_property_ = new rviz_common::properties::FloatProperty( + "Arrow Width", 0.5f, "arrow width", this, SLOT(updateWrenchVisuals())); + + history_length_property_ = new rviz_common::properties::IntProperty( + "History Length", 1, "Number of prior measurements to display.", this, + SLOT(updateHistoryLength())); + + history_length_property_->setMin(1); + history_length_property_->setMax(100000); +} + +void WrenchDisplay::onInitialize() +{ + RTDClass::onInitialize(); + updateHistoryLength(); +} + +WrenchDisplay::~WrenchDisplay() = default; + +void WrenchDisplay::reset() +{ + RTDClass::reset(); + visuals_.clear(); +} + +void WrenchDisplay::updateWrenchVisuals() +{ + float alpha = alpha_property_->getFloat(); + float force_scale = force_scale_property_->getFloat(); + float torque_scale = torque_scale_property_->getFloat(); + float width = width_property_->getFloat(); + Ogre::ColourValue force_color = force_color_property_->getOgreColor(); + Ogre::ColourValue torque_color = torque_color_property_->getOgreColor(); + + for (const auto & visual : visuals_) { + visual->setForceColor(force_color.r, force_color.g, force_color.b, alpha); + visual->setTorqueColor(torque_color.r, torque_color.g, torque_color.b, alpha); + visual->setForceScale(force_scale); + visual->setTorqueScale(torque_scale); + visual->setWidth(width); + } +} + +void WrenchDisplay::updateHistoryLength() +{ + while (visuals_.size() > static_cast(history_length_property_->getInt())) { + visuals_.pop_front(); + } +} + +bool validateFloats(const geometry_msgs::msg::WrenchStamped & msg) +{ + return rviz_common::validateFloats(msg.wrench.force) && + rviz_common::validateFloats(msg.wrench.torque); +} + +void WrenchDisplay::processMessage(geometry_msgs::msg::WrenchStamped::ConstSharedPtr msg) +{ + if (!validateFloats(*msg)) { + setStatus(rviz_common::properties::StatusProperty::Error, "Topic", + "Message contained invalid floating point values (nans or infs)"); + return; + } + + Ogre::Quaternion orientation; + Ogre::Vector3 position; + if (!context_->getFrameManager()->getTransform( + msg->header.frame_id, msg->header.stamp, position, orientation)) + { + setMissingTransformToFixedFrame(msg->header.frame_id); + return; + } + + if (position.isNaN()) { + RVIZ_COMMON_LOG_ERROR( + "Wrench position contains NaNs. Skipping render as long as the position is invalid"); + return; + } + + if (visuals_.size() >= static_cast(history_length_property_->getInt())) { + visuals_.pop_front(); + } + + auto visual = createWrenchVisual(msg, orientation, position); + + visuals_.push_back(visual); +} + +std::shared_ptr WrenchDisplay::createWrenchVisual( + const geometry_msgs::msg::WrenchStamped::ConstSharedPtr & msg, + const Ogre::Quaternion & orientation, + const Ogre::Vector3 & position) +{ + std::shared_ptr visual; + visual = std::make_shared(context_->getSceneManager(), scene_node_); + + Ogre::Vector3 force( + static_cast(msg->wrench.force.x), + static_cast(msg->wrench.force.y), + static_cast(msg->wrench.force.z)); + Ogre::Vector3 torque( + static_cast(msg->wrench.torque.x), + static_cast(msg->wrench.torque.y), + static_cast(msg->wrench.torque.z)); + visual->setWrench(force, torque); + visual->setFramePosition(position); + visual->setFrameOrientation(orientation); + + float alpha = alpha_property_->getFloat(); + float force_scale = force_scale_property_->getFloat(); + float torque_scale = torque_scale_property_->getFloat(); + float width = width_property_->getFloat(); + Ogre::ColourValue force_color = force_color_property_->getOgreColor(); + Ogre::ColourValue torque_color = torque_color_property_->getOgreColor(); + visual->setForceColor(force_color.r, force_color.g, force_color.b, alpha); + visual->setTorqueColor(torque_color.r, torque_color.g, torque_color.b, alpha); + visual->setForceScale(force_scale); + visual->setTorqueScale(torque_scale); + visual->setWidth(width); + + return visual; +} + +} // namespace displays +} // namespace rviz_default_plugins + +#include "pluginlib/class_list_macros.hpp" +PLUGINLIB_EXPORT_CLASS(rviz_default_plugins::displays::WrenchDisplay, rviz_common::Display) diff --git a/rviz_default_plugins/test/reference_images/wrenches_are_displayed_ref.png b/rviz_default_plugins/test/reference_images/wrenches_are_displayed_ref.png new file mode 100644 index 000000000..a9eb9d33d Binary files /dev/null and b/rviz_default_plugins/test/reference_images/wrenches_are_displayed_ref.png differ diff --git a/rviz_default_plugins/test/rviz_default_plugins/displays/wrench/wrench_stamped_display_visual_test.cpp b/rviz_default_plugins/test/rviz_default_plugins/displays/wrench/wrench_stamped_display_visual_test.cpp new file mode 100644 index 000000000..ade3ad429 --- /dev/null +++ b/rviz_default_plugins/test/rviz_default_plugins/displays/wrench/wrench_stamped_display_visual_test.cpp @@ -0,0 +1,61 @@ +/* + * Copyright (c) 2018, 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. + */ + +#include +#include + +#include "rviz_visual_testing_framework/visual_test_fixture.hpp" +#include "rviz_visual_testing_framework/visual_test_publisher.hpp" + +#include "../../page_objects/wrench_display_page_object.hpp" +#include "../../publishers/wrench_publisher.hpp" + +TEST_F(VisualTestFixture, wrenches_are_displayed) { + auto wrench_publisher = std::make_shared(); + auto wrench_visual_publisher = + std::make_unique(wrench_publisher, "wrench_frame"); + + setCamPose(Ogre::Vector3(10, 10, 16)); + setCamLookAt(Ogre::Vector3(0, 0, 0)); + + auto wrench_display = addDisplay(); + wrench_display->setTopic("/wrench"); + wrench_display->setForceColor(255, 255, 0); + wrench_display->setTorqueColor(0, 255, 255); + wrench_display->setForceScale(2); + wrench_display->setTorqueScale(2); + wrench_display->setWidth(4); + + captureMainWindow(); + + wrench_display->setAlpha(0.0f); + captureMainWindow("empty_scene"); + + assertScreenShotsIdentity(); +} diff --git a/rviz_default_plugins/test/rviz_default_plugins/page_objects/wrench_display_page_object.cpp b/rviz_default_plugins/test/rviz_default_plugins/page_objects/wrench_display_page_object.cpp new file mode 100644 index 000000000..3bc700c99 --- /dev/null +++ b/rviz_default_plugins/test/rviz_default_plugins/page_objects/wrench_display_page_object.cpp @@ -0,0 +1,78 @@ +/* + * 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. + */ + +#include "wrench_display_page_object.hpp" + +#include +#include + +WrenchDisplayPageObject::WrenchDisplayPageObject() +: BasePageObject(0, "Wrench") +{} + +void WrenchDisplayPageObject::setTopic(QString topic) +{ + setComboBox("Topic", topic); + waitForFirstMessage(); +} + +void WrenchDisplayPageObject::setAlpha(float alpha) +{ + setFloat("Alpha", alpha); +} + +void WrenchDisplayPageObject::setForceColor(int r, int g, int b) +{ + setColorCode("Force Color", r, g, b); +} + +void WrenchDisplayPageObject::setTorqueColor(int r, int g, int b) +{ + setColorCode("Torque Color", r, g, b); +} + +void WrenchDisplayPageObject::setForceScale(float scale) +{ + setFloat("Force Arrow Scale", scale); +} + +void WrenchDisplayPageObject::setTorqueScale(float scale) +{ + setFloat("Torque Arrow Scale", scale); +} + +void WrenchDisplayPageObject::setWidth(float width) +{ + setFloat("Arrow Width", width); +} + +void WrenchDisplayPageObject::setHistoryLength(int history) +{ + setInt("History Length", history); +} diff --git a/rviz_default_plugins/test/rviz_default_plugins/page_objects/wrench_display_page_object.hpp b/rviz_default_plugins/test/rviz_default_plugins/page_objects/wrench_display_page_object.hpp new file mode 100644 index 000000000..18db85117 --- /dev/null +++ b/rviz_default_plugins/test/rviz_default_plugins/page_objects/wrench_display_page_object.hpp @@ -0,0 +1,50 @@ +/* + * 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__PAGE_OBJECTS__WRENCH_DISPLAY_PAGE_OBJECT_HPP_ +#define RVIZ_DEFAULT_PLUGINS__PAGE_OBJECTS__WRENCH_DISPLAY_PAGE_OBJECT_HPP_ + +#include "rviz_visual_testing_framework/page_objects/base_page_object.hpp" + +class WrenchDisplayPageObject : public BasePageObject +{ +public: + WrenchDisplayPageObject(); + + void setTopic(QString topic); + void setAlpha(float alpha); + void setForceColor(int r, int g, int b); + void setTorqueColor(int r, int g, int b); + void setForceScale(float scale); + void setTorqueScale(float scale); + void setWidth(float width); + void setHistoryLength(int history); +}; + +#endif // RVIZ_DEFAULT_PLUGINS__PAGE_OBJECTS__WRENCH_DISPLAY_PAGE_OBJECT_HPP_ diff --git a/rviz_default_plugins/test/rviz_default_plugins/publishers/wrench_publisher.hpp b/rviz_default_plugins/test/rviz_default_plugins/publishers/wrench_publisher.hpp new file mode 100644 index 000000000..aa04db013 --- /dev/null +++ b/rviz_default_plugins/test/rviz_default_plugins/publishers/wrench_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__WRENCH_PUBLISHER_HPP_ +#define RVIZ_DEFAULT_PLUGINS__PUBLISHERS__WRENCH_PUBLISHER_HPP_ + +#include +#include + +#include "rclcpp/rclcpp.hpp" + +#include "geometry_msgs/msg/wrench_stamped.hpp" +#include "std_msgs/msg/header.hpp" + +using namespace std::chrono_literals; //NOLINT + +namespace nodes +{ + +class WrenchPublisher : public rclcpp::Node +{ +public: + WrenchPublisher(); + +private: + geometry_msgs::msg::WrenchStamped createWrenchMessage(); + + rclcpp::TimerBase::SharedPtr timer_; + rclcpp::Publisher::SharedPtr publisher_; +}; + +WrenchPublisher::WrenchPublisher() +: Node("wrench_publisher") +{ + publisher_ = this->create_publisher("wrench", 10); + + auto timer_callback = + [this]() -> void { + auto message = createWrenchMessage(); + this->publisher_->publish(message); + }; + timer_ = this->create_wall_timer(500ms, timer_callback); +} + +geometry_msgs::msg::WrenchStamped WrenchPublisher::createWrenchMessage() +{ + geometry_msgs::msg::WrenchStamped wrenchMessage; + + wrenchMessage.header = std_msgs::msg::Header(); + wrenchMessage.header.frame_id = "wrench_frame"; + wrenchMessage.header.stamp = rclcpp::Clock().now(); + + wrenchMessage.wrench.force.x = 1; + wrenchMessage.wrench.force.y = 3; + wrenchMessage.wrench.force.z = 2; + + wrenchMessage.wrench.torque.x = 3; + wrenchMessage.wrench.torque.y = 2; + wrenchMessage.wrench.torque.z = 1; + + return wrenchMessage; +} + +} // namespace nodes + +#endif // RVIZ_DEFAULT_PLUGINS__PUBLISHERS__WRENCH_PUBLISHER_HPP_ diff --git a/rviz_rendering/CMakeLists.txt b/rviz_rendering/CMakeLists.txt index 7f7769ed2..18434b7a8 100644 --- a/rviz_rendering/CMakeLists.txt +++ b/rviz_rendering/CMakeLists.txt @@ -102,6 +102,7 @@ add_library(rviz_rendering SHARED src/rviz_rendering/objects/point_cloud.cpp src/rviz_rendering/objects/point_cloud_renderable.cpp src/rviz_rendering/objects/shape.cpp + src/rviz_rendering/objects/wrench_visual.cpp test/rviz_rendering/ogre_testing_environment.cpp test/rviz_rendering/scene_graph_introspection.cpp ) @@ -258,6 +259,17 @@ if(BUILD_TESTING) 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}) + if(TARGET wrench_visual_test_target) + target_link_libraries(wrench_visual_test_target + rviz_ogre_vendor::OgreMain + rviz_rendering + Qt5::Widgets # explicitly do this for include directories (not necessary for external use) + ) + endif() endif() list(APPEND ${PROJECT_NAME}_CONFIG_EXTRAS diff --git a/rviz_rendering/include/rviz_rendering/objects/wrench_visual.hpp b/rviz_rendering/include/rviz_rendering/objects/wrench_visual.hpp new file mode 100644 index 000000000..a23cda84a --- /dev/null +++ b/rviz_rendering/include/rviz_rendering/objects/wrench_visual.hpp @@ -0,0 +1,120 @@ +/* + * Copyright (c) 2019, Martin Idel and others + * 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__WRENCH_VISUAL_HPP_ +#define RVIZ_RENDERING__OBJECTS__WRENCH_VISUAL_HPP_ + +#include + +#include "rviz_rendering/visibility_control.hpp" + +namespace Ogre +{ +class Vector3; +class Quaternion; +} + +namespace rviz_rendering +{ +class Arrow; +class BillboardLine; +} + +namespace rviz_rendering +{ + +/* + * Each instance of WrenchVisual represents the visualization of a single + * sensor_msgs::msg::WrenchStamped message. + */ +class WrenchVisual +{ +public: + RVIZ_RENDERING_PUBLIC + WrenchVisual(Ogre::SceneManager * scene_manager, Ogre::SceneNode * parent_node); + + RVIZ_RENDERING_PUBLIC + virtual ~WrenchVisual(); + + // Configure the visual to show the given force and torque vectors + RVIZ_RENDERING_PUBLIC + void setWrench(const Ogre::Vector3 & force, const Ogre::Vector3 & torque); + + // Set the pose of the coordinate frame the message refers to. + RVIZ_RENDERING_PUBLIC + void setFramePosition(const Ogre::Vector3 & position); + + RVIZ_RENDERING_PUBLIC + void setFrameOrientation(const Ogre::Quaternion & orientation); + + RVIZ_RENDERING_PUBLIC + void setForceColor(float r, float g, float b, float a); + + RVIZ_RENDERING_PUBLIC + void setTorqueColor(float r, float g, float b, float a); + + RVIZ_RENDERING_PUBLIC + void setForceScale(float scale); + + RVIZ_RENDERING_PUBLIC + void setTorqueScale(float scale); + + RVIZ_RENDERING_PUBLIC + void setWidth(float width); + + RVIZ_RENDERING_PUBLIC + void setVisible(bool visible); + +private: + void createTorqueDirectionCircle(const Ogre::Quaternion & orientation) const; + void setTorqueDirectionArrow(const Ogre::Quaternion & orientation) const; + Ogre::Quaternion getDirectionOfRotationRelativeToTorque( + const Ogre::Vector3 & torque, const Ogre::Vector3 & axis_z) const; + void updateForceArrow() const; + void updateTorque() const; + + std::shared_ptr arrow_force_; + std::shared_ptr arrow_torque_; + std::shared_ptr circle_torque_; + std::shared_ptr circle_arrow_torque_; + Ogre::Vector3 force_arrow_direction_; + Ogre::Vector3 torque_arrow_direction_; + float force_scale_; + float torque_scale_; + float width_; + + Ogre::SceneNode * frame_node_; + Ogre::SceneNode * force_node_; + Ogre::SceneNode * torque_node_; + Ogre::SceneManager * scene_manager_; +}; + +} // namespace rviz_rendering + +#endif // RVIZ_RENDERING__OBJECTS__WRENCH_VISUAL_HPP_ diff --git a/rviz_rendering/src/rviz_rendering/objects/wrench_visual.cpp b/rviz_rendering/src/rviz_rendering/objects/wrench_visual.cpp new file mode 100644 index 000000000..4a91ec1ca --- /dev/null +++ b/rviz_rendering/src/rviz_rendering/objects/wrench_visual.cpp @@ -0,0 +1,189 @@ +/* + * Copyright (c) 2019, Martin Idel and others + * 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 + +#include "rviz_rendering/objects/arrow.hpp" +#include "rviz_rendering/objects/billboard_line.hpp" + +#include "rviz_rendering/objects/wrench_visual.hpp" + +namespace rviz_rendering +{ + +WrenchVisual::WrenchVisual(Ogre::SceneManager * scene_manager, Ogre::SceneNode * parent_node) +: force_arrow_direction_(Ogre::Vector3::ZERO), + torque_arrow_direction_(Ogre::Vector3::ZERO), + force_scale_(1), + torque_scale_(1), + width_(1) +{ + scene_manager_ = scene_manager; + + frame_node_ = parent_node->createChildSceneNode(); + force_node_ = frame_node_->createChildSceneNode(); + torque_node_ = frame_node_->createChildSceneNode(); + + arrow_force_ = std::make_shared(scene_manager_, force_node_); + arrow_torque_ = std::make_shared(scene_manager_, torque_node_); + circle_torque_ = std::make_shared(scene_manager_, torque_node_); + circle_arrow_torque_ = std::make_shared(scene_manager_, torque_node_); +} + +WrenchVisual::~WrenchVisual() +{ + scene_manager_->destroySceneNode(frame_node_); +} + +void WrenchVisual::setWrench(const Ogre::Vector3 & force, const Ogre::Vector3 & torque) +{ + force_arrow_direction_ = force; + torque_arrow_direction_ = torque; + + updateForceArrow(); + updateTorque(); +} + +void WrenchVisual::updateForceArrow() const +{ + const auto force_arrow_length = force_arrow_direction_.length() * force_scale_; + const bool show_force = (force_arrow_length > width_); + if (show_force) { + arrow_force_->setScale(Ogre::Vector3(force_arrow_length, width_, width_)); + arrow_force_->setDirection(force_arrow_direction_); + } + force_node_->setVisible(show_force); +} + +void WrenchVisual::updateTorque() const +{ + const auto torque_arrow_length = torque_arrow_direction_.length() * torque_scale_; + const bool show_torque = (torque_arrow_length > width_); + if (show_torque) { + arrow_torque_->setScale(Ogre::Vector3(torque_arrow_length, width_, width_)); + arrow_torque_->setDirection(torque_arrow_direction_); + Ogre::Vector3 axis_z(0, 0, 1); + Ogre::Quaternion orientation = getDirectionOfRotationRelativeToTorque( + torque_arrow_direction_, axis_z); + setTorqueDirectionArrow(orientation); + createTorqueDirectionCircle(orientation); + } + torque_node_->setVisible(show_torque); +} + +Ogre::Quaternion WrenchVisual::getDirectionOfRotationRelativeToTorque( + const Ogre::Vector3 & torque, + const Ogre::Vector3 & axis_z) const +{ + Ogre::Quaternion orientation = axis_z.getRotationTo(torque); + if (std::isnan(orientation.x) || + std::isnan(orientation.y) || + std::isnan(orientation.z) ) + { + orientation = Ogre::Quaternion::IDENTITY; + } + return orientation; +} + +void WrenchVisual::setTorqueDirectionArrow(const Ogre::Quaternion & orientation) const +{ + const auto torque_arrow_length = torque_arrow_direction_.length() * torque_scale_; + circle_arrow_torque_->set(0, width_ * 0.1f, width_ * 0.1f * 1.0f, width_ * 0.1f * 2.0f); + circle_arrow_torque_->setDirection(orientation * Ogre::Vector3(0, 1, 0)); + circle_arrow_torque_->setPosition( + orientation * Ogre::Vector3(torque_arrow_length / 4, 0, torque_arrow_length / 2)); +} + +void WrenchVisual::createTorqueDirectionCircle(const Ogre::Quaternion & orientation) const +{ + const auto torque_arrow_length = torque_arrow_direction_.length() * torque_scale_; + circle_torque_->clear(); + circle_torque_->setLineWidth(width_ * 0.05f); + for (int i = 4; i <= 32; i++) { + Ogre::Vector3 point = Ogre::Vector3( + static_cast((torque_arrow_length / 4) * cos(i * 2 * M_PI / 32)), + static_cast((torque_arrow_length / 4) * sin(i * 2 * M_PI / 32)), + torque_arrow_length / 2); + circle_torque_->addPoint(orientation * point); + } +} + +void WrenchVisual::setFramePosition(const Ogre::Vector3 & position) +{ + frame_node_->setPosition(position); +} + +void WrenchVisual::setFrameOrientation(const Ogre::Quaternion & orientation) +{ + frame_node_->setOrientation(orientation); +} + +void WrenchVisual::setForceColor(float r, float g, float b, float a) +{ + arrow_force_->setColor(r, g, b, a); +} + +void WrenchVisual::setTorqueColor(float r, float g, float b, float a) +{ + arrow_torque_->setColor(r, g, b, a); + circle_torque_->setColor(r, g, b, a); + circle_arrow_torque_->setColor(r, g, b, a); +} + +void WrenchVisual::setForceScale(float scale) +{ + force_scale_ = scale; + updateForceArrow(); +} + +void WrenchVisual::setTorqueScale(float scale) +{ + torque_scale_ = scale; + updateTorque(); +} + +void WrenchVisual::setWidth(float width) +{ + width_ = width; + updateForceArrow(); + updateTorque(); +} + +void WrenchVisual::setVisible(bool visible) +{ + frame_node_->setVisible(visible); +} + +} // namespace rviz_rendering diff --git a/rviz_rendering/test/rviz_rendering/objects/wrench_visual_test.cpp b/rviz_rendering/test/rviz_rendering/objects/wrench_visual_test.cpp new file mode 100644 index 000000000..49e30f6de --- /dev/null +++ b/rviz_rendering/test/rviz_rendering/objects/wrench_visual_test.cpp @@ -0,0 +1,117 @@ +/* + * 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 +#include +#include +#include + +#include "test/rviz_rendering/ogre_testing_environment.hpp" +#include "test/rviz_rendering/scene_graph_introspection.hpp" +#include "rviz_rendering/objects/wrench_visual.hpp" +#include "rviz_rendering/objects/billboard_line.hpp" +#include "rviz_rendering/objects/arrow.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; +} + +class WrenchVisualTestFixture : public ::testing::Test +{ +protected: + static void SetUpTestCase() + { + testing_environment_ = std::make_shared(); + testing_environment_->setUpOgreTestEnvironment(); + } + + static std::shared_ptr testing_environment_; +}; + +std::shared_ptr +WrenchVisualTestFixture::testing_environment_ = nullptr; + +Ogre::SceneNode * findForceArrow(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(WrenchVisualTestFixture, setWrench_sets_force_arrow_correctly) { + auto scene_manager = Ogre::Root::getSingletonPtr()->createSceneManager(); + auto root_node = scene_manager->getRootSceneNode(); + + auto wrench_visual = std::make_shared(scene_manager, root_node); + + wrench_visual->setForceScale(1); + wrench_visual->setWidth(0.2f); + + wrench_visual->setWrench(Ogre::Vector3(3, 0, 0), Ogre::Vector3(0, 1, 0)); + + auto arrows = rviz_rendering::findAllArrows(root_node); + EXPECT_THAT(arrows, SizeIs(3u)); + auto force_arrow = findForceArrow(root_node); + EXPECT_THAT(force_arrow->getScale(), Vector3Eq(Ogre::Vector3(0.2f, 3, 0.2f))); +} + +TEST_F(WrenchVisualTestFixture, setWrench_hides_force_arrow_for_larger_width_than_scale) { + auto scene_manager = Ogre::Root::getSingletonPtr()->createSceneManager(); + auto root_node = scene_manager->getRootSceneNode(); + + auto wrench_visual = std::make_shared(scene_manager, root_node); + + wrench_visual->setWrench(Ogre::Vector3(1, 0, 0), Ogre::Vector3(0, 1, 0)); + + wrench_visual->setForceScale(0.2f); + wrench_visual->setWidth(10); + + auto arrows = rviz_rendering::findAllArrows(root_node); + EXPECT_THAT(arrows, SizeIs(3u)); + auto force_arrow = findForceArrow(root_node); + EXPECT_THAT(force_arrow->getScale(), Vector3Eq(Ogre::Vector3(1, 1, 1))); + auto it = force_arrow->getAttachedObjectIterator(); + while (it.hasMoreElements()) { + EXPECT_THAT(it.getNext()->isVisible(), IsFalse()); + } +}