Skip to content

Commit

Permalink
feat(obstacle_cruise_planner): obstacle type dependent slow down for …
Browse files Browse the repository at this point in the history 
…obstacle cruise planner (autowarefoundation#5208)

* Add slow down parameters dependant on obstacle type

Signed-off-by: Daniel Sanchez <[email protected]>

* divide obstacle parameters based on obstacle type

Signed-off-by: Daniel Sanchez <[email protected]>

* preserve obstacle type for velocity calculation

Signed-off-by: Daniel Sanchez <[email protected]>

* added pre-commit changes

Signed-off-by: Daniel Sanchez <[email protected]>

* change sample config params

Signed-off-by: Daniel Sanchez <[email protected]>

* Update Readme

Signed-off-by: Daniel Sanchez <[email protected]>

* Update README

Signed-off-by: Daniel Sanchez <[email protected]>

* small refactor for cleaner code

Signed-off-by: Daniel Sanchez <[email protected]>

---------

Signed-off-by: Daniel Sanchez <[email protected]>
Signed-off-by: Daniel Sanchez <[email protected]>
  • Loading branch information
@danielsanchezaran
danielsanchezaran committed Oct 6, 2023
1 parent 10a2376 commit ac6842f
Show file tree
Hide file tree
Showing 6 changed files with 101 additions and 38 deletions.
21 changes: 13 additions & 8 deletions planning/obstacle_cruise_planner/README.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -230,14 +230,19 @@ $$

### Slow down planning

| Parameter | Type | Description |
| ---------------------------- | ------ | ------------------------------------------------------------------- |
| `slow_down.min_lat_velocity` | double | minimum velocity to linearly calculate slow down velocity [m] |
| `slow_down.max_lat_velocity` | double | maximum velocity to linearly calculate slow down velocity [m] |
| `slow_down.min_lat_margin` | double | minimum lateral margin to linearly calculate slow down velocity [m] |
| `slow_down.max_lat_margin` | double | maximum lateral margin to linearly calculate slow down velocity [m] |

The role of the slow down planning is inserting slow down velocity in the trajectory where the trajectory points are close to the obstacles.
| Parameter | Type | Description |
| ----------------------------------------------- | -------------- | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------ |
| `slow_down.labels` | vector(string) | A vector of labels for customizing obstacle-label-based slow down behavior. Each label represents an obstacle type that will be treated differently when applying slow down. The possible labels are ("default" (Mandatory), "unknown","car","truck","bus","trailer","motorcycle","bicycle" or "pedestrian") |
| `slow_down.default.min_lat_velocity` | double | minimum velocity to linearly calculate slow down velocity [m]. Note: This default value will be used when the detected obstacle label does not match any of the slow_down.labels |
| `slow_down.default.max_lat_velocity` | double | maximum velocity to linearly calculate slow down velocity [m]. Note: This default value will be used when the detected obstacle label does not match any of the slow_down.labels |
| `slow_down.default.min_lat_margin` | double | minimum lateral margin to linearly calculate slow down velocity [m]. Note: This default value will be used when the detected obstacle label does not match any of the slow_down.labels |
| `slow_down.default.max_lat_margin` | double | maximum lateral margin to linearly calculate slow down velocity [m]. Note: This default value will be used when the detected obstacle label does not match any of the slow_down.labels |
| `(optional) slow_down."label".min_lat_velocity` | double | minimum velocity to linearly calculate slow down velocity [m]. Note: only for obstacles specified in `slow_down.labels` |
| `(optional) slow_down."label".max_lat_velocity` | double | maximum velocity to linearly calculate slow down velocity [m]. Note: only for obstacles specified in `slow_down.labels` |
| `(optional) slow_down."label".min_lat_margin` | double | minimum lateral margin to linearly calculate slow down velocity [m]. Note: only for obstacles specified in `slow_down.labels` |
| `(optional) slow_down."label".max_lat_margin` | double | maximum lateral margin to linearly calculate slow down velocity [m]. Note: only for obstacles specified in `slow_down.labels` |

The role of the slow down planning is inserting slow down velocity in the trajectory where the trajectory points are close to the obstacles. The parameters can be customized depending on the obstacle type (see `slow_down.labels`), making it possible to adjust the slow down behavior depending if the obstacle is a pedestrian, bicycle, car, etc.

The closest point on the obstacle to the ego's trajectory is calculated.
Then, the slow down velocity is calculated by linear interpolation with the distance between the point and trajectory as follows.
Expand Down
17 changes: 13 additions & 4 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 @@ -172,10 +172,19 @@

slow_down:
# parameters to calculate slow down velocity by linear interpolation
min_lat_margin: 0.2
max_lat_margin: 1.0
min_ego_velocity: 2.0
max_ego_velocity: 8.0
labels:
- "default"
- "pedestrian"
default:
min_lat_margin: 0.2
max_lat_margin: 1.0
min_ego_velocity: 2.0
max_ego_velocity: 8.0
pedestrian:
min_lat_margin: 0.5
max_lat_margin: 1.5
min_ego_velocity: 1.0
max_ego_velocity: 2.0

time_margin_on_target_velocity: 1.5 # [s]

Expand Down
8 changes: 5 additions & 3 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 @@ -133,19 +133,21 @@ struct SlowDownObstacle : public TargetObstacleInterface
{
SlowDownObstacle(
const std::string & arg_uuid, const rclcpp::Time & arg_stamp,
const geometry_msgs::msg::Pose & arg_pose, const double arg_lon_velocity,
const double arg_lat_velocity, const double arg_precise_lat_dist,
const ObjectClassification & object_classification, const geometry_msgs::msg::Pose & arg_pose,
const double arg_lon_velocity, const double arg_lat_velocity, const double arg_precise_lat_dist,
const geometry_msgs::msg::Point & arg_front_collision_point,
const geometry_msgs::msg::Point & arg_back_collision_point)
: TargetObstacleInterface(arg_uuid, arg_stamp, arg_pose, arg_lon_velocity, arg_lat_velocity),
precise_lat_dist(arg_precise_lat_dist),
front_collision_point(arg_front_collision_point),
back_collision_point(arg_back_collision_point)
back_collision_point(arg_back_collision_point),
classification(object_classification)
{
}
double precise_lat_dist; // for efficient calculation
geometry_msgs::msg::Point front_collision_point;
geometry_msgs::msg::Point back_collision_point;
ObjectClassification classification;
};

struct LongitudinalInfo
Expand Down
80 changes: 64 additions & 16 deletions planning/obstacle_cruise_planner/include/obstacle_cruise_planner/planner_interface.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -26,6 +26,7 @@
#include <optional>
#include <string>
#include <tuple>
#include <unordered_map>
#include <utility>
#include <vector>

Expand Down Expand Up @@ -220,12 +221,42 @@ class PlannerInterface

struct SlowDownParam
{
std::vector<std::string> obstacle_labels{"default"};
std::unordered_map<uint8_t, std::string> types_map;
struct ObstacleSpecificParams
{
double max_lat_margin;
double min_lat_margin;
double max_ego_velocity;
double min_ego_velocity;
};
explicit SlowDownParam(rclcpp::Node & node)
{
max_lat_margin = node.declare_parameter<double>("slow_down.max_lat_margin");
min_lat_margin = node.declare_parameter<double>("slow_down.min_lat_margin");
max_ego_velocity = node.declare_parameter<double>("slow_down.max_ego_velocity");
min_ego_velocity = node.declare_parameter<double>("slow_down.min_ego_velocity");
types_map = {{ObjectClassification::UNKNOWN, "unknown"},
{ObjectClassification::CAR, "car"},
{ObjectClassification::TRUCK, "truck"},
{ObjectClassification::BUS, "bus"},
{ObjectClassification::TRAILER, "trailer"},
{ObjectClassification::MOTORCYCLE, "motorcycle"},
{ObjectClassification::BICYCLE, "bicycle"},
{ObjectClassification::PEDESTRIAN, "pedestrian"}};
obstacle_labels =
node.declare_parameter<std::vector<std::string>>("slow_down.labels", obstacle_labels);
// obstacle label dependant parameters
for (const auto & label : obstacle_labels) {
ObstacleSpecificParams params;
params.max_lat_margin =
node.declare_parameter<double>("slow_down." + label + ".max_lat_margin");
params.min_lat_margin =
node.declare_parameter<double>("slow_down." + label + ".min_lat_margin");
params.max_ego_velocity =
node.declare_parameter<double>("slow_down." + label + ".max_ego_velocity");
params.min_ego_velocity =
node.declare_parameter<double>("slow_down." + label + ".min_ego_velocity");
obstacle_to_param_struct_map.emplace(std::make_pair(label, params));
}

// common parameters
time_margin_on_target_velocity =
node.declare_parameter<double>("slow_down.time_margin_on_target_velocity");
lpf_gain_slow_down_vel = node.declare_parameter<double>("slow_down.lpf_gain_slow_down_vel");
Expand All @@ -234,16 +265,35 @@ class PlannerInterface
node.declare_parameter<double>("slow_down.lpf_gain_dist_to_slow_down");
}

ObstacleSpecificParams getObstacleParamByLabel(const ObjectClassification & label_id) const
{
const std::string label = types_map.at(label_id.label);
if (obstacle_to_param_struct_map.count(label) > 0) {
return obstacle_to_param_struct_map.at(label);
}
return obstacle_to_param_struct_map.at("default");
}

void onParam(const std::vector<rclcpp::Parameter> & parameters)
{
tier4_autoware_utils::updateParam<double>(
parameters, "slow_down.max_lat_margin", max_lat_margin);
tier4_autoware_utils::updateParam<double>(
parameters, "slow_down.min_lat_margin", min_lat_margin);
tier4_autoware_utils::updateParam<double>(
parameters, "slow_down.max_ego_velocity", max_ego_velocity);
tier4_autoware_utils::updateParam<double>(
parameters, "slow_down.min_ego_velocity", min_ego_velocity);
// obstacle type dependant parameters
for (const auto & label : obstacle_labels) {
auto & param_by_obstacle_label = obstacle_to_param_struct_map[label];
tier4_autoware_utils::updateParam<double>(
parameters, "slow_down." + label + ".max_lat_margin",
param_by_obstacle_label.max_lat_margin);
tier4_autoware_utils::updateParam<double>(
parameters, "slow_down." + label + ".min_lat_margin",
param_by_obstacle_label.min_lat_margin);
tier4_autoware_utils::updateParam<double>(
parameters, "slow_down." + label + ".max_ego_velocity",
param_by_obstacle_label.max_ego_velocity);
tier4_autoware_utils::updateParam<double>(
parameters, "slow_down." + label + ".min_ego_velocity",
param_by_obstacle_label.min_ego_velocity);
}

// common parameters
tier4_autoware_utils::updateParam<double>(
parameters, "slow_down.time_margin_on_target_velocity", time_margin_on_target_velocity);
tier4_autoware_utils::updateParam<double>(
Expand All @@ -254,10 +304,8 @@ class PlannerInterface
parameters, "slow_down.lpf_gain_dist_to_slow_down", lpf_gain_dist_to_slow_down);
}

double max_lat_margin;
double min_lat_margin;
double max_ego_velocity;
double min_ego_velocity;
std::unordered_map<std::string, ObstacleSpecificParams> obstacle_to_param_struct_map;

double time_margin_on_target_velocity;
double lpf_gain_slow_down_vel;
double lpf_gain_lat_dist;
Expand Down
6 changes: 3 additions & 3 deletions planning/obstacle_cruise_planner/src/node.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1106,9 +1106,9 @@ std::optional<SlowDownObstacle> ObstacleCruisePlannerNode::createSlowDownObstacl
}

const auto [tangent_vel, normal_vel] = projectObstacleVelocityToTrajectory(traj_points, obstacle);
return SlowDownObstacle{obstacle.uuid, obstacle.stamp, obstacle.pose,
tangent_vel, normal_vel, precise_lat_dist,
front_collision_point, back_collision_point};
return SlowDownObstacle{obstacle.uuid, obstacle.stamp, obstacle.classification,
obstacle.pose, tangent_vel, normal_vel,
precise_lat_dist, front_collision_point, back_collision_point};
}

