From d73bc47f3f3778f91a8ac469e7f65ad2a5740449 Mon Sep 17 00:00:00 2001 From: danielsanchezaran Date: Fri, 19 Jan 2024 13:16:52 +0900 Subject: [PATCH] feat(map_based_prediction): use obstacle acceleration for map prediction (#6072) * add acc filtering and decaying acc to model Signed-off-by: Daniel Sanchez * fixed compilation problem, acc is used to predict search_dist Signed-off-by: Daniel Sanchez * Use an equivalent velocity to calculate paths Signed-off-by: Daniel Sanchez * change decaying factor to T/4 Signed-off-by: Daniel Sanchez * coment out cerr for evaluation Signed-off-by: Daniel Sanchez * simplify code Signed-off-by: Daniel Sanchez * comments Signed-off-by: Daniel Sanchez * add missing constant to decaying acc model Signed-off-by: Daniel Sanchez * Use an equivalent velocity to calculate paths Signed-off-by: Daniel Sanchez * add missing constant to decaying acc model Signed-off-by: Daniel Sanchez * Implement lanelet speed limit for acc consideration Signed-off-by: Daniel Sanchez * add option to activate on and off acceleration feature Signed-off-by: Daniel Sanchez * add params Signed-off-by: Daniel Sanchez * add params Signed-off-by: Daniel Sanchez * delete unused class Signed-off-by: Daniel Sanchez * update docs Signed-off-by: Daniel Sanchez * delete comments Signed-off-by: Daniel Sanchez * fix comments Signed-off-by: Daniel Sanchez * update comments, refactor code Signed-off-by: Daniel Sanchez * remove unused line Signed-off-by: Daniel Sanchez --------- Signed-off-by: Daniel Sanchez Signed-off-by: Daniel Sanchez --- perception/map_based_prediction/README.md | 48 ++++++++-- .../config/map_based_prediction.param.yaml | 3 + .../map_based_prediction_node.hpp | 9 +- .../map_based_prediction/path_generator.hpp | 20 ++++- .../src/map_based_prediction_node.cpp | 86 +++++++++++++++--- .../src/path_generator.cpp | 87 +++++++++++++++++-- 6 files changed, 224 insertions(+), 29 deletions(-) diff --git a/perception/map_based_prediction/README.md b/perception/map_based_prediction/README.md index ec2f57963332..2419cdb01079 100644 --- a/perception/map_based_prediction/README.md +++ b/perception/map_based_prediction/README.md @@ -124,7 +124,7 @@ 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 +#### Pruning predicted paths with lateral acceleration constraint (for vehicle obstacles) 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. @@ -136,11 +136,47 @@ Currently we provide three parameters to tune the lateral acceleration constrain 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] | +| 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] | + +## Using Vehicle Acceleration for Path Prediction (for Vehicle Obstacles) + +By default, the `map_based_prediction` module uses the current obstacle's velocity to compute its predicted path length. However, it is possible to use the obstacle's current acceleration to calculate its predicted path's length. + +### Decaying Acceleration Model + +Since this module tries to predict the vehicle's path several seconds into the future, it is not practical to consider the current vehicle's acceleration as constant (it is not assumed the vehicle will be accelerating for `prediction_time_horizon` seconds after detection). Instead, a decaying acceleration model is used. With the decaying acceleration model, a vehicle's acceleration is modeled as: + +$\ a(t) = a\_{t0} \cdot e^{-\lambda \cdot t} $ + +where $\ a\_{t0} $ is the vehicle acceleration at the time of detection $\ t0 $, and $\ \lambda $ is the decay constant $\ \lambda = \ln(2) / hl $ and $\ hl $ is the exponential's half life. + +Furthermore, the integration of $\ a(t) $ over time gives us equations for velocity, $\ v(t) $ and distance $\ x(t) $ as: + +$\ v(t) = v*{t0} + a*{t0} \* (1/\lambda) \cdot (1 - e^{-\lambda \cdot t}) $ + +and + +$\ x(t) = x*{t0} + (v*{t0} + a*{t0} \* (1/\lambda)) \cdot t + a*{t0}(1/λ^2)(e^{-\lambda \cdot t} - 1) $ + +With this model, the influence of the vehicle's detected instantaneous acceleration on the predicted path's length is diminished but still considered. This feature also considers that the obstacle might not accelerate past its road's speed limit (multiplied by a tunable factor). + +Currently, we provide three parameters to tune the use of obstacle acceleration for path prediction: + +- `use_vehicle_acceleration`: to enable the feature. +- `acceleration_exponential_half_life`: The decaying acceleration model considers that the current vehicle acceleration will be halved after this many seconds. +- `speed_limit_multiplier`: Set the vehicle type obstacle's maximum predicted speed as the legal speed limit in that lanelet times this value. This value should be at least equal or greater than 1.0. + +You can change these parameters in `rosparam` in the table below. + +| Param Name | Default Value | +| ------------------------------------ | -------------- | +| `use_vehicle_acceleration` | `false` [bool] | +| `acceleration_exponential_half_life` | `2.5` [s] | +| `speed_limit_multiplier` | `1.5` [] | ### Path prediction for crosswalk users 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 fe4d2a51ec6f..fdd64174de9b 100644 --- a/perception/map_based_prediction/config/map_based_prediction.param.yaml +++ b/perception/map_based_prediction/config/map_based_prediction.param.yaml @@ -16,6 +16,9 @@ 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 + use_vehicle_acceleration: false # whether to consider current vehicle acceleration when predicting paths or not + speed_limit_multiplier: 1.5 # When using vehicle acceleration. Set vehicle's maximum predicted speed as the legal speed limit in that lanelet times this value + acceleration_exponential_half_life: 2.5 # [s] When using vehicle acceleration. The decaying acceleration model considers that the current vehicle acceleration will be halved after this many seconds # 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 02db91b98911..f61dc75ffb85 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 @@ -22,6 +22,7 @@ #include #include +#include #include #include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" @@ -89,6 +90,7 @@ struct LaneletData struct PredictedRefPath { float probability; + double speed_limit; PosePath path; Maneuver maneuver; }; @@ -175,6 +177,10 @@ class MapBasedPredictionNode : public rclcpp::Node double max_lateral_accel_; double min_acceleration_before_curve_; + bool use_vehicle_acceleration_; + double speed_limit_multiplier_; + double acceleration_exponential_half_life_; + // Stop watch StopWatch stop_watch_; @@ -231,7 +237,8 @@ class MapBasedPredictionNode : public rclcpp::Node void addReferencePaths( const TrackedObject & object, const lanelet::routing::LaneletPaths & candidate_paths, const float path_probability, const ManeuverProbability & maneuver_probability, - const Maneuver & maneuver, std::vector & reference_paths); + const Maneuver & maneuver, std::vector & reference_paths, + const double speed_limit = 0.0); std::vector convertPathType(const lanelet::routing::LaneletPaths & paths); void updateFuturePossibleLanelets( diff --git a/perception/map_based_prediction/include/map_based_prediction/path_generator.hpp b/perception/map_based_prediction/include/map_based_prediction/path_generator.hpp index 4da4f62be2ed..6dfc4a8db9e2 100644 --- a/perception/map_based_prediction/include/map_based_prediction/path_generator.hpp +++ b/perception/map_based_prediction/include/map_based_prediction/path_generator.hpp @@ -91,7 +91,7 @@ class PathGenerator PredictedPath generatePathForOffLaneVehicle(const TrackedObject & object); PredictedPath generatePathForOnLaneVehicle( - const TrackedObject & object, const PosePath & ref_paths); + const TrackedObject & object, const PosePath & ref_paths, const double speed_limit = 0.0); PredictedPath generatePathForCrosswalkUser( const TrackedObject & object, const CrosswalkEdgePoints & reachable_crosswalk) const; @@ -99,17 +99,30 @@ class PathGenerator PredictedPath generatePathToTargetPoint( const TrackedObject & object, const Eigen::Vector2d & point) const; + void setUseVehicleAcceleration(const bool use_vehicle_acceleration) + { + use_vehicle_acceleration_ = use_vehicle_acceleration; + } + + void setAccelerationHalfLife(const double acceleration_exponential_half_life) + { + acceleration_exponential_half_life_ = acceleration_exponential_half_life; + } + private: // Parameters double time_horizon_; double lateral_time_horizon_; double sampling_time_interval_; double min_crosswalk_user_velocity_; + bool use_vehicle_acceleration_; + double acceleration_exponential_half_life_; // Member functions PredictedPath generateStraightPath(const TrackedObject & object) const; - PredictedPath generatePolynomialPath(const TrackedObject & object, const PosePath & ref_path); + PredictedPath generatePolynomialPath( + const TrackedObject & object, const PosePath & ref_path, const double speed_limit = 0.0); FrenetPath generateFrenetPath( const FrenetPoint & current_point, const FrenetPoint & target_point, const double max_length); @@ -125,7 +138,8 @@ class PathGenerator const TrackedObject & object, const FrenetPath & frenet_predicted_path, const PosePath & ref_path); - FrenetPoint getFrenetPoint(const TrackedObject & object, const PosePath & ref_path); + FrenetPoint getFrenetPoint( + const TrackedObject & object, const PosePath & ref_path, const double speed_limit = 0.0); }; } // 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 6a6f24081655..c949eae21aef 100644 --- a/perception/map_based_prediction/src/map_based_prediction_node.cpp +++ b/perception/map_based_prediction/src/map_based_prediction_node.cpp @@ -24,7 +24,6 @@ #include #include #include -#include #include @@ -385,11 +384,7 @@ bool withinRoadLanelet( const TrackedObject & object, const lanelet::LaneletMapPtr & lanelet_map_ptr, const bool use_yaw_information = false) { - using Point = boost::geometry::model::d2::point_xy; - const auto & obj_pos = object.kinematics.pose_with_covariance.pose.position; - const Point p_object{obj_pos.x, obj_pos.y}; - lanelet::BasicPoint2d search_point(obj_pos.x, obj_pos.y); // nearest lanelet constexpr double search_radius = 10.0; // [m] @@ -788,10 +783,18 @@ MapBasedPredictionNode::MapBasedPredictionNode(const rclcpp::NodeOptions & node_ prediction_time_horizon_rate_for_validate_lane_length_ = declare_parameter("prediction_time_horizon_rate_for_validate_shoulder_lane_length"); + use_vehicle_acceleration_ = declare_parameter("use_vehicle_acceleration"); + speed_limit_multiplier_ = declare_parameter("speed_limit_multiplier"); + acceleration_exponential_half_life_ = + declare_parameter("acceleration_exponential_half_life"); + path_generator_ = std::make_shared( prediction_time_horizon_, lateral_control_time_horizon_, prediction_sampling_time_interval_, min_crosswalk_user_velocity_); + path_generator_->setUseVehicleAcceleration(use_vehicle_acceleration_); + path_generator_->setAccelerationHalfLife(acceleration_exponential_half_life_); + sub_objects_ = this->create_subscription( "~/input/objects", 1, std::bind(&MapBasedPredictionNode::objectsCallback, this, std::placeholders::_1)); @@ -817,6 +820,13 @@ rcl_interfaces::msg::SetParametersResult MapBasedPredictionNode::onParam( updateParam(parameters, "min_acceleration_before_curve", min_acceleration_before_curve_); updateParam( parameters, "check_lateral_acceleration_constraints", check_lateral_acceleration_constraints_); + updateParam(parameters, "use_vehicle_acceleration", use_vehicle_acceleration_); + updateParam(parameters, "speed_limit_multiplier", speed_limit_multiplier_); + updateParam( + parameters, "acceleration_exponential_half_life", acceleration_exponential_half_life_); + + path_generator_->setUseVehicleAcceleration(use_vehicle_acceleration_); + path_generator_->setAccelerationHalfLife(acceleration_exponential_half_life_); rcl_interfaces::msg::SetParametersResult result; result.successful = true; @@ -1010,7 +1020,7 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt for (const auto & ref_path : ref_paths) { PredictedPath predicted_path = path_generator_->generatePathForOnLaneVehicle( - yaw_fixed_transformed_object, ref_path.path); + yaw_fixed_transformed_object, ref_path.path, ref_path.speed_limit); if (predicted_path.path.empty()) continue; if (!check_lateral_acceleration_constraints_) { @@ -1555,14 +1565,63 @@ std::vector MapBasedPredictionNode::getPredictedReferencePath( object.kinematics.twist_with_covariance.twist.linear.x, object.kinematics.twist_with_covariance.twist.linear.y); + // Using a decaying acceleration model. Consult the README for more information about the model. + const double obj_acc = (use_vehicle_acceleration_) + ? std::hypot( + object.kinematics.acceleration_with_covariance.accel.linear.x, + object.kinematics.acceleration_with_covariance.accel.linear.y) + : 0.0; + const double t_h = prediction_time_horizon_; + const double λ = std::log(2) / acceleration_exponential_half_life_; + + auto get_search_distance_with_decaying_acc = [&]() -> double { + const double acceleration_distance = + obj_acc * (1.0 / λ) * t_h + obj_acc * (1.0 / std::pow(λ, 2)) * (std::exp(-λ * t_h) - 1); + double search_dist = acceleration_distance + obj_vel * t_h; + return search_dist; + }; + + auto get_search_distance_with_partial_acc = [&](const double final_speed) -> double { + constexpr double epsilon = 1E-5; + if (std::abs(obj_acc) < epsilon) { + // Assume constant speed + return obj_vel * t_h; + } + // Time to reach final speed + const double t_f = (-1.0 / λ) * std::log(1 - ((final_speed - obj_vel) * λ) / obj_acc); + // It is assumed the vehicle accelerates until final_speed is reached and + // then continues at constant speed for the rest of the time horizon + const double search_dist = + // Distance covered while accelerating + obj_acc * (1.0 / λ) * t_f + obj_acc * (1.0 / std::pow(λ, 2)) * (std::exp(-λ * t_f) - 1) + + obj_vel * t_f + + // Distance covered at constant speed + final_speed * (t_h - t_f); + return search_dist; + }; + std::vector all_ref_paths; + for (const auto & current_lanelet_data : current_lanelets_data) { - // parameter for lanelet::routing::PossiblePathsParams - const double search_dist = prediction_time_horizon_ * obj_vel + - lanelet::utils::getLaneletLength3d(current_lanelet_data.lanelet); + const lanelet::traffic_rules::SpeedLimitInformation limit = + traffic_rules_ptr_->speedLimit(current_lanelet_data.lanelet); + const double legal_speed_limit = static_cast(limit.speedLimit.value()); + + double final_speed_after_acceleration = + obj_vel + obj_acc * (1.0 / λ) * (1.0 - std::exp(-λ * t_h)); + + const double final_speed_limit = legal_speed_limit * speed_limit_multiplier_; + const bool final_speed_surpasses_limit = final_speed_after_acceleration > final_speed_limit; + const bool object_has_surpassed_limit_already = obj_vel > final_speed_limit; + + double search_dist = (final_speed_surpasses_limit && !object_has_surpassed_limit_already) + ? get_search_distance_with_partial_acc(final_speed_limit) + : get_search_distance_with_decaying_acc(); + search_dist += lanelet::utils::getLaneletLength3d(current_lanelet_data.lanelet); + lanelet::routing::PossiblePathsParams possible_params{search_dist, {}, 0, false, true}; const double validate_time_horizon = - prediction_time_horizon_ * prediction_time_horizon_rate_for_validate_lane_length_; + t_h * prediction_time_horizon_rate_for_validate_lane_length_; // lambda function to get possible paths for isolated lanelet // isolated is often caused by lanelet with no connection e.g. shoulder-lane @@ -1644,7 +1703,8 @@ std::vector MapBasedPredictionNode::getPredictedReferencePath( // Step4. add candidate reference paths to the all_ref_paths const float path_prob = current_lanelet_data.probability; const auto addReferencePathsLocal = [&](const auto & paths, const auto & maneuver) { - addReferencePaths(object, paths, path_prob, maneuver_prob, maneuver, all_ref_paths); + addReferencePaths( + object, paths, path_prob, maneuver_prob, maneuver, all_ref_paths, final_speed_limit); }; addReferencePathsLocal(left_paths, Maneuver::LEFT_LANE_CHANGE); addReferencePathsLocal(right_paths, Maneuver::RIGHT_LANE_CHANGE); @@ -1966,7 +2026,8 @@ void MapBasedPredictionNode::updateFuturePossibleLanelets( void MapBasedPredictionNode::addReferencePaths( const TrackedObject & object, const lanelet::routing::LaneletPaths & candidate_paths, const float path_probability, const ManeuverProbability & maneuver_probability, - const Maneuver & maneuver, std::vector & reference_paths) + const Maneuver & maneuver, std::vector & reference_paths, + const double speed_limit) { if (!candidate_paths.empty()) { updateFuturePossibleLanelets(object, candidate_paths); @@ -1976,6 +2037,7 @@ void MapBasedPredictionNode::addReferencePaths( predicted_path.probability = maneuver_probability.at(maneuver) * path_probability; predicted_path.path = converted_path; predicted_path.maneuver = maneuver; + predicted_path.speed_limit = speed_limit; reference_paths.push_back(predicted_path); } } diff --git a/perception/map_based_prediction/src/path_generator.cpp b/perception/map_based_prediction/src/path_generator.cpp index d3f610583577..413a0e233186 100644 --- a/perception/map_based_prediction/src/path_generator.cpp +++ b/perception/map_based_prediction/src/path_generator.cpp @@ -149,13 +149,13 @@ PredictedPath PathGenerator::generatePathForOffLaneVehicle(const TrackedObject & } PredictedPath PathGenerator::generatePathForOnLaneVehicle( - const TrackedObject & object, const PosePath & ref_paths) + const TrackedObject & object, const PosePath & ref_paths, const double speed_limit) { if (ref_paths.size() < 2) { return generateStraightPath(object); } - return generatePolynomialPath(object, ref_paths); + return generatePolynomialPath(object, ref_paths, speed_limit); } PredictedPath PathGenerator::generateStraightPath(const TrackedObject & object) const @@ -178,11 +178,11 @@ PredictedPath PathGenerator::generateStraightPath(const TrackedObject & object) } PredictedPath PathGenerator::generatePolynomialPath( - const TrackedObject & object, const PosePath & ref_path) + const TrackedObject & object, const PosePath & ref_path, const double speed_limit) { // Get current Frenet Point const double ref_path_len = motion_utils::calcArcLength(ref_path); - const auto current_point = getFrenetPoint(object, ref_path); + const auto current_point = getFrenetPoint(object, ref_path, speed_limit); // Step1. Set Target Frenet Point // Note that we do not set position s, @@ -384,7 +384,8 @@ PredictedPath PathGenerator::convertToPredictedPath( return predicted_path; } -FrenetPoint PathGenerator::getFrenetPoint(const TrackedObject & object, const PosePath & ref_path) +FrenetPoint PathGenerator::getFrenetPoint( + const TrackedObject & object, const PosePath & ref_path, const double speed_limit) { FrenetPoint frenet_point; const auto obj_point = object.kinematics.pose_with_covariance.pose.position; @@ -400,10 +401,82 @@ FrenetPoint PathGenerator::getFrenetPoint(const TrackedObject & object, const Po static_cast(tf2::getYaw(ref_path.at(nearest_segment_idx).orientation)); const float delta_yaw = obj_yaw - lane_yaw; + const float ax = + (use_vehicle_acceleration_) + ? static_cast(object.kinematics.acceleration_with_covariance.accel.linear.x) + : 0.0; + const float ay = + (use_vehicle_acceleration_) + ? static_cast(object.kinematics.acceleration_with_covariance.accel.linear.y) + : 0.0; + + // Using a decaying acceleration model. Consult the README for more information about the model. + const double t_h = time_horizon_; + const float λ = std::log(2) / acceleration_exponential_half_life_; + + auto have_same_sign = [](double a, double b) -> bool { + return (a >= 0.0 && b >= 0.0) || (a < 0.0 && b < 0.0); + }; + + auto get_acceleration_adjusted_velocity = [&](const double v, const double a) { + constexpr double epsilon = 1E-5; + if (std::abs(a) < epsilon) { + // Assume constant speed + return v; + } + // Get velocity after time horizon + const auto terminal_velocity = v + a * (1.0 / λ) * (1 - std::exp(-λ * t_h)); + + // If vehicle is decelerating, make sure its speed does not change signs (we assume it will, at + // most stop, not reverse its direction) + if (!have_same_sign(terminal_velocity, v)) { + // we assume a forwards moving vehicle will not decelerate to 0 and then move backwards + // if the velocities don't have the same sign, calculate when the vehicle reaches 0 speed -> + // time t_stop + + // 0 = Vo + acc(1/λ)(1-e^(-λt_stop)) + // e^(-λt_stop) = 1 - (-Vo* λ)/acc + // t_stop = (-1/λ)*ln(1 - (-Vo* λ)/acc) + // t_stop = (-1/λ)*ln(1 + (Vo* λ)/acc) + auto t_stop = (-1.0 / λ) * std::log(1 + (v * λ / a)); + + // Calculate the distance traveled until stopping + auto distance_to_reach_zero_speed = + v * t_stop + a * t_stop * (1.0 / λ) + a * (1.0 / std::pow(λ, 2)) * (std::exp(-λ * t_h) - 1); + // Output an equivalent constant speed + return distance_to_reach_zero_speed / t_h; + } + + // if the vehicle speed limit is not surpassed we return an equivalent speed = x(T) / T + // alternatively, if the vehicle is still accelerating and has surpassed the speed limit. + // assume it will continue accelerating (reckless driving) + const bool object_has_surpassed_limit_already = v > speed_limit; + if (terminal_velocity < speed_limit || object_has_surpassed_limit_already) + return v + a * (1.0 / λ) + (a / (t_h * std::pow(λ, 2))) * (std::exp(-λ * t_h) - 1); + + // It is assumed the vehicle accelerates until final_speed is reached and + // then continues at constant speed for the rest of the time horizon + // So, we calculate the time it takes to reach the speed limit and compute how far the vehicle + // would go if it accelerated until reaching the speed limit, and then continued at a constant + // speed. + const double t_f = (-1.0 / λ) * std::log(1 - ((speed_limit - v) * λ) / a); + const double distance_covered = + // Distance covered while accelerating + a * (1.0 / λ) * t_f + a * (1.0 / std::pow(λ, 2)) * (std::exp(-λ * t_f) - 1) + v * t_f + + // Distance covered at constant speed for the rest of the horizon time + speed_limit * (t_h - t_f); + return distance_covered / t_h; + }; + + const float acceleration_adjusted_velocity_x = get_acceleration_adjusted_velocity(vx, ax); + const float acceleration_adjusted_velocity_y = get_acceleration_adjusted_velocity(vy, ay); + frenet_point.s = motion_utils::calcSignedArcLength(ref_path, 0, nearest_segment_idx) + l; frenet_point.d = motion_utils::calcLateralOffset(ref_path, obj_point); - frenet_point.s_vel = vx * std::cos(delta_yaw) - vy * std::sin(delta_yaw); - frenet_point.d_vel = vx * std::sin(delta_yaw) + vy * std::cos(delta_yaw); + frenet_point.s_vel = acceleration_adjusted_velocity_x * std::cos(delta_yaw) - + acceleration_adjusted_velocity_y * std::sin(delta_yaw); + frenet_point.d_vel = acceleration_adjusted_velocity_x * std::sin(delta_yaw) + + acceleration_adjusted_velocity_y * std::cos(delta_yaw); frenet_point.s_acc = 0.0; frenet_point.d_acc = 0.0;