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 123d0dda93b7a..def2a2af78b37 100644 --- a/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml +++ b/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml @@ -183,15 +183,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 d8c31fb35df9b..2e1e165a78952 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 @@ -49,6 +49,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; @@ -183,11 +188,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; @@ -203,15 +209,17 @@ class PlannerInterface double precise_lat_dist; std::optional start_point{std::nullopt}; std::optional end_point{std::nullopt}; + bool is_moving; }; double calculateMarginFromObstacleOnCurve( const PlannerData & planner_data, const StopObstacle & stop_obstacle) const; 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 { @@ -223,6 +231,7 @@ class PlannerInterface struct SlowDownParam { std::vector obstacle_labels{"default"}; + std::vector obstacle_moving_classification{"static", "moving"}; std::unordered_map types_map; struct ObstacleSpecificParams { @@ -233,28 +242,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 @@ -264,34 +268,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 @@ -313,7 +332,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_; // previous trajectory and distance to stop // NOTE: Previous trajectory is memorized to deal with nearest index search for overlapping or diff --git a/planning/obstacle_cruise_planner/src/planner_interface.cpp b/planning/obstacle_cruise_planner/src/planner_interface.cpp index 76469364cfb39..47f62016df53b 100644 --- a/planning/obstacle_cruise_planner/src/planner_interface.cpp +++ b/planning/obstacle_cruise_planner/src/planner_interface.cpp @@ -561,9 +561,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_, @@ -648,7 +656,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_ @@ -663,10 +671,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( @@ -689,15 +698,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 =