refactor(motion_velocity_smoother): remove unused optional (#1906)

* remove unused optional

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* pre-commit

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

Author: TakaHoribe

planning/behavior_velocity_planner/include/utilization/trajectory_utils.hpp

@@ -108,21 +108,20 @@ inline bool smoothPath(
     *traj_lateral_acc_filtered, v0, current_pose, planner_data->ego_nearest_dist_threshold,
     planner_data->ego_nearest_yaw_threshold);
   const size_t traj_resampled_closest = motion_utils::findFirstNearestIndexWithSoftConstraints(
-    *traj_resampled, current_pose, planner_data->ego_nearest_dist_threshold,
+    traj_resampled, current_pose, planner_data->ego_nearest_dist_threshold,
     planner_data->ego_nearest_yaw_threshold);
   std::vector<TrajectoryPoints> debug_trajectories;
   // Clip trajectory from closest point
   TrajectoryPoints clipped;
   TrajectoryPoints traj_smoothed;
   clipped.insert(
-    clipped.end(), traj_resampled->begin() + traj_resampled_closest, traj_resampled->end());
+    clipped.end(), traj_resampled.begin() + traj_resampled_closest, traj_resampled.end());
   if (!smoother->apply(v0, a0, clipped, traj_smoothed, debug_trajectories)) {
     std::cerr << "[behavior_velocity][trajectory_utils]: failed to smooth" << std::endl;
     return false;
   }
   traj_smoothed.insert(
-    traj_smoothed.begin(), traj_resampled->begin(),
-    traj_resampled->begin() + traj_resampled_closest);
+    traj_smoothed.begin(), traj_resampled.begin(), traj_resampled.begin() + traj_resampled_closest);
 
   out_path = convertTrajectoryPointsToPath(traj_smoothed);
   return true;

planning/motion_velocity_smoother/include/motion_velocity_smoother/resample.hpp

@@ -43,12 +43,12 @@ struct ResampleParam
   double sparse_min_interval_distance;  // minimum points-interval length for sparse sampling [m]
 };
 
