From f2029306059dd8bfaab67c389516a2eda76b5995 Mon Sep 17 00:00:00 2001 From: danielsanchezaran Date: Tue, 14 Nov 2023 11:03:49 +0900 Subject: [PATCH] feat(obstacle_cruise_planner): use obstacle velocity based obstacle parameters (#5510) * Add extra tag for moving obstacle type Signed-off-by: Daniel Sanchez * add static and moving as parameter postfixes Signed-off-by: Daniel Sanchez * set hysteresis-based obstacle moving classification Signed-off-by: Daniel Sanchez * update config params Signed-off-by: Daniel Sanchez * Use speed norm for object classification Signed-off-by: Daniel Sanchez * changed '_' for '.' to make the code more consistent Signed-off-by: Daniel Sanchez * update documentation Signed-off-by: Daniel Sanchez * move the obstacle moving check to generateSlowDownTrajectory Signed-off-by: Daniel Sanchez * remove unnecessary reference Signed-off-by: Daniel Sanchez * add const to certain variables Signed-off-by: Daniel Sanchez --------- Signed-off-by: Daniel Sanchez Signed-off-by: Daniel Sanchez --- planning/obstacle_cruise_planner/README.md | 32 +++--- .../config/obstacle_cruise_planner.param.yaml | 31 ++++-- .../planner_interface.hpp | 104 +++++++++++------- .../src/planner_interface.cpp | 24 ++-- 4 files changed, 120 insertions(+), 71 deletions(-) diff --git a/planning/obstacle_cruise_planner/README.md b/planning/obstacle_cruise_planner/README.md index 260f302791079..532761b1f0cb7 100644 --- a/planning/obstacle_cruise_planner/README.md +++ b/planning/obstacle_cruise_planner/README.md @@ -230,19 +230,25 @@ $$ ### Slow down planning -| 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. +| 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.static.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 and the obstacle is considered to be static, or not moving | +| `slow_down.default.static.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 and the obstacle is considered to be static, or not moving | +| `slow_down.default.static.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 and the obstacle is considered to be static, or not moving | +| `slow_down.default.static.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 and the obstacle is considered to be static, or not moving | +| `slow_down.default.moving.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 and the obstacle is considered to be moving | +| `slow_down.default.moving.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 and the obstacle is considered to be moving | +| `slow_down.default.moving.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 and the obstacle is considered to be moving | +| `slow_down.default.moving.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 and the obstacle is considered to be moving | +| `(optional) slow_down."label".(static & moving).min_lat_velocity` | double | minimum velocity to linearly calculate slow down velocity [m]. Note: only for obstacles specified in `slow_down.labels`. Requires a `static` and a `moving` value | +| `(optional) slow_down."label".(static & moving).max_lat_velocity` | double | maximum velocity to linearly calculate slow down velocity [m]. Note: only for obstacles specified in `slow_down.labels`. Requires a `static` and a `moving` value | +| `(optional) slow_down."label".(static & moving).min_lat_margin` | double | minimum lateral margin to linearly calculate slow down velocity [m]. Note: only for obstacles specified in `slow_down.labels`. Requires a `static` and a `moving` value | +| `(optional) slow_down."label".(static & moving).max_lat_margin` | double | maximum lateral margin to linearly calculate slow down velocity [m]. Note: only for obstacles specified in `slow_down.labels`. Requires a `static` and a `moving` value | + +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. Each obstacle type has a `static` and a `moving` parameter set, so it is possible to customize the slow down response of the ego vehicle according to the obstacle type and if it is moving or not. If an obstacle is determined to be moving, the corresponding `moving` set of parameters will be used to compute the vehicle slow down, otherwise, the `static` parameters will be used. The `static` and `moving` separation is useful for customizing the ego vehicle slow down behavior to, for example, slow down more significantly when passing stopped vehicles that might cause occlusion or that might suddenly open its doors. + +An obstacle is classified as `static` if its total speed is less than the `moving_object_speed_threshold` parameter. Furthermore, a hysteresis based approach is used to avoid chattering, it uses the `moving_object_hysteresis_range` parameter range and the obstacle's previous state (`moving` or `static`) to determine if the obstacle is moving or not. In other words, if an obstacle was previously classified as `static`, it will not change its classification to `moving` unless its total speed is greater than `moving_object_speed_threshold` + `moving_object_hysteresis_range`. Likewise, an obstacle previously classified as `moving`, will only change to `static` if its speed is lower than `moving_object_speed_threshold` - `moving_object_hysteresis_range`. 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 20629fc9667eb..13b2a698afe8c 100644 --- a/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml +++ b/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml @@ -176,15 +176,30 @@ - "default" - "pedestrian" default: - min_lat_margin: 0.2 - max_lat_margin: 1.0 - min_ego_velocity: 2.0 - max_ego_velocity: 8.0 + moving: + min_lat_margin: 0.8 + max_lat_margin: 1.3 + min_ego_velocity: 0.5 + max_ego_velocity: 1.5 + static: + 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 + moving: + min_lat_margin: 0.8 + max_lat_margin: 1.3 + min_ego_velocity: 0.5 + max_ego_velocity: 0.8 + static: + min_lat_margin: 0.8 + max_lat_margin: 1.3 + min_ego_velocity: 1.0 + max_ego_velocity: 2.0 + + moving_object_speed_threshold: 0.5 # [m/s] how fast the object needs to move to be considered as "moving" + moving_object_hysteresis_range: 0.1 # [m/s] hysteresis range used to prevent chattering when obstacle moves close to moving_object_speed_threshold time_margin_on_target_velocity: 1.5 # [s] 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 b3fd21535e261..10eab4523e0b4 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 @@ -48,6 +48,11 @@ class PlannerInterface node.create_publisher("/planning/velocity_factors/obstacle_cruise", 1); stop_speed_exceeded_pub_ = node.create_publisher("~/output/stop_speed_exceeded", 1); + + moving_object_speed_threshold = + node.declare_parameter("slow_down.moving_object_speed_threshold"); + moving_object_hysteresis_range = + node.declare_parameter("slow_down.moving_object_hysteresis_range"); } PlannerInterface() = default; @@ -172,11 +177,12 @@ class PlannerInterface const std::string & arg_uuid, const std::vector & traj_points, const std::optional & start_idx, const std::optional & end_idx, const double arg_target_vel, const double arg_feasible_target_vel, - const double arg_precise_lat_dist) + const double arg_precise_lat_dist, const bool is_moving) : uuid(arg_uuid), target_vel(arg_target_vel), feasible_target_vel(arg_feasible_target_vel), - precise_lat_dist(arg_precise_lat_dist) + precise_lat_dist(arg_precise_lat_dist), + is_moving(is_moving) { if (start_idx) { start_point = traj_points.at(*start_idx).pose; @@ -192,13 +198,15 @@ class PlannerInterface double precise_lat_dist; std::optional start_point{std::nullopt}; std::optional end_point{std::nullopt}; + bool is_moving; }; double calculateSlowDownVelocity( - const SlowDownObstacle & obstacle, const std::optional & prev_output) const; + const SlowDownObstacle & obstacle, const std::optional & prev_output, + const bool is_obstacle_moving) const; std::optional> calculateDistanceToSlowDownWithConstraints( const PlannerData & planner_data, const std::vector & traj_points, const SlowDownObstacle & obstacle, const std::optional & prev_output, - const double dist_to_ego) const; + const double dist_to_ego, const bool is_obstacle_moving) const; struct SlowDownInfo { @@ -210,6 +218,7 @@ class PlannerInterface struct SlowDownParam { std::vector obstacle_labels{"default"}; + std::vector obstacle_moving_classification{"static", "moving"}; std::unordered_map types_map; struct ObstacleSpecificParams { @@ -220,28 +229,23 @@ class PlannerInterface }; explicit SlowDownParam(rclcpp::Node & node) { - 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)); + for (const auto & movement_postfix : obstacle_moving_classification) { + ObstacleSpecificParams params; + params.max_lat_margin = node.declare_parameter( + "slow_down." + label + "." + movement_postfix + ".max_lat_margin"); + params.min_lat_margin = node.declare_parameter( + "slow_down." + label + "." + movement_postfix + ".min_lat_margin"); + params.max_ego_velocity = node.declare_parameter( + "slow_down." + label + "." + movement_postfix + ".max_ego_velocity"); + params.min_ego_velocity = node.declare_parameter( + "slow_down." + label + "." + movement_postfix + ".min_ego_velocity"); + obstacle_to_param_struct_map.emplace( + std::make_pair(label + "." + movement_postfix, params)); + } } // common parameters @@ -251,34 +255,49 @@ class PlannerInterface lpf_gain_lat_dist = node.declare_parameter("slow_down.lpf_gain_lat_dist"); lpf_gain_dist_to_slow_down = node.declare_parameter("slow_down.lpf_gain_dist_to_slow_down"); + + 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"}}; } - ObstacleSpecificParams getObstacleParamByLabel(const ObjectClassification & label_id) const + ObstacleSpecificParams getObstacleParamByLabel( + const ObjectClassification & label_id, const bool is_obstacle_moving) 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"); + const std::string label = + (types_map.count(label_id.label) > 0) ? types_map.at(label_id.label) : "default"; + const std::string movement_postfix = (is_obstacle_moving) ? "moving" : "static"; + return (obstacle_to_param_struct_map.count(label + "." + movement_postfix) > 0) + ? obstacle_to_param_struct_map.at(label + "." + movement_postfix) + : obstacle_to_param_struct_map.at("default." + movement_postfix); } void onParam(const std::vector & parameters) { // 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); + for (const auto & movement_postfix : obstacle_moving_classification) { + if (obstacle_to_param_struct_map.count(label + "." + movement_postfix) < 1) continue; + auto & param_by_obstacle_label = + obstacle_to_param_struct_map.at(label + "." + movement_postfix); + tier4_autoware_utils::updateParam( + parameters, "slow_down." + label + "." + movement_postfix + ".max_lat_margin", + param_by_obstacle_label.max_lat_margin); + tier4_autoware_utils::updateParam( + parameters, "slow_down." + label + "." + movement_postfix + ".min_lat_margin", + param_by_obstacle_label.min_lat_margin); + tier4_autoware_utils::updateParam( + parameters, "slow_down." + label + "." + movement_postfix + ".max_ego_velocity", + param_by_obstacle_label.max_ego_velocity); + tier4_autoware_utils::updateParam( + parameters, "slow_down." + label + "." + movement_postfix + ".min_ego_velocity", + param_by_obstacle_label.min_ego_velocity); + } } // common parameters @@ -300,7 +319,8 @@ class PlannerInterface double lpf_gain_dist_to_slow_down; }; SlowDownParam slow_down_param_; - + double moving_object_speed_threshold; + double moving_object_hysteresis_range; std::vector prev_slow_down_output_; }; diff --git a/planning/obstacle_cruise_planner/src/planner_interface.cpp b/planning/obstacle_cruise_planner/src/planner_interface.cpp index a7fc55a3d7740..c3dccce140e07 100644 --- a/planning/obstacle_cruise_planner/src/planner_interface.cpp +++ b/planning/obstacle_cruise_planner/src/planner_interface.cpp @@ -417,9 +417,17 @@ std::vector PlannerInterface::generateSlowDownTrajectory( const auto & obstacle = obstacles.at(i); const auto prev_output = getObjectFromUuid(prev_slow_down_output_, obstacle.uuid); + const bool is_obstacle_moving = [&]() -> bool { + const auto object_vel_norm = std::hypot(obstacle.velocity, obstacle.lat_velocity); + if (!prev_output) return object_vel_norm > moving_object_speed_threshold; + if (prev_output->is_moving) + return object_vel_norm > moving_object_speed_threshold - moving_object_hysteresis_range; + return object_vel_norm > moving_object_speed_threshold + moving_object_hysteresis_range; + }(); + // calculate slow down start distance, and insert slow down velocity const auto dist_vec_to_slow_down = calculateDistanceToSlowDownWithConstraints( - planner_data, slow_down_traj_points, obstacle, prev_output, dist_to_ego); + planner_data, slow_down_traj_points, obstacle, prev_output, dist_to_ego, is_obstacle_moving); if (!dist_vec_to_slow_down) { RCLCPP_INFO_EXPRESSION( rclcpp::get_logger("ObstacleCruisePlanner::PlannerInterface"), enable_debug_info_, @@ -504,7 +512,7 @@ std::vector PlannerInterface::generateSlowDownTrajectory( // update prev_slow_down_output_ new_prev_slow_down_output.push_back(SlowDownOutput{ obstacle.uuid, slow_down_traj_points, slow_down_start_idx, slow_down_end_idx, - stable_slow_down_vel, feasible_slow_down_vel, obstacle.precise_lat_dist}); + stable_slow_down_vel, feasible_slow_down_vel, obstacle.precise_lat_dist, is_obstacle_moving}); } // update prev_slow_down_output_ @@ -519,10 +527,11 @@ std::vector PlannerInterface::generateSlowDownTrajectory( } double PlannerInterface::calculateSlowDownVelocity( - const SlowDownObstacle & obstacle, const std::optional & prev_output) const + const SlowDownObstacle & obstacle, const std::optional & prev_output, + const bool is_obstacle_moving) const { - const auto & p = slow_down_param_.getObstacleParamByLabel(obstacle.classification); - + const auto & p = + slow_down_param_.getObstacleParamByLabel(obstacle.classification, is_obstacle_moving); const double stable_precise_lat_dist = [&]() { if (prev_output) { return signal_processing::lowpassFilter( @@ -545,15 +554,14 @@ std::optional> PlannerInterface::calculateDistanceToSlowDownWithConstraints( const PlannerData & planner_data, const std::vector & traj_points, const SlowDownObstacle & obstacle, const std::optional & prev_output, - const double dist_to_ego) const + const double dist_to_ego, const bool is_obstacle_moving) const { 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); const double obstacle_vel = obstacle.velocity; - // calculate slow down velocity - const double slow_down_vel = calculateSlowDownVelocity(obstacle, prev_output); + const double slow_down_vel = calculateSlowDownVelocity(obstacle, prev_output, is_obstacle_moving); // calculate distance to collision points const double dist_to_front_collision =