Merge branch 'beta/v0.41' into cherry-pick/10023

launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml

@@ -2,13 +2,31 @@
   <arg name="interface_input_topic" default="/planning/scenario_planning/lane_driving/behavior_planning/path"/>
   <arg name="interface_output_topic" default="/planning/scenario_planning/lane_driving/trajectory"/>
 
+  <arg name="launch_obstacle_stop_module" default="true"/>
+  <arg name="launch_obstacle_slow_down_module" default="true"/>
+  <arg name="launch_obstacle_cruise_module" default="true"/>
   <arg name="launch_dynamic_obstacle_stop_module" default="true"/>
   <arg name="launch_out_of_lane_module" default="true"/>
   <arg name="launch_obstacle_velocity_limiter_module" default="true"/>
   <arg name="launch_module_list_end" default="&quot;&quot;]"/>
 
   <!-- assemble launch config for motion velocity planner -->
   <arg name="motion_velocity_planner_launch_modules" default="["/>
+  <let
+    name="motion_velocity_planner_launch_modules"
+    value="$(eval &quot;'$(var motion_velocity_planner_launch_modules)' + 'autoware::motion_velocity_planner::ObstacleStopModule, '&quot;)"
+    if="$(var launch_obstacle_stop_module)"
+  />
+  <let
+    name="motion_velocity_planner_launch_modules"
+    value="$(eval &quot;'$(var motion_velocity_planner_launch_modules)' + 'autoware::motion_velocity_planner::ObstacleSlowDownModule, '&quot;)"
+    if="$(var launch_obstacle_slow_down_module)"
+  />
+  <let
+    name="motion_velocity_planner_launch_modules"
+    value="$(eval &quot;'$(var motion_velocity_planner_launch_modules)' + 'autoware::motion_velocity_planner::ObstacleCruiseModule, '&quot;)"
+    if="$(var launch_obstacle_cruise_module)"
+  />
   <let
     name="motion_velocity_planner_launch_modules"
     value="$(eval &quot;'$(var motion_velocity_planner_launch_modules)' + 'autoware::motion_velocity_planner::OutOfLaneModule, '&quot;)"
@@ -131,6 +149,8 @@
         <remap from="~/input/virtual_traffic_light_states" to="/perception/virtual_traffic_light_states"/>
         <remap from="~/input/occupancy_grid" to="/perception/occupancy_grid_map/map"/>
         <remap from="~/output/trajectory" to="motion_velocity_planner/trajectory"/>
+        <remap from="~/output/velocity_limit" to="/planning/scenario_planning/max_velocity_candidates"/>
+        <remap from="~/output/clear_velocity_limit" to="/planning/scenario_planning/clear_velocity_limit"/>
         <remap from="~/output/stop_reasons" to="/planning/scenario_planning/status/stop_reasons"/>
         <remap from="~/output/velocity_factors" to="/planning/velocity_factors/motion_velocity_planner"/>
         <!-- params -->
@@ -141,6 +161,9 @@
         <param from="$(var velocity_smoother_param_path)"/>
         <param from="$(var motion_velocity_planner_velocity_smoother_type_param_path)"/>
         <param from="$(var motion_velocity_planner_param_path)"/>
+        <param from="$(var motion_velocity_planner_obstacle_stop_module_param_path)"/>
+        <param from="$(var motion_velocity_planner_obstacle_slow_down_module_param_path)"/>
+        <param from="$(var motion_velocity_planner_obstacle_cruise_module_param_path)"/>
         <param from="$(var motion_velocity_planner_dynamic_obstacle_stop_module_param_path)"/>
         <param from="$(var motion_velocity_planner_out_of_lane_module_param_path)"/>
         <param from="$(var motion_velocity_planner_obstacle_velocity_limiter_param_path)"/>

planning/.pages

@@ -63,6 +63,9 @@ nav:
     - 'Motion Velocity Planner':
       - 'About Motion Velocity Planner': planning/motion_velocity_planner/autoware_motion_velocity_planner_node_universe/
       - 'Available Modules':
+        - 'Obstacle Stop': planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/
+        - 'Obstacle Slow Down': planning/motion_velocity_planner/autoware_motion_velocity_obstacle_slow_down_module/
+        - 'Obstacle Cruise': planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/
         - 'Dynamic Obstacle Stop': planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/
         - 'Out of Lane': planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/
         - 'Obstacle Velocity Limiter': planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/

planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/dynamic_obstacle_stop_module.cpp

