refactor(motion_velocity_smoother): remove unneccesary optional (#2811)

* remove optional for lateral acc filter Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com> * apply to behavior_velocity_planner Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com> --------- Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>
autowarefoundation · Feb 6, 2023 · 69b9085 · 69b9085
1 parent e91af6f
commit 69b9085
Show file tree

Hide file tree

Showing 6 changed files with 18 additions and 38 deletions.
diff --git a/planning/behavior_velocity_planner/include/utilization/trajectory_utils.hpp b/planning/behavior_velocity_planner/include/utilization/trajectory_utils.hpp
@@ -105,7 +105,7 @@ inline bool smoothPath(
 
   // Resample trajectory with ego-velocity based interval distances
   auto traj_resampled = smoother->resampleTrajectory(
-    *traj_lateral_acc_filtered, v0, current_pose, planner_data->ego_nearest_dist_threshold,
+    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,

diff --git a/...er/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp b/...er/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp
@@ -77,7 +77,7 @@ class AnalyticalJerkConstrainedSmoother : public SmootherBase
     [[maybe_unused]] const double nearest_dist_threshold,
     [[maybe_unused]] const double nearest_yaw_threshold) const override;
 
-  boost::optional<TrajectoryPoints> applyLateralAccelerationFilter(
+  TrajectoryPoints applyLateralAccelerationFilter(
     const TrajectoryPoints & input, [[maybe_unused]] const double v0,
     [[maybe_unused]] const double a0,
     [[maybe_unused]] const bool enable_smooth_limit) const override;

diff --git a/...ning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/smoother_base.hpp b/...ning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/smoother_base.hpp
@@ -71,12 +71,12 @@ class SmootherBase
     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;
 
-  virtual boost::optional<TrajectoryPoints> applyLateralAccelerationFilter(
+  virtual TrajectoryPoints applyLateralAccelerationFilter(
     const TrajectoryPoints & input, [[maybe_unused]] const double v0 = 0.0,
     [[maybe_unused]] const double a0 = 0.0,
     [[maybe_unused]] const bool enable_smooth_limit = false) const;
 
-  boost::optional<TrajectoryPoints> applySteeringRateLimit(const TrajectoryPoints & input) const;
+  TrajectoryPoints applySteeringRateLimit(const TrajectoryPoints & input) const;
 
   double getMaxAccel() const;
   double getMinDecel() const;

diff --git a/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp b/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp
@@ -523,30 +523,24 @@ bool MotionVelocitySmootherNode::smoothVelocity(
   const TrajectoryPoints & input, const size_t input_closest,
   TrajectoryPoints & traj_smoothed) const
 {
+  if (input.empty()) {
+    return false;  // cannot apply smoothing
+  }
+
   // Calculate initial motion for smoothing
   const auto [initial_motion, type] = calcInitialMotion(input, input_closest);
 
   // Lateral acceleration limit
   const auto traj_lateral_acc_filtered =
     smoother_->applyLateralAccelerationFilter(input, initial_motion.vel, initial_motion.acc, true);
-  if (!traj_lateral_acc_filtered) {
-    RCLCPP_ERROR(get_logger(), "Fail to do traj_lateral_acc_filtered");
-
-    return false;
-  }
 
   // Steering angle rate limit
   const auto traj_steering_rate_limited =
-    smoother_->applySteeringRateLimit(*traj_lateral_acc_filtered);
-  if (!traj_steering_rate_limited) {
-    RCLCPP_ERROR(get_logger(), "Fail to do traj_steering_rate_limited");
-
-    return false;
-  }
+    smoother_->applySteeringRateLimit(traj_lateral_acc_filtered);
 
   // Resample trajectory with ego-velocity based interval distance
   auto traj_resampled = smoother_->resampleTrajectory(
-    *traj_steering_rate_limited, current_odometry_ptr_->twist.twist.linear.x,
+    traj_steering_rate_limited, current_odometry_ptr_->twist.twist.linear.x,
     current_odometry_ptr_->pose.pose, node_param_.ego_nearest_dist_threshold,
     node_param_.ego_nearest_yaw_threshold);
 
@@ -593,7 +587,7 @@ bool MotionVelocitySmootherNode::smoothVelocity(
   RCLCPP_DEBUG(get_logger(), "smoothVelocity : traj_smoothed.size() = %lu", traj_smoothed.size());
   if (publish_debug_trajs_) {
     {
-      auto tmp = *traj_lateral_acc_filtered;
+      auto tmp = traj_lateral_acc_filtered;
       if (is_reverse_) flipVelocity(tmp);
       pub_trajectory_latacc_filtered_->publish(toTrajectoryMsg(tmp));
     }
@@ -603,7 +597,7 @@ bool MotionVelocitySmootherNode::smoothVelocity(
       pub_trajectory_resampled_->publish(toTrajectoryMsg(tmp));
     }
     {
-      auto tmp = *traj_steering_rate_limited;
+      auto tmp = traj_steering_rate_limited;
       if (is_reverse_) flipVelocity(tmp);
       pub_trajectory_steering_rate_limited_->publish(toTrajectoryMsg(tmp));
     }

diff --git a/...rc/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp b/...rc/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp
@@ -277,16 +277,12 @@ TrajectoryPoints AnalyticalJerkConstrainedSmoother::resampleTrajectory(
   return output;
 }
 
-boost::optional<TrajectoryPoints> AnalyticalJerkConstrainedSmoother::applyLateralAccelerationFilter(
+TrajectoryPoints AnalyticalJerkConstrainedSmoother::applyLateralAccelerationFilter(
   const TrajectoryPoints & input, [[maybe_unused]] const double v0,
   [[maybe_unused]] const double a0, [[maybe_unused]] const bool enable_smooth_limit) const
 {
-  if (input.empty()) {
-    return boost::none;
-  }
-
   if (input.size() < 3) {
-    return boost::optional<TrajectoryPoints>(input);  // cannot calculate lateral acc. do nothing.
+    return input;  // cannot calculate lateral acc. do nothing.
   }
 
   // Interpolate with constant interval distance for lateral acceleration calculation.

diff --git a/planning/motion_velocity_smoother/src/smoother/smoother_base.cpp b/planning/motion_velocity_smoother/src/smoother/smoother_base.cpp
@@ -69,16 +69,12 @@ double SmootherBase::getMaxJerk() const { return base_param_.max_jerk; }
 
 double SmootherBase::getMinJerk() const { return base_param_.min_jerk; }
 
-boost::optional<TrajectoryPoints> SmootherBase::applyLateralAccelerationFilter(
+TrajectoryPoints SmootherBase::applyLateralAccelerationFilter(
   const TrajectoryPoints & input, [[maybe_unused]] const double v0,
   [[maybe_unused]] const double a0, [[maybe_unused]] const bool enable_smooth_limit) const
 {
-  if (input.empty()) {
-    return boost::none;
-  }
-
   if (input.size() < 3) {
-    return boost::optional<TrajectoryPoints>(input);  // cannot calculate lateral acc. do nothing.
+    return input;  // cannot calculate lateral acc. do nothing.
   }
 
   // Interpolate with constant interval distance for lateral acceleration calculation.
@@ -135,16 +131,10 @@ boost::optional<TrajectoryPoints> SmootherBase::applyLateralAccelerationFilter(
   return output;
 }
 
-boost::optional<TrajectoryPoints> SmootherBase::applySteeringRateLimit(
-  const TrajectoryPoints & input) const
+TrajectoryPoints SmootherBase::applySteeringRateLimit(const TrajectoryPoints & input) const
 {
-  if (input.empty()) {
-    return boost::none;
-  }
-
   if (input.size() < 3) {
-    return boost::optional<TrajectoryPoints>(
-      input);  // cannot calculate the desired velocity. do nothing.
+    return input;  // cannot calculate the desired velocity. do nothing.
   }
   // Interpolate with constant interval distance for lateral acceleration calculation.
   std::vector<double> out_arclength;