diff --git a/.github/workflows/abi.yaml b/.github/workflows/abi.yaml index a002ba967a..da410fa9fa 100644 --- a/.github/workflows/abi.yaml +++ b/.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' }} diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index 56c9c84481..a6c0f0768c 100644 --- a/.github/workflows/ci.yaml +++ b/.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 diff --git a/src/rviz/default_plugin/CMakeLists.txt b/src/rviz/default_plugin/CMakeLists.txt index c6ebc4076a..c940f8af49 100644 --- a/src/rviz/default_plugin/CMakeLists.txt +++ b/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 diff --git a/src/rviz/default_plugin/marker_utils.cpp b/src/rviz/default_plugin/marker_utils.cpp index 03048d2650..759f3c2d9b 100644 --- a/src/rviz/default_plugin/marker_utils.cpp +++ b/src/rviz/default_plugin/marker_utils.cpp @@ -30,6 +30,7 @@ #include "marker_utils.h" #include #include +#include #include #include #include @@ -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: diff --git a/src/rviz/default_plugin/markers/arrow_strip_marker.cpp b/src/rviz/default_plugin/markers/arrow_strip_marker.cpp new file mode 100644 index 0000000000..6679351096 --- /dev/null +++ b/src/rviz/default_plugin/markers/arrow_strip_marker.cpp @@ -0,0 +1,96 @@ +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include + +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(0.0, std::min(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 diff --git a/src/rviz/default_plugin/markers/arrow_strip_marker.h b/src/rviz/default_plugin/markers/arrow_strip_marker.h new file mode 100644 index 0000000000..f3fb7703ae --- /dev/null +++ b/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 arrows_; +}; +} // namespace rviz + +#endif diff --git a/src/test/send_arrow_strip_marker.py b/src/test/send_arrow_strip_marker.py new file mode 100755 index 0000000000..8cf33139d5 --- /dev/null +++ b/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)