Skip to content

Commit

Permalink
refactor(obstacle_cruise_planner): clean up the code (autowarefoundat…
Browse files Browse the repository at this point in the history 
…ion#2526)

* refactor obstacle_cruise_planner

Signed-off-by: Takayuki Murooka <[email protected]>

* update

Signed-off-by: Takayuki Murooka <[email protected]>

* update

Signed-off-by: Takayuki Murooka <[email protected]>

* add vel lpf and disable_target_acceleration

Signed-off-by: Takayuki Murooka <[email protected]>

* update

Signed-off-by: Takayuki Murooka <[email protected]>

* fix README.md

Signed-off-by: Takayuki Murooka <[email protected]>

Signed-off-by: Takayuki Murooka <[email protected]>
  • Loading branch information
@takayuki5168
takayuki5168 authored Dec 21, 2022
1 parent 078857d commit febbc13
Show file tree
Hide file tree
Showing 17 changed files with 385 additions and 539 deletions.
58 changes: 39 additions & 19 deletions planning/obstacle_cruise_planner/README.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,30 +4,29 @@

The `obstacle_cruise_planner` package has following modules.

- obstacle stop planning
- inserting a stop point in the trajectory when there is a static obstacle on the trajectory.
- adaptive cruise planning
- sending an external velocity limit to `motion_velocity_smoother` when there is a dynamic obstacle to cruise on the trajectory
- Stop planning
- stop when there is a static obstacle near the trajectory.
- Cruise planning
- slow down when there is a dynamic obstacle to cruise near the trajectory

## Interfaces

### Input topics

| Name | Type | Description |
| ----------------------------- | ----------------------------------------------- | --------------------------------- |
| `~/input/trajectory` | autoware_auto_planning_msgs::Trajectory | input trajectory |
| `~/input/smoothed_trajectory` | autoware_auto_planning_msgs::Trajectory | trajectory with smoothed velocity |
| `~/input/objects` | autoware_auto_perception_msgs::PredictedObjects | dynamic objects |
| `~/input/odometry` | nav_msgs::msg::Odometry | ego odometry |
| Name | Type | Description |
| -------------------- | ----------------------------------------------- | ---------------- |
| `~/input/trajectory` | autoware_auto_planning_msgs::Trajectory | input trajectory |
| `~/input/objects` | autoware_auto_perception_msgs::PredictedObjects | dynamic objects |
| `~/input/odometry` | nav_msgs::msg::Odometry | ego odometry |

### Output topics

| Name | Type | Description |
| ------------------------------ | ---------------------------------------------- | ------------------------------------- |
| `~output/trajectory` | autoware_auto_planning_msgs::Trajectory | output trajectory |
| `~output/velocity_limit` | tier4_planning_msgs::VelocityLimit | velocity limit for cruising |
| `~output/clear_velocity_limit` | tier4_planning_msgs::VelocityLimitClearCommand | clear command for velocity limit |
| `~output/stop_reasons` | tier4_planning_msgs::StopReasonArray | reasons that make the vehicle to stop |
| Name | Type | Description |
| ------------------------------- | ---------------------------------------------- | ------------------------------------- |
| `~/output/trajectory` | autoware_auto_planning_msgs::Trajectory | output trajectory |
| `~/output/velocity_limit` | tier4_planning_msgs::VelocityLimit | velocity limit for cruising |
| `~/output/clear_velocity_limit` | tier4_planning_msgs::VelocityLimitClearCommand | clear command for velocity limit |
| `~/output/stop_reasons` | tier4_planning_msgs::StopReasonArray | reasons that make the vehicle to stop |

## Design

Expand Down Expand Up @@ -176,16 +175,37 @@ This includes not only cruising a front vehicle, but also reacting a cut-in and
The safe distance is calculated dynamically based on the Responsibility-Sensitive Safety (RSS) by the following equation.

$$
d = 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}},
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$ 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.
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 |
| --------------------------------- | ------ | ----------------------------------------------------------------------------- |
| `common.idling_time` | double | idling time for the ego to detect the front vehicle starting deceleration [s] |
| `common.min_object_accel_for_rss` | double | front obstacle's acceleration [m/ss] |
| `common.min_ego_accel_for_rss` | double | ego's acceleration for RSS [m/ss] |
| `common.min_object_accel_for_rss` | double | front obstacle's acceleration for RSS [m/ss] |

The detailed formulation is as follows.

$$
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})
$$

