feat(objects_of_interest_marker_interface): add name text marker (aut…

…owarefoundation#5690) Signed-off-by: Fumiya Watanabe <[email protected]>
YoshiRi · Nov 29, 2023 · 7b45fab · 7b45fab
1 parent 97b59d9
commit 7b45fab
Show file tree

Hide file tree

Showing 4 changed files with 45 additions and 4 deletions.
diff --git a/...f_interest_marker_interface/include/objects_of_interest_marker_interface/marker_utils.hpp b/...f_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 <tier4_autoware_utils/geometry/boost_polygon_utils.hpp>
@@ -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/...ace/include/objects_of_interest_marker_interface/objects_of_interest_marker_interface.hpp b/...ace/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<visualization_msgs::msg::MarkerArray>::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
@@ -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
@@ -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());
   }