From a04f825bc542618fb86a347b45ae90162c1ff17e Mon Sep 17 00:00:00 2001 From: danielsanchezaran Date: Wed, 27 Dec 2023 11:29:55 +0900 Subject: [PATCH] feat(map_based_prediction): prediction with acc constraints (#5960) * WIP add acc constraints to discard paths Signed-off-by: Daniel Sanchez * Add lat acc constraints to predicted paths Signed-off-by: Daniel Sanchez * Delete debug print Signed-off-by: Daniel Sanchez * delete unused var Signed-off-by: Daniel Sanchez * delete unused var Signed-off-by: Daniel Sanchez * WIP Signed-off-by: Daniel Sanchez * make lat acc calculation with yaw rate Signed-off-by: Daniel Sanchez * eliminate debug prints Signed-off-by: Daniel Sanchez * delete unused variable Signed-off-by: Daniel Sanchez * refactor rename function Signed-off-by: Daniel Sanchez * keep a copy of the straightest path in case all paths are cleared Signed-off-by: Daniel Sanchez * refactor and add constraints check parameter Signed-off-by: Daniel Sanchez * update documentation Signed-off-by: Daniel Sanchez * add braces for readability (style guide) Signed-off-by: Daniel Sanchez * fix review issues Signed-off-by: Daniel Sanchez --------- Signed-off-by: Daniel Sanchez Signed-off-by: Daniel Sanchez --- perception/map_based_prediction/README.md | 20 ++- .../config/map_based_prediction.param.yaml | 3 + .../map_based_prediction_node.hpp | 125 +++++++++++++++++- .../src/map_based_prediction_node.cpp | 68 +++++++++- 4 files changed, 211 insertions(+), 5 deletions(-) diff --git a/perception/map_based_prediction/README.md b/perception/map_based_prediction/README.md index 6d3d7f5e035a9..ec2f57963332f 100644 --- a/perception/map_based_prediction/README.md +++ b/perception/map_based_prediction/README.md @@ -66,7 +66,7 @@ Lane change logics is illustrated in the figure below.An example of how to tune ### Tuning lane change detection logic -Currently we provide two parameters to tune lane change detection: +Currently we provide three parameters to tune lane change detection: - `dist_threshold_to_bound_`: maximum distance from lane boundary allowed for lane changing vehicle - `time_threshold_to_bound_`: maximum time allowed for lane change vehicle to reach the boundary @@ -124,6 +124,24 @@ See paper [2] for more details. `lateral_control_time_horizon` parameter supports the tuning of the lateral path shape. This parameter is used to calculate the time to reach the reference path. The smaller the value, the more the path will be generated to reach the reference path quickly. (Mostly the center of the lane.) +#### Pruning predicted paths with lateral acceleration constraint + +It is possible to apply a maximum lateral acceleration constraint to generated vehicle paths. This check verifies if it is possible for the vehicle to perform the predicted path without surpassing a lateral acceleration threshold `max_lateral_accel` when taking a curve. If it is not possible, it checks if the vehicle can slow down on time to take the curve with a deceleration of `min_acceleration_before_curve` and comply with the constraint. If that is also not possible, the path is eliminated. + +Currently we provide three parameters to tune the lateral acceleration constraint: + +- `check_lateral_acceleration_constraints_`: to enable the constraint check. +- `max_lateral_accel_`: max acceptable lateral acceleration for predicted paths (absolute value). +- `min_acceleration_before_curve_`: the minimum acceleration the vehicle would theoretically use to slow down before a curve is taken (must be negative). + +You can change these parameters in rosparam in the table below. + +| param name | default value | +| ----------------------------------------- | -------------- | +| `check_lateral_acceleration_constraints_` | `false` [bool] | +| `max_lateral_accel` | `2.0` [m/s^2] | +| `min_acceleration_before_curve` | `-2.0` [m/s^2] | + ### Path prediction for crosswalk users This module treats **Pedestrians** and **Bicycles** as objects using the crosswalk, and outputs prediction path based on map and estimated object's velocity, assuming the object has intention to cross the crosswalk, if the objects satisfies at least one of the following conditions: diff --git a/perception/map_based_prediction/config/map_based_prediction.param.yaml b/perception/map_based_prediction/config/map_based_prediction.param.yaml index 69ff96a263f4b..fe4d2a51ec6f3 100644 --- a/perception/map_based_prediction/config/map_based_prediction.param.yaml +++ b/perception/map_based_prediction/config/map_based_prediction.param.yaml @@ -13,6 +13,9 @@ sigma_yaw_angle_deg: 5.0 #[angle degree] object_buffer_time_length: 2.0 #[s] history_time_length: 1.0 #[s] + check_lateral_acceleration_constraints: false # whether to check if the predicted path complies with lateral acceleration constraints + max_lateral_accel: 2.0 # [m/ss] max acceptable lateral acceleration for predicted vehicle paths + min_acceleration_before_curve: -2.0 # [m/ss] min acceleration a vehicle might take to decelerate before a curve # parameter for shoulder lane prediction prediction_time_horizon_rate_for_validate_shoulder_lane_length: 0.8 diff --git a/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp b/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp index bdd9ad89bc025..02ca62a61717c 100644 --- a/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp +++ b/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp @@ -16,11 +16,15 @@ #define MAP_BASED_PREDICTION__MAP_BASED_PREDICTION_NODE_HPP_ #include "map_based_prediction/path_generator.hpp" +#include "tf2/LinearMath/Quaternion.h" +#include "tier4_autoware_utils/geometry/geometry.hpp" +#include "tier4_autoware_utils/ros/update_param.hpp" #include #include #include +#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" #include #include #include @@ -34,6 +38,7 @@ #include #include +#include #include #include #include @@ -99,9 +104,10 @@ using autoware_auto_perception_msgs::msg::PredictedPath; using autoware_auto_perception_msgs::msg::TrackedObject; using autoware_auto_perception_msgs::msg::TrackedObjectKinematics; using autoware_auto_perception_msgs::msg::TrackedObjects; +using autoware_auto_planning_msgs::msg::TrajectoryPoint; using tier4_autoware_utils::StopWatch; using tier4_debug_msgs::msg::StringStamped; - +using TrajectoryPoints = std::vector; class MapBasedPredictionNode : public rclcpp::Node { public: @@ -123,6 +129,11 @@ class MapBasedPredictionNode : public rclcpp::Node std::shared_ptr routing_graph_ptr_; std::shared_ptr traffic_rules_ptr_; + // parameter update + OnSetParametersCallbackHandle::SharedPtr set_param_res_; + rcl_interfaces::msg::SetParametersResult onParam( + const std::vector & parameters); + // Pose Transform Listener tier4_autoware_utils::TransformListener transform_listener_{this}; @@ -160,6 +171,10 @@ class MapBasedPredictionNode : public rclcpp::Node bool consider_only_routable_neighbours_; double reference_path_resolution_; + bool check_lateral_acceleration_constraints_; + double max_lateral_accel_; + double min_acceleration_before_curve_; + // Stop watch StopWatch stop_watch_; @@ -237,6 +252,114 @@ class MapBasedPredictionNode : public rclcpp::Node Maneuver predictObjectManeuverByLatDiffDistance( const TrackedObject & object, const LaneletData & current_lanelet_data, const double object_detected_time); + + // NOTE: This function is copied from the motion_velocity_smoother package. + // TODO(someone): Consolidate functions and move them to a common + inline std::vector calcTrajectoryCurvatureFrom3Points( + const TrajectoryPoints & trajectory, size_t idx_dist) + { + using tier4_autoware_utils::calcCurvature; + using tier4_autoware_utils::getPoint; + + if (trajectory.size() < 3) { + const std::vector k_arr(trajectory.size(), 0.0); + return k_arr; + } + + // if the idx size is not enough, change the idx_dist + const auto max_idx_dist = static_cast(std::floor((trajectory.size() - 1) / 2.0)); + idx_dist = std::max(1ul, std::min(idx_dist, max_idx_dist)); + + if (idx_dist < 1) { + throw std::logic_error("idx_dist less than 1 is not expected"); + } + + // calculate curvature by circle fitting from three points + std::vector k_arr(trajectory.size(), 0.0); + + for (size_t i = 1; i + 1 < trajectory.size(); i++) { + double curvature = 0.0; + const auto p0 = getPoint(trajectory.at(i - std::min(idx_dist, i))); + const auto p1 = getPoint(trajectory.at(i)); + const auto p2 = getPoint(trajectory.at(i + std::min(idx_dist, trajectory.size() - 1 - i))); + try { + curvature = calcCurvature(p0, p1, p2); + } catch (std::exception const & e) { + // ...code that handles the error... + RCLCPP_WARN(rclcpp::get_logger("map_based_prediction"), "%s", e.what()); + if (i > 1) { + curvature = k_arr.at(i - 1); // previous curvature + } else { + curvature = 0.0; + } + } + k_arr.at(i) = curvature; + } + // copy curvatures for the last and first points; + k_arr.at(0) = k_arr.at(1); + k_arr.back() = k_arr.at((trajectory.size() - 2)); + + return k_arr; + } + + inline TrajectoryPoints toTrajectoryPoints(const PredictedPath & path, const double velocity) + { + TrajectoryPoints out_trajectory; + std::for_each( + path.path.begin(), path.path.end(), [&out_trajectory, velocity](const auto & pose) { + TrajectoryPoint p; + p.pose = pose; + p.longitudinal_velocity_mps = velocity; + out_trajectory.push_back(p); + }); + return out_trajectory; + }; + + inline bool isLateralAccelerationConstraintSatisfied( + const TrajectoryPoints & trajectory [[maybe_unused]], const double delta_time) + { + if (trajectory.size() < 3) return true; + const double max_lateral_accel_abs = std::fabs(max_lateral_accel_); + + double arc_length = 0.0; + for (size_t i = 1; i < trajectory.size(); ++i) { + const auto current_pose = trajectory.at(i).pose; + const auto next_pose = trajectory.at(i - 1).pose; + // Compute distance between poses + const double delta_s = std::hypot( + next_pose.position.x - current_pose.position.x, + next_pose.position.y - current_pose.position.y); + arc_length += delta_s; + + // Compute change in heading + tf2::Quaternion q_current, q_next; + tf2::convert(current_pose.orientation, q_current); + tf2::convert(next_pose.orientation, q_next); + double delta_theta = q_current.angleShortestPath(q_next); + // Handle wrap-around + if (delta_theta > M_PI) { + delta_theta -= 2.0 * M_PI; + } else if (delta_theta < -M_PI) { + delta_theta += 2.0 * M_PI; + } + + const double yaw_rate = std::max(delta_theta / delta_time, 1.0E-5); + + const double current_speed = std::abs(trajectory.at(i).longitudinal_velocity_mps); + // Compute lateral acceleration + const double lateral_acceleration = std::abs(current_speed * yaw_rate); + if (lateral_acceleration < max_lateral_accel_abs) continue; + + const double v_curvature_max = std::sqrt(max_lateral_accel_abs / yaw_rate); + const double t = + (v_curvature_max - current_speed) / min_acceleration_before_curve_; // acc is negative + const double distance_to_slow_down = + current_speed * t + 0.5 * min_acceleration_before_curve_ * std::pow(t, 2); + + if (distance_to_slow_down > arc_length) return false; + } + return true; + }; }; } // namespace map_based_prediction diff --git a/perception/map_based_prediction/src/map_based_prediction_node.cpp b/perception/map_based_prediction/src/map_based_prediction_node.cpp index f89c83e9c2d5a..0091fc6bc38cb 100644 --- a/perception/map_based_prediction/src/map_based_prediction_node.cpp +++ b/perception/map_based_prediction/src/map_based_prediction_node.cpp @@ -740,6 +740,12 @@ MapBasedPredictionNode::MapBasedPredictionNode(const rclcpp::NodeOptions & node_ sigma_yaw_angle_deg_ = declare_parameter("sigma_yaw_angle_deg"); object_buffer_time_length_ = declare_parameter("object_buffer_time_length"); history_time_length_ = declare_parameter("history_time_length"); + + check_lateral_acceleration_constraints_ = + declare_parameter("check_lateral_acceleration_constraints"); + max_lateral_accel_ = declare_parameter("max_lateral_accel"); + min_acceleration_before_curve_ = declare_parameter("min_acceleration_before_curve"); + { // lane change detection lane_change_detection_method_ = declare_parameter("lane_change_detection.method"); @@ -789,6 +795,25 @@ MapBasedPredictionNode::MapBasedPredictionNode(const rclcpp::NodeOptions & node_ pub_debug_markers_ = this->create_publisher("maneuver", rclcpp::QoS{1}); pub_calculation_time_ = create_publisher("~/debug/calculation_time", 1); + + set_param_res_ = this->add_on_set_parameters_callback( + std::bind(&MapBasedPredictionNode::onParam, this, std::placeholders::_1)); +} + +rcl_interfaces::msg::SetParametersResult MapBasedPredictionNode::onParam( + const std::vector & parameters) +{ + using tier4_autoware_utils::updateParam; + + updateParam(parameters, "max_lateral_accel", max_lateral_accel_); + updateParam(parameters, "min_acceleration_before_curve", min_acceleration_before_curve_); + updateParam( + parameters, "check_lateral_acceleration_constraints", check_lateral_acceleration_constraints_); + + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + result.reason = "success"; + return result; } PredictedObjectKinematics MapBasedPredictionNode::convertToPredictedKinematics( @@ -972,16 +997,53 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt } // Generate Predicted Path std::vector predicted_paths; + double min_avg_curvature = std::numeric_limits::max(); + PredictedPath path_with_smallest_avg_curvature; + for (const auto & ref_path : ref_paths) { PredictedPath predicted_path = path_generator_->generatePathForOnLaneVehicle( yaw_fixed_transformed_object, ref_path.path); - if (predicted_path.path.empty()) { + if (predicted_path.path.empty()) continue; + + if (!check_lateral_acceleration_constraints_) { + predicted_path.confidence = ref_path.probability; + predicted_paths.push_back(predicted_path); continue; } - predicted_path.confidence = ref_path.probability; - predicted_paths.push_back(predicted_path); + + // Check lat. acceleration constraints + const auto trajectory_with_const_velocity = + toTrajectoryPoints(predicted_path, abs_obj_speed); + + if (isLateralAccelerationConstraintSatisfied( + trajectory_with_const_velocity, prediction_sampling_time_interval_)) { + predicted_path.confidence = ref_path.probability; + predicted_paths.push_back(predicted_path); + continue; + } + + // Calculate curvature assuming the trajectory points interval is constant + // In case all paths are deleted, a copy of the straightest path is kept + + constexpr double curvature_calculation_distance = 2.0; + constexpr double points_interval = 1.0; + const size_t idx_dist = static_cast( + std::max(static_cast((curvature_calculation_distance) / points_interval), 1)); + const auto curvature_v = + calcTrajectoryCurvatureFrom3Points(trajectory_with_const_velocity, idx_dist); + if (curvature_v.empty()) { + continue; + } + const auto curvature_avg = + std::accumulate(curvature_v.begin(), curvature_v.end(), 0.0) / curvature_v.size(); + if (curvature_avg < min_avg_curvature) { + min_avg_curvature = curvature_avg; + path_with_smallest_avg_curvature = predicted_path; + path_with_smallest_avg_curvature.confidence = ref_path.probability; + } } + if (predicted_paths.empty()) predicted_paths.push_back(path_with_smallest_avg_curvature); // Normalize Path Confidence and output the predicted object float sum_confidence = 0.0;