feat(interest_objects_marker_interface): change marker type

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

planning/behavior_path_planner/src/marker_utils/utils.cpp

@@ -18,6 +18,7 @@
 #include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp"
 #include "behavior_path_planner/utils/path_utils.hpp"
 
+#include <tier4_autoware_utils/geometry/boost_polygon_utils.hpp>
 #include <tier4_autoware_utils/ros/marker_helper.hpp>
 #include <tier4_autoware_utils/ros/uuid_helper.hpp>
 
@@ -45,6 +46,7 @@ CollisionCheckDebugPair createObjectDebug(const ExtendedPredictedObject & obj)
 {
   CollisionCheckDebug debug;
   debug.current_obj_pose = obj.initial_pose.pose;
+  debug.extended_obj_polygon = tier4_autoware_utils::toPolygon2d(obj.initial_pose.pose, obj.shape);
   debug.current_twist = obj.initial_twist.twist;
   return {tier4_autoware_utils::toHexString(obj.uuid), debug};
 }

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

@@ -315,7 +315,8 @@ void LaneChangeInterface::setObjectDebugVisualizationTmp()
 {
   const auto debug_data = module_type_->getDebugData();
   for (const auto & [uuid, data] : debug_data) {
-    interest_objects_marker_interface_.insertObjectStatus(data.current_obj_pose, 1.5, data.is_safe);
+    interest_objects_marker_interface_.insertObjectStatus(
+      data.current_obj_pose, data.extended_obj_polygon, 1.5, data.is_safe);
   }
   return;
 }

planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp

@@ -318,6 +318,8 @@ std::vector<Polygon2d> getCollidedPolygons(
     const auto interpolated_data = getInterpolatedPoseWithVelocityAndPolygonStamped(
       predicted_ego_path, current_time, ego_vehicle_info);
     if (!interpolated_data) {
+      debug.expected_obj_pose = obj_pose;
+      debug.extended_obj_polygon = obj_polygon;
       continue;
     }
     const auto & ego_pose = interpolated_data->pose;

planning/interest_objects_marker_interface/include/interest_objects_marker_interface/interest_objects_marker_interface.hpp

@@ -16,6 +16,7 @@
 #define INTEREST_OBJECTS_MARKER_INTERFACE__INTEREST_OBJECTS_MARKER_INTERFACE_HPP_
 #include "rclcpp/rclcpp.hpp"
 
+#include <tier4_autoware_utils/geometry/boost_polygon_utils.hpp>
 #include <tier4_autoware_utils/ros/marker_helper.hpp>
 
 #include <geometry_msgs/msg/pose.hpp>
@@ -28,12 +29,14 @@
 namespace interest_objects_marker_interface
 {
 using geometry_msgs::msg::Pose;
+using tier4_autoware_utils::Polygon2d;
 using visualization_msgs::msg::Marker;
 using visualization_msgs::msg::MarkerArray;
 
 struct ObjectStatus
 {
   Pose pose{};
+  Polygon2d polygon{};
   double height{0.0};
   bool safe{false};
 };
@@ -42,7 +45,8 @@ class InterestObjectsMarkerInterface
 {
 public:
   InterestObjectsMarkerInterface(rclcpp::Node * node, const std::string & name);
-  void insertObjectStatus(const Pose & pose, const double obj_height, const bool safe);
+  void insertObjectStatus(
+    const Pose & pose, const Polygon2d & polygon, const double obj_height, const bool safe);
   void publishMarkerArray();
 
 private:
@@ -52,8 +56,6 @@ class InterestObjectsMarkerInterface
 
   std::string name_;
   std::string topic_namespace_ = "/planning/interest_objects_marker";
-
-  mutable std::mutex mutex_;
 };
 
 }  // namespace interest_objects_marker_interface

planning/interest_objects_marker_interface/src/interest_objects_marker_interface.cpp

@@ -13,38 +13,134 @@
 // limitations under the License.
 
 #include "interest_objects_marker_interface/interest_objects_marker_interface.hpp"
+
+#include <tier4_autoware_utils/geometry/geometry.hpp>
+#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 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::createQuaternion;
+using tier4_autoware_utils::createTranslation;
 
-Marker createArrowMarker(const size_t & id, const ObjectStatus & status, const std::string & name)
+using tier4_autoware_utils::cos;
+using tier4_autoware_utils::getRPY;
+using tier4_autoware_utils::pi;
+using tier4_autoware_utils::sin;
+
+ColorRGBA getColor(const bool safe)
 {
-  const float r = status.safe ? 0.0f : 0.75f;
-  const float g = status.safe ? 0.75f : 0.0f;
-  const float b = 0.0f;
+  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, 0.95);
+}
+
+double calcAzimuthAngleFromPolygon(const Polygon2d & polygon)
+{
+  const size_t polygon_size = polygon.outer().size();
+
+  const auto polygon_from = polygon.outer().at(polygon_size - 2);
+  Point p_from;
+  p_from.x = polygon_from.x();
+  p_from.y = polygon_from.y();
+  p_from.z = 0.0;
+
+  const auto polygon_to = polygon.outer().back();
+  Point p_to;
+  p_to.x = polygon_to.x();
+  p_to.y = polygon_to.y();
+  p_to.z = 0.0;
+
+  return calcAzimuthAngle(p_from, p_to);
+}
+
+Marker createCircleMarker(
+  const size_t & id, const ObjectStatus & status, const std::string & name, const double radius)
+{
+  if (status.polygon.outer().size() < 2) {
+    return Marker{};
+  }
+
+  const auto azimuth = calcAzimuthAngleFromPolygon(status.polygon);
 
   Marker marker = createDefaultMarker(
-    "map", rclcpp::Clock{RCL_ROS_TIME}.now(), name, id, Marker::ARROW,
-    createMarkerScale(0.25, 0.5, 0.5), createMarkerColor(r, g, b, 0.999));
+    "map", rclcpp::Clock{RCL_ROS_TIME}.now(), name + "_circle", id, Marker::LINE_STRIP,
+    createMarkerScale(0.25, 0.0, 0.0), getColor(status.safe));
 
