From 5f77755c1c745754379a455ed0ac310109de1ea7 Mon Sep 17 00:00:00 2001 From: danielsanchezaran Date: Thu, 12 Oct 2023 12:42:09 +0900 Subject: [PATCH] =?UTF-8?q?feat(obstacle=5Fcruise=5Fplanner):=20obstacle?= =?UTF-8?q?=20type=20dependent=20slow=20down=20for=20=E2=80=A6=20(#920)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit feat(obstacle_cruise_planner): obstacle type dependent slow down for obstacle cruise planner (#5208) * Add slow down parameters dependant on obstacle type * divide obstacle parameters based on obstacle type * preserve obstacle type for velocity calculation * added pre-commit changes * change sample config params * Update Readme * Update README * small refactor for cleaner code --------- Signed-off-by: Daniel Sanchez Signed-off-by: Daniel Sanchez --- planning/obstacle_cruise_planner/README.md | 21 +++-- .../config/obstacle_cruise_planner.param.yaml | 17 +++- .../common_structs.hpp | 8 +- .../planner_interface.hpp | 80 +++++++++++++++---- planning/obstacle_cruise_planner/src/node.cpp | 6 +- .../src/planner_interface.cpp | 7 +- 6 files changed, 101 insertions(+), 38 deletions(-) diff --git a/planning/obstacle_cruise_planner/README.md b/planning/obstacle_cruise_planner/README.md index 314f9dcc29393..260f302791079 100644 --- a/planning/obstacle_cruise_planner/README.md +++ b/planning/obstacle_cruise_planner/README.md @@ -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. diff --git a/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml b/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml index f3f1932c44c43..93536452a1e9a 100644 --- a/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml +++ b/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml @@ -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] diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp index faa7cfcd8e96a..ed753d1a16ca3 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp @@ -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 diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/planner_interface.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/planner_interface.hpp index 879f2e0b8b43d..f084d45de07c7 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/planner_interface.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/planner_interface.hpp @@ -26,6 +26,7 @@ #include #include #include +#include #include #include @@ -220,12 +221,42 @@ class PlannerInterface struct SlowDownParam { + std::vector obstacle_labels{"default"}; + std::unordered_map 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("slow_down.max_lat_margin"); - min_lat_margin = node.declare_parameter("slow_down.min_lat_margin"); - max_ego_velocity = node.declare_parameter("slow_down.max_ego_velocity"); - min_ego_velocity = node.declare_parameter("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>("slow_down.labels", obstacle_labels); + // obstacle label dependant parameters + for (const auto & label : obstacle_labels) { + ObstacleSpecificParams params; + params.max_lat_margin = + node.declare_parameter("slow_down." + label + ".max_lat_margin"); + params.min_lat_margin = + node.declare_parameter("slow_down." + label + ".min_lat_margin"); + params.max_ego_velocity = + node.declare_parameter("slow_down." + label + ".max_ego_velocity"); + params.min_ego_velocity = + node.declare_parameter("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("slow_down.time_margin_on_target_velocity"); lpf_gain_slow_down_vel = node.declare_parameter("slow_down.lpf_gain_slow_down_vel"); @@ -234,16 +265,35 @@ class PlannerInterface node.declare_parameter("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 & parameters) { - tier4_autoware_utils::updateParam( - parameters, "slow_down.max_lat_margin", max_lat_margin); - tier4_autoware_utils::updateParam( - parameters, "slow_down.min_lat_margin", min_lat_margin); - tier4_autoware_utils::updateParam( - parameters, "slow_down.max_ego_velocity", max_ego_velocity); - tier4_autoware_utils::updateParam( - 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( + parameters, "slow_down." + label + ".max_lat_margin", + param_by_obstacle_label.max_lat_margin); + tier4_autoware_utils::updateParam( + parameters, "slow_down." + label + ".min_lat_margin", + param_by_obstacle_label.min_lat_margin); + tier4_autoware_utils::updateParam( + parameters, "slow_down." + label + ".max_ego_velocity", + param_by_obstacle_label.max_ego_velocity); + tier4_autoware_utils::updateParam( + parameters, "slow_down." + label + ".min_ego_velocity", + param_by_obstacle_label.min_ego_velocity); + } + + // common parameters tier4_autoware_utils::updateParam( parameters, "slow_down.time_margin_on_target_velocity", time_margin_on_target_velocity); tier4_autoware_utils::updateParam( @@ -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 obstacle_to_param_struct_map; + double time_margin_on_target_velocity; double lpf_gain_slow_down_vel; double lpf_gain_lat_dist; diff --git a/planning/obstacle_cruise_planner/src/node.cpp b/planning/obstacle_cruise_planner/src/node.cpp index a4554128651b7..f62c6f0b2841e 100644 --- a/planning/obstacle_cruise_planner/src/node.cpp +++ b/planning/obstacle_cruise_planner/src/node.cpp @@ -1107,9 +1107,9 @@ std::optional 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( diff --git a/planning/obstacle_cruise_planner/src/planner_interface.cpp b/planning/obstacle_cruise_planner/src/planner_interface.cpp index 4656988750e65..6c50f2c32a2a1 100644 --- a/planning/obstacle_cruise_planner/src/planner_interface.cpp +++ b/planning/obstacle_cruise_planner/src/planner_interface.cpp @@ -662,7 +662,7 @@ std::vector PlannerInterface::generateSlowDownTrajectory( double PlannerInterface::calculateSlowDownVelocity( const SlowDownObstacle & obstacle, const std::optional & 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) { @@ -688,7 +688,6 @@ PlannerInterface::calculateDistanceToSlowDownWithConstraints( const SlowDownObstacle & obstacle, const std::optional & 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); @@ -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 =