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 aea6429083323..c6c447770a4ff 100644 --- a/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp +++ b/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp @@ -471,17 +471,23 @@ void MotionVelocitySmootherNode::onCurrentTrajectory(const Trajectory::ConstShar external_velocity_limit_ptr_ = nullptr; // Trajectory must be either all forward or all backward. - bool is_forward_only = std::all_of(input_points.begin(), input_points.end(), - [](const auto& tp) { return tp.longitudinal_velocity_mps >= 0.0; }); - bool is_backward_only = std::all_of(input_points.begin(), input_points.end(), - [](const auto& tp) { return tp.longitudinal_velocity_mps <= 0.0; }); + bool is_forward_only = std::all_of(input_points.begin(), input_points.end(), [](const auto & tp) { + return tp.longitudinal_velocity_mps >= 0.0; + }); + bool is_backward_only = std::all_of( + input_points.begin(), input_points.end(), + [](const auto & tp) { return tp.longitudinal_velocity_mps <= 0.0; }); if (!is_forward_only && !is_backward_only) { - RCLCPP_ERROR(get_logger(), "Input trajectory contains both forward and backward parts (unsupported). Trajectory will be ignored."); + RCLCPP_ERROR( + get_logger(), + "Input trajectory contains both forward and backward parts (unsupported). Trajectory will be " + "ignored."); return; } - // Backward trajectories are handled by "flipping" the all the poses (orientation, velocity, steering, etc), processing trajectories as if the vehicle was driving forward and flipping back the result. - // Note: if trajectory is both forward and backward, it means it is filled with 0.0. + // Backward trajectories are handled by "flipping" the all the poses (orientation, velocity, + // steering, etc), processing trajectories as if the vehicle was driving forward and flipping back + // the result. Note: if trajectory is both forward and backward, it means it is filled with 0.0. // In such case we don't need to do anything. if (!is_forward_only && is_backward_only) { // flip the trajectory @@ -556,7 +562,7 @@ TrajectoryPoints MotionVelocitySmootherNode::calcTrajectoryVelocity( traj_input, input_closest, node_param_.extract_ahead_dist, node_param_.extract_behind_dist); if (traj_extracted.empty()) { RCLCPP_WARN(get_logger(), "Fail to extract the path from the input trajectory"); - is_reverse_ = false; // previous output is never flipped + is_reverse_ = false; // previous output is never flipped return prev_output_; } @@ -585,7 +591,7 @@ TrajectoryPoints MotionVelocitySmootherNode::calcTrajectoryVelocity( // Smoothing velocity if (!smoothVelocity(traj_extracted, traj_extracted_closest, output)) { - is_reverse_ = false; // previous output is never flipped + is_reverse_ = false; // previous output is never flipped return prev_output_; } @@ -619,7 +625,8 @@ bool MotionVelocitySmootherNode::smoothVelocity( : traj_lateral_acc_filtered; // Note: if input trajectory poses have been flipped, ego pose/velocity should be flipped too. - const auto curr_pose = is_reverse_ ? flipPose(current_odometry_ptr_->pose.pose) : current_odometry_ptr_->pose.pose; + const auto curr_pose = + is_reverse_ ? flipPose(current_odometry_ptr_->pose.pose) : current_odometry_ptr_->pose.pose; const auto curr_vel = (is_reverse_ ? -1.0 : 1.0) * current_odometry_ptr_->twist.twist.linear.x; // Resample trajectory with ego-velocity based interval distance @@ -771,7 +778,8 @@ MotionVelocitySmootherNode::calcInitialMotion( const TrajectoryPoints & input_traj, const size_t input_closest) const { // ego motion w.r.t trajectory orientation - const double vehicle_speed = (is_reverse_ ? -1.0 : 1.0) * current_odometry_ptr_->twist.twist.linear.x; + const double vehicle_speed = + (is_reverse_ ? -1.0 : 1.0) * current_odometry_ptr_->twist.twist.linear.x; const double vehicle_acceleration = current_acceleration_ptr_->accel.accel.linear.x; // always > 0 const double target_vel = input_traj.at(input_closest).longitudinal_velocity_mps; @@ -783,7 +791,8 @@ MotionVelocitySmootherNode::calcInitialMotion( } // when velocity tracking deviation is large - const double desired_vel = (is_reverse_ ? -1.0 : 1.0) * current_closest_point_from_prev_output_->longitudinal_velocity_mps; + const double desired_vel = + (is_reverse_ ? -1.0 : 1.0) * current_closest_point_from_prev_output_->longitudinal_velocity_mps; const double desired_acc = current_closest_point_from_prev_output_->acceleration_mps2; const double vel_error = vehicle_speed - desired_vel; @@ -1077,13 +1086,15 @@ Trajectory MotionVelocitySmootherNode::toTrajectoryMsg( size_t MotionVelocitySmootherNode::findNearestIndexFromEgo(const TrajectoryPoints & points) const { - const auto curr_pose = is_reverse_ ? flipPose(current_odometry_ptr_->pose.pose) : current_odometry_ptr_->pose.pose; + const auto curr_pose = + is_reverse_ ? flipPose(current_odometry_ptr_->pose.pose) : current_odometry_ptr_->pose.pose; return motion_utils::findFirstNearestIndexWithSoftConstraints( points, curr_pose, node_param_.ego_nearest_dist_threshold, node_param_.ego_nearest_yaw_threshold); } -geometry_msgs::msg::Pose MotionVelocitySmootherNode::flipPose(const geometry_msgs::msg::Pose & pose) const +geometry_msgs::msg::Pose MotionVelocitySmootherNode::flipPose( + const geometry_msgs::msg::Pose & pose) const { geometry_msgs::msg::Pose output = pose; @@ -1098,7 +1109,6 @@ geometry_msgs::msg::Pose MotionVelocitySmootherNode::flipPose(const geometry_msg return output; } - void MotionVelocitySmootherNode::flipTrajectory(TrajectoryPoints & points) const { for (auto & pt : points) { @@ -1133,7 +1143,8 @@ TrajectoryPoint MotionVelocitySmootherNode::calcProjectedTrajectoryPoint( TrajectoryPoint MotionVelocitySmootherNode::calcProjectedTrajectoryPointFromEgo( const TrajectoryPoints & trajectory) const { - auto curr_pose = is_reverse_ ? flipPose(current_odometry_ptr_->pose.pose) : current_odometry_ptr_->pose.pose; + auto curr_pose = + is_reverse_ ? flipPose(current_odometry_ptr_->pose.pose) : current_odometry_ptr_->pose.pose; return calcProjectedTrajectoryPoint(trajectory, curr_pose); } diff --git a/planning/motion_velocity_smoother/src/trajectory_utils.cpp b/planning/motion_velocity_smoother/src/trajectory_utils.cpp index d528e82358002..de2c0d32cd5d1 100644 --- a/planning/motion_velocity_smoother/src/trajectory_utils.cpp +++ b/planning/motion_velocity_smoother/src/trajectory_utils.cpp @@ -92,11 +92,17 @@ TrajectoryPoint calcInterpolatedTrajectoryPoint( seg_pt.longitudinal_velocity_mps, next_pt.longitudinal_velocity_mps, prop); traj_p.acceleration_mps2 = interpolation::lerp(seg_pt.acceleration_mps2, next_pt.acceleration_mps2, prop); - traj_p.heading_rate_rps = interpolation::lerp(seg_pt.heading_rate_rps, next_pt.heading_rate_rps, prop); - traj_p.lateral_velocity_mps = interpolation::lerp(seg_pt.lateral_velocity_mps, next_pt.lateral_velocity_mps, prop); - traj_p.front_wheel_angle_rad = interpolation::lerp(seg_pt.front_wheel_angle_rad, next_pt.front_wheel_angle_rad, prop); - traj_p.rear_wheel_angle_rad = interpolation::lerp(seg_pt.rear_wheel_angle_rad, next_pt.rear_wheel_angle_rad, prop); - traj_p.time_from_start = rclcpp::Duration(seg_pt.time_from_start) + (rclcpp::Duration(next_pt.time_from_start) - rclcpp::Duration(seg_pt.time_from_start)) * prop; + traj_p.heading_rate_rps = + interpolation::lerp(seg_pt.heading_rate_rps, next_pt.heading_rate_rps, prop); + traj_p.lateral_velocity_mps = + interpolation::lerp(seg_pt.lateral_velocity_mps, next_pt.lateral_velocity_mps, prop); + traj_p.front_wheel_angle_rad = + interpolation::lerp(seg_pt.front_wheel_angle_rad, next_pt.front_wheel_angle_rad, prop); + traj_p.rear_wheel_angle_rad = + interpolation::lerp(seg_pt.rear_wheel_angle_rad, next_pt.rear_wheel_angle_rad, prop); + traj_p.time_from_start = + rclcpp::Duration(seg_pt.time_from_start) + + (rclcpp::Duration(next_pt.time_from_start) - rclcpp::Duration(seg_pt.time_from_start)) * prop; } return traj_p;