Feature/add arrow strip marker

rviz_common/include/rviz_common/msg_conversions.hpp

@@ -33,7 +33,9 @@
 
 #include <OgreVector.h>
 #include <OgreQuaternion.h>
+#include <OgreColourValue.h>
 
+#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_

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

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 <memory>
+#include <vector>
+
+#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<std::unique_ptr<rviz_rendering::Arrow>> arrows_;
+};
+
+}  // namespace markers
+}  // namespace displays
+}  // namespace rviz_default_plugins
+
+#endif // RVIZ_DEFAULT_PLUGINS__DISPLAYS__MARKER__MARKERS__ARROW_STRIP_MARKER_HPP_

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_));

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<double>(0.0, std::min<double>(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<float>(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

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 <algorithm>
+#include <memory>
+#include <string>
+
+#include <OgreEntity.h>
+#include <OgreQuaternion.h>
+#include <OgreSceneNode.h>
+#include <OgreSceneManager.h>
+#include <OgreVector.h>
+
+#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<rviz_rendering::Arrow> arrow =
+      std::make_unique<rviz_rendering::Arrow>(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<float>(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

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<MarkerBase> MarkerFactory::createMarkerForType(
     case visualization_msgs::msg::Marker::ARROW:
       return std::make_shared<ArrowMarker>(owner_, context_, parent_node_);
 
+    case visualization_msgs::msg::Marker::ARROW_STRIP:
+      return std::make_shared<ArrowStripMarker>(owner_, context_, parent_node_);
     case visualization_msgs::msg::Marker::LINE_STRIP:
       return std::make_shared<LineStripMarker>(owner_, context_, parent_node_);
 

rviz_default_plugins/src/rviz_default_plugins/displays/odometry/odometry_display.cpp

@@ -209,11 +209,10 @@ void OdometryDisplay::updateAxes(const std::unique_ptr<rviz_rendering::Axes> & a
 
 void OdometryDisplay::updateArrow(const std::unique_ptr<rviz_rendering::Arrow> & 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()