diff --git a/rviz_common/include/rviz_common/msg_conversions.hpp b/rviz_common/include/rviz_common/msg_conversions.hpp index 27493128a..0df9ebdf3 100644 --- a/rviz_common/include/rviz_common/msg_conversions.hpp +++ b/rviz_common/include/rviz_common/msg_conversions.hpp @@ -33,7 +33,9 @@ #include #include +#include +#include "std_msgs/msg/color_rgba.hpp" #include "geometry_msgs/msg/point.hpp" #include "geometry_msgs/msg/vector3.hpp" #include "geometry_msgs/msg/quaternion.hpp" @@ -58,6 +60,11 @@ static inline Ogre::Quaternion quaternionMsgToOgre(const geometry_msgs::msg::Qua return Ogre::Quaternion(m.w, m.x, m.y, m.z); } +static inline Ogre::ColourValue colorMsgToOgre(const std_msgs::msg::ColorRGBA & m) +{ + return Ogre::ColourValue(m.r, m.g, m.b, m.a); +} + static inline geometry_msgs::msg::Point pointOgreToMsg(const Ogre::Vector3 & o) { geometry_msgs::msg::Point m; @@ -79,6 +86,13 @@ static inline geometry_msgs::msg::Quaternion quaternionOgreToMsg(const Ogre::Qua return m; } +static inline std_msgs::msg::ColorRGBA colorOgreToMsg(const Ogre::ColourValue & o) +{ + std_msgs::msg::ColorRGBA m; + m.r = o.r; m.g = o.g; m.b = o.b; m.a = o.a; + return m; +} + } // namespace rviz_common #endif // RVIZ_COMMON__MSG_CONVERSIONS_HPP_ diff --git a/rviz_default_plugins/CMakeLists.txt b/rviz_default_plugins/CMakeLists.txt index 1bdc96b17..33e5e8301 100644 --- a/rviz_default_plugins/CMakeLists.txt +++ b/rviz_default_plugins/CMakeLists.txt @@ -43,6 +43,8 @@ else() set(SKIP_VISUAL_TESTS "SKIP_TEST") endif() +set(SKIP_VISUAL_TESTS "") + if(MSVC) set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /bigobj") endif() @@ -162,6 +164,7 @@ set(rviz_default_plugins_source_files src/rviz_default_plugins/displays/map/palette_builder.cpp src/rviz_default_plugins/displays/map/swatch.cpp src/rviz_default_plugins/displays/marker/markers/arrow_marker.cpp + src/rviz_default_plugins/displays/marker/markers/arrow_strip_marker.cpp src/rviz_default_plugins/displays/marker/markers/line_list_marker.cpp src/rviz_default_plugins/displays/marker/markers/line_marker_base.cpp src/rviz_default_plugins/displays/marker/markers/line_strip_marker.cpp @@ -416,6 +419,7 @@ if(BUILD_TESTING) ament_add_gmock(marker_test test/rviz_default_plugins/displays/marker/markers/arrow_marker_test.cpp + test/rviz_default_plugins/displays/marker/markers/arrow_strip_marker_test.cpp test/rviz_default_plugins/displays/marker/markers/line_marker_test.cpp test/rviz_default_plugins/displays/marker/markers/mesh_resource_marker_test.cpp test/rviz_default_plugins/displays/marker/markers/points_marker_test.cpp diff --git a/rviz_default_plugins/include/rviz_default_plugins/displays/marker/markers/arrow_strip_marker.hpp b/rviz_default_plugins/include/rviz_default_plugins/displays/marker/markers/arrow_strip_marker.hpp new file mode 100644 index 000000000..744d3c2f7 --- /dev/null +++ b/rviz_default_plugins/include/rviz_default_plugins/displays/marker/markers/arrow_strip_marker.hpp @@ -0,0 +1,76 @@ +/* + * Copyright (c) 2024, Open Source Robotics Foundation, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef RVIZ_DEFAULT_PLUGINS__DISPLAYS__MARKER__MARKERS__ARROW_STRIP_MARKER_HPP_ +#define RVIZ_DEFAULT_PLUGINS__DISPLAYS__MARKER__MARKERS__ARROW_STRIP_MARKER_HPP_ + +#include +#include + +#include "marker_base.hpp" + +#include "rviz_rendering/objects/arrow.hpp" +#include "rviz_default_plugins/visibility_control.hpp" + +namespace Ogre +{ +class SceneNode; +} // namespace Ogre +namespace rviz_common +{ +class DisplayContext; +} // namespace rviz_common + +namespace rviz_default_plugins +{ +namespace displays +{ +namespace markers +{ + +class RVIZ_DEFAULT_PLUGINS_PUBLIC ArrowStripMarker : public MarkerBase +{ +public: + ArrowStripMarker( + MarkerCommon * owner, rviz_common::DisplayContext * context, + Ogre::SceneNode * parent_node); + ~ArrowStripMarker() override = default; + +protected: + void onNewMessage( + const MarkerConstSharedPtr & old_message, + const MarkerConstSharedPtr & new_message) override; + std::vector> arrows_; +}; + +} // namespace markers +} // namespace displays +} // namespace rviz_default_plugins + +#endif // RVIZ_DEFAULT_PLUGINS__DISPLAYS__MARKER__MARKERS__ARROW_STRIP_MARKER_HPP_ \ No newline at end of file diff --git a/rviz_default_plugins/src/rviz_default_plugins/displays/interactive_markers/interactive_marker_control.cpp b/rviz_default_plugins/src/rviz_default_plugins/displays/interactive_markers/interactive_marker_control.cpp index aba59ee6d..30de2edb0 100644 --- a/rviz_default_plugins/src/rviz_default_plugins/displays/interactive_markers/interactive_marker_control.cpp +++ b/rviz_default_plugins/src/rviz_default_plugins/displays/interactive_markers/interactive_marker_control.cpp @@ -58,6 +58,7 @@ #include "rviz_default_plugins/displays/marker/markers/shape_marker.hpp" #include "rviz_default_plugins/displays/marker/markers/arrow_marker.hpp" +#include "rviz_default_plugins/displays/marker/markers/arrow_strip_marker.hpp" #include "rviz_default_plugins/displays/marker/markers/line_list_marker.hpp" #include "rviz_default_plugins/displays/marker/markers/line_strip_marker.hpp" #include "rviz_default_plugins/displays/marker/markers/points_marker.hpp" @@ -123,7 +124,11 @@ void InteractiveMarkerControl::makeMarkers( marker.reset(new markers::ArrowMarker(nullptr, context_, markers_node_)); } break; - + case visualization_msgs::msg::Marker::ARROW_STRIP: + { + marker.reset(new markers::ArrowStripMarker(nullptr, context_, markers_node_)); + } + break; case visualization_msgs::msg::Marker::LINE_STRIP: { marker.reset(new markers::LineStripMarker(nullptr, context_, markers_node_)); diff --git a/rviz_default_plugins/src/rviz_default_plugins/displays/marker/markers/arrow_marker.cpp b/rviz_default_plugins/src/rviz_default_plugins/displays/marker/markers/arrow_marker.cpp index c43bf2e83..519a7e98f 100644 --- a/rviz_default_plugins/src/rviz_default_plugins/displays/marker/markers/arrow_marker.cpp +++ b/rviz_default_plugins/src/rviz_default_plugins/displays/marker/markers/arrow_marker.cpp @@ -103,8 +103,7 @@ void ArrowMarker::onNewMessage( setPosition(pos); setOrientation(orientation); - arrow_->setColor( - new_message->color.r, new_message->color.g, new_message->color.b, new_message->color.a); + arrow_->setColor(rviz_common::colorMsgToOgre(new_message->color)); if (new_message->points.size() == 2) { setArrowFromPoints(new_message); @@ -127,35 +126,23 @@ void ArrowMarker::setArrowFromPoints(const MarkerConstSharedPtr & message) { last_arrow_set_from_points_ = true; - // compute translation & rotation from the two points - Ogre::Vector3 point1 = rviz_common::pointMsgToOgre(message->points[0]); - Ogre::Vector3 point2 = rviz_common::pointMsgToOgre(message->points[1]); - - Ogre::Vector3 direction = point2 - point1; - float distance = direction.length(); - - float head_length_proportion = 0.23f; // see default in arrow.hpp: shaft:head ratio of 1:0.3 - float head_length = head_length_proportion * distance; - if (message->scale.z != 0.0) { - double length = message->scale.z; - head_length = std::max(0.0, std::min(length, distance)); // clamp + Ogre::Vector3 start = rviz_common::pointMsgToOgre(message->points[0]); + Ogre::Vector3 end = rviz_common::pointMsgToOgre(message->points[1]); + arrow_->setEndpoints(start, end); + arrow_->setShaftDiameter(message->scale.x); + arrow_->setHeadDiameter(message->scale.y); + float head_length = std::clamp(message->scale.z, 0.0, arrow_->getLength()); + if (head_length > 0.0) { + arrow_->setShaftHeadRatio(head_length - arrow_->getLength(), head_length); + } else { + arrow_->setShaftHeadRatio(3, 1); // default 3:1 ratio from arrow.hpp } - float shaft_length = distance - head_length; - - arrow_->set(shaft_length, message->scale.x, head_length, message->scale.y); - - direction.normalise(); - - // for some reason the arrow goes into the y direction by default - Ogre::Quaternion orient = Ogre::Vector3::NEGATIVE_UNIT_Z.getRotationTo(direction); // if scale.x and scale.y are 0, then nothing is shown if (owner_ && (message->scale.x + message->scale.y == 0.0f)) { owner_->setMarkerStatus( getID(), rviz_common::properties::StatusProperty::Warn, "Scale of 0 in both x and y"); } - arrow_->setPosition(point1); - arrow_->setOrientation(orient); } void ArrowMarker::setArrow(const MarkerConstSharedPtr & message) @@ -171,14 +158,15 @@ void ArrowMarker::setArrow(const MarkerConstSharedPtr & message) getID(), rviz_common::properties::StatusProperty::Warn, "Scale of 0 in one of x/y/z"); } arrow_->setScale(rviz_common::vector3MsgToOgre(message->scale)); - - Ogre::Quaternion orient = Ogre::Vector3::NEGATIVE_UNIT_Z.getRotationTo(Ogre::Vector3(1, 0, 0)); - arrow_->setOrientation(orient); + arrow_->setDirection(Ogre::Vector3::UNIT_Z); } void ArrowMarker::setDefaultProportions() { - arrow_->set(0.77f, 1.0f, 0.23f, 2.0f); + arrow_->setShaftLength(0.77); + arrow_->setShaftDiameter(1); + arrow_->setHeadLength(0.23); + arrow_->setHeadDiameter(2); } } // namespace markers diff --git a/rviz_default_plugins/src/rviz_default_plugins/displays/marker/markers/arrow_strip_marker.cpp b/rviz_default_plugins/src/rviz_default_plugins/displays/marker/markers/arrow_strip_marker.cpp new file mode 100644 index 000000000..d45e282f5 --- /dev/null +++ b/rviz_default_plugins/src/rviz_default_plugins/displays/marker/markers/arrow_strip_marker.cpp @@ -0,0 +1,125 @@ +/* + * Copyright (c) 2024, Open Source Robotics Foundation, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include "rviz_default_plugins/displays/marker/markers/arrow_strip_marker.hpp" + +#include +#include +#include + +#include +#include +#include +#include +#include + +#include "rviz_rendering/objects/arrow.hpp" +#include "rviz_rendering/objects/shape.hpp" +#include "rviz_common/display_context.hpp" +#include "rviz_common/msg_conversions.hpp" + +#include "rviz_default_plugins/displays/marker/marker_common.hpp" +#include "rviz_default_plugins/displays/marker/markers/marker_selection_handler.hpp" + +namespace rviz_default_plugins +{ +namespace displays +{ +namespace markers +{ +ArrowStripMarker::ArrowStripMarker( + MarkerCommon * owner, rviz_common::DisplayContext * context, + Ogre::SceneNode * parent_node) +: MarkerBase(owner, context, parent_node) +{ +} + +void ArrowStripMarker::onNewMessage( + const MarkerConstSharedPtr & old_message, + const MarkerConstSharedPtr & new_message) +{ + (void) old_message; + + assert(new_message->type == visualization_msgs::msg::Marker::ARROW_STRIP); + + Ogre::Vector3 pos, scale; + Ogre::Quaternion orient; + if (!transform(new_message, pos, orient, scale)) { // NOLINT: is super class method + scene_node_->setVisible(false); + return; + } + + scene_node_->setVisible(true); + setPosition(pos); + setOrientation(orient); + + arrows_.clear(); + + handler_.reset(new MarkerSelectionHandler(this, MarkerID(new_message->ns, new_message->id), + context_)); + if (new_message->points.size() < 2) { + std::string error = "Too few points to define an arrow strip."; + if (owner_) { + owner_->setMarkerStatus(getID(), rviz_common::properties::StatusProperty::Error, error); + } + RVIZ_COMMON_LOG_DEBUG(error); + scene_node_->setVisible(false); + return; + } + + // if scale.x and scale.y are 0, then nothing is shown + if (owner_ && (new_message->scale.x + new_message->scale.y == 0.0f)) { + owner_->setMarkerStatus( + getID(), rviz_common::properties::StatusProperty::Warn, "Scale of 0 in both x and y"); + return; + } + + for (unsigned i = 0; i < new_message->points.size() - 1; i++) { + Ogre::Vector3 start = rviz_common::pointMsgToOgre(new_message->points.at(i)); + Ogre::Vector3 end = rviz_common::pointMsgToOgre(new_message->points.at(i + 1)); + std::unique_ptr arrow = + std::make_unique(context_->getSceneManager(), scene_node_); + arrow->setEndpoints(start, end); + arrow->setShaftDiameter(new_message->scale.x); + arrow->setHeadDiameter(new_message->scale.y); + float head_length = std::clamp(new_message->scale.z, 0.0, arrow->getLength()); + if (head_length > 0.0) { + arrow->setShaftHeadRatio(head_length - arrow->getLength(), head_length); + } else { + arrow->setShaftHeadRatio(3, 1); // default 3:1 ratio from arrow.hpp + } + arrow->setColor(rviz_common::colorMsgToOgre(new_message->color)); + handler_->addTrackedObjects(arrow->getSceneNode()); + arrows_.push_back(std::move(arrow)); + } +} + +} // namespace markers +} // namespace displays +} // namespace rviz_default_plugins \ No newline at end of file diff --git a/rviz_default_plugins/src/rviz_default_plugins/displays/marker/markers/marker_factory.cpp b/rviz_default_plugins/src/rviz_default_plugins/displays/marker/markers/marker_factory.cpp index 3757afa1f..6be7def23 100644 --- a/rviz_default_plugins/src/rviz_default_plugins/displays/marker/markers/marker_factory.cpp +++ b/rviz_default_plugins/src/rviz_default_plugins/displays/marker/markers/marker_factory.cpp @@ -42,6 +42,7 @@ #include "rviz_default_plugins/displays/marker/markers/marker_base.hpp" #include "rviz_default_plugins/displays/marker/markers/arrow_marker.hpp" +#include "rviz_default_plugins/displays/marker/markers/arrow_strip_marker.hpp" #include "rviz_default_plugins/displays/marker/markers/line_list_marker.hpp" #include "rviz_default_plugins/displays/marker/markers/line_strip_marker.hpp" #include "rviz_default_plugins/displays/marker/markers/mesh_resource_marker.hpp" @@ -77,6 +78,8 @@ std::shared_ptr MarkerFactory::createMarkerForType( case visualization_msgs::msg::Marker::ARROW: return std::make_shared(owner_, context_, parent_node_); + case visualization_msgs::msg::Marker::ARROW_STRIP: + return std::make_shared(owner_, context_, parent_node_); case visualization_msgs::msg::Marker::LINE_STRIP: return std::make_shared(owner_, context_, parent_node_); diff --git a/rviz_default_plugins/src/rviz_default_plugins/displays/odometry/odometry_display.cpp b/rviz_default_plugins/src/rviz_default_plugins/displays/odometry/odometry_display.cpp index bbe9c4104..c79cbfbb8 100644 --- a/rviz_default_plugins/src/rviz_default_plugins/displays/odometry/odometry_display.cpp +++ b/rviz_default_plugins/src/rviz_default_plugins/displays/odometry/odometry_display.cpp @@ -209,11 +209,10 @@ void OdometryDisplay::updateAxes(const std::unique_ptr & a void OdometryDisplay::updateArrow(const std::unique_ptr & arrow) { - arrow->set( - shaft_length_property_->getFloat(), - shaft_radius_property_->getFloat(), - head_length_property_->getFloat(), - head_radius_property_->getFloat()); + arrow->setShaftLength(shaft_length_property_->getFloat()); + arrow->setShaftDiameter(shaft_radius_property_->getFloat()); + arrow->setHeadLength(head_length_property_->getFloat()); + arrow->setHeadDiameter(head_radius_property_->getFloat()); } void OdometryDisplay::updateShapeChoice() diff --git a/rviz_default_plugins/src/rviz_default_plugins/displays/path/path_display.cpp b/rviz_default_plugins/src/rviz_default_plugins/displays/path/path_display.cpp index 9569be512..6c5a30e02 100644 --- a/rviz_default_plugins/src/rviz_default_plugins/displays/path/path_display.cpp +++ b/rviz_default_plugins/src/rviz_default_plugins/displays/path/path_display.cpp @@ -329,11 +329,10 @@ void PathDisplay::updatePoseArrowGeometry() { for (auto & arrow_vect : arrow_chain_) { for (auto & arrow : arrow_vect) { - arrow->set( - pose_arrow_shaft_length_property_->getFloat(), - pose_arrow_shaft_diameter_property_->getFloat(), - pose_arrow_head_length_property_->getFloat(), - pose_arrow_head_diameter_property_->getFloat()); + arrow->setShaftLength(pose_arrow_shaft_length_property_->getFloat()); + arrow->setShaftDiameter(pose_arrow_shaft_diameter_property_->getFloat()); + arrow->setHeadLength(pose_arrow_head_length_property_->getFloat()); + arrow->setHeadDiameter(pose_arrow_head_diameter_property_->getFloat()); } } context_->queueRender(); @@ -536,12 +535,10 @@ void PathDisplay::updateArrowMarkers( for (size_t i = 0; i < num_points; ++i) { QColor color = pose_arrow_color_property_->getColor(); arrow_vect[i]->setColor(color.redF(), color.greenF(), color.blueF(), 1.0f); - - arrow_vect[i]->set( - pose_arrow_shaft_length_property_->getFloat(), - pose_arrow_shaft_diameter_property_->getFloat(), - pose_arrow_head_length_property_->getFloat(), - pose_arrow_head_diameter_property_->getFloat()); + arrow_vect[i]->setShaftLength(pose_arrow_shaft_length_property_->getFloat()); + arrow_vect[i]->setShaftDiameter(pose_arrow_shaft_diameter_property_->getFloat()); + arrow_vect[i]->setHeadLength(pose_arrow_head_length_property_->getFloat()); + arrow_vect[i]->setHeadDiameter(pose_arrow_head_diameter_property_->getFloat()); const geometry_msgs::msg::Point & pos = msg->poses[i].pose.position; arrow_vect[i]->setPosition(transform * rviz_common::pointMsgToOgre(pos)); Ogre::Quaternion orientation(rviz_common::quaternionMsgToOgre(msg->poses[i].pose.orientation)); diff --git a/rviz_default_plugins/src/rviz_default_plugins/displays/pose/pose_display.cpp b/rviz_default_plugins/src/rviz_default_plugins/displays/pose/pose_display.cpp index 225f24513..69ce1dac0 100644 --- a/rviz_default_plugins/src/rviz_default_plugins/displays/pose/pose_display.cpp +++ b/rviz_default_plugins/src/rviz_default_plugins/displays/pose/pose_display.cpp @@ -154,11 +154,10 @@ void PoseDisplay::updateColorAndAlpha() void PoseDisplay::updateArrowGeometry() { - arrow_->set( - shaft_length_property_->getFloat(), - shaft_radius_property_->getFloat(), - head_length_property_->getFloat(), - head_radius_property_->getFloat()); + arrow_->setShaftLength(shaft_length_property_->getFloat()); + arrow_->setShaftDiameter(shaft_radius_property_->getFloat()); + arrow_->setHeadLength(head_length_property_->getFloat()); + arrow_->setHeadDiameter(head_radius_property_->getFloat()); context_->queueRender(); } 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 index c6b3066a3..d23d2d886 100644 --- 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 @@ -362,12 +362,10 @@ void PoseArrayDisplay::updateArrow2dGeometry() 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() - ); + arrow->setShaftLength(arrow3d_shaft_length_property_->getFloat()); + arrow->setShaftDiameter(arrow3d_shaft_radius_property_->getFloat()); + arrow->setHeadLength(arrow3d_head_length_property_->getFloat()); + arrow->setHeadDiameter(arrow3d_head_radius_property_->getFloat()); } context_->queueRender(); } diff --git a/rviz_default_plugins/src/rviz_default_plugins/displays/pose_covariance/pose_with_covariance_display.cpp b/rviz_default_plugins/src/rviz_default_plugins/displays/pose_covariance/pose_with_covariance_display.cpp index 734d59881..7e3978221 100644 --- a/rviz_default_plugins/src/rviz_default_plugins/displays/pose_covariance/pose_with_covariance_display.cpp +++ b/rviz_default_plugins/src/rviz_default_plugins/displays/pose_covariance/pose_with_covariance_display.cpp @@ -158,11 +158,10 @@ void PoseWithCovarianceDisplay::updateColorAndAlpha() void PoseWithCovarianceDisplay::updateArrowGeometry() { - arrow_->set( - shaft_length_property_->getFloat(), - shaft_radius_property_->getFloat(), - head_length_property_->getFloat(), - head_radius_property_->getFloat() ); + arrow_->setShaftLength(shaft_length_property_->getFloat()); + arrow_->setShaftDiameter(shaft_radius_property_->getFloat()); + arrow_->setHeadLength(head_length_property_->getFloat()); + arrow_->setHeadDiameter(head_radius_property_->getFloat()); context_->queueRender(); } diff --git a/rviz_default_plugins/src/rviz_default_plugins/displays/tf/frame_info.cpp b/rviz_default_plugins/src/rviz_default_plugins/displays/tf/frame_info.cpp index 1ee114692..1e96513e7 100644 --- a/rviz_default_plugins/src/rviz_default_plugins/displays/tf/frame_info.cpp +++ b/rviz_default_plugins/src/rviz_default_plugins/displays/tf/frame_info.cpp @@ -225,8 +225,10 @@ void FrameInfo::updateParentArrow( float shaft_length = distance - head_length; // aleeper: This was changed from 0.02 and 0.08 to 0.01 and 0.04 // to match proper radius handling in arrow.cpp - parent_arrow_->set(shaft_length, 0.01f * scale, head_length, 0.04f * scale); - + parent_arrow_->setShaftLength(shaft_length); + parent_arrow_->setShaftDiameter(0.01f * scale); + parent_arrow_->setHeadLength(head_length); + parent_arrow_->setHeadDiameter(0.04f * scale); parent_arrow_->setPosition(position); parent_arrow_->setOrientation(orient); } else { diff --git a/rviz_default_plugins/test/rviz_default_plugins/displays/marker/marker_common_test.cpp b/rviz_default_plugins/test/rviz_default_plugins/displays/marker/marker_common_test.cpp index 12aaca5e3..25b592933 100644 --- a/rviz_default_plugins/test/rviz_default_plugins/displays/marker/marker_common_test.cpp +++ b/rviz_default_plugins/test/rviz_default_plugins/displays/marker/marker_common_test.cpp @@ -80,7 +80,7 @@ class MarkerCommonFixture : public DisplayTestFixture visualization_msgs::msg::Marker::SharedPtr createSharedPtrMessage( int32_t action, int32_t type, int id = 0) { - auto marker = createMessageWithPoints(type); + auto marker = createMessageWithTwoPoints(type); marker.action = action; marker.id = id; return std::make_shared(marker); diff --git a/rviz_default_plugins/test/rviz_default_plugins/displays/marker/marker_messages.cpp b/rviz_default_plugins/test/rviz_default_plugins/displays/marker/marker_messages.cpp index a93af2ce6..2097e55bd 100644 --- a/rviz_default_plugins/test/rviz_default_plugins/displays/marker/marker_messages.cpp +++ b/rviz_default_plugins/test/rviz_default_plugins/displays/marker/marker_messages.cpp @@ -105,7 +105,7 @@ visualization_msgs::msg::Marker createDefaultMessage(int32_t type) return marker; } -visualization_msgs::msg::Marker createMessageWithPoints(int32_t type) +visualization_msgs::msg::Marker createMessageWithTwoPoints(int32_t type) { auto marker = createDefaultMessage(type); marker.points.push_back(create_point(2, 0, 0)); @@ -113,9 +113,18 @@ visualization_msgs::msg::Marker createMessageWithPoints(int32_t type) return marker; } +visualization_msgs::msg::Marker createMessageWithThreePoints(int32_t type) +{ + auto marker = createDefaultMessage(type); + marker.points.push_back(create_point(2, 0, 0)); + marker.points.push_back(create_point(1, 1, 0)); + marker.points.push_back(create_point(0, 2, 0)); + return marker; +} + visualization_msgs::msg::Marker createMessageWithColorPerPoint(int32_t type) { - auto marker = createMessageWithPoints(type); + auto marker = createMessageWithTwoPoints(type); marker.colors.push_back(color(1.0f, 0.0f, 0.5f, 0.5f)); marker.colors.push_back(color(0.5f, 0.6f, 0.0f, 0.3f)); return marker; diff --git a/rviz_default_plugins/test/rviz_default_plugins/displays/marker/marker_messages.hpp b/rviz_default_plugins/test/rviz_default_plugins/displays/marker/marker_messages.hpp index 3f62920b5..75becef6b 100644 --- a/rviz_default_plugins/test/rviz_default_plugins/displays/marker/marker_messages.hpp +++ b/rviz_default_plugins/test/rviz_default_plugins/displays/marker/marker_messages.hpp @@ -43,7 +43,9 @@ std_msgs::msg::ColorRGBA color(float r, float g, float b, float a); visualization_msgs::msg::Marker createDefaultMessage(int32_t type); -visualization_msgs::msg::Marker createMessageWithPoints(int32_t type); +visualization_msgs::msg::Marker createMessageWithTwoPoints(int32_t type); + +visualization_msgs::msg::Marker createMessageWithThreePoints(int32_t type); visualization_msgs::msg::Marker createMessageWithColorPerPoint(int32_t type); } // namespace testing 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 c31cd55ef..5d891f63a 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 @@ -65,7 +65,7 @@ TEST_F(MarkersTestFixture, setMessage_makes_the_scene_node_invisible_if_invalid_ TEST_F(MarkersTestFixture, incomplete_message_sets_scene_node_to_not_visible) { marker_ = makeMarker(); - auto incomplete_message = createMessageWithPoints(visualization_msgs::msg::Marker::ARROW); + auto incomplete_message = createMessageWithTwoPoints(visualization_msgs::msg::Marker::ARROW); incomplete_message.points.pop_back(); marker_->setMessage(incomplete_message); @@ -90,7 +90,7 @@ TEST_F(MarkersTestFixture, setMessage_sets_positions_and_orientations_from_point marker_ = makeMarker(); mockValidTransform(); - auto message = createMessageWithPoints(visualization_msgs::msg::Marker::ARROW); + auto message = createMessageWithTwoPoints(visualization_msgs::msg::Marker::ARROW); marker_->setMessage(message); auto first_point = Ogre::Vector3( @@ -115,7 +115,7 @@ TEST_F(MarkersTestFixture, setMessage_ignores_points_if_thery_are_more_than_two) mockValidTransform(); geometry_msgs::msg::Point point; - auto message = createMessageWithPoints(visualization_msgs::msg::Marker::ARROW); + auto message = createMessageWithTwoPoints(visualization_msgs::msg::Marker::ARROW); message.points.push_back(point); marker_->setMessage(message); @@ -128,7 +128,7 @@ TEST_F(MarkersTestFixture, setMessage_ignores_old_message) { marker_ = makeMarker(); mockValidTransform(); - marker_->setMessage(createMessageWithPoints(visualization_msgs::msg::Marker::ARROW)); + marker_->setMessage(createMessageWithTwoPoints(visualization_msgs::msg::Marker::ARROW)); marker_->setMessage(createDefaultMessage(visualization_msgs::msg::Marker::ARROW)); rviz_default_plugins::assertArrowWithTransform( diff --git a/rviz_default_plugins/test/rviz_default_plugins/displays/marker/markers/arrow_strip_marker_test.cpp b/rviz_default_plugins/test/rviz_default_plugins/displays/marker/markers/arrow_strip_marker_test.cpp new file mode 100644 index 000000000..2288269b0 --- /dev/null +++ b/rviz_default_plugins/test/rviz_default_plugins/displays/marker/markers/arrow_strip_marker_test.cpp @@ -0,0 +1,163 @@ +// 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 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 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 "visualization_msgs/msg/marker.hpp" +#include "rviz_rendering/objects/arrow.hpp" +#include "rviz_rendering/objects/shape.hpp" + +#include "rviz_default_plugins/displays/marker/markers/arrow_strip_marker.hpp" + +#include "../../../scene_graph_introspection.hpp" +#include "markers_test_fixture.hpp" +#include "../marker_messages.hpp" + +using namespace ::testing; // NOLINT + +// 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); +const auto default_arrow_scale_ = Ogre::Vector3(1, 0.2f, 0.2f); + +TEST_F(MarkersTestFixture, setMessage_makes_arrow_strip_invisible_if_invalid_transform) { + marker_ = makeMarker(); + EXPECT_CALL(*frame_manager_, transform(_, _, _, _, _)).WillOnce(Return(false)); // NOLINT + + marker_->setMessage(createDefaultMessage(visualization_msgs::msg::Marker::ARROW_STRIP)); + + EXPECT_TRUE(rviz_default_plugins::noArrowsAreVisible(scene_manager_->getRootSceneNode())); +} + +TEST_F(MarkersTestFixture, incomplete_message_sets_arrow_strip_to_not_visible) { + marker_ = makeMarker(); + + auto incomplete_message = createMessageWithTwoPoints(visualization_msgs::msg::Marker::ARROW_STRIP); + incomplete_message.points.pop_back(); + + marker_->setMessage(incomplete_message); + + EXPECT_TRUE(rviz_default_plugins::noArrowsAreVisible(scene_manager_->getRootSceneNode())); +} + +TEST_F(MarkersTestFixture, setMessage_sets_arrow_strip_positions_and_orientations_correctly) { + marker_ = makeMarker(); + mockValidTransform(); + + marker_->setMessage(createDefaultMessage(visualization_msgs::msg::Marker::ARROW_STRIP)); + + EXPECT_THAT(marker_->getPosition(), Vector3Eq(Ogre::Vector3(0, 1, 0))); + EXPECT_THAT(marker_->getOrientation(), QuaternionEq(Ogre::Quaternion(0, 0, 1, 0))); + + rviz_default_plugins::assertArrowWithTransform( + scene_manager_, default_arrow_position_, default_arrow_scale_, default_arrow_orientation_); +} + +TEST_F(MarkersTestFixture, setMessage_sets_arrow_strip_positions_and_orientations_from_two_points_correctly) { + marker_ = makeMarker(); + mockValidTransform(); + + auto message = createMessageWithTwoPoints(visualization_msgs::msg::Marker::ARROW_STRIP); + marker_->setMessage(message); + + auto p1 = Ogre::Vector3( + message.points[0].x, message.points[0].y, message.points[0].z); + auto p2 = Ogre::Vector3( + message.points[1].x, message.points[1].y, message.points[1].z); + + auto direction = p2 - p1; + direction.normalise(); + + auto expected_arrow_orientation = Ogre::Vector3::NEGATIVE_UNIT_Z.getRotationTo(direction) * + Ogre::Quaternion(Ogre::Degree(-90), Ogre::Vector3::UNIT_X); + + Ogre::Vector3 expected_arrow_scale(1, 1, 1); + + EXPECT_THAT(marker_->getPosition(), Vector3Eq(Ogre::Vector3(0, 1, 0))); + EXPECT_THAT(marker_->getOrientation(), QuaternionEq(Ogre::Quaternion(0, 0, 1, 0))); + + rviz_default_plugins::assertArrowWithTransform( + scene_manager_, p1, expected_arrow_scale, expected_arrow_orientation); +} + +TEST_F(MarkersTestFixture, setMessage_sets_arrow_strip_positions_and_orientations_from_three_points_correctly) { + marker_ = makeMarker(); + mockValidTransform(); + + auto message = createMessageWithThreePoints(visualization_msgs::msg::Marker::ARROW_STRIP); + marker_->setMessage(message); + + auto p1 = Ogre::Vector3( + message.points[0].x, message.points[0].y, message.points[0].z); + auto p2 = Ogre::Vector3( + message.points[1].x, message.points[1].y, message.points[1].z); + auto p3 = Ogre::Vector3( + message.points[2].x, message.points[2].y, message.points[2].z); + + auto d1 = p2 - p1; + auto d2 = p3 - p2; + + d1.normalise(); + d2.normalise(); + + auto q1 = Ogre::Vector3::NEGATIVE_UNIT_Z.getRotationTo(d1) * + Ogre::Quaternion(Ogre::Degree(-90), Ogre::Vector3::UNIT_X); + auto q2 = Ogre::Vector3::NEGATIVE_UNIT_Z.getRotationTo(d2) * + Ogre::Quaternion(Ogre::Degree(-90), Ogre::Vector3::UNIT_X); + + Ogre::Vector3 expected_arrow_scale(1, 1, 1); + + EXPECT_THAT(marker_->getPosition(), Vector3Eq(Ogre::Vector3(0, 1, 0))); + EXPECT_THAT(marker_->getOrientation(), QuaternionEq(Ogre::Quaternion(0, 0, 1, 0))); + + std::array expected_arrow_points = {p1, p2}; + std::array expected_arrow_orientations = {q1, q2}; + std::array expected_arrow_scales = {expected_arrow_scale, expected_arrow_scale}; + + rviz_default_plugins::assertArrowsWithTransforms<2>( + scene_manager_, expected_arrow_points, expected_arrow_scales, expected_arrow_orientations); +} + +TEST_F(MarkersTestFixture, setMessage_arrow_strip_ignores_old_message) { + marker_ = makeMarker(); + mockValidTransform(); + + marker_->setMessage(createMessageWithTwoPoints(visualization_msgs::msg::Marker::ARROW_STRIP)); + marker_->setMessage(createDefaultMessage(visualization_msgs::msg::Marker::ARROW_STRIP)); + + 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/marker/markers/points_marker_test.cpp b/rviz_default_plugins/test/rviz_default_plugins/displays/marker/markers/points_marker_test.cpp index 6920b9c4a..bc20ad2c4 100644 --- a/rviz_default_plugins/test/rviz_default_plugins/displays/marker/markers/points_marker_test.cpp +++ b/rviz_default_plugins/test/rviz_default_plugins/displays/marker/markers/points_marker_test.cpp @@ -56,7 +56,7 @@ TEST_F(MarkersTestFixture, setMessage_sets_points_correctly) { marker_ = makeMarker(); mockValidTransform(); - marker_->setMessage(createMessageWithPoints(visualization_msgs::msg::Marker::SPHERE_LIST)); + marker_->setMessage(createMessageWithTwoPoints(visualization_msgs::msg::Marker::SPHERE_LIST)); auto point_cloud = rviz_default_plugins::findOnePointCloud(scene_manager_->getRootSceneNode()); float expected_bounding_radius = 2.236068f; @@ -68,7 +68,7 @@ TEST_F(MarkersTestFixture, setMessage_sets_position_and_orientation_correctly) { marker_ = makeMarker(); mockValidTransform(); - marker_->setMessage(createMessageWithPoints(visualization_msgs::msg::Marker::CUBE_LIST)); + marker_->setMessage(createMessageWithTwoPoints(visualization_msgs::msg::Marker::CUBE_LIST)); EXPECT_THAT(marker_->getPosition(), Vector3Eq(Ogre::Vector3(0, 1, 0))); EXPECT_THAT(marker_->getOrientation(), QuaternionEq(Ogre::Quaternion(0, 0, 1, 0))); @@ -78,7 +78,7 @@ TEST_F(MarkersTestFixture, setMessage_sets_single_color_correctly) { marker_ = makeMarker(); mockValidTransform(); - marker_->setMessage(createMessageWithPoints(visualization_msgs::msg::Marker::SPHERE_LIST)); + marker_->setMessage(createMessageWithTwoPoints(visualization_msgs::msg::Marker::SPHERE_LIST)); auto point_cloud = rviz_default_plugins::findOnePointCloud(scene_manager_->getRootSceneNode()); Ogre::ColourValue expected_color(0.0f, 1.0f, 1.0f, 1.0f); 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 3dc8c742d..f96f7056c 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 @@ -82,6 +82,23 @@ bool arrowIsVisible(Ogre::SceneNode * scene_node) return arrow_head->isVisible() && arrow_shaft->isVisible(); } +bool noArrowsAreVisible(Ogre::SceneNode * scene_node) +{ + auto arrow_heads = findAllEntitiesByMeshName(scene_node, "rviz_cone.mesh"); + auto arrow_shafts = findAllEntitiesByMeshName(scene_node, "rviz_cylinder.mesh"); + for (const auto & arrow_head : arrow_heads) { + if (arrow_head->isVisible()) { + return false; + } + } + for (const auto & arrow_shaft : arrow_shafts) { + if (arrow_shaft->isVisible()) { + return false; + } + } + return true; +} + std::vector findAllEntitiesByMeshName( Ogre::SceneNode * scene_node, const Ogre::String & resource_name) { 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 4bf69bc26..804af1cb5 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 @@ -78,9 +78,30 @@ bool axesAreVisible(Ogre::SceneNode * scene_node); bool arrowIsVisible(Ogre::SceneNode * scene_manager); +bool noArrowsAreVisible(Ogre::SceneNode * scene_manager); + std::vector findAllArrows(Ogre::SceneNode * scene_node); Ogre::SceneNode * findOneArrow(Ogre::SceneNode * scene_node); +template +void assertArrowsWithTransforms( + Ogre::SceneManager * scene_manager, + std::array positions, + std::array scales, + std::array orientations +) { + auto arrow_scene_nodes = findAllArrows(scene_manager->getRootSceneNode()); + ASSERT_EQ(arrow_scene_nodes.size(), N); + for (unsigned i = 0; i < N; i++) { + auto arrow_node = arrow_scene_nodes.at(i); + ASSERT_TRUE(arrow_node); + EXPECT_THAT(arrow_node->getPosition(), Vector3Eq(positions.at(i))); + // Have to mangle the scale because of the default orientation of the cylinders (see arrow.cpp). + EXPECT_THAT(arrow_node->getScale(), Vector3Eq(Ogre::Vector3(scales.at(i).z, scales.at(i).x, scales.at(i).y))); + EXPECT_THAT(arrow_node->getOrientation(), QuaternionEq(orientations.at(i))); + } +} + std::vector findAllAxes(Ogre::SceneNode * scene_node); Ogre::SceneNode * findOneAxes(Ogre::SceneNode * scene_node); diff --git a/rviz_rendering/include/rviz_rendering/objects/arrow.hpp b/rviz_rendering/include/rviz_rendering/objects/arrow.hpp index b01683c41..6ee45fd0a 100644 --- a/rviz_rendering/include/rviz_rendering/objects/arrow.hpp +++ b/rviz_rendering/include/rviz_rendering/objects/arrow.hpp @@ -67,7 +67,8 @@ class RVIZ_RENDERING_PUBLIC Arrow : public Object * \brief Constructor * * @param scene_manager The scene manager to use to construct any necessary objects - * @param parent_node A scene node to use as the parent of this object. If NULL, uses the root scene node. + * @param parent_node A scene node to use as the parent of this object. If NULL, uses the root + scene node. * @param shaft_length Length of the arrow's shaft * @param shaft_diameter Diameter of the arrow's shaft * @param head_length Length of the arrow's head @@ -79,6 +80,25 @@ class RVIZ_RENDERING_PUBLIC Arrow : public Object float head_length = 0.3f, float head_diameter = 0.2f); virtual ~Arrow(); + /** + * \brief Sets the position, direction, and length of this arrow from two endpoints. + * @param start Vector to the start of the arrow. + * @param end Vector to the end of the arrow. + */ + void setEndpoints(const Ogre::Vector3 & start, const Ogre::Vector3 & end); + + /** + * \brief Gets the total length of this arrow. + * @return The combined length of the shaft and head. + */ + float getLength(); + + /** + * \brief Sets the total length of this arrow whilst keeping the shaft and head proportions constant. + * @param length The combined length of the shaft and head. + */ + void setLength(float length); + /** * \brief Set the parameters for this arrow * @@ -89,6 +109,38 @@ class RVIZ_RENDERING_PUBLIC Arrow : public Object */ void set(float shaft_length, float shaft_diameter, float head_length, float head_diameter); + /** + * \brief Sets the shaft and head proportions whilst keeping the total length constant. + * @param shaft_weight Unnormalized weight given to the shaft of this arrow. + * @param head_weight Unnormalized weight given to the head of this arrow. + */ + void setShaftHeadRatio(float shaft_weight, float head_weight); + + /** + * \brief Set the size of this arrow's shaft. + * @param shaft_length Length of the arrow's shaft. + * @param shaft_diameter Diameter of the arrow's shaft. + */ + void setShaftLength(float shaft_length); + + /** + * \brief Set the diameter of this arrow's shaft. + * @param shaft_diameter Diameter of the arrow's shaft. + */ + void setShaftDiameter(float shaft_diameter); + + /** + * \brief Set the length of this arrow's head. + * @param head_length Length of the arrow's head. + */ + void setHeadLength(float head_length); + + /** + * \brief Set the size of this arrow's head. + * @param head_diameter Diameter of the arrow's head. + */ + void setHeadDiameter(float head_diameter); + /** * \brief Set the color of this arrow. Sets both the head and shaft color to the same value. Values are in the range [0, 1] * diff --git a/rviz_rendering/src/rviz_rendering/objects/arrow.cpp b/rviz_rendering/src/rviz_rendering/objects/arrow.cpp index 2be5293d2..11285925a 100644 --- a/rviz_rendering/src/rviz_rendering/objects/arrow.cpp +++ b/rviz_rendering/src/rviz_rendering/objects/arrow.cpp @@ -58,9 +58,10 @@ Arrow::Arrow( shaft_ = new Shape(Shape::Cylinder, scene_manager_, scene_node_); head_ = new Shape(Shape::Cone, scene_manager_, scene_node_); head_->setOffset(Ogre::Vector3(0.0f, 0.5f, 0.0f)); - - set(shaft_length, shaft_diameter, head_length, head_diameter); - + setShaftLength(shaft_length); + setShaftDiameter(shaft_diameter); + setHeadLength(head_length); + setHeadDiameter(head_diameter); setOrientation(Ogre::Quaternion::IDENTITY); } @@ -72,13 +73,73 @@ Arrow::~Arrow() scene_manager_->destroySceneNode(scene_node_); } +void Arrow::setEndpoints(const Ogre::Vector3 & start, const Ogre::Vector3 & end) +{ + Ogre::Vector3 direction = end - start; + setPosition(start); + setDirection(direction.normalisedCopy()); + setLength(direction.length()); +} + +float Arrow::getLength() +{ + float shaft_length = shaft_->getRootNode()->getScale().y; + float head_length = head_->getRootNode()->getScale().y; + return shaft_length + head_length; +} + +void Arrow::setLength(float length) +{ + float shaft_length = shaft_->getRootNode()->getScale().y; + float head_length = head_->getRootNode()->getScale().y; + float shaft_proportion = shaft_length / (shaft_length + head_length); + float head_proportion = 1.0 - shaft_proportion; + setShaftLength(length * shaft_proportion); + setHeadLength(length * head_proportion); +} + void Arrow::set(float shaft_length, float shaft_diameter, float head_length, float head_diameter) { - shaft_->setScale(Ogre::Vector3(shaft_diameter, shaft_length, shaft_diameter)); - shaft_->setPosition(Ogre::Vector3(0.0f, shaft_length / 2.0f, 0.0f) ); + setShaftLength(shaft_length); + setShaftDiameter(shaft_diameter); + setHeadLength(head_length); + setHeadDiameter(head_diameter); +} - head_->setScale(Ogre::Vector3(head_diameter, head_length, head_diameter) ); - head_->setPosition(Ogre::Vector3(0.0f, shaft_length, 0.0f) ); +void Arrow::setShaftHeadRatio(float shaft_weight, float head_weight) +{ + float shaft_proportion = shaft_weight / (shaft_weight + head_weight); + float head_proportion = 1.0 - shaft_proportion; + float shaft_length = shaft_->getRootNode()->getScale().y; + float head_length = head_->getRootNode()->getScale().y; + setShaftLength((shaft_length + head_length) * shaft_proportion); + setHeadLength((shaft_length + head_length) * head_proportion); +} + +void Arrow::setShaftLength(float shaft_length) +{ + Ogre::Vector3 scale = shaft_->getRootNode()->getScale(); + shaft_->setScale(Ogre::Vector3(scale.x, shaft_length, scale.z)); + shaft_->setPosition(Ogre::Vector3(0.0f, shaft_length / 2.0f, 0.0f)); + head_->setPosition(Ogre::Vector3(0.0f, shaft_length, 0.0f)); +} + +void Arrow::setShaftDiameter(float shaft_diameter) +{ + Ogre::Vector3 scale = shaft_->getRootNode()->getScale(); + shaft_->setScale(Ogre::Vector3(shaft_diameter, scale.y, shaft_diameter)); +} + +void Arrow::setHeadLength(float head_length) +{ + Ogre::Vector3 scale = head_->getRootNode()->getScale(); + head_->setScale(Ogre::Vector3(scale.x, head_length, scale.z)); +} + +void Arrow::setHeadDiameter(float head_diameter) +{ + Ogre::Vector3 scale = head_->getRootNode()->getScale(); + head_->setScale(Ogre::Vector3(head_diameter, scale.y, head_diameter)); } void Arrow::setColor(const Ogre::ColourValue & c) diff --git a/rviz_rendering/src/rviz_rendering/objects/effort_visual.cpp b/rviz_rendering/src/rviz_rendering/objects/effort_visual.cpp index 11248b89a..f3ce3987e 100644 --- a/rviz_rendering/src/rviz_rendering/objects/effort_visual.cpp +++ b/rviz_rendering/src/rviz_rendering/objects/effort_visual.cpp @@ -112,7 +112,10 @@ void EffortVisual::setEffort(const std::string & joint_name, double effort, doub effort_value = static_cast(fabs(effort) + 0.05f); } - effort_arrow_[joint_name]->set(0, width_ * 2.0f, width_ * 2.0f * 1.0f, width_ * 2.0f * 2.0f); + effort_arrow_[joint_name]->setShaftLength(0); + effort_arrow_[joint_name]->setShaftDiameter(width_ * 2.0f); + effort_arrow_[joint_name]->setHeadLength(width_ * 2.0f); + effort_arrow_[joint_name]->setHeadDiameter(width_ * 4.0f); if (effort > 0) { effort_arrow_[joint_name]->setDirection(orientation_[joint_name] * Ogre::Vector3(-1, 0, 0)); } else { diff --git a/rviz_rendering/src/rviz_rendering/objects/screw_visual.cpp b/rviz_rendering/src/rviz_rendering/objects/screw_visual.cpp index bae139f71..6655bcd43 100644 --- a/rviz_rendering/src/rviz_rendering/objects/screw_visual.cpp +++ b/rviz_rendering/src/rviz_rendering/objects/screw_visual.cpp @@ -92,7 +92,10 @@ void ScrewVisual::setScrew(const Ogre::Vector3 & linear, const Ogre::Vector3 & a orientation = Ogre::Quaternion::IDENTITY; } // circle_arrow_angular_->setScale(Ogre::Vector3(width_, width_, 0.05)); - circle_arrow_angular_->set(0, width_ * 0.1f, width_ * 0.1f * 1.0f, width_ * 0.1f * 2.0f); + circle_arrow_angular_->setShaftLength(0); + circle_arrow_angular_->setShaftDiameter(width_ * 0.1f); + circle_arrow_angular_->setHeadLength(width_ * 0.1f); + circle_arrow_angular_->setHeadDiameter(width_ * 0.2f); circle_arrow_angular_->setDirection(orientation * Ogre::Vector3(0, 1, 0)); circle_arrow_angular_->setPosition( orientation * diff --git a/rviz_rendering/src/rviz_rendering/objects/wrench_visual.cpp b/rviz_rendering/src/rviz_rendering/objects/wrench_visual.cpp index 678b6120e..be0ee996d 100644 --- a/rviz_rendering/src/rviz_rendering/objects/wrench_visual.cpp +++ b/rviz_rendering/src/rviz_rendering/objects/wrench_visual.cpp @@ -121,7 +121,10 @@ Ogre::Quaternion WrenchVisual::getDirectionOfRotationRelativeToTorque( 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_->setShaftLength(0); + circle_arrow_torque_->setShaftDiameter(width_ * 0.1f); + circle_arrow_torque_->setHeadLength(width_ * 0.1f); + circle_arrow_torque_->setHeadDiameter(width_ * 0.2f); 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));