@@ -97,19 +97,21 @@ void DynamicObstacleStopModule::update_parameters(const std::vector<rclcpp::Para
 }
 
 VelocityPlanningResult DynamicObstacleStopModule::plan(
-  const std::vector<autoware_planning_msgs::msg::TrajectoryPoint> & ego_trajectory_points,
+  [[maybe_unused]] const std::vector<autoware_planning_msgs::msg::TrajectoryPoint> &
+    raw_trajectory_points,
+  const std::vector<autoware_planning_msgs::msg::TrajectoryPoint> & smoothed_trajectory_points,
   const std::shared_ptr<const PlannerData> planner_data)
 {
   VelocityPlanningResult result;
   debug_data_.reset_data();
-  if (ego_trajectory_points.size() < 2) return result;
+  if (smoothed_trajectory_points.size() < 2) return result;
 
   autoware::universe_utils::StopWatch<std::chrono::microseconds> stopwatch;
   stopwatch.tic();
   stopwatch.tic("preprocessing");
   dynamic_obstacle_stop::EgoData ego_data;
   ego_data.pose = planner_data->current_odometry.pose.pose;
-  ego_data.trajectory = ego_trajectory_points;
+  ego_data.trajectory = smoothed_trajectory_points;
   autoware::motion_utils::removeOverlapPoints(ego_data.trajectory);
   ego_data.first_trajectory_idx =
     autoware::motion_utils::findNearestSegmentIndex(ego_data.trajectory, ego_data.pose.position);
@@ -164,7 +166,7 @@ VelocityPlanningResult DynamicObstacleStopModule::plan(
     if (stop_pose) {
       result.stop_points.push_back(stop_pose->position);
       planning_factor_interface_->add(
-        ego_trajectory_points, ego_data.pose, *stop_pose, PlanningFactor::STOP,
+        smoothed_trajectory_points, ego_data.pose, *stop_pose, PlanningFactor::STOP,
         SafetyFactorArray{});
       create_virtual_walls();
     }

planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/dynamic_obstacle_stop_module.hpp

@@ -36,7 +36,8 @@ class DynamicObstacleStopModule : public PluginModuleInterface
   void publish_planning_factor() override { planning_factor_interface_->publish(); };
   void update_parameters(const std::vector<rclcpp::Parameter> & parameters) override;
   VelocityPlanningResult plan(
-    const std::vector<autoware_planning_msgs::msg::TrajectoryPoint> & ego_trajectory_points,
+    const std::vector<autoware_planning_msgs::msg::TrajectoryPoint> & raw_trajectory_points,
+    const std::vector<autoware_planning_msgs::msg::TrajectoryPoint> & smoothed_trajectory_points,
     const std::shared_ptr<const PlannerData> planner_data) override;
   std::string get_module_name() const override { return module_name_; }
 

planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/object_filtering.cpp

@@ -96,12 +96,12 @@ bool is_unavoidable(
 };
 
 std::vector<autoware_perception_msgs::msg::PredictedObject> filter_predicted_objects(
-  const std::vector<PlannerData::Object> & objects, const EgoData & ego_data,
+  const std::vector<std::shared_ptr<PlannerData::Object>> & objects, const EgoData & ego_data,
   const PlannerParam & params, const double hysteresis)
 {
   std::vector<autoware_perception_msgs::msg::PredictedObject> filtered_objects;
   for (const auto & object : objects) {
-    const auto & predicted_object = object.predicted_object;
+    const auto & predicted_object = object->predicted_object;
     const auto is_not_too_slow =
       predicted_object.kinematics.initial_twist_with_covariance.twist.linear.x >=
       params.minimum_object_velocity;

planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/object_filtering.hpp

@@ -76,7 +76,7 @@ bool is_unavoidable(
 /// @param hysteresis [m] extra distance threshold used for filtering
 /// @return filtered predicted objects
 std::vector<autoware_perception_msgs::msg::PredictedObject> filter_predicted_objects(
-  const std::vector<PlannerData::Object> & objects, const EgoData & ego_data,
+  const std::vector<std::shared_ptr<PlannerData::Object>> & objects, const EgoData & ego_data,
   const PlannerParam & params, const double hysteresis);
 
 }  // namespace autoware::motion_velocity_planner::dynamic_obstacle_stop

planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/types.hpp

@@ -53,23 +53,23 @@ struct PlannerParam
 
 struct EgoData
 {
-  TrajectoryPoints trajectory;
+  TrajectoryPoints trajectory{};
   size_t first_trajectory_idx{};
-  double longitudinal_offset_to_first_trajectory_idx;  // [m]
-  geometry_msgs::msg::Pose pose;
-  autoware::universe_utils::MultiPolygon2d trajectory_footprints;
-  Rtree rtree;
-  std::optional<geometry_msgs::msg::Pose> earliest_stop_pose;
+  double longitudinal_offset_to_first_trajectory_idx{};  // [m]
+  geometry_msgs::msg::Pose pose{};
+  autoware::universe_utils::MultiPolygon2d trajectory_footprints{};
+  Rtree rtree{};
+  std::optional<geometry_msgs::msg::Pose> earliest_stop_pose{};
 };
 
 /// @brief debug data
 struct DebugData
 {
-  autoware::universe_utils::MultiPolygon2d obstacle_footprints;
+  autoware::universe_utils::MultiPolygon2d obstacle_footprints{};
   size_t prev_dynamic_obstacles_nb{};
-  autoware::universe_utils::MultiPolygon2d ego_footprints;
+  autoware::universe_utils::MultiPolygon2d ego_footprints{};
   size_t prev_ego_footprints_nb{};
-  std::optional<geometry_msgs::msg::Pose> stop_pose;
+  std::optional<geometry_msgs::msg::Pose> stop_pose{};
   size_t prev_collisions_nb{};
   double z{};
   void reset_data()
@@ -82,8 +82,8 @@ struct DebugData
 
 struct Collision
 {
-  geometry_msgs::msg::Point point;
-  std::string object_uuid;
+  geometry_msgs::msg::Point point{};
+  std::string object_uuid{};
 };
 }  // namespace autoware::motion_velocity_planner::dynamic_obstacle_stop
 

planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/test/test_object_filtering.cpp

@@ -205,7 +205,7 @@ TEST(TestObjectFiltering, isUnavoidable)
 TEST(TestObjectFiltering, filterPredictedObjects)
 {
   using autoware::motion_velocity_planner::dynamic_obstacle_stop::filter_predicted_objects;
-  std::vector<autoware::motion_velocity_planner::PlannerData::Object> objects;
+  std::vector<std::shared_ptr<autoware::motion_velocity_planner::PlannerData::Object>> objects;
   autoware::motion_velocity_planner::dynamic_obstacle_stop::EgoData ego_data;
   autoware::motion_velocity_planner::dynamic_obstacle_stop::PlannerParam params;
   double hysteresis{};

planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/CMakeLists.txt

@@ -0,0 +1,12 @@
+cmake_minimum_required(VERSION 3.14)
+project(autoware_motion_velocity_obstacle_cruise_module)
+
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+pluginlib_export_plugin_description_file(autoware_motion_velocity_planner_node_universe plugins.xml)
+
+ament_auto_add_library(${PROJECT_NAME} SHARED
+  DIRECTORY src
+)
+
+ament_auto_package(INSTALL_TO_SHARE config)

planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/README.md

@@ -0,0 +1,118 @@
+# Obstacle Cruise
+
+## Role
+
+The `obstacle_cruise` module does the cruise planning against a dynamic obstacle in front of the ego.
+
+## Activation
+
+This module is activated if the launch parameter `launch_obstacle_cruise_module` is set to true.
+
+## Inner-workings / Algorithms
+
+### Obstacle Filtering
+
+The obstacles meeting the following condition are determined as obstacles for cruising.
+
+- The lateral distance from the object to the ego's trajectory is smaller than `obstacle_filtering.max_lat_margin`.
+
+- The object type is for cruising according to `obstacle_filtering.object_type.*`.
+- The object is not crossing the ego's trajectory (\*1).
+- If the object is inside the trajectory.
+  - The object type is for inside cruising according to `obstacle_filtering.object_type.inside.*`.
+  - The object velocity is larger than `obstacle_filtering.obstacle_velocity_threshold_from_cruise`.
+- If the object is outside the trajectory.
+  - The object type is for outside cruising according to `obstacle_filtering.object_type.outside.*`.
+  - The object velocity is larger than `obstacle_filtering.outside_obstacle.obstacle_velocity_threshold`.
+  - The highest confident predicted path collides with the ego's trajectory.
+  - Its collision's period is larger than `obstacle_filtering.outside_obstacle.ego_obstacle_overlap_time_threshold`.
+
+#### NOTE
+
+##### \*1: Crossing obstacles
+
+Crossing obstacle is the object whose orientation's yaw angle against the ego's trajectory is smaller than `obstacle_filtering.crossing_obstacle.obstacle_traj_angle_threshold`.
+
+##### Yield for vehicles that might cut in into the ego's lane
+
+It is also possible to yield (cruise) behind vehicles in neighbor lanes if said vehicles might cut in the ego vehicle's current lane.
+
+The obstacles meeting the following condition are determined as obstacles for yielding (cruising).
+
+- The object type is for cruising according to `obstacle_filtering.object_type.*` and it is moving with a speed greater than `obstacle_filtering.yield.stopped_obstacle_velocity_threshold`.
+- The object is not crossing the ego's trajectory (\*1).
+- There is another object of type `obstacle_filtering.object_type.*` stopped in front of the moving obstacle.
+- The lateral distance (using the ego's trajectory as reference) between both obstacles is less than `obstacle_filtering.yield.max_lat_dist_between_obstacles`
+- Both obstacles, moving and stopped, are within `obstacle_filtering.yield.lat_distance_threshold` and `obstacle_filtering.yield.lat_distance_threshold` + `obstacle_filtering.yield.max_lat_dist_between_obstacles` lateral distance from the ego's trajectory respectively.
+
+If the above conditions are met, the ego vehicle will cruise behind the moving obstacle, yielding to it so it can cut in into the ego's lane to avoid the stopped obstacle.
+
+### Cruise Planning
+
+The role of the cruise planning is keeping a safe distance with dynamic vehicle objects with smoothed velocity transition.
+This includes not only cruising a front vehicle, but also reacting a cut-in and cut-out vehicle.
+
+The safe distance is calculated dynamically based on the Responsibility-Sensitive Safety (RSS) by the following equation.
+
+$$
+d_{rss} = v_{ego} t_{idling} + \frac{1}{2} a_{ego} t_{idling}^2 + \frac{v_{ego}^2}{2 a_{ego}} - \frac{v_{obstacle}^2}{2 a_{obstacle}},
+$$
+
+assuming that $d_{rss}$ is the calculated safe distance, $t_{idling}$ is the idling time for the ego to detect the front vehicle's deceleration, $v_{ego}$ is the ego's current velocity, $v_{obstacle}$ is the front obstacle's current velocity, $a_{ego}$ is the ego's acceleration, and $a_{obstacle}$ is the obstacle's acceleration.
+These values are parameterized as follows. Other common values such as ego's minimum acceleration is defined in `common.param.yaml`.
+
+| Parameter                                  | Type   | Description                                                                   |
+| ------------------------------------------ | ------ | ----------------------------------------------------------------------------- |
+| `cruise_planning.idling_time`              | double | idling time for the ego to detect the front vehicle starting deceleration [s] |
+| `cruise_planning.min_ego_accel_for_rss`    | double | ego's acceleration for RSS [m/ss]                                             |
+| `cruise_planning.min_object_accel_for_rss` | double | front obstacle's acceleration for RSS [m/ss]                                  |
+
+The detailed formulation is as follows.
+
+$$
+\begin{align}
+d_{error} & = d - d_{rss} \\
+d_{normalized} & = lpf(d_{error} / d_{obstacle}) \\
+d_{quad, normalized} & = sign(d_{normalized}) *d_{normalized}*d_{normalized} \\
+v_{pid} & = pid(d_{quad, normalized}) \\
+v_{add} & = v_{pid} > 0 ? v_{pid}* w_{acc} : v_{pid} \\
+v_{target} & = max(v_{ego} + v_{add}, v_{min, cruise})
+\end{align}
+$$
+
+| Variable          | Description                             |
+| ----------------- | --------------------------------------- |
+| `d`               | actual distance to obstacle             |
+| `d_{rss}`         | ideal distance to obstacle based on RSS |
+| `v_{min, cruise}` | `min_cruise_target_vel`                 |
+| `w_{acc}`         | `output_ratio_during_accel`             |
+| `lpf(val)`        | apply low-pass filter to `val`          |
+| `pid(val)`        | apply pid to `val`                      |
+
+#### Algorithm selection for cruise planner
+
+Currently, only a PID-based planner is supported.
+Each planner will be explained in the following.
+
+| Parameter                   | Type   | Description                                                  |
+| --------------------------- | ------ | ------------------------------------------------------------ |
+| `option.planning_algorithm` | string | cruise and stop planning algorithm, selected from "pid_base" |
+
+#### PID-based planner
+
+In order to keep the safe distance, the target velocity and acceleration is calculated and sent as an external velocity limit to the velocity smoothing package (`velocity_smoother` by default).
+The target velocity and acceleration is respectively calculated with the PID controller according to the error between the reference safe distance and the actual distance.
+
+#### Optimization-based planner
+
+under construction
+
+## Debugging
+
+### Obstacle for cruise
+
+Orange sphere which is an obstacle for cruise is visualized by `obstacles_to_cruise` in the `~/debug/marker` topic.
+
+Orange wall which means a safe distance to cruise if the ego's front meets the wall is visualized in the `~/debug/cruise/virtual_wall` topic.
+
+![cruise_visualization](./docs/cruise_visualization.png)