void ObstacleCruisePlannerNode::checkConsistency(
Expand Down
7 changes: 3 additions & 4 deletions planning/obstacle_cruise_planner/src/planner_interface.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -662,7 +662,7 @@ std::vector<TrajectoryPoint> PlannerInterface::generateSlowDownTrajectory(
double PlannerInterface::calculateSlowDownVelocity(
const SlowDownObstacle & obstacle, const std::optional<SlowDownOutput> & prev_output) const
{
const auto & p = slow_down_param_;
const auto & p = slow_down_param_.getObstacleParamByLabel(obstacle.classification);

const double stable_precise_lat_dist = [&]() {
if (prev_output) {
Expand All @@ -688,7 +688,6 @@ PlannerInterface::calculateDistanceToSlowDownWithConstraints(
const SlowDownObstacle & obstacle, const std::optional<SlowDownOutput> & prev_output,
const double dist_to_ego) const
{
const auto & p = slow_down_param_;
const double abs_ego_offset = planner_data.is_driving_forward
? std::abs(vehicle_info_.max_longitudinal_offset_m)
: std::abs(vehicle_info_.min_longitudinal_offset_m);
Expand Down Expand Up @@ -725,8 +724,8 @@ PlannerInterface::calculateDistanceToSlowDownWithConstraints(

// calculate distance during deceleration, slow down preparation, and slow down
const double min_slow_down_prepare_dist = 3.0;
const double slow_down_prepare_dist =
std::max(min_slow_down_prepare_dist, slow_down_vel * p.time_margin_on_target_velocity);
const double slow_down_prepare_dist = std::max(
min_slow_down_prepare_dist, slow_down_vel * slow_down_param_.time_margin_on_target_velocity);
const double deceleration_dist = offset_dist_to_collision + dist_to_front_collision -
abs_ego_offset - dist_to_ego - slow_down_prepare_dist;
const double slow_down_dist =
Expand Down

0 comments on commit ac6842f

Please sign in to comment.