feat(static_obstacle_avoidance): show markers when system requests op…

…erator support (autowarefoundation#7994) Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>
tier4 · Jul 29, 2024 · 2dc8253 · 2dc8253
1 parent dc058fe
commit 2dc8253
Show file tree

Hide file tree

Showing 3 changed files with 80 additions and 1 deletion.
diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/debug.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/debug.hpp
@@ -47,6 +47,9 @@ MarkerArray createTargetObjectsMarkerArray(const ObjectDataArray & objects, cons
 MarkerArray createOtherObjectsMarkerArray(
   const ObjectDataArray & objects, const ObjectInfo & info, const bool verbose);
 
+MarkerArray createAmbiguousObjectsMarkerArray(
+  const ObjectDataArray & objects, const Pose & ego_pose, const std::string & policy);
+
 MarkerArray createStopTargetObjectMarkerArray(const AvoidancePlanningData & data);
 
 MarkerArray createDebugMarkerArray(

diff --git a/planning/behavior_path_avoidance_module/src/debug.cpp b/planning/behavior_path_avoidance_module/src/debug.cpp
@@ -456,7 +456,7 @@ MarkerArray createOtherObjectsMarkerArray(
   appendMarkerArray(
     createObjectsCubeMarkerArray(
       filtered_objects, "others_" + ns + "_cube", createMarkerScale(3.0, 1.5, 1.5),
-      createMarkerColor(0.0, 1.0, 0.0, 0.8)),
+      createMarkerColor(0.5, 0.5, 0.5, 0.8)),
     &msg);
   appendMarkerArray(
     createObjectInfoMarkerArray(filtered_objects, "others_" + ns + "_info", verbose), &msg);
@@ -466,6 +466,77 @@ MarkerArray createOtherObjectsMarkerArray(
   return msg;
 }
 
+MarkerArray createAmbiguousObjectsMarkerArray(
+  const ObjectDataArray & objects, const Pose & ego_pose, const std::string & policy)
+{
+  MarkerArray msg;
+
+  if (policy != "manual") {
+    return msg;
+  }
+
+  for (const auto & object : objects) {
+    if (!object.is_ambiguous || !object.is_avoidable) {
+      continue;
+    }
+
+    {
+      auto marker = createDefaultMarker(
+        "map", rclcpp::Clock{RCL_ROS_TIME}.now(), "ambiguous_target", 0L, Marker::ARROW,
+        createMarkerScale(0.5, 1.0, 1.0), createMarkerColor(1.0, 1.0, 0.0, 0.999));
+
+      Point src, dst;
+      src = object.getPosition();
+      src.z += 4.0;
+      dst = object.getPosition();
+      dst.z += 2.0;
+
+      marker.points.push_back(src);
+      marker.points.push_back(dst);
+      marker.id = uuidToInt32(object.object.object_id);
+
+      msg.markers.push_back(marker);
+    }
+
+    {
+      auto marker = createDefaultMarker(
+        "map", rclcpp::Clock{RCL_ROS_TIME}.now(), "ambiguous_target_text", 0L,
+        Marker::TEXT_VIEW_FACING, createMarkerScale(0.5, 0.5, 0.5),
+        createMarkerColor(1.0, 1.0, 0.0, 1.0));
+
+      marker.id = uuidToInt32(object.object.object_id);
+      marker.pose = object.getPose();
+      marker.pose.position.z += 4.5;
+      std::ostringstream string_stream;
+      string_stream << "SHOULD AVOID?";
+      marker.text = string_stream.str();
+      marker.color = createMarkerColor(1.0, 1.0, 0.0, 0.999);
+      marker.scale = createMarkerScale(0.8, 0.8, 0.8);
+      msg.markers.push_back(marker);
+    }
+
+    {
+      auto marker = createDefaultMarker(
+        "map", rclcpp::Clock{RCL_ROS_TIME}.now(), "request_text", 0L, Marker::TEXT_VIEW_FACING,
+        createMarkerScale(0.5, 0.5, 0.5), createMarkerColor(1.0, 1.0, 0.0, 1.0));
+
+      marker.id = uuidToInt32(object.object.object_id);
+      marker.pose = ego_pose;
+      marker.pose.position.z += 2.0;
+      std::ostringstream string_stream;
+      string_stream << "SYSTEM REQUESTS OPERATOR SUPPORT.";
+      marker.text = string_stream.str();
+      marker.color = createMarkerColor(1.0, 1.0, 0.0, 0.999);
+      marker.scale = createMarkerScale(0.8, 0.8, 0.8);
+      msg.markers.push_back(marker);
+    }
+
+    return msg;
+  }
+
+  return msg;
+}
+
 MarkerArray createStopTargetObjectMarkerArray(const AvoidancePlanningData & data)
 {
   MarkerArray msg;

diff --git a/planning/behavior_path_avoidance_module/src/scene.cpp b/planning/behavior_path_avoidance_module/src/scene.cpp
@@ -1385,13 +1385,18 @@ void AvoidanceModule::updateRTCData()
 
 void AvoidanceModule::updateInfoMarker(const AvoidancePlanningData & data) const
 {
+  using utils::avoidance::createAmbiguousObjectsMarkerArray;
   using utils::avoidance::createStopTargetObjectMarkerArray;
   using utils::avoidance::createTargetObjectsMarkerArray;
 
   info_marker_.markers.clear();
   appendMarkerArray(
     createTargetObjectsMarkerArray(data.target_objects, "target_objects"), &info_marker_);
   appendMarkerArray(createStopTargetObjectMarkerArray(data), &info_marker_);
+  appendMarkerArray(
+    createAmbiguousObjectsMarkerArray(
+      data.target_objects, getEgoPose(), parameters_->policy_ambiguous_vehicle),
+    &info_marker_);
 }
 
 void AvoidanceModule::updateDebugMarker(