refactor(interest_objects_marker_interface): fix interface

Signed-off-by: Fumiya Watanabe <[email protected]>

planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp

@@ -319,8 +319,10 @@ void LaneChangeInterface::setObjectDebugVisualizationTmp(const bool is_approved)
   const auto debug_data =
     is_approved ? module_type_->getAfterApprovalDebugData() : module_type_->getDebugData();
   for (const auto & [uuid, data] : debug_data) {
-    interest_objects_marker_interface_.insertObjectStatus(
-      data.current_obj_pose, data.obj_shape, data.is_safe);
+    const auto color = data.is_safe ? interest_objects_marker_interface::ColorName::GREEN
+                                    : interest_objects_marker_interface::ColorName::RED;
+    interest_objects_marker_interface_.insertObjectData(
+      data.current_obj_pose, data.obj_shape, color);
   }
   return;
 }

planning/interest_objects_marker_interface/CMakeLists.txt

@@ -13,6 +13,7 @@ find_package(ament_cmake_auto REQUIRED)
 ament_auto_find_build_dependencies()
 
 ament_auto_add_library(interest_objects_marker_interface SHARED
+  src/coloring.cpp
   src/interest_objects_marker_interface.cpp
 )
 

planning/interest_objects_marker_interface/include/interest_objects_marker_interface/coloring.hpp

@@ -0,0 +1,31 @@
+// Copyright 2023 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef INTEREST_OBJECTS_MARKER_INTERFACE__COLORING_HPP_
+#define INTEREST_OBJECTS_MARKER_INTERFACE__COLORING_HPP_
+#include "interest_objects_marker_interface/marker_data.hpp"
+
+#include <tier4_autoware_utils/ros/marker_helper.hpp>
+
+#include <std_msgs/msg/color_rgba.hpp>
+
+namespace interest_objects_marker_interface::coloring
+{
+std_msgs::msg::ColorRGBA getGreen(const float alpha);
+std_msgs::msg::ColorRGBA getAmber(const float alpha);
+std_msgs::msg::ColorRGBA getRed(const float alpha);
+std_msgs::msg::ColorRGBA getWhite(const float alpha);
+}  // namespace interest_objects_marker_interface::coloring
+
+#endif  // INTEREST_OBJECTS_MARKER_INTERFACE__COLORING_HPP_

planning/interest_objects_marker_interface/include/interest_objects_marker_interface/interest_objects_marker_interface.hpp

@@ -14,47 +14,45 @@
 
 #ifndef INTEREST_OBJECTS_MARKER_INTERFACE__INTEREST_OBJECTS_MARKER_INTERFACE_HPP_
 #define INTEREST_OBJECTS_MARKER_INTERFACE__INTEREST_OBJECTS_MARKER_INTERFACE_HPP_
-#include "rclcpp/rclcpp.hpp"
+#include "interest_objects_marker_interface/coloring.hpp"
+#include "interest_objects_marker_interface/marker_data.hpp"
 
+#include <rclcpp/rclcpp.hpp>
 #include <tier4_autoware_utils/geometry/boost_polygon_utils.hpp>
 #include <tier4_autoware_utils/ros/marker_helper.hpp>
 
 #include <autoware_auto_perception_msgs/msg/predicted_object.hpp>
 #include <geometry_msgs/msg/pose.hpp>
+#include <std_msgs/msg/color_rgba.hpp>
 #include <visualization_msgs/msg/marker_array.hpp>
 
