[pull] main from autowarefoundation:main

common/motion_utils/include/motion_utils/marker/marker_helper.hpp

@@ -32,6 +32,15 @@ visualization_msgs::msg::MarkerArray createSlowDownVirtualWallMarker(
 visualization_msgs::msg::MarkerArray createDeadLineVirtualWallMarker(
   const geometry_msgs::msg::Pose & pose, const std::string & module_name, const rclcpp::Time & now,
   const int32_t id);
+
+visualization_msgs::msg::MarkerArray createDeletedStopVirtualWallMarker(
+  const rclcpp::Time & now, const int32_t id);
+
+visualization_msgs::msg::MarkerArray createDeletedSlowDownVirtualWallMarker(
+  const rclcpp::Time & now, const int32_t id);
+
+visualization_msgs::msg::MarkerArray createDeletedDeadLineVirtualWallMarker(
+  const rclcpp::Time & now, const int32_t id);
 }  // namespace motion_utils
 
 #endif  // MOTION_UTILS__MARKER__MARKER_HELPER_HPP_

common/motion_utils/src/marker/marker_helper.cpp

@@ -17,6 +17,7 @@
 #include <string>
 
 using tier4_autoware_utils::createDefaultMarker;
+using tier4_autoware_utils::createDeletedDefaultMarker;
 using tier4_autoware_utils::createMarkerColor;
 using tier4_autoware_utils::createMarkerScale;
 
@@ -57,6 +58,25 @@ inline visualization_msgs::msg::MarkerArray createVirtualWallMarkerArray(
   return marker_array;
 }
 
+inline visualization_msgs::msg::MarkerArray createDeletedVirtualWallMarkerArray(
+  const std::string & ns_prefix, const rclcpp::Time & now, const int32_t id)
+{
+  visualization_msgs::msg::MarkerArray marker_array;
+
+  // Virtual Wall
+  {
+    auto marker = createDeletedDefaultMarker(now, ns_prefix + "virtual_wall", id);
+    marker_array.markers.push_back(marker);
+  }
+
+  // Factor Text
+  {
+    auto marker = createDeletedDefaultMarker(now, ns_prefix + "factor_text", id);
+    marker_array.markers.push_back(marker);
+  }
+
+  return marker_array;
+}
 }  // namespace
 
 namespace motion_utils
@@ -84,4 +104,22 @@ visualization_msgs::msg::MarkerArray createDeadLineVirtualWallMarker(
   return createVirtualWallMarkerArray(
     pose, module_name, "dead_line_", now, id, createMarkerColor(0.0, 1.0, 0.0, 0.5));
 }
+
+visualization_msgs::msg::MarkerArray createDeletedStopVirtualWallMarker(
+  const rclcpp::Time & now, const int32_t id)
+{
+  return createDeletedVirtualWallMarkerArray("stop_", now, id);
+}
+
+visualization_msgs::msg::MarkerArray createDeletedSlowDownVirtualWallMarker(
+  const rclcpp::Time & now, const int32_t id)
+{
+  return createDeletedVirtualWallMarkerArray("slow_down_", now, id);
+}
+
+visualization_msgs::msg::MarkerArray createDeletedDeadLineVirtualWallMarker(
+  const rclcpp::Time & now, const int32_t id)
+{
+  return createDeletedVirtualWallMarkerArray("dead_line_", now, id);
+}
 }  // namespace motion_utils

common/tier4_autoware_utils/include/tier4_autoware_utils/ros/marker_helper.hpp

@@ -88,6 +88,19 @@ inline visualization_msgs::msg::Marker createDefaultMarker(
   return marker;
 }
 
