Feature/add arrow strip marker

.github/workflows/abi.yaml

@@ -15,7 +15,7 @@ jobs:
         distro: [noetic]
 
     env:
-      UPSTREAM_WORKSPACE: github:rhaschke/python_qt_binding#silent-external-warnings
+      UPSTREAM_WORKSPACE: github:rhaschke/python_qt_binding#silent-external-warnings github:rr-tom-noble/common_msgs#feature/add-arrow-strip-marker-type
       CCACHE_DIR: ${{ github.workspace }}/.ccache
       BASEDIR: /home/runner/work
       DOCKER_IMAGE: rhaschke/ici:rviz-${{ matrix.distro }}-${{ matrix.repo || 'ros' }}

.github/workflows/ci.yaml

@@ -23,7 +23,7 @@ jobs:
 
     env:
       CXXFLAGS: "-Werror -Wall -Wextra -Wwrite-strings -Wunreachable-code -Wpointer-arith -Wredundant-decls -Wno-strict-aliasing -Wno-sign-compare"
-      UPSTREAM_WORKSPACE: github:rhaschke/python_qt_binding#silent-external-warnings
+      UPSTREAM_WORKSPACE: github:rhaschke/python_qt_binding#silent-external-warnings github:rr-tom-noble/common_msgs#feature/add-arrow-strip-marker-type
       AFTER_INSTALL_TARGET_DEPENDENCIES: apt install -qq -y libogre-${{ matrix.ogre }}-dev
       CATKIN_LINT: true
       CCACHE_DIR: ${{ github.workspace }}/.ccache

src/rviz/default_plugin/CMakeLists.txt

