feat(motion_utils): add detail and pass type to VirtualWall (autoware…

…foundation#9940) Signed-off-by: Mamoru Sobue <[email protected]> Signed-off-by: TetsuKawa <[email protected]>
TetsuKawa · Jan 20, 2025 · dea525e · dea525e
1 parent 3b3263c
commit dea525e
Show file tree

Hide file tree

Showing 4 changed files with 63 additions and 4 deletions.
diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/marker/marker_helper.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/marker/marker_helper.hpp
@@ -40,6 +40,11 @@ visualization_msgs::msg::MarkerArray createDeadLineVirtualWallMarker(
   const double longitudinal_offset = 0.0, const std::string & ns_prefix = "",
   const bool is_driving_forward = true);
 
+visualization_msgs::msg::MarkerArray createIntendedPassVirtualMarker(
+  const geometry_msgs::msg::Pose & pose, const std::string & module_name, const rclcpp::Time & now,
+  const int32_t id, const double longitudinal_offset, const std::string & ns_prefix,
+  const bool is_driving_forward);
+
 visualization_msgs::msg::MarkerArray createDeletedStopVirtualWallMarker(
   const rclcpp::Time & now, const int32_t id);
 

diff --git a/...utoware_motion_utils/include/autoware/motion_utils/marker/virtual_wall_marker_creator.hpp b/...utoware_motion_utils/include/autoware/motion_utils/marker/virtual_wall_marker_creator.hpp
@@ -29,12 +29,13 @@ namespace autoware::motion_utils
 {
 
 /// @brief type of virtual wall associated with different marker styles and namespace
-enum VirtualWallType { stop, slowdown, deadline };
+enum VirtualWallType { stop, slowdown, deadline, pass };
 /// @brief virtual wall to be visualized in rviz
 struct VirtualWall
 {
   geometry_msgs::msg::Pose pose{};
   std::string text{};
+  std::string detail{};
   std::string ns{};
   VirtualWallType style = stop;
   double longitudinal_offset{};

diff --git a/common/autoware_motion_utils/src/marker/marker_helper.cpp b/common/autoware_motion_utils/src/marker/marker_helper.cpp
@@ -54,7 +54,7 @@ inline visualization_msgs::msg::MarkerArray createVirtualWallMarkerArray(
   {
     auto marker = createDefaultMarker(
       "map", now, ns_prefix + "factor_text", id, visualization_msgs::msg::Marker::TEXT_VIEW_FACING,
-      createMarkerScale(0.0, 0.0, 1.0), createMarkerColor(1.0, 1.0, 1.0, 1.0));
+      createMarkerScale(0.0, 0.0, 1.0 /*font size*/), createMarkerColor(1.0, 1.0, 1.0, 1.0));
 
     marker.pose = vehicle_front_pose;
     marker.pose.position.z += 2.0;
@@ -85,6 +85,41 @@ inline visualization_msgs::msg::MarkerArray createDeletedVirtualWallMarkerArray(
 
   return marker_array;
 }
+
+inline visualization_msgs::msg::MarkerArray createIntendedPassArrowMarkerArray(
+  const geometry_msgs::msg::Pose & vehicle_front_pose, const std::string & module_name,
+  const std::string & ns_prefix, const rclcpp::Time & now, const int32_t id,
+  const std_msgs::msg::ColorRGBA & color)
+{
+  visualization_msgs::msg::MarkerArray marker_array;
+
+  // Arrow
+  {
+    auto marker = createDefaultMarker(
+      "map", now, ns_prefix + "direction", id, visualization_msgs::msg::Marker::ARROW,
+      createMarkerScale(2.5 /*length*/, 0.15 /*width*/, 1.0 /*height*/), color);
+
+    marker.pose = vehicle_front_pose;
+
+    marker_array.markers.push_back(marker);
+  }
+
+  // Factor Text
+  {
+    auto marker = createDefaultMarker(
+      "map", now, ns_prefix + "factor_text", id, visualization_msgs::msg::Marker::TEXT_VIEW_FACING,
+      createMarkerScale(0.0, 0.0, 0.4 /*font size*/), createMarkerColor(1.0, 1.0, 1.0, 1.0));
+
+    marker.pose = vehicle_front_pose;
+    marker.pose.position.z += 2.0;
+    marker.text = module_name;
+
+    marker_array.markers.push_back(marker);
+  }
+
+  return marker_array;
+}
+
 }  // namespace
 
 namespace autoware::motion_utils
@@ -125,6 +160,18 @@ visualization_msgs::msg::MarkerArray createDeadLineVirtualWallMarker(
     createMarkerColor(0.0, 1.0, 0.0, 0.5));
 }
 
+visualization_msgs::msg::MarkerArray createIntendedPassVirtualMarker(
+  const geometry_msgs::msg::Pose & pose, const std::string & module_name, const rclcpp::Time & now,
+  const int32_t id, const double longitudinal_offset, const std::string & ns_prefix,
+  const bool is_driving_forward)
+{
+  const auto pose_with_offset = autoware::universe_utils::calcOffsetPose(
+    pose, longitudinal_offset * (is_driving_forward ? 1.0 : -1.0), 0.0, 0.0);
+  return createIntendedPassArrowMarkerArray(
+    pose_with_offset, module_name, ns_prefix + "intended_pass_", now, id,
+    createMarkerColor(0.77, 0.77, 0.77, 0.5));
+}
+
 visualization_msgs::msg::MarkerArray createDeletedStopVirtualWallMarker(
   const rclcpp::Time & now, const int32_t id)
 {

diff --git a/common/autoware_motion_utils/src/marker/virtual_wall_marker_creator.cpp b/common/autoware_motion_utils/src/marker/virtual_wall_marker_creator.cpp
@@ -63,10 +63,16 @@ visualization_msgs::msg::MarkerArray VirtualWallMarkerCreator::create_markers(
       case deadline:
         create_fn = autoware::motion_utils::createDeadLineVirtualWallMarker;
         break;
+      case pass:
+        create_fn = autoware::motion_utils::createIntendedPassVirtualMarker;
+        break;
     }
+    const auto wall_text = virtual_wall.detail.empty()
+                             ? virtual_wall.text
+                             : virtual_wall.text + "(" + virtual_wall.detail + ")";
     auto markers = create_fn(
-      virtual_wall.pose, virtual_wall.text, now, 0, virtual_wall.longitudinal_offset,
-      virtual_wall.ns, virtual_wall.is_driving_forward);
+      virtual_wall.pose, wall_text, now, 0, virtual_wall.longitudinal_offset, virtual_wall.ns,
+      virtual_wall.is_driving_forward);
     for (auto & marker : markers.markers) {
       marker.id = static_cast<int>(marker_count_per_namespace_[marker.ns].current++);
       marker_array.markers.push_back(marker);