Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

fix(motion_velocity_smoother): add current_closest_point_from_prev_output #2399

Merged
merged 2 commits into from
Nov 29, 2022
Merged

fix(motion_velocity_smoother): add current_closest_point_from_prev_output #2399

Show file tree
Hide file tree
Changes from 1 commit
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
62a03f7
fix(motion_velocity_smoother): add current_closest_point_from_prev_ou…
mkuri Nov 29, 2022
1e39701
Merge branch 'main' into fix/update-prev-closest-point
mkuri Nov 29, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 1 addition & 0 deletions ...tion_velocity_smoother/include/motion_velocity_smoother/motion_velocity_smoother_node.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -90,6 +90,7 @@ class MotionVelocitySmootherNode : public rclcpp::Node

// previous trajectory point closest to ego vehicle
boost::optional<TrajectoryPoint> prev_closest_point_{};
boost::optional<TrajectoryPoint> current_closest_point_from_prev_output_{};

tier4_autoware_utils::SelfPoseListener self_pose_listener_{this};

Expand Down
26 changes: 16 additions & 10 deletions planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -300,7 +300,7 @@ void MotionVelocitySmootherNode::calcExternalVelocityLimit()
}

// on the first time, apply directly
if (prev_output_.empty() || !prev_closest_point_) {
if (prev_output_.empty() || !current_closest_point_from_prev_output_) {
external_velocity_limit_.velocity = external_velocity_limit_ptr_->max_velocity;
pub_velocity_limit_->publish(*external_velocity_limit_ptr_);
return;
Expand All @@ -316,8 +316,8 @@ void MotionVelocitySmootherNode::calcExternalVelocityLimit()
if (
std::fabs((external_velocity_limit_.velocity - external_velocity_limit_ptr_->max_velocity)) >
eps) {
const double v0 = prev_closest_point_->longitudinal_velocity_mps;
const double a0 = prev_closest_point_->acceleration_mps2;
const double v0 = current_closest_point_from_prev_output_->longitudinal_velocity_mps;
const double a0 = current_closest_point_from_prev_output_->acceleration_mps2;

if (isEngageStatus(v0)) {
max_velocity_with_deceleration_ = external_velocity_limit_ptr_->max_velocity;
Expand Down Expand Up @@ -409,7 +409,7 @@ void MotionVelocitySmootherNode::onCurrentTrajectory(const Trajectory::ConstShar
node_param_.ego_nearest_yaw_threshold);
const auto closest_point = trajectory_utils::calcInterpolatedTrajectoryPoint(
prev_output_, current_pose_ptr_->pose, current_seg_idx);
prev_closest_point_ = closest_point;
current_closest_point_from_prev_output_ = closest_point;
}

// calculate distance to insert external velocity limit
Expand Down Expand Up @@ -706,20 +706,20 @@ MotionVelocitySmootherNode::calcInitialMotion(
InitializeType type{};

// first time
if (!prev_closest_point_) {
if (!current_closest_point_from_prev_output_) {
initial_motion.vel = vehicle_speed;
initial_motion.acc = 0.0;
type = InitializeType::INIT;
return std::make_pair(initial_motion, type);
}

// when velocity tracking deviation is large
const double desired_vel{prev_closest_point_->longitudinal_velocity_mps};
const double desired_vel{current_closest_point_from_prev_output_->longitudinal_velocity_mps};
const double vel_error{vehicle_speed - std::fabs(desired_vel)};
if (std::fabs(vel_error) > node_param_.replan_vel_deviation) {
type = InitializeType::LARGE_DEVIATION_REPLAN;
initial_motion.vel = vehicle_speed; // use current vehicle speed
initial_motion.acc = prev_closest_point_->acceleration_mps2;
initial_motion.acc = current_closest_point_from_prev_output_->acceleration_mps2;
RCLCPP_DEBUG(
get_logger(),
"calcInitialMotion : Large deviation error for speed control. Use current speed for "
Expand Down Expand Up @@ -760,10 +760,10 @@ MotionVelocitySmootherNode::calcInitialMotion(
}
}

// normal update: use closest in prev_output
// normal update: use closest in current_closest_point_from_prev_output
type = InitializeType::NORMAL;
initial_motion.vel = prev_closest_point_->longitudinal_velocity_mps;
initial_motion.acc = prev_closest_point_->acceleration_mps2;
initial_motion.vel = current_closest_point_from_prev_output_->longitudinal_velocity_mps;
initial_motion.acc = current_closest_point_from_prev_output_->acceleration_mps2;
RCLCPP_DEBUG(
get_logger(),
"calcInitialMotion : normal update. v0 = %f, a0 = %f, vehicle_speed = %f, target_vel = %f",
Expand Down Expand Up @@ -942,6 +942,12 @@ void MotionVelocitySmootherNode::publishClosestState(const TrajectoryPoints & tr
void MotionVelocitySmootherNode::updatePrevValues(const TrajectoryPoints & final_result)
{
prev_output_ = final_result;
const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints(
final_result, current_pose_ptr_->pose, node_param_.ego_nearest_dist_threshold,
node_param_.ego_nearest_yaw_threshold);
const auto closest_point = trajectory_utils::calcInterpolatedTrajectoryPoint(
final_result, current_pose_ptr_->pose, current_seg_idx);
prev_closest_point_ = closest_point;
}

MotionVelocitySmootherNode::AlgorithmType MotionVelocitySmootherNode::getAlgorithmType(
Expand Down