@@ -22,6 +22,7 @@ set(SOURCE_FILES
   marker_display.cpp
   marker_utils.cpp
   markers/arrow_marker.cpp
+  markers/arrow_strip_marker.cpp
   markers/line_list_marker.cpp
   markers/line_strip_marker.cpp
   markers/marker_base.cpp

src/rviz/default_plugin/marker_utils.cpp

@@ -30,6 +30,7 @@
 #include "marker_utils.h"
 #include <rviz/default_plugin/markers/shape_marker.h>
 #include <rviz/default_plugin/markers/arrow_marker.h>
+#include <rviz/default_plugin/markers/arrow_strip_marker.h>
 #include <rviz/default_plugin/markers/line_list_marker.h>
 #include <rviz/default_plugin/markers/line_strip_marker.h>
 #include <rviz/default_plugin/marker_display.h>
@@ -68,6 +69,9 @@ createMarker(int marker_type, MarkerDisplay* owner, DisplayContext* context, Ogr
   case visualization_msgs::Marker::LINE_LIST:
     return new rviz::LineListMarker(owner, context, parent_node);
 
+  case visualization_msgs::Marker::ARROW_STRIP:
+    return new rviz::ArrowStripMarker(owner, context, parent_node);
+
   case visualization_msgs::Marker::SPHERE_LIST:
   case visualization_msgs::Marker::CUBE_LIST:
   case visualization_msgs::Marker::POINTS:
@@ -109,6 +113,8 @@ QString getMarkerTypeName(unsigned int type)
     return "Line Strip";
   case visualization_msgs::Marker::LINE_LIST:
     return "Line List";
+  case visualization_msgs::Marker::ARROW_STRIP:
+    return "Arrow Strip";
   case visualization_msgs::Marker::POINTS:
     return "Points";
   case visualization_msgs::Marker::TEXT_VIEW_FACING:
@@ -312,6 +318,14 @@ void checkPointsNotEmpty(const visualization_msgs::Marker& marker,
       increaseLevel(::ros::console::levels::Error, level);
     }
     break;
+  case visualization_msgs::Marker::ARROW_STRIP:
+    if (marker.points.size() <= 1)
+    {
+      addSeparatorIfRequired(ss);
+      ss << "At least two points are required for an ARROW_STRIP marker.";
+      increaseLevel(::ros::console::levels::Error, level);
+    }
+    break;
   default:
     break;
   }
@@ -459,7 +473,15 @@ bool checkMarkerMsg(const visualization_msgs::Marker& marker, MarkerDisplay* own
     checkTextEmpty(marker, ss, level);
     checkMeshEmpty(marker, ss, level);
     break;
-
+  case visualization_msgs::Marker::ARROW_STRIP:
+    checkQuaternion(marker, ss, level);
+    checkScale(marker, ss, level);
+    checkColor(marker, ss, level);
+    checkPointsNotEmpty(marker, ss, level);
+    checkColorsEmpty(marker, ss, level);
+    checkTextEmpty(marker, ss, level);
+    checkMeshEmpty(marker, ss, level);
+    break;
   case visualization_msgs::Marker::SPHERE_LIST:
   case visualization_msgs::Marker::CUBE_LIST:
   case visualization_msgs::Marker::TRIANGLE_LIST:

src/rviz/default_plugin/markers/arrow_strip_marker.cpp

@@ -0,0 +1,96 @@
+#include <rviz/ogre_helpers/ogre_vector.h>
+#include <OgreQuaternion.h>
+#include <OgreSceneNode.h>
+#include <OgreSceneManager.h>
+#include <OgreEntity.h>
+
+#include <rviz/default_plugin/marker_display.h>
+#include <rviz/default_plugin/markers/marker_selection_handler.h>
+#include <rviz/display_context.h>
+#include <rviz/ogre_helpers/arrow.h>
+#include <rviz/ogre_helpers/shape.h>
+#include <rviz/validate_floats.h>
+
+#include <rviz/default_plugin/markers/arrow_strip_marker.h>
+
+namespace rviz
+{
+ArrowStripMarker::ArrowStripMarker(MarkerDisplay* owner,
+                                   DisplayContext* context,
+                                   Ogre::SceneNode* parent_node)
+  : MarkerBase(owner, context, parent_node)
+{
+}
+
+ArrowStripMarker::~ArrowStripMarker()
+{
+  for (Arrow* arrow : arrows_)
+  {
+    delete arrow;
+  }
+}
+
+void ArrowStripMarker::onNewMessage(const MarkerConstPtr& /*old_message*/,
+                                    const MarkerConstPtr& new_message)
+{
+  ROS_ASSERT(new_message->type == visualization_msgs::Marker::ARROW_STRIP);
+
+  Ogre::Vector3 pos, scale;
+  Ogre::Quaternion orient;
+  if (!transform(new_message, pos, orient, scale))
+  {
+    scene_node_->setVisible(false);
+    return;
+  }
+
+  scene_node_->setVisible(true);
+  setPosition(pos);
+  setOrientation(orient);
+
+  for (Arrow* arrow : arrows_)
+  {
+    delete arrow;
+    arrows_.clear();
+  }
+
+  handler_.reset(new MarkerSelectionHandler(this, MarkerID(new_message->ns, new_message->id), context_));
+  if (new_message->points.size() < 2)
+  {
+    return;
+  }
+
+  geometry_msgs::Point p = new_message->points.at(0);
+  Ogre::Vector3 arrow_start(p.x, p.y, p.z);
+  for (int i = 1; i < new_message->points.size(); i++)
+  {
+    p = new_message->points.at(i);
+    Ogre::Vector3 arrow_end(p.x, p.y, p.z);
+    if (!validateFloats(p))
+    {
+      ROS_WARN("Marker '%s/%d': invalid point[%d] (%.2f, %.2f, %.2f)", new_message->ns.c_str(),
+               new_message->id, i, p.x, p.y, p.z);
+      continue;
+    }
+
+    arrows_.push_back(new Arrow(context_->getSceneManager(), scene_node_));
+    Ogre::Vector3 direction = arrow_end - arrow_start;
+    float distance = direction.length();
+    float head_length_proportion = 0.23;
+    float head_length = head_length_proportion * distance;
+    if (new_message->scale.z != 0.0)
+    {
+      float length = new_message->scale.z;
+      head_length = std::max<double>(0.0, std::min<double>(length, distance)); // clamp
+    }
+    float shaft_length = distance - head_length;
+    arrows_.back()->set(shaft_length, new_message->scale.x, head_length, new_message->scale.y);
+    arrows_.back()->setPosition(arrow_start);
+    arrows_.back()->setDirection(direction.normalisedCopy());
+    arrows_.back()->setColor(new_message->color.r, new_message->color.g, new_message->color.b,
+                             new_message->color.a);
+    handler_->addTrackedObjects(arrows_.back()->getSceneNode());
+    arrow_start = arrow_end;
+  }
+}
+
+} // namespace rviz

src/rviz/default_plugin/markers/arrow_strip_marker.h

@@ -0,0 +1,29 @@
+#ifndef RVIZ_ARROW_STRIP_MARKER_H
+#define RVIZ_ARROW_STRIP_MARKER_H
+
+#include "marker_base.h"
+
+namespace Ogre
+{
+class SceneNode;
+}
+
+namespace rviz
+{
+class Arrow;
+class DisplayContext;
+
+class ArrowStripMarker : public MarkerBase
+{
+public:
+  ArrowStripMarker(MarkerDisplay* owner, DisplayContext* context, Ogre::SceneNode* parent_node);
+  ~ArrowStripMarker();
+
+protected:
+  void onNewMessage(const MarkerConstPtr& old_message, const MarkerConstPtr& new_message) override;
+
+  std::vector<Arrow*> arrows_;
+};
+} // namespace rviz
+
+#endif

src/test/send_arrow_strip_marker.py

@@ -0,0 +1,66 @@
+#!/usr/bin/env python
+
+import rospy
+import math
+from visualization_msgs.msg import Marker
+from geometry_msgs.msg import Pose, Point, Vector3, Quaternion
+from std_msgs.msg import ColorRGBA
+
+
+FRAME_ID = "map"
+rospy.init_node("marker_test")
+marker_pub = rospy.Publisher("marker_test", Marker, queue_size=1)
+
+
+def generate_arrows(points, pose, scale, color):
+    m = Marker()
+    m.action = Marker.ADD
+    m.header.frame_id = FRAME_ID
+    m.header.stamp = rospy.Time.now()
+    m.ns = "arrow_strip"
+    m.id = 0
+    m.type = Marker.ARROW_STRIP
+    m.points = points
+    m.pose = pose
+    m.scale = scale
+    m.color = color
+    return m
+
+
+def generate_circle(radius, samples):
+    points = []
+    angle_step = (2 * math.pi) / samples
+    for i in range(samples):
+        x = radius * math.sin(angle_step * i)
+        y = radius * math.cos(angle_step * i)
+        points.append(Point(x, y, 0))
+    points.append(points[0])
+    points.append(Point())
+    return points
+
+
+def to_quaternion(axis, angle):
+    return Quaternion(
+        axis[0] * math.sin(angle / 2),
+        axis[1] * math.sin(angle / 2),
+        axis[2] * math.sin(angle / 2),
+        math.cos(angle / 2),
+    )
+
+
+points = generate_circle(radius=1, samples=50)
+arrows = generate_arrows(
+    points=points,
+    pose=Pose(Point(0, 0, 0), Quaternion(0, 0, 0, 1)),
+    scale=Vector3(0.01, 0.05, 0.05),
+    color=ColorRGBA(1.0, 0, 0, 1.0),
+)
+angle = 0
+
+while not rospy.is_shutdown():
+    angle = (angle + 1) % 360
+    arrows.pose.orientation = to_quaternion(
+        [1, 0, 0], (float)(angle) * (math.pi / 180.0)
+    )
+    marker_pub.publish(arrows)
+    rospy.sleep(0.05)