Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

feat(obstacle_cruise_planner): obstacle type dependent slow down for … #920

Merged
merged 1 commit into from
Oct 12, 2023
Merged

feat(obstacle_cruise_planner): obstacle type dependent slow down for … #920

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
21 changes: 13 additions & 8 deletions planning/obstacle_cruise_planner/README.md
View file
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -1107,9 +1107,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
Open in desktop
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
Loading