-boost::optional<TrajectoryPoints> resampleTrajectory(
+TrajectoryPoints resampleTrajectory(
   const TrajectoryPoints & input, const double v_current,
   const geometry_msgs::msg::Pose & current_pose, const double nearest_dist_threshold,
   const double nearest_yaw_threshold, const ResampleParam & param, const bool use_zoh_for_v = true);
 
-boost::optional<TrajectoryPoints> resampleTrajectory(
+TrajectoryPoints resampleTrajectory(
   const TrajectoryPoints & input, const geometry_msgs::msg::Pose & current_pose,
   const double nearest_dist_threshold, const double nearest_yaw_threshold,
   const ResampleParam & param, const double nominal_ds, const bool use_zoh_for_v = true);

planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp

@@ -71,7 +71,7 @@ class AnalyticalJerkConstrainedSmoother : public SmootherBase
     const double initial_vel, const double initial_acc, const TrajectoryPoints & input,
     TrajectoryPoints & output, std::vector<TrajectoryPoints> & debug_trajectories) override;
 
-  boost::optional<TrajectoryPoints> resampleTrajectory(
+  TrajectoryPoints resampleTrajectory(
     const TrajectoryPoints & input, [[maybe_unused]] const double v0,
     [[maybe_unused]] const geometry_msgs::msg::Pose & current_pose,
     [[maybe_unused]] const double nearest_dist_threshold,

planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/jerk_filtered_smoother.hpp

@@ -46,7 +46,7 @@ class JerkFilteredSmoother : public SmootherBase
     const double initial_vel, const double initial_acc, const TrajectoryPoints & input,
     TrajectoryPoints & output, std::vector<TrajectoryPoints> & debug_trajectories) override;
 
-  boost::optional<TrajectoryPoints> resampleTrajectory(
+  TrajectoryPoints resampleTrajectory(
     const TrajectoryPoints & input, [[maybe_unused]] const double v0,
     const geometry_msgs::msg::Pose & current_pose, const double nearest_dist_threshold,
     const double nearest_yaw_threshold) const override;

planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp

@@ -44,7 +44,7 @@ class L2PseudoJerkSmoother : public SmootherBase
     const double initial_vel, const double initial_acc, const TrajectoryPoints & input,
     TrajectoryPoints & output, std::vector<TrajectoryPoints> & debug_trajectories) override;
 
-  boost::optional<TrajectoryPoints> resampleTrajectory(
+  TrajectoryPoints resampleTrajectory(
     const TrajectoryPoints & input, const double v0, const geometry_msgs::msg::Pose & current_pose,
     const double nearest_dist_threshold, const double nearest_yaw_threshold) const override;
 

planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp

@@ -44,7 +44,7 @@ class LinfPseudoJerkSmoother : public SmootherBase
     const double initial_vel, const double initial_acc, const TrajectoryPoints & input,
     TrajectoryPoints & output, std::vector<TrajectoryPoints> & debug_trajectories) override;
 
-  boost::optional<TrajectoryPoints> resampleTrajectory(
+  TrajectoryPoints resampleTrajectory(
     const TrajectoryPoints & input, const double v0, const geometry_msgs::msg::Pose & current_pose,
     const double nearest_dist_threshold, const double nearest_yaw_threshold) const override;
 

planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/smoother_base.hpp

@@ -65,7 +65,7 @@ class SmootherBase
     const double initial_vel, const double initial_acc, const TrajectoryPoints & input,
     TrajectoryPoints & output, std::vector<TrajectoryPoints> & debug_trajectories) = 0;
 
-  virtual boost::optional<TrajectoryPoints> resampleTrajectory(
+  virtual TrajectoryPoints resampleTrajectory(
     const TrajectoryPoints & input, const double v0, const geometry_msgs::msg::Pose & current_pose,
     const double nearest_dist_threshold, const double nearest_yaw_threshold) const = 0;
 

planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp

@@ -401,14 +401,10 @@ void MotionVelocitySmootherNode::onCurrentTrajectory(const Trajectory::ConstShar
     output, current_odometry_ptr_->twist.twist.linear.x, current_pose_ptr_->pose,
     node_param_.ego_nearest_dist_threshold, node_param_.ego_nearest_yaw_threshold,
     node_param_.post_resample_param, false);
-  if (!output_resampled) {
-    RCLCPP_WARN(get_logger(), "Failed to get the resampled output trajectory");
-    return;
-  }
 
   // Set 0 at the end of the trajectory
-  if (!output_resampled->empty()) {
-    output_resampled->back().longitudinal_velocity_mps = 0.0;
+  if (!output_resampled.empty()) {
+    output_resampled.back().longitudinal_velocity_mps = 0.0;
   }
 
   // update previous step infomation
@@ -417,11 +413,11 @@ void MotionVelocitySmootherNode::onCurrentTrajectory(const Trajectory::ConstShar
   // for reverse velocity
   // NOTE: this process must be in the end of the process
   if (is_reverse_) {
-    flipVelocity(*output_resampled);
+    flipVelocity(output_resampled);
   }
 
   // publish message
-  publishTrajectory(*output_resampled);
+  publishTrajectory(output_resampled);
 
   // publish debug message
   publishStopDistance(output);
@@ -523,24 +519,21 @@ bool MotionVelocitySmootherNode::smoothVelocity(
     *traj_steering_rate_limited, current_odometry_ptr_->twist.twist.linear.x,
     current_pose_ptr_->pose, node_param_.ego_nearest_dist_threshold,
     node_param_.ego_nearest_yaw_threshold);
-  if (!traj_resampled) {
-    RCLCPP_WARN(get_logger(), "Fail to do resampling before the optimization");
-    return false;
-  }
-  const size_t traj_resampled_closest = findNearestIndexFromEgo(*traj_resampled);
+
+  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;
+  if (!traj_resampled.empty()) {
+    traj_resampled.back().longitudinal_velocity_mps = 0.0;
   }
 
   // Publish Closest Resample Trajectory Velocity
-  publishClosestVelocity(*traj_resampled, current_pose_ptr_->pose, debug_closest_max_velocity_);
+  publishClosestVelocity(traj_resampled, current_pose_ptr_->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());
+    clipped.end(), traj_resampled.begin() + traj_resampled_closest, traj_resampled.end());
 
   std::vector<TrajectoryPoints> debug_trajectories;
   if (!smoother_->apply(
@@ -552,8 +545,7 @@ bool MotionVelocitySmootherNode::smoothVelocity(
   overwriteStopPoint(clipped, traj_smoothed);
 
   traj_smoothed.insert(
-    traj_smoothed.begin(), traj_resampled->begin(),
-    traj_resampled->begin() + traj_resampled_closest);
+    traj_smoothed.begin(), traj_resampled.begin(), traj_resampled.begin() + traj_resampled_closest);
 
   // For the endpoint of the trajectory
   if (!traj_smoothed.empty()) {
@@ -575,7 +567,7 @@ bool MotionVelocitySmootherNode::smoothVelocity(
       pub_trajectory_latacc_filtered_->publish(toTrajectoryMsg(tmp));
     }
     {
-      auto tmp = *traj_resampled;
+      auto tmp = traj_resampled;
       if (is_reverse_) flipVelocity(tmp);
       pub_trajectory_resampled_->publish(toTrajectoryMsg(tmp));
     }
@@ -588,8 +580,8 @@ bool MotionVelocitySmootherNode::smoothVelocity(
     if (!debug_trajectories.empty()) {
       for (auto & debug_trajectory : debug_trajectories) {
         debug_trajectory.insert(
-          debug_trajectory.begin(), traj_resampled->begin(),
-          traj_resampled->begin() + traj_resampled_closest);
+          debug_trajectory.begin(), traj_resampled.begin(),
+          traj_resampled.begin() + traj_resampled_closest);
         for (size_t i = 0; i < traj_resampled_closest; ++i) {
           debug_trajectory.at(i).longitudinal_velocity_mps =
             debug_trajectory.at(traj_resampled_closest).longitudinal_velocity_mps;

planning/motion_velocity_smoother/src/resample.cpp

@@ -24,7 +24,7 @@ namespace motion_velocity_smoother
 {
 namespace resampling
 {
-boost::optional<TrajectoryPoints> resampleTrajectory(
+TrajectoryPoints resampleTrajectory(
   const TrajectoryPoints & input, const double v_current,
   const geometry_msgs::msg::Pose & current_pose, const double nearest_dist_threshold,
   const double nearest_yaw_threshold, const ResampleParam & param, const bool use_zoh_for_v)
@@ -142,7 +142,7 @@ boost::optional<TrajectoryPoints> resampleTrajectory(
   return output;
 }
 
-boost::optional<TrajectoryPoints> resampleTrajectory(
+TrajectoryPoints resampleTrajectory(
   const TrajectoryPoints & input, const geometry_msgs::msg::Pose & current_pose,
   const double nearest_dist_threshold, const double nearest_yaw_threshold,
   const ResampleParam & param, const double nominal_ds, const bool use_zoh_for_v)
@@ -187,7 +187,7 @@ boost::optional<TrajectoryPoints> resampleTrajectory(
   double dist_i{0.0};
   bool is_zero_point_included{false};
   bool is_endpoint_included{false};
-  while (true) {
+  while (rclcpp::ok()) {
     double ds = nominal_ds;
     if (start_stop_arclength_value <= dist_i && dist_i <= stop_arclength_value) {
       // Dense sampling before the stop point

planning/motion_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp

@@ -231,7 +231,7 @@ bool AnalyticalJerkConstrainedSmoother::apply(
   return true;
 }
 
-boost::optional<TrajectoryPoints> AnalyticalJerkConstrainedSmoother::resampleTrajectory(
+TrajectoryPoints AnalyticalJerkConstrainedSmoother::resampleTrajectory(
   const TrajectoryPoints & input, [[maybe_unused]] const double v0,
   [[maybe_unused]] const geometry_msgs::msg::Pose & current_pose,
   [[maybe_unused]] const double nearest_dist_threshold,
@@ -240,7 +240,7 @@ boost::optional<TrajectoryPoints> AnalyticalJerkConstrainedSmoother::resampleTra
   TrajectoryPoints output;
   if (input.empty()) {
     RCLCPP_WARN(logger_, "Input trajectory is empty");
-    return {};
+    return input;
   }
 
   const double ds = 1.0 / static_cast<double>(smoother_param_.resample.num_resample);
@@ -274,7 +274,7 @@ boost::optional<TrajectoryPoints> AnalyticalJerkConstrainedSmoother::resampleTra
 
   output.push_back(input.back());
 
-  return boost::optional<TrajectoryPoints>(output);
+  return output;
 }
 
 boost::optional<TrajectoryPoints> AnalyticalJerkConstrainedSmoother::applyLateralAccelerationFilter(

planning/motion_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp

@@ -24,6 +24,8 @@
 #include <numeric>
 #include <vector>
 
+#define VERBOSE_TRAJECTORY_VELOCITY false
+
 namespace motion_velocity_smoother
 {
 JerkFilteredSmoother::JerkFilteredSmoother(rclcpp::Node & node) : SmootherBase(node)
@@ -71,8 +73,6 @@ bool JerkFilteredSmoother::apply(
     return true;
   }
 
-  bool TMP_SHOW_DEBUG_INFO = false;
-
   const auto ts = std::chrono::system_clock::now();
 
   const double a_max = base_param_.max_accel;
@@ -93,28 +93,29 @@ bool JerkFilteredSmoother::apply(
   const auto filtered =
     mergeFilteredTrajectory(v0, a0, a_min, j_min, forward_filtered, backward_filtered);
 
-  // Set debug trajectories
-  debug_trajectories.resize(3);
-  debug_trajectories[0] = forward_filtered;
-  debug_trajectories[1] = backward_filtered;
-  debug_trajectories[2] = filtered;
-
   // Resample TrajectoryPoints for Optimization
   // TODO(planning/control team) deal with overlapped lanes with the same direction
   const auto initial_traj_pose = filtered.front().pose;
-  auto opt_resampled_trajectory = resampling::resampleTrajectory(
-    filtered, v0, initial_traj_pose, std::numeric_limits<double>::max(),
-    std::numeric_limits<double>::max(), base_param_.resample_param);
 
-  if (!opt_resampled_trajectory) {
-    RCLCPP_WARN(logger_, "Resample failed!");
-    return false;
-  }
+  const auto resample = [&](const auto & trajectory) {
+    return resampling::resampleTrajectory(
+      trajectory, v0, initial_traj_pose, std::numeric_limits<double>::max(),
+      std::numeric_limits<double>::max(), base_param_.resample_param);
+  };
+
+  auto opt_resampled_trajectory = resample(filtered);
+
+  // Set debug trajectories
+  debug_trajectories.resize(3);
+  debug_trajectories[0] = resample(forward_filtered);
+  debug_trajectories[1] = resample(backward_filtered);
+  debug_trajectories[2] = resample(filtered);
+
   // Ensure terminal velocity is zero
-  opt_resampled_trajectory->back().longitudinal_velocity_mps = 0.0;
+  opt_resampled_trajectory.back().longitudinal_velocity_mps = 0.0;
 
   // If Resampled Size is too small, we don't do optimization
-  if (opt_resampled_trajectory->size() == 1) {
+  if (opt_resampled_trajectory.size() == 1) {
     // No need to do optimization
     output.front().longitudinal_velocity_mps = v0;
     output.front().acceleration_mps2 = a0;
@@ -127,7 +128,7 @@ bool JerkFilteredSmoother::apply(
   // to avoid getting 0 as a stop point, search zero velocity index from 1.
   // the size of the resampled trajectory must not be less than 2.
   const auto zero_vel_id = motion_utils::searchZeroVelocityIndex(
-    *opt_resampled_trajectory, 1, opt_resampled_trajectory->size());
+    opt_resampled_trajectory, 1, opt_resampled_trajectory.size());
 
   if (!zero_vel_id) {
     RCLCPP_WARN(logger_, "opt_resampled_trajectory must have stop point.");
@@ -137,14 +138,14 @@ bool JerkFilteredSmoother::apply(
   // Clip trajectory from 0 to zero_vel_id (the size becomes zero_vel_id_ + 1)
   const size_t N = *zero_vel_id + 1;
 
-  output = *opt_resampled_trajectory;
+  output = opt_resampled_trajectory;
 
   const std::vector<double> interval_dist_arr =
-    trajectory_utils::calcTrajectoryIntervalDistance(*opt_resampled_trajectory);
+    trajectory_utils::calcTrajectoryIntervalDistance(opt_resampled_trajectory);
 
   std::vector<double> v_max_arr(N, 0.0);
   for (size_t i = 0; i < N; ++i) {
-    v_max_arr.at(i) = opt_resampled_trajectory->at(i).longitudinal_velocity_mps;
+    v_max_arr.at(i) = opt_resampled_trajectory.at(i).longitudinal_velocity_mps;
   }
 
   /*
@@ -306,44 +307,29 @@ bool JerkFilteredSmoother::apply(
 
   qp_solver_.logUnsolvedStatus("[motion_velocity_smoother]");
 
-  if (TMP_SHOW_DEBUG_INFO) {
-    // jerk calculation
-    std::vector<double> p_jerk, jerk_converted, jerk_ideal;
-    for (size_t i = 0; i < input.size() - 1; ++i) {
-      double v_ref0 = filtered.at(i).longitudinal_velocity_mps;
-      double v0 = output.at(i).longitudinal_velocity_mps;
-      double a0 = output.at(i).acceleration_mps2;
-      double a1 = output.at(i + 1).acceleration_mps2;
-      double ds = interval_dist_arr.at(i);
-      p_jerk.push_back((a1 - a0) / ds);
-      jerk_converted.push_back((a1 - a0) / ds * v_ref0);
-      jerk_ideal.push_back((a1 - a0) / ds * v0);
-    }
-    p_jerk.push_back(0.0);
-    jerk_converted.push_back(0.0);
-    jerk_ideal.push_back(0.0);
-
-    // print
-    size_t i = 0;
-    auto getVx = [&i](const TrajectoryPoints & trajectory) {
-      return trajectory.at(i).longitudinal_velocity_mps;
-    };
-    auto getAx = [&i](const TrajectoryPoints & trajectory) {
-      return trajectory.at(i).acceleration_mps2;
-    };
-    printf("v0 = %.3f, a0 = %.3f\n", v0, a0);
-    for (; i < input.size(); ++i) {
-      double gamma = optval.at(IDX_GAMMA0 + i);
-      printf(
-        "i = %lu, input: %.3f, filt_f: (%.3f, %.3f), filt_b: (%.3f, %.3f), filt_fb: (%.3f, %.3f), "
-        "opt: (%.3f, %f), p_jerk = %.3f, p_jerk*v_ref: %.3f (min: %.3f, mac: %.3f), p_jerk*v_opt "
-        "(to "
-        "check): %.3f, gamma: %.3f\n",
-        i, getVx(input), getVx(forward_filtered), getAx(forward_filtered), getVx(backward_filtered),
-        getAx(backward_filtered), getVx(filtered), getAx(filtered), getVx(output), getAx(output),
-        p_jerk.at(i), jerk_converted.at(i), j_min, j_max, jerk_ideal.at(i), gamma);
+  const int status_polish = std::get<2>(result);
+  if (status_polish != 1) {
+    const auto msg = status_polish == 0    ? "unperformed"
+                     : status_polish == -1 ? "unsuccessful"
+                                           : "unknown";
+    RCLCPP_WARN(logger_, "osqp polish process failed : %s. The result may be inaccurate", msg);
+  }
+
+  if (VERBOSE_TRAJECTORY_VELOCITY) {
+    const auto s_output = trajectory_utils::calcArclengthArray(output);
+
+    std::cerr << "\n\n" << std::endl;
+    for (size_t i = 0; i < N; ++i) {
+      const auto v_opt = output.at(i).longitudinal_velocity_mps;
+      const auto a_opt = output.at(i).acceleration_mps2;
+      const auto ds = i < interval_dist_arr.size() ? interval_dist_arr.at(i) : 0.0;
+      const auto v_rs = i < opt_resampled_trajectory.size()
+                          ? opt_resampled_trajectory.at(i).longitudinal_velocity_mps
+                          : 0.0;
+      RCLCPP_INFO(
+        logger_, "i =  %4lu | s: %5f | ds: %5f | rs: %9f | op_v: %10f | op_a: %10f |", i,
+        s_output.at(i), ds, v_rs, v_opt, a_opt);
     }
-    printf("\n");
   }
 
   return true;
@@ -467,7 +453,7 @@ TrajectoryPoints JerkFilteredSmoother::mergeFilteredTrajectory(
   return merged;
 }
 
-boost::optional<TrajectoryPoints> JerkFilteredSmoother::resampleTrajectory(
+TrajectoryPoints JerkFilteredSmoother::resampleTrajectory(
   const TrajectoryPoints & input, [[maybe_unused]] const double v0,
   const geometry_msgs::msg::Pose & current_pose, const double nearest_dist_threshold,
   const double nearest_yaw_threshold) const