+inline visualization_msgs::msg::Marker createDeletedDefaultMarker(
+  const rclcpp::Time & now, const std::string & ns, const int32_t id)
+{
+  visualization_msgs::msg::Marker marker;
+
+  marker.header.stamp = now;
+  marker.ns = ns;
+  marker.id = id;
+  marker.action = visualization_msgs::msg::Marker::DELETE;
+
+  return marker;
+}
+
 inline void appendMarkerArray(
   const visualization_msgs::msg::MarkerArray & additional_marker_array,
   visualization_msgs::msg::MarkerArray * marker_array,

launch/tier4_planning_launch/launch/scenario_planning/parking.launch.py

@@ -73,6 +73,7 @@ def launch_setup(context, *args, **kwargs):
                         "map_frame": "map",
                         "update_rate": 10.0,
                         "use_wayarea": True,
+                        "use_parkinglot": True,
                         "use_objects": True,
                         "use_points": True,
                         "grid_min_value": 0.0,

planning/behavior_velocity_planner/detection-area-design.md

@@ -1,33 +1,34 @@
-### Detection Area
+## Detection Area
 
-#### Role
+### Role
 
 If pointcloud is detected in a detection area defined on a map, the stop planning will be executed at the predetermined point.
 
 ![brief](./docs/detection_area/detection_area.svg)
 
-#### Activation Timing
+### Activation Timing
 
 This module is activated when there is a detection area on the target lane.
 
-### Algorithm
+### Module Parameters
+
+| Parameter                   | Type   | Description                                                                                        |
+| --------------------------- | ------ | -------------------------------------------------------------------------------------------------- |
+| `use_dead_line`             | bool   | [-] weather to use dead line or not                                                                |
+| `use_pass_judge_line`       | bool   | [-] weather to use pass judge line or not                                                          |
+| `state_clear_time`          | double | [s] when the vehicle is stopping for certain time without incoming obstacle, move to STOPPED state |
+| `stop_margin`               | double | [m] a margin that the vehicle tries to stop before stop_line                                       |
+| `dead_line_margin`          | double | [m] ignore threshold that vehicle behind is collide with ego vehicle or not                        |
+| `hold_stop_margin_distance` | double | [m] parameter for restart prevention (See Algorithm section)                                       |
+
+### Inner-workings / Algorithm
 
 1. Gets a detection area and stop line from map information and confirms if there is pointcloud in the detection area
 2. Inserts stop point l[m] in front of the stop line
 3. Inserts a pass judge point to a point where the vehicle can stop with a max deceleration
 4. Sets velocity as zero behind the stop line when the ego-vehicle is in front of the pass judge point
 5. If the ego vehicle has passed the pass judge point already, it doesn’t stop and pass through.
 
-#### Module Parameters
-
-| Parameter             | Type   | Description                                                                                        |
-| --------------------- | ------ | -------------------------------------------------------------------------------------------------- |
-| `stop_margin`         | double | [m] a margin that the vehicle tries to stop before stop_line                                       |
-| `use_dead_line`       | bool   | [-] weather to use dead line or not                                                                |
-| `dead_line_margin`    | double | [m] ignore threshold that vehicle behind is collide with ego vehicle or not                        |
-| `use_pass_judge_line` | bool   | [-] weather to use pass judge line or not                                                          |
-| `state_clear_time`    | double | [s] when the vehicle is stopping for certain time without incoming obstacle, move to STOPPED state |
-
 #### Flowchart
 
 ```plantuml
@@ -87,3 +88,24 @@ endif
 stop
 @enduml
 ```
+
+#### Restart prevention
+
+If it needs X meters (e.g. 0.5 meters) to stop once the vehicle starts moving due to the poor vehicle control performance, the vehicle goes over the stopping position that should be strictly observed when the vehicle starts to moving in order to approach the near stop point (e.g. 0.3 meters away).
+
+This module has parameter `hold_stop_margin_distance` in order to prevent from these redundant restart. If the vehicle is stopped within `hold_stop_margin_distance` meters from stop point of the module (\_front_to_stop_line < hold_stop_margin_distance), the module judges that the vehicle has already stopped for the module's stop point and plans to keep stopping current position even if the vehicle is stopped due to other factors.
+
+<figure markdown>
+  ![example](docs/detection_area/restart_prevention.svg){width=1000}
+  <figcaption>parameters</figcaption>
+</figure>
+
+<figure markdown>
+  ![example](docs/detection_area/restart.svg){width=1000}
+  <figcaption>outside the hold_stop_margin_distance</figcaption>
+</figure>
+
+<figure markdown>
+  ![example](docs/detection_area/keep_stopping.svg){width=1000}
+  <figcaption>inside the hold_stop_margin_distance</figcaption>
+</figure>