From 70477783bce848eea6980679220c8c8cf85b7a98 Mon Sep 17 00:00:00 2001 From: Azumi Suzuki <38061530+s-azumi@users.noreply.github.com> Date: Fri, 3 Mar 2023 21:40:28 +0900 Subject: [PATCH] fix: for pr299 function to work (#301) Signed-off-by: Azumi Suzuki --- .../src/motion_velocity_smoother_node.cpp | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) diff --git a/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp b/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp index 3ff4f7b7ebd1d..5ceb73d95e3dd 100644 --- a/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp +++ b/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp @@ -284,13 +284,6 @@ void MotionVelocitySmootherNode::onExternalVelocityLimit(const VelocityLimit::Co constexpr double eps = 1.0E-04; const double margin = node_param_.margin_to_insert_external_velocity_limit; - // Set distance as zero if ego vehicle is stopped and external velocity limit is zero - if ( - std::fabs(current_odometry_ptr_->twist.twist.linear.x) < eps && - external_velocity_limit_ < eps) { - external_velocity_limit_dist_ = 0.0; - } - // calculate distance and maximum velocity // to decelerate to external velocity limit with jerk and acceleration // constraints. @@ -425,6 +418,14 @@ void MotionVelocitySmootherNode::updateDataForExternalVelocityLimit() return; } + // Set distance as zero if ego vehicle is stopped and external velocity limit is zero + constexpr double eps = 1.0E-04; + if ( + std::fabs(current_odometry_ptr_->twist.twist.linear.x) < eps && + external_velocity_limit_ < eps) { + external_velocity_limit_dist_ = 0.0; + } + // calculate distance to insert external velocity limit const double travel_dist = calcTravelDistance(); external_velocity_limit_dist_ -= travel_dist;