Skip to content

Commit

Permalink
cleanup and doc
Browse files Browse the repository at this point in the history 
Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp>
  • Loading branch information
@maxime-clem
maxime-clem committed May 24, 2024
1 parent 735b78b commit b84fe84
Show file tree
Hide file tree
Showing 7 changed files with 604 additions and 2,648 deletions.
7 changes: 0 additions & 7 deletions ..._velocity_planner_common/include/autoware_motion_velocity_planner_common/planner_data.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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,
Expand Down
82 changes: 41 additions & 41 deletions planning/motion_velocity_planner/autoware_motion_velocity_planner_node/README.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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/).

Check warning on line 14 in planning/motion_velocity_planner/autoware_motion_velocity_planner_node/README.md

View workflow job for this annotation

GitHub Actions / spell-check-partial 

Unknown word (overhange)

![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.
3 changes: 0 additions & 3 deletions ...y_planner/autoware_motion_velocity_planner_node/config/motion_velocity_planner.param.yaml
View file
Original file line number Diff line number Diff line change
@@ -1,5 +1,2 @@
/**:
ros__parameters:
stop_line_extend_length: 5.0
system_delay: 0.5
delay_response_time: 0.5
2,523 changes: 0 additions & 2,523 deletions ...tion_velocity_planner_node/docs/BehaviorVelocityPlanner-Architecture.drawio.svg
View file

This file was deleted.

538 changes: 538 additions & 0 deletions ...n_velocity_planner_node/docs/MotionVelocityPlanner-InternalInterface.drawio.svg
View file
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
62 changes: 3 additions & 59 deletions ..._planner/autoware_motion_velocity_planner_node/schema/motion_velocity_planner.schema.json
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
}
},
Expand All @@ -71,7 +15,7 @@
"type": "object",
"properties": {
"ros__parameters": {
"$ref": "#/definitions/behavior_velocity_planner"
"$ref": "#/definitions/motion_velocity_planner"
}
},
"required": ["ros__parameters"],
Expand Down
37 changes: 22 additions & 15 deletions planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand All @@ -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")) {
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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 =
Expand All @@ -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);
Expand All @@ -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;
Expand Down

0 comments on commit b84fe84

Please sign in to comment.