fix(motion_velocity_smoother): improve backward trajectory handling

planning/motion_velocity_smoother/include/motion_velocity_smoother/motion_velocity_smoother_node.hpp

@@ -203,7 +203,7 @@ class MotionVelocitySmootherNode : public rclcpp::Node
 
   AlgorithmType getAlgorithmType(const std::string & algorithm_name) const;
 
-  TrajectoryPoints calcTrajectoryVelocity(const TrajectoryPoints & traj_input) const;
+  TrajectoryPoints calcTrajectoryVelocity(const TrajectoryPoints & traj_input);
 
   bool smoothVelocity(
     const TrajectoryPoints & input, const size_t input_closest,
@@ -266,8 +266,8 @@ class MotionVelocitySmootherNode : public rclcpp::Node
 
   // helper functions
   size_t findNearestIndexFromEgo(const TrajectoryPoints & points) const;
-  bool isReverse(const TrajectoryPoints & points) const;
-  void flipVelocity(TrajectoryPoints & points) const;
+  geometry_msgs::msg::Pose flipPose(const geometry_msgs::msg::Pose & pose) const;
+  void flipTrajectory(TrajectoryPoints & points) const;
   void publishStopWatchTime();
 
   std::unique_ptr<tier4_autoware_utils::LoggerLevelConfigure> logger_configure_;

planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp

@@ -1,4 +1,4 @@
 // Copyright 2021 Tier IV, Inc.
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -22,6 +22,8 @@
 
 #include <vehicle_info_util/vehicle_info_util.hpp>
 
+#include <tf2/LinearMath/Quaternion.h>
+
 #include <algorithm>
 #include <chrono>
 #include <limits>
@@ -359,7 +361,7 @@
   if (
     std::fabs((external_velocity_limit_.velocity - external_velocity_limit_ptr_->max_velocity)) >
     eps) {
-    const double v0 = current_closest_point_from_prev_output_->longitudinal_velocity_mps;
+    const double v0 = std::fabs(current_closest_point_from_prev_output_->longitudinal_velocity_mps);
     const double a0 = current_closest_point_from_prev_output_->acceleration_mps2;
 
     if (isEngageStatus(v0)) {
@@ -468,39 +470,58 @@
   // ignore current external velocity limit next time
   external_velocity_limit_ptr_ = nullptr;
 
-  // For negative velocity handling, multiple -1 to velocity if it is for reverse.
-  // NOTE: this process must be in the beginning of the process
-  is_reverse_ = isReverse(input_points);
-  if (is_reverse_) {
-    flipVelocity(input_points);
+  // 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; });
+  if (!is_forward_only && !is_backward_only) {
+    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.
+  // In such case we don't need to do anything.
+  if (!is_forward_only && is_backward_only) {
+    // flip the trajectory
+    is_reverse_ = true;
+    flipTrajectory(input_points);
   }
 
-  const auto output = calcTrajectoryVelocity(input_points);
+  // Note: only works with forward trajectories
+  auto output = calcTrajectoryVelocity(input_points);
   if (output.empty()) {
     RCLCPP_WARN(get_logger(), "Output Point is empty");
     return;
   }
 
+  if (is_reverse_) {
+    // flip back orientation
+    is_reverse_ = false;
+    flipTrajectory(output);
+  }
+
   // Note that output velocity is resampled by linear interpolation
   auto output_resampled = resampling::resampleTrajectory(
     output, current_odometry_ptr_->twist.twist.linear.x, current_odometry_ptr_->pose.pose,
     node_param_.ego_nearest_dist_threshold, node_param_.ego_nearest_yaw_threshold,
     node_param_.post_resample_param, false);
 
   // Set 0 at the end of the trajectory
   if (!output_resampled.empty()) {
     output_resampled.back().longitudinal_velocity_mps = 0.0;
   }
 
   // update previous step infomation
   updatePrevValues(output);
 
-  // for reverse velocity
-  // NOTE: this process must be in the end of the process
-  if (is_reverse_) {
-    flipVelocity(output_resampled);
-  }
-
   // publish message
   publishTrajectory(output_resampled);
 
@@ -530,7 +551,7 @@
 }
 
 TrajectoryPoints MotionVelocitySmootherNode::calcTrajectoryVelocity(
-  const TrajectoryPoints & traj_input) const
+  const TrajectoryPoints & traj_input)
 {
   TrajectoryPoints output{};  // velocity is optimized by qp solver
 
@@ -541,13 +562,14 @@
     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
     return prev_output_;
   }
 
   // Debug
   if (publish_debug_trajs_) {
     auto tmp = traj_extracted;
-    if (is_reverse_) flipVelocity(tmp);
+    if (is_reverse_) flipTrajectory(tmp);
     pub_trajectory_raw_->publish(toTrajectoryMsg(tmp));
   }
 
@@ -563,12 +585,13 @@
   // Debug
   if (publish_debug_trajs_) {
     auto tmp = traj_extracted;
-    if (is_reverse_) flipVelocity(tmp);
+    if (is_reverse_) flipTrajectory(tmp);
     pub_trajectory_vel_lim_->publish(toTrajectoryMsg(traj_extracted));
   }
 
   // Smoothing velocity
   if (!smoothVelocity(traj_extracted, traj_extracted_closest, output)) {
+    is_reverse_ = false;  // previous output is never flipped
     return prev_output_;
   }
 
@@ -601,67 +624,74 @@
       ? smoother_->applySteeringRateLimit(traj_lateral_acc_filtered, false)
       : 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_vel = (is_reverse_ ? -1.0 : 1.0) * current_odometry_ptr_->twist.twist.linear.x;
+
   // Resample trajectory with ego-velocity based interval distance
   auto traj_resampled = smoother_->resampleTrajectory(
-    traj_steering_rate_limited, current_odometry_ptr_->twist.twist.linear.x,
-    current_odometry_ptr_->pose.pose, node_param_.ego_nearest_dist_threshold,
+    traj_steering_rate_limited, curr_vel, curr_pose, node_param_.ego_nearest_dist_threshold,
     node_param_.ego_nearest_yaw_threshold);
 
   const size_t traj_resampled_closest = findNearestIndexFromEgo(traj_resampled);
 
   // Set 0[m/s] in the terminal point
   if (!traj_resampled.empty()) {
     traj_resampled.back().longitudinal_velocity_mps = 0.0;
   }
 
   // Publish Closest Resample Trajectory Velocity
-  publishClosestVelocity(
-    traj_resampled, current_odometry_ptr_->pose.pose, debug_closest_max_velocity_);
+  {
+    auto tmp = traj_resampled;
+    if (is_reverse_) flipTrajectory(tmp);
+    publishClosestVelocity(tmp, current_odometry_ptr_->pose.pose, debug_closest_max_velocity_);
+  }
 
   // Clip trajectory from closest point
   TrajectoryPoints clipped;
   clipped.insert(
     clipped.end(), traj_resampled.begin() + traj_resampled_closest, traj_resampled.end());
 
   std::vector<TrajectoryPoints> debug_trajectories;
   if (!smoother_->apply(
         initial_motion.vel, initial_motion.acc, clipped, traj_smoothed, debug_trajectories)) {
     RCLCPP_WARN(get_logger(), "Fail to solve optimization.");
   }
 
   // Set 0 velocity after input-stop-point
   overwriteStopPoint(clipped, traj_smoothed);
 
   traj_smoothed.insert(
     traj_smoothed.begin(), traj_resampled.begin(), traj_resampled.begin() + traj_resampled_closest);
 
   // For the endpoint of the trajectory
   if (!traj_smoothed.empty()) {
     traj_smoothed.back().longitudinal_velocity_mps = 0.0;
   }
 
   // Max velocity filter for safety
   trajectory_utils::applyMaximumVelocityLimit(
     traj_resampled_closest, traj_smoothed.size(), node_param_.max_velocity, traj_smoothed);
 
   // Insert behind velocity for output's consistency
   insertBehindVelocity(traj_resampled_closest, type, traj_smoothed);
 
   RCLCPP_DEBUG(get_logger(), "smoothVelocity : traj_smoothed.size() = %lu", traj_smoothed.size());
   if (publish_debug_trajs_) {
     {
       auto tmp = traj_lateral_acc_filtered;
-      if (is_reverse_) flipVelocity(tmp);
+      if (is_reverse_) flipTrajectory(tmp);
       pub_trajectory_latacc_filtered_->publish(toTrajectoryMsg(tmp));
     }
     {
       auto tmp = traj_resampled;
-      if (is_reverse_) flipVelocity(tmp);
+      if (is_reverse_) flipTrajectory(tmp);
       pub_trajectory_resampled_->publish(toTrajectoryMsg(tmp));
     }
     {
       auto tmp = traj_steering_rate_limited;
-      if (is_reverse_) flipVelocity(tmp);
+      if (is_reverse_) flipTrajectory(tmp);
       pub_trajectory_steering_rate_limited_->publish(toTrajectoryMsg(tmp));
     }
 
@@ -747,20 +777,24 @@
 MotionVelocitySmootherNode::calcInitialMotion(
   const TrajectoryPoints & input_traj, const size_t input_closest) const
 {
-  const double vehicle_speed = std::fabs(current_odometry_ptr_->twist.twist.linear.x);
+  // 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_acceleration = current_acceleration_ptr_->accel.accel.linear.x;
-  const double target_vel = std::fabs(input_traj.at(input_closest).longitudinal_velocity_mps);
+  // always > 0
+  const double target_vel = input_traj.at(input_closest).longitudinal_velocity_mps;
 
   // first time
   if (!current_closest_point_from_prev_output_) {
     Motion initial_motion = {vehicle_speed, 0.0};
     return {initial_motion, InitializeType::EGO_VELOCITY};
   }
 
   // when velocity tracking deviation is large
-  const double desired_vel = 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 - std::fabs(desired_vel);
+  const double vel_error = vehicle_speed - desired_vel;
 
   if (std::fabs(vel_error) > node_param_.replan_vel_deviation) {
     RCLCPP_DEBUG(
@@ -884,10 +918,7 @@
     0, traj.size(), max_velocity_with_deceleration_, traj);
 
   // insert the point at the distance of external velocity limit
-  const auto & current_pose = current_odometry_ptr_->pose.pose;
-  const size_t closest_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints(
-    traj, current_pose, node_param_.ego_nearest_dist_threshold,
-    node_param_.ego_nearest_yaw_threshold);
+  const size_t closest_seg_idx = findNearestIndexFromEgo(traj);
   const auto inserted_index =
     motion_utils::insertTargetPoint(closest_seg_idx, external_velocity_limit_.dist, traj);
   if (!inserted_index) {
@@ -940,9 +971,9 @@
       return;
     }
     if (is_reverse_) {
-      flipVelocity(debug_trajectories_tmp.at(0));
-      flipVelocity(debug_trajectories_tmp.at(1));
-      flipVelocity(debug_trajectories_tmp.at(2));
+      flipTrajectory(debug_trajectories_tmp.at(0));
+      flipTrajectory(debug_trajectories_tmp.at(1));
+      flipTrajectory(debug_trajectories_tmp.at(2));
     }
     pub_forward_filtered_trajectory_->publish(toTrajectoryMsg(debug_trajectories_tmp.at(0)));
     pub_backward_filtered_trajectory_->publish(toTrajectoryMsg(debug_trajectories_tmp.at(1)));
@@ -1042,7 +1073,7 @@
 {
   const double vehicle_speed = std::fabs(current_odometry_ptr_->twist.twist.linear.x);
   const double engage_vel_thr = node_param_.engage_velocity * node_param_.engage_exit_ratio;
-  return vehicle_speed < engage_vel_thr && target_vel >= node_param_.engage_velocity;
+  return vehicle_speed < engage_vel_thr && std::fabs(target_vel) >= node_param_.engage_velocity;
 }
 
 Trajectory MotionVelocitySmootherNode::toTrajectoryMsg(
@@ -1055,22 +1086,40 @@
 
 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;
   return motion_utils::findFirstNearestIndexWithSoftConstraints(
-    points, current_odometry_ptr_->pose.pose, node_param_.ego_nearest_dist_threshold,
+    points, curr_pose, node_param_.ego_nearest_dist_threshold,
     node_param_.ego_nearest_yaw_threshold);
 }
 
-bool MotionVelocitySmootherNode::isReverse(const TrajectoryPoints & points) const
+geometry_msgs::msg::Pose MotionVelocitySmootherNode::flipPose(
+  const geometry_msgs::msg::Pose & pose) const
 {
-  if (points.empty()) return true;
+  geometry_msgs::msg::Pose output = pose;
+
+  // rotation by 180 degrees
+  tf2::Quaternion q_rot;
+  q_rot.setRPY(0, 0, M_PI);
 
-  return std::any_of(
-    points.begin(), points.end(), [](auto & pt) { return pt.longitudinal_velocity_mps < 0; });
+  tf2::Quaternion q;
+  tf2::fromMsg(pose.orientation, q);
+  q *= q_rot;
+  output.orientation = tf2::toMsg(q);
+  return output;
 }
-void MotionVelocitySmootherNode::flipVelocity(TrajectoryPoints & points) const
+
+void MotionVelocitySmootherNode::flipTrajectory(TrajectoryPoints & points) const
 {
   for (auto & pt : points) {
-    pt.longitudinal_velocity_mps *= -1.0;
+    pt.pose = flipPose(pt.pose);
+    pt.longitudinal_velocity_mps = -pt.longitudinal_velocity_mps;
+    pt.lateral_velocity_mps = -pt.lateral_velocity_mps;
+    // NOTE(VRichardJP) this is actually a directed acceleration so it should not be flipped
+    // pt.acceleration_mps2 = -pt.acceleration_mps2;
+    pt.heading_rate_rps = -pt.heading_rate_rps;
+    pt.front_wheel_angle_rad = -pt.front_wheel_angle_rad;
+    pt.rear_wheel_angle_rad = -pt.rear_wheel_angle_rad;
   }
 }
 
@@ -1094,7 +1143,9 @@
 TrajectoryPoint MotionVelocitySmootherNode::calcProjectedTrajectoryPointFromEgo(
   const TrajectoryPoints & trajectory) const
 {
-  return calcProjectedTrajectoryPoint(trajectory, 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);
 }
 
 }  // namespace motion_velocity_smoother

planning/motion_velocity_smoother/src/trajectory_utils.cpp

@@ -92,6 +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;
   }
 
   return traj_p;