From d99e56b3968d01233c0367a824dac154154beb9c Mon Sep 17 00:00:00 2001 From: Fumiya Watanabe Date: Mon, 27 Nov 2023 17:45:46 +0900 Subject: [PATCH] feat(objects_of_interest_marker_interface): add name text marker (#5690) Signed-off-by: Fumiya Watanabe --- .../marker_utils.hpp | 13 ++++++++++ .../objects_of_interest_marker_interface.hpp | 10 ++++++++ .../src/marker_utils.cpp | 24 ++++++++++++++++--- .../objects_of_interest_marker_interface.cpp | 2 +- 4 files changed, 45 insertions(+), 4 deletions(-) diff --git a/planning/objects_of_interest_marker_interface/include/objects_of_interest_marker_interface/marker_utils.hpp b/planning/objects_of_interest_marker_interface/include/objects_of_interest_marker_interface/marker_utils.hpp index cff45e5c1a9eb..c1fae5ecc4bec 100644 --- a/planning/objects_of_interest_marker_interface/include/objects_of_interest_marker_interface/marker_utils.hpp +++ b/planning/objects_of_interest_marker_interface/include/objects_of_interest_marker_interface/marker_utils.hpp @@ -14,6 +14,7 @@ #ifndef OBJECTS_OF_INTEREST_MARKER_INTERFACE__MARKER_UTILS_HPP_ #define OBJECTS_OF_INTEREST_MARKER_INTERFACE__MARKER_UTILS_HPP_ +#include "objects_of_interest_marker_interface/coloring.hpp" #include "objects_of_interest_marker_interface/marker_data.hpp" #include @@ -56,6 +57,18 @@ visualization_msgs::msg::Marker createCircleMarker( const size_t id, const ObjectMarkerData & data, const std::string & name, const double radius, const double height_offset, const double line_width = 0.1); +/** + * @brief Create text marker visualizing module name + * @param id Marker id + * @param data Object marker data + * @param name Module name + * @param height_offset Height offset of target marker + * @param text_size Text size + */ +visualization_msgs::msg::Marker createNameTextMarker( + const size_t id, const ObjectMarkerData & data, const std::string & name, + const double height_offset, const double text_size = 0.5); + /** * @brief Create target marker from object marker data * @param id Marker id diff --git a/planning/objects_of_interest_marker_interface/include/objects_of_interest_marker_interface/objects_of_interest_marker_interface.hpp b/planning/objects_of_interest_marker_interface/include/objects_of_interest_marker_interface/objects_of_interest_marker_interface.hpp index 79bab6d5faf69..d5a19fede4fc2 100644 --- a/planning/objects_of_interest_marker_interface/include/objects_of_interest_marker_interface/objects_of_interest_marker_interface.hpp +++ b/planning/objects_of_interest_marker_interface/include/objects_of_interest_marker_interface/objects_of_interest_marker_interface.hpp @@ -78,6 +78,16 @@ class ObjectsOfInterestMarkerInterface */ static std_msgs::msg::ColorRGBA getColor(const ColorName & color_name, const float alpha = 0.99f); + /** + * @brief Get module name including this interface + */ + std::string getName() const { return name_; } + + /** + * @brief Get height offset + */ + double getHeightOffset() const { return height_offset_; } + private: rclcpp::Publisher::SharedPtr pub_marker_; diff --git a/planning/objects_of_interest_marker_interface/src/marker_utils.cpp b/planning/objects_of_interest_marker_interface/src/marker_utils.cpp index 71414eae4dbe7..23f33c0b8115a 100644 --- a/planning/objects_of_interest_marker_interface/src/marker_utils.cpp +++ b/planning/objects_of_interest_marker_interface/src/marker_utils.cpp @@ -35,7 +35,7 @@ Marker createArrowMarker( const double head_height = 0.5 * arrow_length; Marker marker = createDefaultMarker( - "map", rclcpp::Clock{RCL_ROS_TIME}.now(), name, id, Marker::ARROW, + "map", rclcpp::Clock{RCL_ROS_TIME}.now(), name + "_arrow", id, Marker::ARROW, createMarkerScale(line_width, head_width, head_height), data.color); const double height = 0.5 * data.shape.dimensions.z; @@ -77,19 +77,37 @@ Marker createCircleMarker( return marker; } +visualization_msgs::msg::Marker createNameTextMarker( + const size_t id, const ObjectMarkerData & data, const std::string & name, + const double height_offset, const double text_size) +{ + Marker marker = createDefaultMarker( + "map", rclcpp::Clock{RCL_ROS_TIME}.now(), name + "_name_text", id, Marker::TEXT_VIEW_FACING, + createMarkerScale(0.0, 0.0, text_size), coloring::getGray(data.color.a)); + + marker.text = name; + + const double height = 0.5 * data.shape.dimensions.z; + marker.pose = data.pose; + marker.pose.position.z += height + height_offset; + + return marker; +} + MarkerArray createTargetMarker( const size_t id, const ObjectMarkerData & data, const std::string & name, const double height_offset, const double arrow_length, const double line_width) { MarkerArray marker_array; - marker_array.markers.push_back( - createArrowMarker(id, data, name + "_arrow", height_offset, arrow_length)); + marker_array.markers.push_back(createArrowMarker(id, data, name, height_offset, arrow_length)); marker_array.markers.push_back(createCircleMarker( id, data, name + "_circle1", 0.5 * arrow_length, height_offset + 0.75 * arrow_length, line_width)); marker_array.markers.push_back(createCircleMarker( id, data, name + "_circle2", 0.75 * arrow_length, height_offset + 0.75 * arrow_length, line_width)); + marker_array.markers.push_back( + createNameTextMarker(id, data, name, height_offset + 1.5 * arrow_length, 0.5 * arrow_length)); return marker_array; } diff --git a/planning/objects_of_interest_marker_interface/src/objects_of_interest_marker_interface.cpp b/planning/objects_of_interest_marker_interface/src/objects_of_interest_marker_interface.cpp index 6261b4791be48..03e71193cc5a5 100644 --- a/planning/objects_of_interest_marker_interface/src/objects_of_interest_marker_interface.cpp +++ b/planning/objects_of_interest_marker_interface/src/objects_of_interest_marker_interface.cpp @@ -57,7 +57,7 @@ void ObjectsOfInterestMarkerInterface::publishMarkerArray() for (size_t i = 0; i < obj_marker_data_array_.size(); ++i) { const auto data = obj_marker_data_array_.at(i); const MarkerArray target_marker = - marker_utils::createTargetMarker(i, data, name_, height_offset_); + marker_utils::createTargetMarker(i, data, getName(), getHeightOffset()); marker_array.markers.insert( marker_array.markers.end(), target_marker.markers.begin(), target_marker.markers.end()); }