-  Point src, dst;
-  src = status.pose.position;
-  src.z += status.height + 1.0;
-  dst = status.pose.position;
-  dst.z += status.height;
+  constexpr size_t num_points = 20;
+  std::vector<Point> points;
+  points.resize(num_points);
 
-  marker.points.push_back(src);
-  marker.points.push_back(dst);
+  for (size_t i = 0; i < num_points; ++i) {
+    const double ratio = static_cast<double>(i) / static_cast<double>(num_points);
+    const double theta = 2 * pi * ratio + azimuth;
+    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;
+    marker.points.push_back(points.at(i));
+  }
+  marker.points.push_back(points.front());
 
   return marker;
 }
+
+MarkerArray createCrosshairMarker(
+  const size_t & id, const ObjectStatus & status, const std::string & name, const double radius)
+{
+  MarkerArray marker_array;
+
+  if (status.polygon.outer().size() < 2) {
+    return marker_array;
+  }
+
+  const auto azimuth = calcAzimuthAngleFromPolygon(status.polygon);
+
+  marker_array.markers.push_back(createCircleMarker(id, status, name, radius));
+
+  for (size_t i = 0; i < 8; ++i) {
+    const double scale = (i % 2 == 0) ? 0.25 : 0.1;
+    const double radius_in = (i % 2 == 0) ? radius * 0.5 : radius * 0.75;
+    const double theta = 2 * pi * static_cast<double>(i) / 8.0 + azimuth;
+
+    Marker marker = createDefaultMarker(
+      "map", rclcpp::Clock{RCL_ROS_TIME}.now(), name + "_cross_" + std::to_string(i), id,
+      Marker::LINE_STRIP, createMarkerScale(scale, 0.0, 0.0), getColor(status.safe));
+
+    Point src;
+    src.x = status.pose.position.x + radius * cos(theta);
+    src.y = status.pose.position.y + radius * sin(theta);
+    src.z = status.pose.position.z;
+    marker.points.push_back(src);
+
+    Point dst;
+    dst.x = status.pose.position.x + radius_in * cos(theta);
+    dst.y = status.pose.position.y + radius_in * sin(theta);
+    dst.z = status.pose.position.z;
+    marker.points.push_back(dst);
+
+    marker_array.markers.push_back(marker);
+  }
+
+  return marker_array;
+}
+
 }  // namespace
 
 namespace interest_objects_marker_interface
@@ -59,10 +155,11 @@ InterestObjectsMarkerInterface::InterestObjectsMarkerInterface(
 }
 
 void InterestObjectsMarkerInterface::insertObjectStatus(
-  const Pose & pose, const double obj_height, const bool safe)
+  const Pose & pose, const Polygon2d & polygon, double obj_height, const bool safe)
 {
   ObjectStatus status;
   status.pose = pose;
+  status.polygon = polygon;
   status.height = obj_height;
   status.safe = safe;
 
@@ -74,8 +171,11 @@ 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 Marker marker = createArrowMarker(i, obj, name_);
-    marker_array.markers.push_back(marker);
+    const double radius = 2.5;
+    const MarkerArray crosshair_markers = createCrosshairMarker(i, obj, name_, radius);
+    marker_array.markers.insert(
+      marker_array.markers.end(), crosshair_markers.markers.begin(),
+      crosshair_markers.markers.end());
   }
 
   pub_marker_->publish(marker_array);