-#include <mutex>
 #include <string>
 #include <vector>
 
 namespace interest_objects_marker_interface
 {
 using autoware_auto_perception_msgs::msg::Shape;
 using geometry_msgs::msg::Pose;
+using std_msgs::msg::ColorRGBA;
 using tier4_autoware_utils::Polygon2d;
 using visualization_msgs::msg::Marker;
 using visualization_msgs::msg::MarkerArray;
 
-struct ObjectStatus
-{
-  Pose pose{};
-  Shape shape{};
-  bool safe{false};
-};
-
 class InterestObjectsMarkerInterface
 {
 public:
   InterestObjectsMarkerInterface(rclcpp::Node * node, const std::string & name);
-  void insertObjectStatus(const Pose & pose, const Shape & shape, const bool safe);
+  void insertObjectData(const Pose & pose, const Shape & shape, const ColorName & color_name);
+  void insertObjectDataWithColor(const Pose & pose, const Shape & shape, const ColorRGBA & color);
   void publishMarkerArray();
   void setHeightOffset(const double offset);
+  static ColorRGBA getColor(const ColorName & color_name, const float alpha = 0.99f);
 
 private:
   rclcpp::Publisher<MarkerArray>::SharedPtr pub_marker_;
 
   double height_offset_{0.0};
-  std::vector<ObjectStatus> obj_status_array_;
+  std::vector<ObjectMarkerData> obj_marker_data_array_;
 
   std::string name_;
   std::string topic_namespace_ = "/planning/debug/interest_objects_marker";

planning/interest_objects_marker_interface/include/interest_objects_marker_interface/marker_data.hpp

@@ -0,0 +1,34 @@
+// Copyright 2023 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef INTEREST_OBJECTS_MARKER_INTERFACE__MARKER_DATA_HPP_
+#define INTEREST_OBJECTS_MARKER_INTERFACE__MARKER_DATA_HPP_
+
+#include <autoware_auto_perception_msgs/msg/predicted_object.hpp>
+#include <geometry_msgs/msg/pose.hpp>
+#include <std_msgs/msg/color_rgba.hpp>
+
+namespace interest_objects_marker_interface
+{
+struct ObjectMarkerData
+{
+  geometry_msgs::msg::Pose pose{};
+  autoware_auto_perception_msgs::msg::Shape shape{};
+  std_msgs::msg::ColorRGBA color;
+};
+
+enum class ColorName { GREEN, AMBER, RED, WHITE };
+}  // namespace interest_objects_marker_interface
+
+#endif  // INTEREST_OBJECTS_MARKER_INTERFACE__MARKER_DATA_HPP_

planning/interest_objects_marker_interface/src/coloring.cpp

@@ -0,0 +1,50 @@
+// Copyright 2023 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "interest_objects_marker_interface/coloring.hpp"
+
+namespace interest_objects_marker_interface::coloring
+{
+std_msgs::msg::ColorRGBA getGreen(const float alpha)
+{
+  const float r = 0.0f;
+  const float g = 211.0 / 255.0;
+  const float b = 141.0 / 255.0;
+  return tier4_autoware_utils::createMarkerColor(r, g, b, alpha);
+}
+
+std_msgs::msg::ColorRGBA getAmber(const float alpha)
+{
+  const float r = 0.0f;
+  const float g = 211.0 / 255.0;
+  const float b = 141.0 / 255.0;
+  return tier4_autoware_utils::createMarkerColor(r, g, b, alpha);
+}
+
+std_msgs::msg::ColorRGBA getRed(const float alpha)
+{
+  const float r = 214.0 / 255.0;
+  const float g = 0.0f;
+  const float b = 77.0 / 255.0;
+  return tier4_autoware_utils::createMarkerColor(r, g, b, alpha);
+}
+
+std_msgs::msg::ColorRGBA getWhite(const float alpha)
+{
+  const float r = 1.0f;
+  const float g = 1.0f;
+  const float b = 1.0f;
+  return tier4_autoware_utils::createMarkerColor(r, g, b, alpha);
+}
+}  // namespace interest_objects_marker_interface::coloring

planning/interest_objects_marker_interface/src/interest_objects_marker_interface.cpp

@@ -18,23 +18,20 @@
 #include <tier4_autoware_utils/math/constants.hpp>
 #include <tier4_autoware_utils/math/trigonometry.hpp>
 
-#include <std_msgs/msg/color_rgba.hpp>
-
 namespace
 {
 using geometry_msgs::msg::Point;
 using geometry_msgs::msg::Pose;
 using geometry_msgs::msg::Transform;
 using geometry_msgs::msg::Vector3;
-using interest_objects_marker_interface::ObjectStatus;
+using interest_objects_marker_interface::ObjectMarkerData;
 using std_msgs::msg::ColorRGBA;
 using tier4_autoware_utils::Polygon2d;
 using visualization_msgs::msg::Marker;
 using visualization_msgs::msg::MarkerArray;
 
 using tier4_autoware_utils::calcAzimuthAngle;
 using tier4_autoware_utils::createDefaultMarker;
-using tier4_autoware_utils::createMarkerColor;
 using tier4_autoware_utils::createMarkerScale;
 using tier4_autoware_utils::createQuaternionFromRPY;
 using tier4_autoware_utils::createTranslation;
@@ -44,28 +41,20 @@ using tier4_autoware_utils::getRPY;
 using tier4_autoware_utils::pi;
 using tier4_autoware_utils::sin;
 
-ColorRGBA getColor(const bool safe, const float alpha)
-{
-  const float r = safe ? 0.0f : 214.0 / 255.0;
-  const float g = safe ? 211.0 / 255.0 : 0.0f;
-  const float b = safe ? 141.0 / 255.0 : 77.0 / 255.0;
-  return createMarkerColor(r, g, b, alpha);
-}
-
 Marker createArrowMarker(
-  const size_t & id, const ObjectStatus & status, const std::string & name,
+  const size_t & id, const ObjectMarkerData & data, const std::string & name,
   const double height_offset, const double arrow_length = 1.0)
 {
   Marker marker = createDefaultMarker(
     "map", rclcpp::Clock{RCL_ROS_TIME}.now(), name, id, Marker::ARROW,
-    createMarkerScale(0.25, 0.5, 0.5), getColor(status.safe, 0.95f));
+    createMarkerScale(0.25, 0.5, 0.5), data.color);
 
-  const double height = 0.5 * status.shape.dimensions.z;
+  const double height = 0.5 * data.shape.dimensions.z;
 
   Point src, dst;
-  src = status.pose.position;
+  src = data.pose.position;
   src.z += height + height_offset + arrow_length;
-  dst = status.pose.position;
+  dst = data.pose.position;
   dst.z += height + height_offset;
 
   marker.points.push_back(src);
@@ -75,42 +64,40 @@ Marker createArrowMarker(
 }
 
 Marker createCircleMarker(
-  const size_t & id, const ObjectStatus & status, const std::string & name, const double radius,
+  const size_t & id, const ObjectMarkerData & data, const std::string & name, const double radius,
   const double height_offset)
 {
   Marker marker = createDefaultMarker(
     "map", rclcpp::Clock{RCL_ROS_TIME}.now(), name, id, Marker::LINE_STRIP,
-    createMarkerScale(0.1, 0.0, 0.0), getColor(status.safe, 0.951f));
+    createMarkerScale(0.1, 0.0, 0.0), data.color);
 
-  const double height = 0.5 * status.shape.dimensions.z;
+  const double height = 0.5 * data.shape.dimensions.z;
 
   constexpr size_t num_points = 20;
-  std::vector<Point> points;
-  points.resize(num_points);
-
   for (size_t i = 0; i < num_points; ++i) {
+    Point point;
     const double ratio = static_cast<double>(i) / static_cast<double>(num_points);
     const double theta = 2 * pi * ratio;
-    points.at(i).x = status.pose.position.x + radius * cos(theta);
-    points.at(i).y = status.pose.position.y + radius * sin(theta);
-    points.at(i).z = status.pose.position.z + height + height_offset;
-    marker.points.push_back(points.at(i));
+    point.x = data.pose.position.x + radius * cos(theta);
+    point.y = data.pose.position.y + radius * sin(theta);
+    point.z = data.pose.position.z + height + height_offset;
+    marker.points.push_back(point);
   }
-  marker.points.push_back(points.front());
+  marker.points.push_back(marker.points.front());
 
   return marker;
 }
 
 MarkerArray createTargetMarker(
-  const size_t & id, const ObjectStatus & status, const std::string & name,
+  const size_t & id, const ObjectMarkerData & data, const std::string & name,
   const double height_offset)
 {
   MarkerArray marker_array;
-  marker_array.markers.push_back(createArrowMarker(id, status, name + "_arrow", height_offset));
+  marker_array.markers.push_back(createArrowMarker(id, data, name + "_arrow", height_offset));
   marker_array.markers.push_back(
-    createCircleMarker(id, status, name + "_circle1", 0.5, height_offset + 0.75));
+    createCircleMarker(id, data, name + "_circle1", 0.5, height_offset + 0.75));
   marker_array.markers.push_back(
-    createCircleMarker(id, status, name + "_circle2", 0.75, height_offset + 0.75));
+    createCircleMarker(id, data, name + "_circle2", 0.75, height_offset + 0.75));
 
   return marker_array;
 }
