diff --git a/rviz/src/rviz/default_plugin/pose_array_display.cpp b/rviz/src/rviz/default_plugin/pose_array_display.cpp deleted file mode 100644 index ddeaa84e0..000000000 --- a/rviz/src/rviz/default_plugin/pose_array_display.cpp +++ /dev/null @@ -1,377 +0,0 @@ -/* - * Copyright (c) 2012, Willow Garage, 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 - -#include "rviz/display_context.h" -#include "rviz/frame_manager.h" -#include "rviz/properties/enum_property.h" -#include "rviz/properties/color_property.h" -#include "rviz/properties/float_property.h" -#include "rviz/validate_floats.h" -#include "rviz/ogre_helpers/arrow.h" -#include "rviz/ogre_helpers/axes.h" - -#include "rviz/default_plugin/pose_array_display.h" - -namespace rviz -{ - -namespace -{ - struct ShapeType - { - enum - { - Arrow2d, - Arrow3d, - Axes, - }; - }; - - Ogre::Vector3 vectorRosToOgre( geometry_msgs::Point const & point ) - { - return Ogre::Vector3( point.x, point.y, point.z ); - } - - Ogre::Quaternion quaternionRosToOgre( geometry_msgs::Quaternion const & quaternion ) - { - return Ogre::Quaternion( quaternion.w, quaternion.x, quaternion.y, quaternion.z ); - } -} - -PoseArrayDisplay::PoseArrayDisplay() - : manual_object_( NULL ) -{ - shape_property_ = new EnumProperty( "Shape", "Arrow (Flat)", "Shape to display the pose as.", - this, SLOT( updateShapeChoice() ) ); - - arrow_color_property_ = new ColorProperty( "Color", QColor( 255, 25, 0 ), "Color to draw the arrows.", - this, SLOT( updateArrowColor() ) ); - - arrow_alpha_property_ = new FloatProperty( "Alpha", 1, "Amount of transparency to apply to the displayed poses.", - this, SLOT( updateArrowColor() ) ); - - arrow2d_length_property_ = new FloatProperty( "Arrow Length", 0.3, "Length of the arrows.", - this, SLOT( updateArrow2dGeometry() ) ); - - arrow3d_head_radius_property_ = new FloatProperty( "Head Radius", 0.03, "Radius of the arrow's head, in meters.", - this, SLOT( updateArrow3dGeometry() ) ); - - arrow3d_head_length_property_ = new FloatProperty( "Head Length", 0.07, "Length of the arrow's head, in meters.", - this, SLOT( updateArrow3dGeometry() ) ); - - arrow3d_shaft_radius_property_ = new FloatProperty( "Shaft Radius", 0.01, "Radius of the arrow's shaft, in meters.", - this, SLOT( updateArrow3dGeometry() ) ); - - arrow3d_shaft_length_property_ = new FloatProperty( "Shaft Length", 0.23, "Length of the arrow's shaft, in meters.", - this, SLOT( updateArrow3dGeometry() ) ); - - axes_length_property_ = new FloatProperty( "Axes Length", 0.3, "Length of each axis, in meters.", - this, SLOT( updateAxesGeometry() ) ); - - axes_radius_property_ = new FloatProperty( "Axes Radius", 0.01, "Radius of each axis, in meters.", - this, SLOT( updateAxesGeometry() ) ); - - shape_property_->addOption( "Arrow (Flat)", ShapeType::Arrow2d ); - shape_property_->addOption( "Arrow (3D)", ShapeType::Arrow3d ); - shape_property_->addOption( "Axes", ShapeType::Axes ); - arrow_alpha_property_->setMin( 0 ); - arrow_alpha_property_->setMax( 1 ); -} - -PoseArrayDisplay::~PoseArrayDisplay() -{ - if ( initialized() ) - { - scene_manager_->destroyManualObject( manual_object_ ); - } -} - -void PoseArrayDisplay::onInitialize() -{ - MFDClass::onInitialize(); - manual_object_ = scene_manager_->createManualObject(); - manual_object_->setDynamic( true ); - scene_node_->attachObject( manual_object_ ); - arrow_node_ = scene_node_->createChildSceneNode(); - axes_node_ = scene_node_->createChildSceneNode(); - updateShapeChoice(); -} - -bool validateFloats( const geometry_msgs::PoseArray& msg ) -{ - return validateFloats( msg.poses ); -} - -void PoseArrayDisplay::processMessage( const geometry_msgs::PoseArray::ConstPtr& msg ) -{ - if( !validateFloats( *msg )) - { - setStatus( StatusProperty::Error, "Topic", - "Message contained invalid floating point values (nans or infs)" ); - return; - } - - if( !setTransform( msg->header ) ) - { - setStatus( StatusProperty::Error, "Topic", "Failed to look up transform" ); - return; - } - - poses_.resize( msg->poses.size() ); - for (std::size_t i = 0; i < msg->poses.size(); ++i) - { - poses_[i].position = vectorRosToOgre( msg->poses[i].position ); - poses_[i].orientation = quaternionRosToOgre( msg->poses[i].orientation ); - } - - updateDisplay(); - context_->queueRender(); -} - -bool PoseArrayDisplay::setTransform( std_msgs::Header const & header ) -{ - Ogre::Vector3 position; - Ogre::Quaternion orientation; - if( !context_->getFrameManager()->getTransform( header, position, orientation ) ) - { - ROS_ERROR( "Error transforming pose '%s' from frame '%s' to frame '%s'", - qPrintable( getName() ), header.frame_id.c_str(), - qPrintable( fixed_frame_ ) ); - return false; - } - scene_node_->setPosition( position ); - scene_node_->setOrientation( orientation ); - return true; -} - -void PoseArrayDisplay::updateArrows2d() -{ - manual_object_->clear(); - - Ogre::ColourValue color = arrow_color_property_->getOgreColor(); - color.a = arrow_alpha_property_->getFloat(); - float length = arrow2d_length_property_->getFloat(); - size_t num_poses = poses_.size(); - manual_object_->estimateVertexCount( num_poses * 6 ); - manual_object_->begin( "BaseWhiteNoLighting", Ogre::RenderOperation::OT_LINE_LIST ); - for( size_t i=0; i < num_poses; ++i ) - { - const Ogre::Vector3 & pos = poses_[i].position; - const Ogre::Quaternion & orient = poses_[i].orientation; - Ogre::Vector3 vertices[6]; - vertices[0] = pos; // back of arrow - vertices[1] = pos + orient * Ogre::Vector3( length, 0, 0 ); // tip of arrow - vertices[2] = vertices[ 1 ]; - vertices[3] = pos + orient * Ogre::Vector3( 0.75*length, 0.2*length, 0 ); - vertices[4] = vertices[ 1 ]; - vertices[5] = pos + orient * Ogre::Vector3( 0.75*length, -0.2*length, 0 ); - - for( int i = 0; i < 6; ++i ) - { - manual_object_->position( vertices[i] ); - manual_object_->colour( color ); - } - } - manual_object_->end(); -} - -void PoseArrayDisplay::updateDisplay() -{ - int shape = shape_property_->getOptionInt(); - switch (shape) { - case ShapeType::Arrow2d: - updateArrows2d(); - arrows3d_.clear(); - axes_.clear(); - break; - case ShapeType::Arrow3d: - updateArrows3d(); - manual_object_->clear(); - axes_.clear(); - break; - case ShapeType::Axes: - updateAxes(); - manual_object_->clear(); - arrows3d_.clear(); - break; - } -} - -void PoseArrayDisplay::updateArrows3d() -{ - while (arrows3d_.size() < poses_.size()) - arrows3d_.push_back(makeArrow3d()); - while (arrows3d_.size() > poses_.size()) - arrows3d_.pop_back(); - - Ogre::Quaternion adjust_orientation( Ogre::Degree(-90), Ogre::Vector3::UNIT_Y ); - for (std::size_t i = 0; i < poses_.size(); ++i) - { - arrows3d_[i].setPosition( poses_[i].position ); - arrows3d_[i].setOrientation( poses_[i].orientation * adjust_orientation ); - } -} - -void PoseArrayDisplay::updateAxes() -{ - while (axes_.size() < poses_.size()) - axes_.push_back(makeAxes()); - while (axes_.size() > poses_.size()) - axes_.pop_back(); - for (std::size_t i = 0; i < poses_.size(); ++i) - { - axes_[i].setPosition( poses_[i].position ); - axes_[i].setOrientation( poses_[i].orientation ); - } -} - -Arrow * PoseArrayDisplay::makeArrow3d() -{ - Ogre::ColourValue color = arrow_color_property_->getOgreColor(); - color.a = arrow_alpha_property_->getFloat(); - - Arrow * arrow = new Arrow( - scene_manager_, - arrow_node_, - arrow3d_shaft_length_property_->getFloat(), - arrow3d_shaft_radius_property_->getFloat(), - arrow3d_head_length_property_->getFloat(), - arrow3d_head_radius_property_->getFloat() - ); - - arrow->setColor(color); - return arrow; -} - -Axes * PoseArrayDisplay::makeAxes() -{ - return new Axes( - scene_manager_, - axes_node_, - axes_length_property_->getFloat(), - axes_radius_property_->getFloat() - ); -} - -void PoseArrayDisplay::reset() -{ - MFDClass::reset(); - if( manual_object_ ) - { - manual_object_->clear(); - } - arrows3d_.clear(); - axes_.clear(); -} - -void PoseArrayDisplay::updateShapeChoice() -{ - int shape = shape_property_->getOptionInt(); - bool use_arrow2d = shape == ShapeType::Arrow2d; - bool use_arrow3d = shape == ShapeType::Arrow3d; - bool use_arrow = use_arrow2d || use_arrow3d; - bool use_axes = shape == ShapeType::Axes; - - arrow_color_property_->setHidden( !use_arrow ); - arrow_alpha_property_->setHidden( !use_arrow ); - - arrow2d_length_property_->setHidden(!use_arrow2d); - - arrow3d_shaft_length_property_->setHidden( !use_arrow3d ); - arrow3d_shaft_radius_property_->setHidden( !use_arrow3d ); - arrow3d_head_length_property_->setHidden( !use_arrow3d ); - arrow3d_head_radius_property_->setHidden( !use_arrow3d ); - - axes_length_property_->setHidden( !use_axes ); - axes_radius_property_->setHidden( !use_axes ); - - if (initialized()) - updateDisplay(); -} - -void PoseArrayDisplay::updateArrowColor() -{ - int shape = shape_property_->getOptionInt(); - Ogre::ColourValue color = arrow_color_property_->getOgreColor(); - color.a = arrow_alpha_property_->getFloat(); - - if (shape == ShapeType::Arrow2d) - { - updateArrows2d(); - } - else if (shape == ShapeType::Arrow3d) - { - for (std::size_t i = 0; i < arrows3d_.size(); ++i) - { - arrows3d_[i].setColor( color ); - } - } - context_->queueRender(); -} - -void PoseArrayDisplay::updateArrow2dGeometry() -{ - updateArrows2d(); - context_->queueRender(); -} - -void PoseArrayDisplay::updateArrow3dGeometry() -{ - for (std::size_t i = 0; i < poses_.size(); ++i) - { - arrows3d_[i].set( - arrow3d_shaft_length_property_->getFloat(), - arrow3d_shaft_radius_property_->getFloat(), - arrow3d_head_length_property_->getFloat(), - arrow3d_head_radius_property_->getFloat() - ); - } - context_->queueRender(); -} - -void PoseArrayDisplay::updateAxesGeometry() -{ - for (std::size_t i = 0; i < poses_.size(); ++i) - { - axes_[i].set( - axes_length_property_->getFloat(), - axes_radius_property_->getFloat() - ); - } - context_->queueRender(); -} - -} // namespace rviz - -#include -PLUGINLIB_EXPORT_CLASS( rviz::PoseArrayDisplay, rviz::Display ) diff --git a/rviz/src/rviz/default_plugin/pose_array_display.h b/rviz/src/rviz/default_plugin/pose_array_display.h deleted file mode 100644 index d73cce68d..000000000 --- a/rviz/src/rviz/default_plugin/pose_array_display.h +++ /dev/null @@ -1,121 +0,0 @@ -/* - * Copyright (c) 2012, Willow Garage, 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_POSE_ARRAY_DISPLAY_H_ -#define RVIZ_POSE_ARRAY_DISPLAY_H_ - -#include - -#include "rviz/message_filter_display.h" - -#include - -namespace Ogre -{ -class ManualObject; -} - -namespace rviz -{ - -class EnumProperty; -class ColorProperty; -class FloatProperty; -class Arrow; -class Axes; - -/** @brief Displays a geometry_msgs/PoseArray message as a bunch of line-drawn arrows. */ -class PoseArrayDisplay: public MessageFilterDisplay -{ -Q_OBJECT -public: - PoseArrayDisplay(); - virtual ~PoseArrayDisplay(); - -protected: - virtual void onInitialize(); - virtual void reset(); - virtual void processMessage( const geometry_msgs::PoseArray::ConstPtr& msg ); - -private: - bool setTransform(std_msgs::Header const & header); - void updateArrows2d(); - void updateArrows3d(); - void updateAxes(); - void updateDisplay(); - Axes * makeAxes(); - Arrow * makeArrow3d(); - - struct OgrePose { - Ogre::Vector3 position; - Ogre::Quaternion orientation; - }; - - std::vector poses_; - boost::ptr_vector arrows3d_; - boost::ptr_vector axes_; - - Ogre::SceneNode * arrow_node_; - Ogre::SceneNode * axes_node_; - Ogre::ManualObject* manual_object_; - - EnumProperty* shape_property_; - ColorProperty* arrow_color_property_; - FloatProperty* arrow_alpha_property_; - - FloatProperty* arrow2d_length_property_; - - FloatProperty* arrow3d_head_radius_property_; - FloatProperty* arrow3d_head_length_property_; - FloatProperty* arrow3d_shaft_radius_property_; - FloatProperty* arrow3d_shaft_length_property_; - - FloatProperty* axes_length_property_; - FloatProperty* axes_radius_property_; - -private Q_SLOTS: - /// Update the interface and visible shapes based on the selected shape type. - void updateShapeChoice(); - - /// Update the arrow color. - void updateArrowColor(); - - /// Update the flat arrow geometry. - void updateArrow2dGeometry(); - - /// Update the 3D arrow geometry. - void updateArrow3dGeometry(); - - /// Update the axes geometry. - void updateAxesGeometry(); -}; - -} // namespace rviz - -#endif /* RVIZ_POSE_ARRAY_DISPLAY_H_ */ diff --git a/rviz_default_plugins/CMakeLists.txt b/rviz_default_plugins/CMakeLists.txt index b42a50c59..5e5ba0b44 100644 --- a/rviz_default_plugins/CMakeLists.txt +++ b/rviz_default_plugins/CMakeLists.txt @@ -44,6 +44,7 @@ set(rviz_default_plugins_headers_to_moc src/rviz_default_plugins/displays/pointcloud/point_cloud_helpers.hpp src/rviz_default_plugins/displays/polygon/polygon_display.hpp src/rviz_default_plugins/displays/pose/pose_display.hpp + src/rviz_default_plugins/displays/pose_array/pose_array_display.hpp src/rviz_default_plugins/displays/robot_model/robot_model_display.hpp src/rviz_default_plugins/displays/tf/frame_info.hpp src/rviz_default_plugins/displays/tf/tf_display.hpp @@ -85,6 +86,8 @@ set(rviz_default_plugins_source_files src/rviz_default_plugins/displays/polygon/polygon_display.cpp src/rviz_default_plugins/displays/pose/pose_display.cpp src/rviz_default_plugins/displays/pose/pose_display_selection_handler.cpp + src/rviz_default_plugins/displays/pose_array/pose_array_display.cpp + src/rviz_default_plugins/displays/pose_array/flat_arrows_array.cpp src/rviz_default_plugins/displays/robot_model/robot_model_display.cpp src/rviz_default_plugins/displays/tf/frame_info.cpp src/rviz_default_plugins/displays/tf/frame_selection_handler.cpp @@ -244,6 +247,16 @@ if(BUILD_TESTING) # target_link_libraries(marker_display_test rviz_default_plugins) # endif() +# TODO(greimela): reenable these tests once they can be run on the ci +# ament_add_gmock(pose_array_display_test +# test/rviz_default_plugins/displays/pose_array/pose_array_display_test.cpp +# test/rviz_default_plugins/displays/display_test_fixture.cpp +# test/rviz_default_plugins/scene_graph_introspection.cpp +# APPEND_ENV AMENT_PREFIX_PATH=${CMAKE_INSTALL_PREFIX} PATH=${CMAKE_INSTALL_PREFIX}/bin;${CMAKE_INSTALL_PREFIX}/opt/rviz_assimp_vendor/bin;${CMAKE_INSTALL_PREFIX}/opt/rviz_yaml_cpp_vendor/bin; +# ${CMAKE_INSTALL_PREFIX}/opt/rviz_ogre_vendor/bin) +# if(TARGET pose_array_display_test) +# target_link_libraries(pose_array_display_test rviz_default_plugins) +# endif() endif() ament_package() diff --git a/rviz_default_plugins/plugins_description.xml b/rviz_default_plugins/plugins_description.xml index c2ff97f51..5ec63d298 100644 --- a/rviz_default_plugins/plugins_description.xml +++ b/rviz_default_plugins/plugins_description.xml @@ -110,6 +110,17 @@ + + + The Pose Array display shows a geometry_msgs/PoseArray message, as a collection of arrows. <a href="http://www.ros.org/wiki/rviz/DisplayTypes/PoseArray">More Information</a>. + + geometry_msgs/PoseArray + + +#include + +#include +#include + +namespace rviz_default_plugins +{ +namespace displays +{ + +FlatArrowsArray::FlatArrowsArray(Ogre::SceneManager * scene_manager) +: scene_manager_(scene_manager), manual_object_(nullptr) {} + +FlatArrowsArray::~FlatArrowsArray() +{ + if (manual_object_) { + scene_manager_->destroyManualObject(manual_object_); + } +} + +void FlatArrowsArray::createAndAttachManualObject(Ogre::SceneNode * scene_node) +{ + manual_object_ = scene_manager_->createManualObject(); + manual_object_->setDynamic(true); + scene_node->attachObject(manual_object_); +} + +void FlatArrowsArray::updateManualObject( + Ogre::ColourValue color, + float alpha, + float length, + const std::vector & poses) +{ + clear(); + + color.a = alpha; + setManualObjectMaterial(); + EnableBlending(color); + + manual_object_->begin( + material_->getName(), Ogre::RenderOperation::OT_LINE_LIST, "rviz_rendering"); + setManualObjectVertices(color, length, poses); + manual_object_->end(); +} + +void FlatArrowsArray::clear() +{ + if (manual_object_) { + manual_object_->clear(); + } +} + +void FlatArrowsArray::setManualObjectMaterial() +{ + static int material_count = 0; + std::string material_name = "FlatArrowsMaterial" + std::to_string(material_count++); + material_ = Ogre::MaterialManager::getSingleton().create(material_name, "rviz_rendering"); + material_->setReceiveShadows(false); + material_->getTechnique(0)->setLightingEnabled(false); +} + +void FlatArrowsArray::EnableBlending(const Ogre::ColourValue & color) +{ + if (color.a < 0.9998) { + material_->setSceneBlending(Ogre::SBT_TRANSPARENT_ALPHA); + material_->setDepthWriteEnabled(false); + } else { + material_->setSceneBlending(Ogre::SBT_REPLACE); + material_->setDepthWriteEnabled(true); + } +} + +void FlatArrowsArray::setManualObjectVertices( + const Ogre::ColourValue & color, + float length, + const std::vector & poses) +{ + manual_object_->estimateVertexCount(poses.size() * 6); + + for (const auto & pose : poses) { + Ogre::Vector3 vertices[6]; + vertices[0] = pose.position; // back of arrow + vertices[1] = pose.position + pose.orientation * Ogre::Vector3(length, 0, 0); // tip of arrow + vertices[2] = vertices[1]; + vertices[3] = + pose.position + pose.orientation * Ogre::Vector3(0.75f * length, 0.2f * length, 0); + vertices[4] = vertices[1]; + vertices[5] = pose.position + pose.orientation * Ogre::Vector3( + 0.75f * length, -0.2f * length, + 0); + + for (const auto & vertex : vertices) { + manual_object_->position(vertex); + manual_object_->colour(color); + } + } +} + +} // namespace displays +} // namespace rviz_default_plugins diff --git a/rviz_default_plugins/src/rviz_default_plugins/displays/pose_array/flat_arrows_array.hpp b/rviz_default_plugins/src/rviz_default_plugins/displays/pose_array/flat_arrows_array.hpp new file mode 100644 index 000000000..fa5eeeddb --- /dev/null +++ b/rviz_default_plugins/src/rviz_default_plugins/displays/pose_array/flat_arrows_array.hpp @@ -0,0 +1,80 @@ +/* + * Copyright (c) 2012, Willow Garage, Inc. + * Copyright (c) 2018, Bosch Software Innovations GmbH. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the 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__POSE_ARRAY__FLAT_ARROWS_ARRAY_HPP_ +#define RVIZ_DEFAULT_PLUGINS__DISPLAYS__POSE_ARRAY__FLAT_ARROWS_ARRAY_HPP_ + +#include + +#include +#include +#include +#include +#include + +#include "pose_array_display.hpp" + +namespace rviz_default_plugins +{ +namespace displays +{ +struct OgrePose; + +class FlatArrowsArray +{ +public: + explicit FlatArrowsArray(Ogre::SceneManager * scene_manager_); + ~FlatArrowsArray(); + + void createAndAttachManualObject(Ogre::SceneNode * scene_node); + void updateManualObject( + Ogre::ColourValue color, + float alpha, + float length, + const std::vector & poses); + void clear(); + +private: + void setManualObjectMaterial(); + void EnableBlending(const Ogre::ColourValue & color); + void setManualObjectVertices( + const Ogre::ColourValue & color, + float length, + const std::vector & poses); + + Ogre::SceneManager * scene_manager_; + Ogre::ManualObject * manual_object_; + Ogre::MaterialPtr material_; +}; + +} // namespace displays +} // namespace rviz_default_plugins + +#endif // RVIZ_DEFAULT_PLUGINS__DISPLAYS__POSE_ARRAY__FLAT_ARROWS_ARRAY_HPP_ diff --git a/rviz_default_plugins/src/rviz_default_plugins/displays/pose_array/pose_array_display.cpp b/rviz_default_plugins/src/rviz_default_plugins/displays/pose_array/pose_array_display.cpp new file mode 100644 index 000000000..ba0ef2a71 --- /dev/null +++ b/rviz_default_plugins/src/rviz_default_plugins/displays/pose_array/pose_array_display.cpp @@ -0,0 +1,402 @@ +/* + * Copyright (c) 2012, Willow Garage, Inc. + * Copyright (c) 2018, Bosch Software Innovations GmbH. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the 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 "pose_array_display.hpp" + +#include +#include + +#include +#include +#include + +#include "rviz_common/logging.hpp" +#include "rviz_common/properties/enum_property.hpp" +#include "rviz_common/properties/color_property.hpp" +#include "rviz_common/properties/float_property.hpp" +#include "rviz_common/validate_floats.hpp" + +#include "rviz_rendering/objects/arrow.hpp" +#include "rviz_rendering/objects/axes.hpp" + +namespace rviz_default_plugins +{ +namespace displays +{ +namespace +{ +struct ShapeType +{ + enum + { + Arrow2d, + Arrow3d, + Axes, + }; +}; + +Ogre::Vector3 vectorRosToOgre(geometry_msgs::msg::Point const & point) +{ + return Ogre::Vector3(point.x, point.y, point.z); +} + +Ogre::Quaternion quaternionRosToOgre(geometry_msgs::msg::Quaternion const & quaternion) +{ + return Ogre::Quaternion(quaternion.w, quaternion.x, quaternion.y, quaternion.z); +} +} // namespace + +PoseArrayDisplay::PoseArrayDisplay( + rviz_common::DisplayContext * display_context, + Ogre::SceneNode * scene_node) +: PoseArrayDisplay() +{ + context_ = display_context; + scene_node_ = scene_node; + scene_manager_ = context_->getSceneManager(); + + arrows2d_ = std::make_unique(scene_manager_); + arrows2d_->createAndAttachManualObject(scene_node); + arrow_node_ = scene_node_->createChildSceneNode(); + axes_node_ = scene_node_->createChildSceneNode(); + updateShapeChoice(); +} + +PoseArrayDisplay::PoseArrayDisplay() +{ + initializeProperties(); + + shape_property_->addOption("Arrow (Flat)", ShapeType::Arrow2d); + shape_property_->addOption("Arrow (3D)", ShapeType::Arrow3d); + shape_property_->addOption("Axes", ShapeType::Axes); + arrow_alpha_property_->setMin(0); + arrow_alpha_property_->setMax(1); +} + +void PoseArrayDisplay::initializeProperties() +{ + shape_property_ = new rviz_common::properties::EnumProperty( + "Shape", "Arrow (Flat)", "Shape to display the pose as.", this, SLOT(updateShapeChoice())); + + arrow_color_property_ = new rviz_common::properties::ColorProperty( + "Color", QColor(255, 25, 0), "Color to draw the arrows.", this, SLOT(updateArrowColor())); + + arrow_alpha_property_ = new rviz_common::properties::FloatProperty( + "Alpha", + 1.0f, + "Amount of transparency to apply to the displayed poses.", + this, + SLOT(updateArrowColor())); + + arrow2d_length_property_ = new rviz_common::properties::FloatProperty( + "Arrow Length", 0.3f, "Length of the arrows.", this, SLOT(updateArrow2dGeometry())); + + arrow3d_head_radius_property_ = new rviz_common::properties::FloatProperty( + "Head Radius", + 0.03f, + "Radius of the arrow's head, in meters.", + this, + SLOT(updateArrow3dGeometry())); + + arrow3d_head_length_property_ = new rviz_common::properties::FloatProperty( + "Head Length", + 0.07f, + "Length of the arrow's head, in meters.", + this, + SLOT(updateArrow3dGeometry())); + + arrow3d_shaft_radius_property_ = new rviz_common::properties::FloatProperty( + "Shaft Radius", + 0.01f, + "Radius of the arrow's shaft, in meters.", + this, + SLOT(updateArrow3dGeometry())); + + arrow3d_shaft_length_property_ = new rviz_common::properties::FloatProperty( + "Shaft Length", + 0.23f, + "Length of the arrow's shaft, in meters.", + this, + SLOT(updateArrow3dGeometry())); + + axes_length_property_ = new rviz_common::properties::FloatProperty( + "Axes Length", + 0.3f, + "Length of each axis, in meters.", + this, + SLOT(updateAxesGeometry())); + + axes_radius_property_ = new rviz_common::properties::FloatProperty( + "Axes Radius", + 0.01f, + "Radius of each axis, in meters.", + this, + SLOT(updateAxesGeometry())); +} + +PoseArrayDisplay::~PoseArrayDisplay() +{ + // because of forward declaration of arrow and axes, destructor cannot be declared in .hpp as + // default +} + +void PoseArrayDisplay::onInitialize() +{ + RTDClass::onInitialize(); + arrows2d_ = std::make_unique(scene_manager_); + arrows2d_->createAndAttachManualObject(scene_node_); + arrow_node_ = scene_node_->createChildSceneNode(); + axes_node_ = scene_node_->createChildSceneNode(); + updateShapeChoice(); +} + +void PoseArrayDisplay::processMessage(const geometry_msgs::msg::PoseArray::ConstSharedPtr msg) +{ + if (!validateFloats(*msg)) { + setStatus( + rviz_common::properties::StatusProperty::Error, + "Topic", + "Message contained invalid floating point values (nans or infs)"); + return; + } + + if (!setTransform(msg->header)) { + setStatus( + rviz_common::properties::StatusProperty::Error, "Topic", "Failed to look up transform"); + return; + } + + poses_.resize(msg->poses.size()); + + for (std::size_t i = 0; i < msg->poses.size(); ++i) { + poses_[i].position = vectorRosToOgre(msg->poses[i].position); + poses_[i].orientation = quaternionRosToOgre(msg->poses[i].orientation); + } + + updateDisplay(); + + context_->queueRender(); +} + +bool PoseArrayDisplay::validateFloats(const geometry_msgs::msg::PoseArray & msg) +{ + return rviz_common::validateFloats(msg.poses); +} + +bool PoseArrayDisplay::setTransform(std_msgs::msg::Header const & header) +{ + Ogre::Vector3 position; + Ogre::Quaternion orientation; + if (!context_->getFrameManager()->getTransform(header, position, orientation)) { + RVIZ_COMMON_LOG_ERROR_STREAM( + "Error transforming pose '" << qPrintable(getName()) << "' from frame '" << + header.frame_id.c_str() << "' to frame '" << qPrintable(fixed_frame_) << "'"); + return false; + } + scene_node_->setPosition(position); + scene_node_->setOrientation(orientation); + return true; +} + +void PoseArrayDisplay::updateDisplay() +{ + int shape = shape_property_->getOptionInt(); + switch (shape) { + case ShapeType::Arrow2d: + updateArrows2d(); + arrows3d_.clear(); + axes_.clear(); + break; + case ShapeType::Arrow3d: + updateArrows3d(); + arrows2d_->clear(); + axes_.clear(); + break; + case ShapeType::Axes: + updateAxes(); + arrows2d_->clear(); + arrows3d_.clear(); + break; + } +} + +void PoseArrayDisplay::updateArrows2d() +{ + arrows2d_->updateManualObject( + arrow_color_property_->getOgreColor(), + arrow_alpha_property_->getFloat(), + arrow2d_length_property_->getFloat(), + poses_); +} + +void PoseArrayDisplay::updateArrows3d() +{ + while (arrows3d_.size() < poses_.size()) { + arrows3d_.push_back(makeArrow3d()); + } + while (arrows3d_.size() > poses_.size()) { + arrows3d_.pop_back(); + } + + Ogre::Quaternion adjust_orientation(Ogre::Degree(-90), Ogre::Vector3::UNIT_Y); + for (std::size_t i = 0; i < poses_.size(); ++i) { + arrows3d_[i]->setPosition(poses_[i].position); + arrows3d_[i]->setOrientation(poses_[i].orientation * adjust_orientation); + } +} + +void PoseArrayDisplay::updateAxes() +{ + while (axes_.size() < poses_.size()) { + axes_.push_back(makeAxes()); + } + while (axes_.size() > poses_.size()) { + axes_.pop_back(); + } + for (std::size_t i = 0; i < poses_.size(); ++i) { + axes_[i]->setPosition(poses_[i].position); + axes_[i]->setOrientation(poses_[i].orientation); + } +} + +std::unique_ptr PoseArrayDisplay::makeArrow3d() +{ + Ogre::ColourValue color = arrow_color_property_->getOgreColor(); + color.a = arrow_alpha_property_->getFloat(); + + auto arrow = std::make_unique( + scene_manager_, + arrow_node_, + arrow3d_shaft_length_property_->getFloat(), + arrow3d_shaft_radius_property_->getFloat(), + arrow3d_head_length_property_->getFloat(), + arrow3d_head_radius_property_->getFloat() + ); + + arrow->setColor(color); + return arrow; +} + +std::unique_ptr PoseArrayDisplay::makeAxes() +{ + return std::make_unique( + scene_manager_, + axes_node_, + axes_length_property_->getFloat(), + axes_radius_property_->getFloat() + ); +} + +void PoseArrayDisplay::reset() +{ + RTDClass::reset(); + arrows2d_->clear(); + arrows3d_.clear(); + axes_.clear(); +} + +void PoseArrayDisplay::updateShapeChoice() +{ + int shape = shape_property_->getOptionInt(); + bool use_arrow2d = shape == ShapeType::Arrow2d; + bool use_arrow3d = shape == ShapeType::Arrow3d; + bool use_axes = shape == ShapeType::Axes; + + arrow_color_property_->setHidden(use_axes); + arrow_alpha_property_->setHidden(use_axes); + + arrow2d_length_property_->setHidden(!use_arrow2d); + + arrow3d_shaft_length_property_->setHidden(!use_arrow3d); + arrow3d_shaft_radius_property_->setHidden(!use_arrow3d); + arrow3d_head_length_property_->setHidden(!use_arrow3d); + arrow3d_head_radius_property_->setHidden(!use_arrow3d); + + axes_length_property_->setHidden(!use_axes); + axes_radius_property_->setHidden(!use_axes); + + if (initialized()) { + updateDisplay(); + } +} + +void PoseArrayDisplay::updateArrowColor() +{ + int shape = shape_property_->getOptionInt(); + Ogre::ColourValue color = arrow_color_property_->getOgreColor(); + color.a = arrow_alpha_property_->getFloat(); + + if (shape == ShapeType::Arrow2d) { + updateArrows2d(); + } else if (shape == ShapeType::Arrow3d) { + for (const auto & arrow : arrows3d_) { + arrow->setColor(color); + } + } + context_->queueRender(); +} + +void PoseArrayDisplay::updateArrow2dGeometry() +{ + updateArrows2d(); + context_->queueRender(); +} + +void PoseArrayDisplay::updateArrow3dGeometry() +{ + for (const auto & arrow : arrows3d_) { + arrow->set( + arrow3d_shaft_length_property_->getFloat(), + arrow3d_shaft_radius_property_->getFloat(), + arrow3d_head_length_property_->getFloat(), + arrow3d_head_radius_property_->getFloat() + ); + } + context_->queueRender(); +} + +void PoseArrayDisplay::updateAxesGeometry() +{ + for (const auto & axis : axes_) { + axis->set(axes_length_property_->getFloat(), axes_radius_property_->getFloat()); + } + context_->queueRender(); +} + +void PoseArrayDisplay::setShape(QString shape) +{ + shape_property_->setValue(shape); +} + +} // namespace displays +} // namespace rviz_default_plugins + +#include // NOLINT +PLUGINLIB_EXPORT_CLASS(rviz_default_plugins::displays::PoseArrayDisplay, rviz_common::Display) diff --git a/rviz_default_plugins/src/rviz_default_plugins/displays/pose_array/pose_array_display.hpp b/rviz_default_plugins/src/rviz_default_plugins/displays/pose_array/pose_array_display.hpp new file mode 100644 index 000000000..3b5926da6 --- /dev/null +++ b/rviz_default_plugins/src/rviz_default_plugins/displays/pose_array/pose_array_display.hpp @@ -0,0 +1,152 @@ +/* + * Copyright (c) 2012, Willow Garage, Inc. + * Copyright (c) 2018, Bosch Software Innovations GmbH. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the 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__POSE_ARRAY__POSE_ARRAY_DISPLAY_HPP_ +#define RVIZ_DEFAULT_PLUGINS__DISPLAYS__POSE_ARRAY__POSE_ARRAY_DISPLAY_HPP_ + +#include +#include + +#include "geometry_msgs/msg/pose_array.hpp" + +#include "rviz_rendering/objects/shape.hpp" +#include "rviz_common/ros_topic_display.hpp" +#include "flat_arrows_array.hpp" + +// TODO(botteroa): Originally the display extended the MessageFilterDisplay. Revisit when available. +// #include "rviz_common/message_filter_display.hpp" + +namespace Ogre +{ +class ManualObject; +} // namespace Ogre + +namespace rviz_common +{ +namespace properties +{ +class EnumProperty; +class ColorProperty; +class FloatProperty; +} // namespace properties +} // namespace rviz_common + +namespace rviz_rendering +{ +class Arrow; +class Axes; +} // namespace rviz_rendering + +namespace rviz_default_plugins +{ +namespace displays +{ +class FlatArrowsArray; +struct OgrePose +{ + Ogre::Vector3 position; + Ogre::Quaternion orientation; +}; + +/** @brief Displays a geometry_msgs/PoseArray message as a bunch of line-drawn arrows. */ +class PoseArrayDisplay : public rviz_common::RosTopicDisplay +{ + Q_OBJECT + +public: + // TODO(botteroa-si): Constructor for testing, remove once ros_nodes can be mocked and call + // initialize instead + PoseArrayDisplay( + rviz_common::DisplayContext * display_context, + Ogre::SceneNode * scene_node); + PoseArrayDisplay(); + ~PoseArrayDisplay() override; + + void processMessage(geometry_msgs::msg::PoseArray::ConstSharedPtr msg) override; + void setShape(QString shape); // for testing + +protected: + void onInitialize() override; + void reset() override; + +private Q_SLOTS: + /// Update the interface and visible shapes based on the selected shape type. + void updateShapeChoice(); + + /// Update the arrow color. + void updateArrowColor(); + + /// Update the flat arrow geometry. + void updateArrow2dGeometry(); + + /// Update the 3D arrow geometry. + void updateArrow3dGeometry(); + + /// Update the axes geometry. + void updateAxesGeometry(); + +private: + void initializeProperties(); + bool validateFloats(const geometry_msgs::msg::PoseArray & msg); + bool setTransform(std_msgs::msg::Header const & header); + void updateDisplay(); + void updateArrows2d(); + void updateArrows3d(); + void updateAxes(); + std::unique_ptr makeAxes(); + std::unique_ptr makeArrow3d(); + + std::vector poses_; + std::unique_ptr arrows2d_; + std::vector> arrows3d_; + std::vector> axes_; + + Ogre::SceneNode * arrow_node_; + Ogre::SceneNode * axes_node_; + + rviz_common::properties::EnumProperty * shape_property_; + rviz_common::properties::ColorProperty * arrow_color_property_; + rviz_common::properties::FloatProperty * arrow_alpha_property_; + + rviz_common::properties::FloatProperty * arrow2d_length_property_; + + rviz_common::properties::FloatProperty * arrow3d_head_radius_property_; + rviz_common::properties::FloatProperty * arrow3d_head_length_property_; + rviz_common::properties::FloatProperty * arrow3d_shaft_radius_property_; + rviz_common::properties::FloatProperty * arrow3d_shaft_length_property_; + + rviz_common::properties::FloatProperty * axes_length_property_; + rviz_common::properties::FloatProperty * axes_radius_property_; +}; + +} // namespace displays +} // namespace rviz_default_plugins + +#endif // RVIZ_DEFAULT_PLUGINS__DISPLAYS__POSE_ARRAY__POSE_ARRAY_DISPLAY_HPP_ diff --git a/rviz_default_plugins/test/rviz_default_plugins/displays/marker/markers/arrow_marker_test.cpp b/rviz_default_plugins/test/rviz_default_plugins/displays/marker/markers/arrow_marker_test.cpp index 3c070316f..52653f76e 100644 --- a/rviz_default_plugins/test/rviz_default_plugins/displays/marker/markers/arrow_marker_test.cpp +++ b/rviz_default_plugins/test/rviz_default_plugins/displays/marker/markers/arrow_marker_test.cpp @@ -57,30 +57,6 @@ using namespace ::testing; // NOLINT -void assertArrowIsNotVisible() -{ - auto arrow_head = rviz_default_plugins::findEntityByMeshName( - MarkersTestFixture::scene_manager_->getRootSceneNode(), "rviz_cone" ".mesh"); - auto arrow_shaft = rviz_default_plugins::findEntityByMeshName( - MarkersTestFixture::scene_manager_->getRootSceneNode(), "rviz_cylinder.mesh"); - - ASSERT_TRUE(!arrow_head->isVisible() && !arrow_shaft->isVisible()); -} - -void assertArrowWithTransform( - Ogre::SceneManager * scene_manager, - Ogre::Vector3 position, - Ogre::Vector3 scale, - Ogre::Quaternion orientation) -{ - auto arrow_scene_node = rviz_default_plugins::findOneArrow(scene_manager->getRootSceneNode()); - ASSERT_TRUE(arrow_scene_node); - EXPECT_VECTOR3_EQ(position, arrow_scene_node->getPosition()); - // Have to mangle the scale because of the default orientation of the cylinders (see arrow.cpp). - EXPECT_VECTOR3_EQ(Ogre::Vector3(scale.z, scale.x, scale.y), arrow_scene_node->getScale()); - EXPECT_QUATERNION_EQ(orientation, arrow_scene_node->getOrientation()); -} - // default orientation is set to (0.5, -0.5, -0.5, -0.5) by arrow marker and arrow. const auto default_arrow_orientation_ = Ogre::Quaternion(0.5f, -0.5f, -0.5f, -0.5f); const auto default_arrow_position_ = Ogre::Vector3(0, 0, 0); @@ -92,7 +68,7 @@ TEST_F(MarkersTestFixture, setMessage_makes_the_scene_node_invisible_if_invalid_ marker_->setMessage(createDefaultMessage(visualization_msgs::msg::Marker::ARROW)); - assertArrowIsNotVisible(); + EXPECT_FALSE(rviz_default_plugins::arrowIsVisible(scene_manager_)); } TEST_F(MarkersTestFixture, incomplete_message_sets_scene_node_to_not_visible) { @@ -103,7 +79,7 @@ TEST_F(MarkersTestFixture, incomplete_message_sets_scene_node_to_not_visible) { marker_->setMessage(incomplete_message); - assertArrowIsNotVisible(); + EXPECT_FALSE(rviz_default_plugins::arrowIsVisible(scene_manager_)); } TEST_F(MarkersTestFixture, setMessage_sets_positions_and_orientations_correctly) { @@ -115,7 +91,7 @@ TEST_F(MarkersTestFixture, setMessage_sets_positions_and_orientations_correctly) EXPECT_VECTOR3_EQ(Ogre::Vector3(0, 1, 0), marker_->getPosition()); EXPECT_QUATERNION_EQ(Ogre::Quaternion(0, 0, 1, 0), marker_->getOrientation()); - assertArrowWithTransform( + rviz_default_plugins::assertArrowWithTransform( scene_manager_, default_arrow_position_, default_arrow_scale_, default_arrow_orientation_); } @@ -139,7 +115,7 @@ TEST_F(MarkersTestFixture, setMessage_sets_positions_and_orientations_from_point EXPECT_VECTOR3_EQ(Ogre::Vector3(0, 1, 0), marker_->getPosition()); EXPECT_QUATERNION_EQ(Ogre::Quaternion(0, 0, 1, 0), marker_->getOrientation()); - assertArrowWithTransform( + rviz_default_plugins::assertArrowWithTransform( scene_manager_, first_point, expected_arrow_scale, expected_arrow_orientation); } @@ -153,7 +129,7 @@ TEST_F(MarkersTestFixture, setMessage_ignores_points_if_thery_are_more_than_two) marker_->setMessage(message); - assertArrowWithTransform( + rviz_default_plugins::assertArrowWithTransform( scene_manager_, default_arrow_position_, default_arrow_scale_, default_arrow_orientation_); } @@ -164,6 +140,6 @@ TEST_F(MarkersTestFixture, setMessage_ignores_old_message) { marker_->setMessage(createMessageWithPoints(visualization_msgs::msg::Marker::ARROW)); marker_->setMessage(createDefaultMessage(visualization_msgs::msg::Marker::ARROW)); - assertArrowWithTransform( + rviz_default_plugins::assertArrowWithTransform( scene_manager_, default_arrow_position_, default_arrow_scale_, default_arrow_orientation_); } diff --git a/rviz_default_plugins/test/rviz_default_plugins/displays/pose_array/pose_array_display_test.cpp b/rviz_default_plugins/test/rviz_default_plugins/displays/pose_array/pose_array_display_test.cpp new file mode 100644 index 000000000..63c6a8655 --- /dev/null +++ b/rviz_default_plugins/test/rviz_default_plugins/displays/pose_array/pose_array_display_test.cpp @@ -0,0 +1,302 @@ +/* + * Copyright (c) 2018, Bosch Software Innovations GmbH. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted (subject to the limitations in the disclaimer + * below) provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS + * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include +#include + +#include +#include + +#include +#include +#include + +#include "rviz_common/properties/float_property.hpp" + +#include "../../../../src/rviz_default_plugins/displays/pose_array/pose_array_display.hpp" + +#include "../display_test_fixture.hpp" +#include "../../scene_graph_introspection.hpp" + +using namespace ::testing; // NOLINT + +class PoseArrayDisplayFixture : public DisplayTestFixture +{ +public: + void SetUp() override + { + DisplayTestFixture::SetUp(); + + display_ = std::make_unique( + context_.get(), + scene_manager_->getRootSceneNode()->createChildSceneNode()); + + arrow_2d_length_property_ = display_->childAt(5); + + for (int i = 3; i < 5; i++) { + common_arrow_properties_.push_back(display_->childAt(i)); + } + for (int i = 6; i < 10; i++) { + arrow_3d_properties_.push_back(display_->childAt(i)); + } + for (int i = 10; i < 12; i++) { + axes_properties_.push_back(display_->childAt(i)); + } + } + + void TearDown() override + { + display_.reset(); + DisplayTestFixture::TearDown(); + } + + std::unique_ptr display_; + rviz_common::properties::Property * arrow_2d_length_property_; + std::vector common_arrow_properties_; + std::vector arrow_3d_properties_; + std::vector axes_properties_; +}; + +geometry_msgs::msg::PoseArray::SharedPtr createMessageWithOnePose() +{ + auto message = std::make_shared(); + message->header = std_msgs::msg::Header(); + message->header.frame_id = "pose_array_frame"; + message->header.stamp = rclcpp::Clock().now(); + + geometry_msgs::msg::Pose pose; + pose.position.x = 1; + pose.position.y = 2; + pose.position.z = 3; + pose.orientation.x = 1; + pose.orientation.y = 0; + pose.orientation.z = 1; + pose.orientation.w = 0; + + message->poses.push_back(pose); + + return message; +} + +void expectAxesAreVisible(Ogre::SceneNode * node) +{ + for (uint16_t i = 0; i < 3; ++i) { + auto child_node = dynamic_cast(node->getChild(i)->getChild(0)); + auto entity = dynamic_cast(child_node->getAttachedObject(0)); + ASSERT_TRUE(entity); + EXPECT_TRUE(entity->isVisible()); + } +} + +TEST_F(PoseArrayDisplayFixture, constructor_set_all_the_properties_in_the_right_order) { + EXPECT_EQ("Color", display_->childAt(3)->getNameStd()); + EXPECT_EQ("Alpha", display_->childAt(4)->getNameStd()); + EXPECT_EQ("Arrow Length", display_->childAt(5)->getNameStd()); + EXPECT_EQ("Head Radius", display_->childAt(6)->getNameStd()); + EXPECT_EQ("Head Length", display_->childAt(7)->getNameStd()); + EXPECT_EQ("Shaft Radius", display_->childAt(8)->getNameStd()); + EXPECT_EQ("Shaft Length", display_->childAt(9)->getNameStd()); + EXPECT_EQ("Axes Length", display_->childAt(10)->getNameStd()); + EXPECT_EQ("Axes Radius", display_->childAt(11)->getNameStd()); +} + +TEST_F(PoseArrayDisplayFixture, at_startup_only_flat_arrows_propertie_are_visible) { + for (const auto & property : common_arrow_properties_) { + EXPECT_FALSE(property->getHidden()); + } + for (const auto & property : arrow_3d_properties_) { + EXPECT_TRUE(property->getHidden()); + } + for (const auto & property : axes_properties_) { + EXPECT_TRUE(property->getHidden()); + } + EXPECT_FALSE(arrow_2d_length_property_->getHidden()); +} + +TEST_F(PoseArrayDisplayFixture, + processMessage_corrctly_manages_property_visibility_from_arrow2d_to_arrow3d) { + mockValidTransform(); + auto msg = createMessageWithOnePose(); + display_->setShape("Arrow (3D)"); + display_->processMessage(msg); + + for (const auto & property : common_arrow_properties_) { + EXPECT_FALSE(property->getHidden()); + } + for (const auto & property : arrow_3d_properties_) { + EXPECT_FALSE(property->getHidden()); + } + for (const auto & property : axes_properties_) { + EXPECT_TRUE(property->getHidden()); + } + EXPECT_TRUE(arrow_2d_length_property_->getHidden()); +} + +TEST_F(PoseArrayDisplayFixture, + processMessage_corrctly_manages_property_visibility_from_arrow2d_to_axes) { + mockValidTransform(); + auto msg = createMessageWithOnePose(); + display_->setShape("Axes"); + display_->processMessage(msg); + + for (const auto & property : common_arrow_properties_) { + EXPECT_TRUE(property->getHidden()); + } + for (const auto & property : arrow_3d_properties_) { + EXPECT_TRUE(property->getHidden()); + } + for (const auto & property : axes_properties_) { + EXPECT_FALSE(property->getHidden()); + } + EXPECT_TRUE(arrow_2d_length_property_->getHidden()); +} + +TEST_F(PoseArrayDisplayFixture, setTransform_with_invalid_message_returns_early) { + mockValidTransform(); + auto msg = createMessageWithOnePose(); + msg->poses[0].position.x = nan("NaN"); + display_->processMessage(msg); + + auto arrows_3d = rviz_default_plugins::findAllArrows(scene_manager_->getRootSceneNode()); + auto axes = rviz_default_plugins::findAllAxes(scene_manager_->getRootSceneNode()); + auto manual_object = + rviz_default_plugins::findOneMovableObject(scene_manager_->getRootSceneNode()); + + // the default position and orientation of the scene node are (0, 0, 0) and (1, 0, 0, 0) + EXPECT_VECTOR3_EQ(Ogre::Vector3(0, 0, 0), display_->getSceneNode()->getPosition()); + EXPECT_QUATERNION_EQ(Ogre::Quaternion(1, 0, 0, 0), display_->getSceneNode()->getOrientation()); + EXPECT_FLOAT_EQ(0, manual_object->getBoundingRadius()); + EXPECT_THAT(arrows_3d, SizeIs(0)); + EXPECT_THAT(axes, SizeIs(0)); +} + +TEST_F(PoseArrayDisplayFixture, setTransform_with_invalid_transform_returns_early) { + EXPECT_CALL(*frame_manager_, getTransform(_, _, _, _)).WillOnce(Return(false)); + + auto msg = createMessageWithOnePose(); + display_->processMessage(msg); + + auto arrows_3d = rviz_default_plugins::findAllArrows(scene_manager_->getRootSceneNode()); + auto axes = rviz_default_plugins::findAllAxes(scene_manager_->getRootSceneNode()); + auto manual_object = + rviz_default_plugins::findOneMovableObject(scene_manager_->getRootSceneNode()); + + // the default position and orientation of the scene node are (0, 0, 0) and (1, 0, 0, 0) + EXPECT_VECTOR3_EQ(Ogre::Vector3(0, 0, 0), display_->getSceneNode()->getPosition()); + EXPECT_QUATERNION_EQ(Ogre::Quaternion(1, 0, 0, 0), display_->getSceneNode()->getOrientation()); + EXPECT_FLOAT_EQ(0, manual_object->getBoundingRadius()); + EXPECT_THAT(arrows_3d, SizeIs(0)); + EXPECT_THAT(axes, SizeIs(0)); +} + +TEST_F(PoseArrayDisplayFixture, setTransform_sets_node_position_and_orientation_correctly) { + mockValidTransform(); + auto msg = createMessageWithOnePose(); + display_->processMessage(msg); + + EXPECT_VECTOR3_EQ(Ogre::Vector3(0, 1, 0), display_->getSceneNode()->getPosition()); + EXPECT_QUATERNION_EQ(Ogre::Quaternion(0, 0, 1, 0), display_->getSceneNode()->getOrientation()); +} + +TEST_F(PoseArrayDisplayFixture, processMessage_sets_manualObject_correctly) { + mockValidTransform(); + auto msg = createMessageWithOnePose(); + display_->processMessage(msg); + + auto manual_object = + rviz_default_plugins::findOneMovableObject(scene_manager_->getRootSceneNode()); + auto manual_objectbounding_radius = 4.17732; + EXPECT_FLOAT_EQ(manual_objectbounding_radius, manual_object->getBoundingRadius()); + EXPECT_VECTOR3_EQ(Ogre::Vector3(0.85, 2, 3.3), manual_object->getBoundingBox().getCenter()); +} + +TEST_F(PoseArrayDisplayFixture, processMessage_sets_arrows3d_correctly) { + mockValidTransform(); + auto msg = createMessageWithOnePose(); + + display_->setShape("Arrow (3D)"); + display_->processMessage(msg); + + auto arrows = rviz_default_plugins::findAllArrows(scene_manager_->getRootSceneNode()); + + // The orientation is first manipulated by the display and then in setOrientation() in arrow.cpp + auto expected_orientation = + Ogre::Quaternion(0, 1, 0, 1) * + Ogre::Quaternion(Ogre::Degree(-90), Ogre::Vector3::UNIT_Y) * + Ogre::Quaternion(Ogre::Degree(-90), Ogre::Vector3::UNIT_X); + expected_orientation.normalise(); + + EXPECT_TRUE(rviz_default_plugins::arrowIsVisible(scene_manager_)); + EXPECT_THAT(arrows, SizeIs(1)); + rviz_default_plugins::assertArrowWithTransform( + scene_manager_, Ogre::Vector3(1, 2, 3), Ogre::Vector3(1, 1, 1), expected_orientation); +} + +TEST_F(PoseArrayDisplayFixture, processMessage_sets_axes_correctly) { + mockValidTransform(); + auto msg = createMessageWithOnePose(); + + display_->setShape("Axes"); + display_->processMessage(msg); + + auto frames = rviz_default_plugins::findAllAxes(scene_manager_->getRootSceneNode()); + + auto expected_orientation = Ogre::Quaternion(0, 1, 0, 1); + expected_orientation.normalise(); + + expectAxesAreVisible(frames[0]); + EXPECT_THAT(frames, SizeIs(1)); + EXPECT_VECTOR3_EQ(Ogre::Vector3(1, 2, 3), frames[0]->getPosition()); + EXPECT_QUATERNION_EQ(expected_orientation, frames[0]->getOrientation()); +} + +TEST_F(PoseArrayDisplayFixture, processMessage_updates_the_display_correctly_after_shape_change) { + mockValidTransform(); + auto msg = createMessageWithOnePose(); + display_->setShape("Arrow (3D)"); + display_->processMessage(msg); + + auto arrows = rviz_default_plugins::findAllArrows(scene_manager_->getRootSceneNode()); + auto frames = rviz_default_plugins::findAllAxes(scene_manager_->getRootSceneNode()); + auto manual_object = + rviz_default_plugins::findOneMovableObject(scene_manager_->getRootSceneNode()); + EXPECT_THAT(arrows, SizeIs(1)); + EXPECT_EQ(0, manual_object->getBoundingRadius()); + EXPECT_THAT(frames, SizeIs(0)); + + display_->setShape("Axes"); + display_->processMessage(msg); + auto post_update_arrows = rviz_default_plugins::findAllArrows(scene_manager_->getRootSceneNode()); + auto post_update_frames = rviz_default_plugins::findAllAxes(scene_manager_->getRootSceneNode()); + EXPECT_THAT(post_update_frames, SizeIs(1)); + EXPECT_EQ(0, manual_object->getBoundingRadius()); + EXPECT_THAT(post_update_arrows, SizeIs(0)); +} diff --git a/rviz_default_plugins/test/rviz_default_plugins/scene_graph_introspection.cpp b/rviz_default_plugins/test/rviz_default_plugins/scene_graph_introspection.cpp index bbb92a909..e7fb40f6f 100644 --- a/rviz_default_plugins/test/rviz_default_plugins/scene_graph_introspection.cpp +++ b/rviz_default_plugins/test/rviz_default_plugins/scene_graph_introspection.cpp @@ -70,6 +70,30 @@ bool vector3NearlyEqual(Ogre::Vector3 expected, Ogre::Vector3 actual) Ogre::Math::Abs(expected.z - actual.z) < 0.0001f; } +bool arrowIsVisible(Ogre::SceneManager * scene_manager) +{ + auto arrow_head = rviz_default_plugins::findEntityByMeshName( + scene_manager->getRootSceneNode(), "rviz_cone.mesh"); + auto arrow_shaft = rviz_default_plugins::findEntityByMeshName( + scene_manager->getRootSceneNode(), "rviz_cylinder.mesh"); + + return arrow_head->isVisible() && arrow_shaft->isVisible(); +} + +void assertArrowWithTransform( + Ogre::SceneManager * scene_manager, + Ogre::Vector3 position, + Ogre::Vector3 scale, + Ogre::Quaternion orientation) +{ + auto arrow_scene_node = rviz_default_plugins::findOneArrow(scene_manager->getRootSceneNode()); + ASSERT_TRUE(arrow_scene_node); + EXPECT_VECTOR3_EQ(position, arrow_scene_node->getPosition()); + // Have to mangle the scale because of the default orientation of the cylinders (see arrow.cpp). + EXPECT_VECTOR3_EQ(Ogre::Vector3(scale.z, scale.x, scale.y), arrow_scene_node->getScale()); + EXPECT_QUATERNION_EQ(orientation, arrow_scene_node->getOrientation()); +} + std::vector findAllEntitiesByMeshName( Ogre::SceneNode * scene_node, const Ogre::String & resource_name) { @@ -77,9 +101,9 @@ std::vector findAllEntitiesByMeshName( findAllOgreObjectByType(scene_node, "Entity"); std::vector correct_entities; - for (size_t i = 0; i < all_entities.size(); ++i) { - if (all_entities[i]->getMesh() && all_entities[i]->getMesh()->getName() == resource_name) { - correct_entities.push_back(all_entities[i]); + for (const auto & entity : all_entities) { + if (entity->getMesh() && entity->getMesh()->getName() == resource_name) { + correct_entities.push_back(entity); } } @@ -144,6 +168,41 @@ std::vector findAllArrows(Ogre::SceneNode * scene_node) return arrows; } +std::vector findAllAxes(Ogre::SceneNode * scene_node) +{ + std::vector axes; + auto all_cylinders = findAllEntitiesByMeshName(scene_node, "rviz_cylinder.mesh"); + if (!all_cylinders.empty()) { + for (size_t i = 0; i < all_cylinders.size(); i++) { + auto first_axis_node = all_cylinders[i] + ->getParentSceneNode() // OffsetNode from shape + ->getParentSceneNode() // SceneNode from shape + ->getParentSceneNode(); // SceneNode from axes + if (first_axis_node) { + for (size_t j = i + 1; j < all_cylinders.size(); j++) { + auto second_axis_node = all_cylinders[j] + ->getParentSceneNode() + ->getParentSceneNode() + ->getParentSceneNode(); + if (second_axis_node && second_axis_node == first_axis_node) { + for (size_t k = j + 1; k < all_cylinders.size(); k++) { + auto third_axis_node = all_cylinders[k] + ->getParentSceneNode() + ->getParentSceneNode() + ->getParentSceneNode(); + if (third_axis_node && third_axis_node == first_axis_node) { + axes.push_back(first_axis_node); + } + } + } + } + } + } + } + + return axes; +} + Ogre::SceneNode * findOneArrow(Ogre::SceneNode * scene_node) { auto arrows = findAllArrows(scene_node); diff --git a/rviz_default_plugins/test/rviz_default_plugins/scene_graph_introspection.hpp b/rviz_default_plugins/test/rviz_default_plugins/scene_graph_introspection.hpp index ddfe7dcf5..12c32d551 100644 --- a/rviz_default_plugins/test/rviz_default_plugins/scene_graph_introspection.hpp +++ b/rviz_default_plugins/test/rviz_default_plugins/scene_graph_introspection.hpp @@ -64,9 +64,18 @@ namespace rviz_default_plugins bool quaternionNearlyEqual(Ogre::Quaternion expected, Ogre::Quaternion actual); bool vector3NearlyEqual(Ogre::Vector3 expected, Ogre::Vector3 actual); +bool arrowIsVisible(Ogre::SceneManager * scene_manager); +void assertArrowWithTransform( + Ogre::SceneManager * scene_manager, + Ogre::Vector3 position, + Ogre::Vector3 scale, + Ogre::Quaternion orientation); + std::vector findAllArrows(Ogre::SceneNode * scene_node); Ogre::SceneNode * findOneArrow(Ogre::SceneNode * scene_node); +std::vector findAllAxes(Ogre::SceneNode * scene_node); + std::vector findAllEntitiesByMeshName( Ogre::SceneNode * scene_node, const Ogre::String & resource_name); Ogre::Entity * findEntityByMeshName(