cleanup and doc

Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp>

Author: maxime-clem

planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware_motion_velocity_planner_common/planner_data.hpp

@@ -57,8 +57,6 @@ struct PlannerData
   explicit PlannerData(rclcpp::Node & node)
   : vehicle_info_(vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo())
   {
-    system_delay = node.declare_parameter<double>("system_delay");
-    delay_response_time = node.declare_parameter<double>("delay_response_time");
   }
 
   // msgs from callbacks that are used for data-ready
@@ -87,11 +85,6 @@ struct PlannerData
   // parameters
   vehicle_info_util::VehicleInfo vehicle_info_;
 
-  // additional parameters
-  double system_delay;
-  double delay_response_time;
-  double stop_line_extend_length;
-
   /**
    *@fn
    *@brief queries the traffic signal information of given Id. if keep_last_observation = true,

planning/motion_velocity_planner/autoware_motion_velocity_planner_node/README.md

@@ -2,57 +2,57 @@
 
 ## Overview
 
-`motion_velocity_planner` is a planner that adjust the trajectory velocity based on the obstacles around the vehicle.
+`motion_velocity_planner` is a planner to adjust the trajectory velocity based on the obstacles around the vehicle.
 It loads modules as plugins. Please refer to the links listed below for detail on each module.
 
-<!-- ![Architecture](./docs/BehaviorVelocityPlanner-Architecture.drawio.svg)
-
-- [Blind Spot](../behavior_velocity_blind_spot_module/README.md)
-- [Crosswalk](../behavior_velocity_crosswalk_module/README.md)
-- [Walkway](../behavior_velocity_walkway_module/README.md)
-- [Detection Area](../behavior_velocity_detection_area_module/README.md)
-- [Intersection](../behavior_velocity_intersection_module/README.md)
-- [MergeFromPrivate](../behavior_velocity_intersection_module/README.md#merge-from-private)
-- [Stop Line](../behavior_velocity_stop_line_module/README.md)
-- [Virtual Traffic Light](../behavior_velocity_virtual_traffic_light_module/README.md)
-- [Traffic Light](../behavior_velocity_traffic_light_module/README.md)
-- [Occlusion Spot](../behavior_velocity_occlusion_spot_module/README.md)
-- [No Stopping Area](../behavior_velocity_no_stopping_area_module/README.md)
-- [Run Out](../behavior_velocity_run_out_module/README.md)
-- [Speed Bump](../behavior_velocity_speed_bump_module/README.md)
-- [Out of Lane](../behavior_velocity_out_of_lane_module/README.md)
-
-When each module plans velocity, it considers based on `base_link`(center of rear-wheel axis) pose.
-So for example, in order to stop at a stop line with the vehicles' front on the stop line, it calculates `base_link` position from the distance between `base_link` to front and modifies path velocity from the `base_link` position.
+![Architecture](./docs/MotionVelocityPlanner-InternalInterface.drawio.svg)
+
+- [Out of Lane](../autoware_motion_velocity_out_of_lane_module/README.md)
+
+Each module calculates stop and slow down points to be inserted in the ego trajectory.
+These points are assumed to correspond to the `base_link` frame of the ego vehicle as it follows the trajectory.
+This means that to stop before a wall, a stop point is inserted in the trajectory at a distance ahead of the wall equal to the vehicle front offset (wheelbase + front overhange, see the [vehicle dimensions](https://autowarefoundation.github.io/autoware-documentation/main/design/autoware-interfaces/components/vehicle-dimensions/).
 
 ![set_stop_velocity](./docs/set_stop_velocity.drawio.svg)
 
 ## Input topics
 
-| Name                                      | Type                                                 | Description                                                                                                                     |
-| ----------------------------------------- | ---------------------------------------------------- | ------------------------------------------------------------------------------------------------------------------------------- |
-| `~input/path_with_lane_id`                | autoware_auto_planning_msgs::msg::PathWithLaneId     | path with lane_id                                                                                                               |
-| `~input/vector_map`                       | autoware_auto_mapping_msgs::msg::HADMapBin           | vector map                                                                                                                      |
-| `~input/vehicle_odometry`                 | nav_msgs::msg::Odometry                              | vehicle velocity                                                                                                                |
-| `~input/dynamic_objects`                  | autoware_auto_perception_msgs::msg::PredictedObjects | dynamic objects                                                                                                                 |
-| `~input/no_ground_pointcloud`             | sensor_msgs::msg::PointCloud2                        | obstacle pointcloud                                                                                                             |
-| `~/input/compare_map_filtered_pointcloud` | sensor_msgs::msg::PointCloud2                        | obstacle pointcloud filtered by compare map. Note that this is used only when the detection method of run out module is Points. |
-| `~input/traffic_signals`                  | autoware_perception_msgs::msg::TrafficSignalArray    | traffic light states                                                                                                            |
+| Name                                   | Type                                                 | Description                   |
+| -------------------------------------- | ---------------------------------------------------- | ----------------------------- |
+| `~/input/trajectory`                   | autoware_auto_planning_msgs::msg::Trajectory         | input trajectory              |
+| `~/input/vector_map`                   | autoware_auto_mapping_msgs::msg::HADMapBin           | vector map                    |
+| `~/input/vehicle_odometry`             | nav_msgs::msg::Odometry                              | vehicle position and velocity |
+| `~/input/accel`                        | geometry_msgs::msg::AccelWithCovarianceStamped       | vehicle acceleration          |
+| `~/input/dynamic_objects`              | autoware_auto_perception_msgs::msg::PredictedObjects | dynamic objects               |
+| `~/input/no_ground_pointcloud`         | sensor_msgs::msg::PointCloud2                        | obstacle pointcloud           |
+| `~/input/traffic_signals`              | autoware_perception_msgs::msg::TrafficSignalArray    | traffic light states          |
+| `~/input/virtual_traffic_light_states` | tier4_v2x_msgs::msg::VirtualTrafficLightStateArray   | virtual traffic light states  |
+| `~/input/occupancy_grid`               | nav_msgs::msg::OccupancyGrid                         | occupancy grid                |
 
 ## Output topics
 
-| Name                   | Type                                      | Description                            |
-| ---------------------- | ----------------------------------------- | -------------------------------------- |
-| `~output/path`         | autoware_auto_planning_msgs::msg::Path    | path to be followed                    |
-| `~output/stop_reasons` | tier4_planning_msgs::msg::StopReasonArray | reasons that cause the vehicle to stop |
+| Name                        | Type                                              | Description                                        |
+| --------------------------- | ------------------------------------------------- | -------------------------------------------------- |
+| `~/output/trajectory`       | autoware_auto_planning_msgs::msg::Trajectory      | Ego trajectory with updated velocity profile       |
+| `~/output/velocity_factors` | autoware_adapi_v1_msgs::msg::VelocityFactorsArray | factors causing change in the ego velocity profile |
+
+## Services
+
+| Name                      | Type                                                     | Description                  |
+| ------------------------- | -------------------------------------------------------- | ---------------------------- |
+| `~/service/load_plugin`   | autoware_motion_velocity_planner_node::srv::LoadPlugin   | To request loading a plugin  |
+| `~/service/unload_plugin` | autoware_motion_velocity_planner_node::srv::UnloadPlugin | To request unloaded a plugin |
 
 ## Node parameters
 
-| Parameter              | Type                 | Description                                                                         |
-| ---------------------- | -------------------- | ----------------------------------------------------------------------------------- |
-| `launch_modules`       | vector&lt;string&gt; | module names to launch                                                              |
-| `forward_path_length`  | double               | forward path length                                                                 |
-| `backward_path_length` | double               | backward path length                                                                |
-| `max_accel`            | double               | (to be a global parameter) max acceleration of the vehicle                          |
-| `system_delay`         | double               | (to be a global parameter) delay time until output control command                  |
-| `delay_response_time`  | double               | (to be a global parameter) delay time of the vehicle's response to control commands | -->
+| Parameter        | Type             | Description            |
+| ---------------- | ---------------- | ---------------------- |
+| `launch_modules` | vector\<string\> | module names to launch |
+
+In addition, the following parameters should be provided to the node:
+
+- [nearest search parameters](https://github.com/autowarefoundation/autoware_launch/blob/main/autoware_launch/config/planning/scenario_planning/common/nearest_search.param.yaml);
+- [vehicle info parameters](https://github.com/autowarefoundation/sample_vehicle_launch/blob/main/sample_vehicle_description/config/vehicle_info.param.yaml);
+- [common planning parameters](https://github.com/autowarefoundation/autoware_launch/blob/main/autoware_launch/config/planning/scenario_planning/common/common.param.yaml);
+- [smoother parameters](https://autowarefoundation.github.io/autoware.universe/main/planning/motion_velocity_smoother/#parameters)
+- Parameters of each plugin that will be loaded.

planning/motion_velocity_planner/autoware_motion_velocity_planner_node/config/motion_velocity_planner.param.yaml

@@ -1,5 +1,2 @@
 /**:
   ros__parameters:
-    stop_line_extend_length: 5.0
-    system_delay: 0.5
-    delay_response_time: 0.5

planning/motion_velocity_planner/autoware_motion_velocity_planner_node/docs/BehaviorVelocityPlanner-Architecture.drawio.svg

@@ -5,64 +5,8 @@
   "definitions": {
     "behavior_velocity_planner": {
       "type": "object",
-      "properties": {
-        "forward_path_length": {
-          "type": "number",
-          "default": "1000.0",
-          "description": "forward path"
-        },
-        "backward_path_length": {
-          "type": "number",
-          "default": "5.0",
-          "description": "backward path"
-        },
-        "behavior_output_path_interval": {
-          "type": "number",
-          "default": "1.0",
-          "description": "the output path will be interpolated by this interval"
-        },
-        "max_accel": {
-          "type": "number",
-          "default": "-2.8",
-          "description": "(to be a global parameter) max acceleration of the vehicle"
-        },
-        "system_delay": {
-          "type": "number",
-          "default": "0.5",
-          "description": "(to be a global parameter) delay time until output control command"
-        },
-        "delay_response_time": {
-          "type": "number",
-          "default": "0.5",
-          "description": "(to be a global parameter) delay time of the vehicle's response to control commands"
-        },
-        "stop_line_extend_length": {
-          "type": "number",
-          "default": "5.0",
-          "description": "extend length of stop line"
-        },
-        "max_jerk": {
-          "type": "number",
-          "default": "-5.0",
-          "description": "max jerk of the vehicle"
-        },
-        "is_publish_debug_path": {
-          "type": "boolean",
-          "default": "false",
-          "description": "is publish debug path?"
-        }
-      },
-      "required": [
-        "forward_path_length",
-        "behavior_output_path_interval",
-        "backward_path_length",
-        "max_accel",
-        "system_delay",
-        "delay_response_time",
-        "stop_line_extend_length",
-        "max_jerk",
-        "is_publish_debug_path"
-      ],
+      "properties": {},
+      "required": [],
       "additionalProperties": false
     }
   },
@@ -71,7 +15,7 @@
       "type": "object",
       "properties": {
         "ros__parameters": {
-          "$ref": "#/definitions/behavior_velocity_planner"
+          "$ref": "#/definitions/motion_velocity_planner"
         }
       },
       "required": ["ros__parameters"],

planning/motion_velocity_planner/autoware_motion_velocity_planner_node/docs/MotionVelocityPlanner-InternalInterface.drawio.svg

@@ -146,9 +146,6 @@ MotionVelocityPlannerNode::MotionVelocityPlannerNode(const rclcpp::NodeOptions &
     "~/service/unload_plugin",
     std::bind(&MotionVelocityPlannerNode::on_unload_plugin, this, _1, _2));
 
-  // set velocity smoother param
-  on_param();
-
   // Publishers
   trajectory_pub_ =
     this->create_publisher<autoware_auto_planning_msgs::msg::Trajectory>("~/output/trajectory", 1);
@@ -157,11 +154,12 @@ MotionVelocityPlannerNode::MotionVelocityPlannerNode(const rclcpp::NodeOptions &
       "~/output/velocity_factors", 1);
 
   // Parameters
-  planner_data_.stop_line_extend_length = declare_parameter<double>("stop_line_extend_length");
   // nearest search
   planner_data_.ego_nearest_dist_threshold =
     declare_parameter<double>("ego_nearest_dist_threshold");
   planner_data_.ego_nearest_yaw_threshold = declare_parameter<double>("ego_nearest_yaw_threshold");
+  // set velocity smoother param
+  on_param();
 
   // Initialize PlannerManager
   for (const auto & name : declare_parameter<std::vector<std::string>>("launch_modules")) {
@@ -199,42 +197,47 @@ void MotionVelocityPlannerNode::on_unload_plugin(
 bool MotionVelocityPlannerNode::is_data_ready() const
 {
   const auto & d = planner_data_;
+  constexpr auto throttle_duration = 3000;  // [ms]
   auto clock = *get_clock();
 
   // from callbacks
   if (!d.current_odometry) {
-    RCLCPP_INFO_THROTTLE(get_logger(), clock, 3000, "Waiting for current odometry");
+    RCLCPP_INFO_THROTTLE(get_logger(), clock, throttle_duration, "Waiting for current odometry");
     return false;
   }
 
   if (!d.current_velocity) {
-    RCLCPP_INFO_THROTTLE(get_logger(), clock, 3000, "Waiting for current velocity");
+    RCLCPP_INFO_THROTTLE(get_logger(), clock, throttle_duration, "Waiting for current velocity");
     return false;
   }
   if (!d.current_acceleration) {
-    RCLCPP_INFO_THROTTLE(get_logger(), clock, 3000, "Waiting for current acceleration");
+    RCLCPP_INFO_THROTTLE(
+      get_logger(), clock, throttle_duration, "Waiting for current acceleration");
     return false;
   }
   if (!d.predicted_objects) {
-    RCLCPP_INFO_THROTTLE(get_logger(), clock, 3000, "Waiting for predicted_objects");
+    RCLCPP_INFO_THROTTLE(get_logger(), clock, throttle_duration, "Waiting for predicted_objects");
     return false;
   }
   if (!d.no_ground_pointcloud) {
-    RCLCPP_INFO_THROTTLE(get_logger(), clock, 3000, "Waiting for pointcloud");
+    RCLCPP_INFO_THROTTLE(get_logger(), clock, throttle_duration, "Waiting for pointcloud");
     return false;
   }
   if (!map_ptr_) {
-    RCLCPP_INFO_THROTTLE(get_logger(), clock, 3000, "Waiting for the initialization of map");
+    RCLCPP_INFO_THROTTLE(
+      get_logger(), clock, throttle_duration, "Waiting for the initialization of map");
     return false;
   }
   if (!d.velocity_smoother_) {
     RCLCPP_INFO_THROTTLE(
-      get_logger(), clock, 3000, "Waiting for the initialization of velocity smoother");
+      get_logger(), clock, throttle_duration,
+      "Waiting for the initialization of velocity smoother");
     return false;
   }
   if (!d.occupancy_grid) {
     RCLCPP_INFO_THROTTLE(
-      get_logger(), clock, 3000, "Waiting for the initialization of occupancy grid map");
+      get_logger(), clock, throttle_duration,
+      "Waiting for the initialization of occupancy grid map");
     return false;
   }
   return true;
@@ -415,7 +418,11 @@ autoware_auto_planning_msgs::msg::Trajectory MotionVelocityPlannerNode::generate
         motion_utils::findNearestSegmentIndex(output_trajectory_msg.points, stop_point);
       const auto insert_idx =
         motion_utils::insertTargetPoint(seg_idx, stop_point, output_trajectory_msg.points);
-      if (insert_idx) output_trajectory_msg.points[*insert_idx].longitudinal_velocity_mps = 0.0;
+      if (insert_idx) {
+        output_trajectory_msg.points[*insert_idx].longitudinal_velocity_mps = 0.0;
+      } else {
+        RCLCPP_WARN(get_logger(), "Failed to insert stop point");
+      }
     }
     for (const auto & slowdown_interval : planning_result.slowdown_intervals) {
       const auto from_seg_idx =
@@ -430,7 +437,7 @@ autoware_auto_planning_msgs::msg::Trajectory MotionVelocityPlannerNode::generate
         for (auto idx = *from_insert_idx; idx <= *to_insert_idx; ++idx)
           output_trajectory_msg.points[idx].longitudinal_velocity_mps = 0.0;
       } else {
-        RCLCPP_WARN(get_logger(), "Failed to insert slowdown points");
+        RCLCPP_WARN(get_logger(), "Failed to insert slowdown point");
       }
     }
     velocity_factors.factors.push_back(planning_result.velocity_factor);
@@ -450,9 +457,9 @@ rcl_interfaces::msg::SetParametersResult MotionVelocityPlannerNode::on_set_param
     planner_manager_.update_module_parameters(parameters);
   }
 
-  updateParam(parameters, "stop_line_extend_length", planner_data_.stop_line_extend_length);
   updateParam(parameters, "ego_nearest_dist_threshold", planner_data_.ego_nearest_dist_threshold);
   updateParam(parameters, "ego_nearest_yaw_threshold", planner_data_.ego_nearest_yaw_threshold);
+  on_param();
 
   rcl_interfaces::msg::SetParametersResult result;
   result.successful = true;