| Variable | Description |
| ----------------- | --------------------------------------- |
| `d` | actual distane 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` |

## Implementation

Expand Down
49 changes: 36 additions & 13 deletions planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,15 +3,18 @@
common:
planning_algorithm: "pid_base" # currently supported algorithm is "pid_base"

is_showing_debug_info: true
is_showing_debug_info: false
disable_stop_planning: false # true

# longitudinal info
idling_time: 1.5 # idling time to detect front vehicle starting deceleration [s]
idling_time: 2.0 # idling time to detect front vehicle starting deceleration [s]
min_ego_accel_for_rss: -1.0 # ego's acceleration to calculate RSS distance [m/ss]
min_object_accel_for_rss: -1.0 # front obstacle's acceleration to calculate RSS distance [m/ss]
safe_distance_margin : 6.0 # This is also used as a stop margin [m]
terminal_safe_distance_margin : 3.0 # Stop margin at the goal. This value cannot exceed safe distance margin. [m]

lpf_gain_for_accel: 0.2 # gain of low pass filter for ego acceleration [-]

nearest_dist_deviation_threshold: 3.0 # [m] for finding nearest index
nearest_yaw_deviation_threshold: 1.57 # [rad] for finding nearest index
min_behavior_stop_margin: 3.0 # [m]
Expand All @@ -25,7 +28,7 @@
bus: true
trailer: true
motorcycle: true
bicycle: false
bicycle: true
pedestrian: false

stop_obstacle_type:
Expand Down Expand Up @@ -62,7 +65,7 @@
stop_obstacle_hold_time_threshold : 1.0 # maximum time for holding closest stop obstacle

ignored_outside_obstacle_type:
unknown: false
unknown: true
car: false
truck: false
bus: false
Expand All @@ -72,21 +75,41 @@
pedestrian: true

pid_based_planner:
# use_predicted_obstacle_pose: false
use_velocity_limit_based_planner: true
error_function_type: quadratic # choose from linear, quadratic

# PID gains to keep safe distance with the front vehicle
kp: 2.5
ki: 0.0
kd: 2.3
velocity_limit_based_planner:
# PID gains to keep safe distance with the front vehicle
kp: 10.0
ki: 0.0
kd: 2.0

min_accel_during_cruise: -2.0 # minimum acceleration during cruise to slow down [m/ss]
output_ratio_during_accel: 0.6 # target acceleration is multiplied with this value while ego accelerates to catch up the front vehicle [-]
vel_to_acc_weight: 12.0 # target acceleration is calculated by (target_velocity - current_velocity) * vel_to_acc_weight [-]

enable_jerk_limit_to_output_acc: false

output_ratio_during_accel: 0.6 # target acceleration is multiplied with this value while ego accelerates to catch up the front vehicle [-]
vel_to_acc_weight: 3.0 # target acceleration is calculated by (target_velocity - current_velocity) * vel_to_acc_weight [-]
disable_target_acceleration: true

velocity_insertion_based_planner:
kp_acc: 6.0
ki_acc: 0.0
kd_acc: 2.0

kp_jerk: 20.0
ki_jerk: 0.0
kd_jerk: 0.0

output_acc_ratio_during_accel: 0.6 # target acceleration is multiplied with this value while ego accelerates to catch up the front vehicle [-]
output_jerk_ratio_during_accel: 1.0 # target acceleration is multiplied with this value while ego accelerates to catch up the front vehicle [-]

enable_jerk_limit_to_output_acc: true

min_accel_during_cruise: -2.0 # minimum acceleration during cruise to slow down [m/ss]
min_cruise_target_vel: 0.0 # minimum target velocity during slow down [m/s]
time_to_evaluate_rss: 0.0

lpf_cruise_gain: 0.2
lpf_normalized_error_cruise_dist_gain: 0.2

optimization_based_planner:
dense_resampling_time_interval: 0.2
Expand Down
86 changes: 58 additions & 28 deletions planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,37 +15,16 @@
#ifndef OBSTACLE_CRUISE_PLANNER__COMMON_STRUCTS_HPP_
#define OBSTACLE_CRUISE_PLANNER__COMMON_STRUCTS_HPP_

#include "obstacle_cruise_planner/type_alias.hpp"
#include "tier4_autoware_utils/tier4_autoware_utils.hpp"

#include <rclcpp/rclcpp.hpp>

#include "autoware_auto_perception_msgs/msg/predicted_objects.hpp"
#include "autoware_auto_planning_msgs/msg/trajectory.hpp"
#include "geometry_msgs/msg/point_stamped.hpp"
#include "visualization_msgs/msg/marker_array.hpp"

#include <boost/optional.hpp>

#include <string>
#include <vector>

using autoware_auto_perception_msgs::msg::ObjectClassification;
using autoware_auto_perception_msgs::msg::PredictedObject;
using autoware_auto_perception_msgs::msg::PredictedPath;
using autoware_auto_perception_msgs::msg::Shape;

namespace
{
std::string toHexString(const unique_identifier_msgs::msg::UUID & id)
{
std::stringstream ss;
for (auto i = 0; i < 16; ++i) {
ss << std::hex << std::setfill('0') << std::setw(2) << +id.uuid[i];
}
return ss.str();
}
} // namespace

struct TargetObstacle
{
TargetObstacle(
Expand All @@ -59,7 +38,7 @@ struct TargetObstacle
velocity_reliable = true;
velocity = aligned_velocity;
classification = object.classification.at(0);
uuid = toHexString(object.object_id);
uuid = tier4_autoware_utils::toHexString(object.object_id);

predicted_paths.clear();
for (const auto & path : object.kinematics.predicted_paths) {
Expand All @@ -85,7 +64,7 @@ struct TargetObstacle
struct ObstacleCruisePlannerData
{
rclcpp::Time current_time;
autoware_auto_planning_msgs::msg::Trajectory traj;
Trajectory traj;
geometry_msgs::msg::Pose current_pose;
double current_vel;
double current_acc;
Expand All @@ -95,6 +74,50 @@ struct ObstacleCruisePlannerData

struct LongitudinalInfo
{
explicit LongitudinalInfo(rclcpp::Node & node)
{
max_accel = node.declare_parameter<double>("normal.max_acc");
min_accel = node.declare_parameter<double>("normal.min_acc");
max_jerk = node.declare_parameter<double>("normal.max_jerk");
min_jerk = node.declare_parameter<double>("normal.min_jerk");
limit_max_accel = node.declare_parameter<double>("limit.max_acc");
limit_min_accel = node.declare_parameter<double>("limit.min_acc");
limit_max_jerk = node.declare_parameter<double>("limit.max_jerk");
limit_min_jerk = node.declare_parameter<double>("limit.min_jerk");

idling_time = node.declare_parameter<double>("common.idling_time");
min_ego_accel_for_rss = node.declare_parameter<double>("common.min_ego_accel_for_rss");
min_object_accel_for_rss = node.declare_parameter<double>("common.min_object_accel_for_rss");

safe_distance_margin = node.declare_parameter<double>("common.safe_distance_margin");
terminal_safe_distance_margin =
node.declare_parameter<double>("common.terminal_safe_distance_margin");
}

void onParam(const std::vector<rclcpp::Parameter> & parameters)
{
tier4_autoware_utils::updateParam<double>(parameters, "normal.max_accel", max_accel);
tier4_autoware_utils::updateParam<double>(parameters, "normal.min_accel", min_accel);
tier4_autoware_utils::updateParam<double>(parameters, "normal.max_jerk", max_jerk);
tier4_autoware_utils::updateParam<double>(parameters, "normal.min_jerk", min_jerk);
tier4_autoware_utils::updateParam<double>(parameters, "limit.max_accel", limit_max_accel);
tier4_autoware_utils::updateParam<double>(parameters, "limit.min_accel", limit_min_accel);
tier4_autoware_utils::updateParam<double>(parameters, "limit.max_jerk", limit_max_jerk);
tier4_autoware_utils::updateParam<double>(parameters, "limit.min_jerk", limit_min_jerk);

tier4_autoware_utils::updateParam<double>(parameters, "common.idling_time", idling_time);
tier4_autoware_utils::updateParam<double>(
parameters, "common.min_ego_accel_for_rss", min_ego_accel_for_rss);
tier4_autoware_utils::updateParam<double>(
parameters, "common.min_object_accel_for_rss", min_object_accel_for_rss);

tier4_autoware_utils::updateParam<double>(
parameters, "common.safe_distance_margin", safe_distance_margin);
tier4_autoware_utils::updateParam<double>(
parameters, "common.terminal_safe_distance_margin", terminal_safe_distance_margin);
}

// common parameter
double max_accel;
double min_accel;
double max_jerk;
Expand All @@ -103,9 +126,13 @@ struct LongitudinalInfo
double limit_min_accel;
double limit_max_jerk;
double limit_min_jerk;

// rss parameter
double idling_time;
double min_ego_accel_for_rss;
double min_object_accel_for_rss;

// distance margin
double safe_distance_margin;
double terminal_safe_distance_margin;
};
Expand All @@ -115,18 +142,21 @@ struct DebugData
std::vector<PredictedObject> intentionally_ignored_obstacles;
std::vector<TargetObstacle> obstacles_to_stop;
std::vector<TargetObstacle> obstacles_to_cruise;
visualization_msgs::msg::MarkerArray stop_wall_marker;
visualization_msgs::msg::MarkerArray cruise_wall_marker;
MarkerArray stop_wall_marker;
MarkerArray cruise_wall_marker;
std::vector<tier4_autoware_utils::Polygon2d> detection_polygons;
std::vector<geometry_msgs::msg::Point> collision_points;
};

struct EgoNearestParam
{
EgoNearestParam(const double arg_dist_threshold, const double arg_yaw_threshold)
: dist_threshold(arg_dist_threshold), yaw_threshold(arg_yaw_threshold)
EgoNearestParam() = default;
explicit EgoNearestParam(rclcpp::Node & node)
{
dist_threshold = node.declare_parameter<double>("ego_nearest_dist_threshold");
yaw_threshold = node.declare_parameter<double>("ego_nearest_yaw_threshold");
}

double dist_threshold;
double yaw_threshold;
};
Expand Down
Loading

0 comments on commit febbc13

Please sign in to comment.