Feature/add arrow strip marker

rviz_common/include/rviz_common/msg_conversions.hpp

@@ -32,7 +32,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"
@@ -57,6 +59,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;
@@ -78,6 +85,12 @@ 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

@@ -151,6 +151,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
@@ -415,6 +416,7 @@ if(BUILD_TESTING)
 
   ament_add_gmock(marker_test
     test/rviz_default_plugins/displays/marker/markers/arrow_marker_test.cpp
+    # TODO: Add arrow strip marker test
     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,40 @@
+#ifndef RVIZ_DEFAULT_PLUGINS__DISPLAYS__MARKER__MARKERS__ARROW_STRIP_MARKER_HPP_
+#define RVIZ_DEFAULT_PLUGINS__DISPLAYS__MARKER__MARKERS__ARROW_STRIP_MARKER_HPP_
+
+#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/src/rviz_default_plugins/displays/interactive_markers/interactive_marker_control.cpp

@@ -57,6 +57,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"
@@ -122,7 +123,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

@@ -102,8 +102,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);
@@ -126,35 +125,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)
@@ -170,14 +157,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,90 @@
+#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

@@ -45,6 +45,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"
@@ -80,6 +81,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

@@ -208,11 +208,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()

rviz_default_plugins/src/rviz_default_plugins/displays/path/path_display.cpp

@@ -328,11 +328,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();
@@ -535,12 +534,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));

rviz_default_plugins/src/rviz_default_plugins/displays/pose/pose_display.cpp

@@ -153,11 +153,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();
 }
 

rviz_default_plugins/src/rviz_default_plugins/displays/pose_array/pose_array_display.cpp

@@ -360,12 +360,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();
 }

rviz_default_plugins/src/rviz_default_plugins/displays/pose_covariance/pose_with_covariance_display.cpp

@@ -157,11 +157,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();
 }