@@ -128,34 +115,56 @@ InterestObjectsMarkerInterface::InterestObjectsMarkerInterface(
   pub_marker_ = node->create_publisher<MarkerArray>(topic_namespace_ + "/" + name, 1);
 }
 
-void InterestObjectsMarkerInterface::insertObjectStatus(
-  const Pose & pose, const Shape & shape, const bool safe)
+void InterestObjectsMarkerInterface::insertObjectData(
+  const Pose & pose, const Shape & shape, const ColorName & color_name)
+{
+  insertObjectDataWithColor(pose, shape, getColor(color_name));
+}
+
+void InterestObjectsMarkerInterface::insertObjectDataWithColor(
+  const Pose & pose, const Shape & shape, const ColorRGBA & color)
 {
-  ObjectStatus status;
-  status.pose = pose;
-  status.shape = shape;
-  status.safe = safe;
+  ObjectMarkerData data;
+  data.pose = pose;
+  data.shape = shape;
+  data.color = color;
 
-  obj_status_array_.push_back(status);
+  obj_marker_data_array_.push_back(data);
 }
 
 void InterestObjectsMarkerInterface::publishMarkerArray()
 {
   MarkerArray marker_array;
-  for (size_t i = 0; i < obj_status_array_.size(); ++i) {
-    const auto obj = obj_status_array_.at(i);
-    const MarkerArray target_marker = createTargetMarker(i, obj, name_, height_offset_);
+  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 = createTargetMarker(i, data, name_, height_offset_);
     marker_array.markers.insert(
       marker_array.markers.end(), target_marker.markers.begin(), target_marker.markers.end());
   }
 
   pub_marker_->publish(marker_array);
-  obj_status_array_.clear();
+  obj_marker_data_array_.clear();
 }
 
 void InterestObjectsMarkerInterface::setHeightOffset(const double offset)
 {
   height_offset_ = offset;
 }
 
+ColorRGBA InterestObjectsMarkerInterface::getColor(const ColorName & color_name, const float alpha)
+{
+  switch (color_name) {
+    case ColorName::GREEN:
+      return coloring::getGreen(alpha);
+    case ColorName::AMBER:
+      return coloring::getAmber(alpha);
+    case ColorName::RED:
+      return coloring::getRed(alpha);
+    case ColorName::WHITE:
+      return coloring::getWhite(alpha);
+    default:
+      return coloring::getWhite(alpha);
+  }
+}
+
 }  // namespace interest_objects_marker_interface