refactor(run_out): boost::optional to std::optional (#5760)

Signed-off-by: Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

Author: zulfaqar-azmi-t4

planning/behavior_velocity_run_out_module/src/dynamic_obstacle.cpp

@@ -238,7 +238,7 @@ pcl::PointCloud<pcl::PointXYZ> extractLateralNearestPoints(
   return lateral_nearest_points;
 }
 
-boost::optional<Eigen::Affine3f> getTransformMatrix(
+std::optional<Eigen::Affine3f> getTransformMatrix(
   const tf2_ros::Buffer & tf_buffer, const std::string & target_frame_id,
   const std::string & source_frame_id, const builtin_interfaces::msg::Time & stamp)
 {

planning/behavior_velocity_run_out_module/src/scene.cpp

@@ -136,7 +136,7 @@ bool RunOutModule::modifyPathVelocity(
   return true;
 }
 
-boost::optional<DynamicObstacle> RunOutModule::detectCollision(
+std::optional<DynamicObstacle> RunOutModule::detectCollision(
   const std::vector<DynamicObstacle> & dynamic_obstacles, const PathWithLaneId & path)
 {
   if (path.points.size() < 2) {
@@ -197,7 +197,7 @@ boost::optional<DynamicObstacle> RunOutModule::detectCollision(
   return {};
 }
 
-boost::optional<DynamicObstacle> RunOutModule::findNearestCollisionObstacle(
+std::optional<DynamicObstacle> RunOutModule::findNearestCollisionObstacle(
   const PathWithLaneId & path, const geometry_msgs::msg::Pose & base_pose,
   std::vector<DynamicObstacle> & dynamic_obstacles) const
 {
@@ -337,7 +337,7 @@ std::vector<DynamicObstacle> RunOutModule::checkCollisionWithObstacles(
 
 // calculate the predicted pose of the obstacle on the predicted path with given travel time
 // assume that the obstacle moves with constant velocity
-boost::optional<geometry_msgs::msg::Pose> RunOutModule::calcPredictedObstaclePose(
+std::optional<geometry_msgs::msg::Pose> RunOutModule::calcPredictedObstaclePose(
   const std::vector<PredictedPath> & predicted_paths, const float travel_time,
   const float velocity_mps) const
 {
@@ -509,7 +509,7 @@ bool RunOutModule::checkCollisionWithPolygon() const
 }
 
 std::optional<geometry_msgs::msg::Pose> RunOutModule::calcStopPoint(
-  const boost::optional<DynamicObstacle> & dynamic_obstacle, const PathWithLaneId & path,
+  const std::optional<DynamicObstacle> & dynamic_obstacle, const PathWithLaneId & path,
   const geometry_msgs::msg::Pose & current_pose, const float current_vel,
   const float current_acc) const
 {
@@ -625,7 +625,7 @@ void RunOutModule::insertStopPoint(
 }
 
 void RunOutModule::insertVelocityForState(
-  const boost::optional<DynamicObstacle> & dynamic_obstacle, const PlannerData planner_data,
+  const std::optional<DynamicObstacle> & dynamic_obstacle, const PlannerData planner_data,
   const PlannerParam & planner_param, const PathWithLaneId & smoothed_path,
   PathWithLaneId & output_path)
 {
@@ -684,7 +684,7 @@ void RunOutModule::insertVelocityForState(
 }
 
 void RunOutModule::insertStoppingVelocity(
-  const boost::optional<DynamicObstacle> & dynamic_obstacle,
+  const std::optional<DynamicObstacle> & dynamic_obstacle,
   const geometry_msgs::msg::Pose & current_pose, const float current_vel, const float current_acc,
   PathWithLaneId & output_path)
 {
@@ -739,7 +739,7 @@ void RunOutModule::applyMaxJerkLimit(
     return;
   }
 
-  const auto stop_point = path.points.at(stop_point_idx.get()).point.pose.position;
+  const auto stop_point = path.points.at(stop_point_idx.value()).point.pose.position;
   const auto dist_to_stop_point =
     motion_utils::calcSignedArcLength(path.points, current_pose.position, stop_point);
 
@@ -749,7 +749,7 @@ void RunOutModule::applyMaxJerkLimit(
     current_vel, dist_to_stop_point);
 
   // overwrite velocity with limited velocity
-  run_out_utils::insertPathVelocityFromIndex(stop_point_idx.get(), jerk_limited_vel, path.points);
+  run_out_utils::insertPathVelocityFromIndex(stop_point_idx.value(), jerk_limited_vel, path.points);
 }
 
 std::vector<DynamicObstacle> RunOutModule::excludeObstaclesOutSideOfPartition(
@@ -782,7 +782,7 @@ std::vector<DynamicObstacle> RunOutModule::excludeObstaclesOutSideOfPartition(
 
 void RunOutModule::publishDebugValue(
   const PathWithLaneId & path, const std::vector<DynamicObstacle> extracted_obstacles,
-  const boost::optional<DynamicObstacle> & dynamic_obstacle,
+  const std::optional<DynamicObstacle> & dynamic_obstacle,
   const geometry_msgs::msg::Pose & current_pose) const
 {
   if (dynamic_obstacle) {

planning/behavior_velocity_run_out_module/src/scene.hpp

@@ -24,12 +24,10 @@
 
 #include <memory>
 #include <optional>
-#include <string>
 #include <vector>
 
 namespace behavior_velocity_planner
 {
-namespace bg = boost::geometry;
 using autoware_auto_perception_msgs::msg::PredictedObjects;
 using autoware_auto_planning_msgs::msg::PathPointWithLaneId;
 using autoware_auto_planning_msgs::msg::PathWithLaneId;
@@ -68,7 +66,7 @@ class RunOutModule : public SceneModuleInterface
   // Function
   Polygons2d createDetectionAreaPolygon(const PathWithLaneId & smoothed_path) const;
 
-  boost::optional<DynamicObstacle> detectCollision(
+  std::optional<DynamicObstacle> detectCollision(
     const std::vector<DynamicObstacle> & dynamic_obstacles, const PathWithLaneId & path_points);
 
   float calcCollisionPositionOfVehicleSide(
@@ -81,11 +79,11 @@ class RunOutModule : public SceneModuleInterface
     const std::vector<DynamicObstacle> & dynamic_obstacles,
     std::vector<geometry_msgs::msg::Point> poly, const float travel_time) const;
 
-  boost::optional<DynamicObstacle> findNearestCollisionObstacle(
+  std::optional<DynamicObstacle> findNearestCollisionObstacle(
     const PathWithLaneId & path, const geometry_msgs::msg::Pose & base_pose,
     std::vector<DynamicObstacle> & dynamic_obstacles) const;
 
-  boost::optional<geometry_msgs::msg::Pose> calcPredictedObstaclePose(
+  std::optional<geometry_msgs::msg::Pose> calcPredictedObstaclePose(
     const std::vector<PredictedPath> & predicted_paths, const float travel_time,
     const float velocity_mps) const;
 
@@ -108,7 +106,7 @@ class RunOutModule : public SceneModuleInterface
     const PoseWithRange & pose_with_range, const float x_offset, const float y_offset) const;
 
   std::optional<geometry_msgs::msg::Pose> calcStopPoint(
-    const boost::optional<DynamicObstacle> & dynamic_obstacle, const PathWithLaneId & path,
+    const std::optional<DynamicObstacle> & dynamic_obstacle, const PathWithLaneId & path,
     const geometry_msgs::msg::Pose & current_pose, const float current_vel,
     const float current_acc) const;
 
@@ -117,12 +115,12 @@ class RunOutModule : public SceneModuleInterface
     autoware_auto_planning_msgs::msg::PathWithLaneId & path);
 
   void insertVelocityForState(
-    const boost::optional<DynamicObstacle> & dynamic_obstacle, const PlannerData planner_data,
+    const std::optional<DynamicObstacle> & dynamic_obstacle, const PlannerData planner_data,
     const PlannerParam & planner_param, const PathWithLaneId & smoothed_path,
     PathWithLaneId & output_path);
 
   void insertStoppingVelocity(
-    const boost::optional<DynamicObstacle> & dynamic_obstacle,
+    const std::optional<DynamicObstacle> & dynamic_obstacle,
     const geometry_msgs::msg::Pose & current_pose, const float current_vel, const float current_acc,
     PathWithLaneId & output_path);
 
@@ -140,7 +138,7 @@ class RunOutModule : public SceneModuleInterface
 
   void publishDebugValue(
     const PathWithLaneId & path, const std::vector<DynamicObstacle> extracted_obstacles,
-    const boost::optional<DynamicObstacle> & dynamic_obstacle,
+    const std::optional<DynamicObstacle> & dynamic_obstacle,
     const geometry_msgs::msg::Pose & current_pose) const;
 
   bool isMomentaryDetection();

planning/behavior_velocity_run_out_module/src/state_machine.hpp

@@ -17,12 +17,9 @@
 
 #include "utils.hpp"
 
-#include <memory>
 #include <string>
 
-namespace behavior_velocity_planner
-{
-namespace run_out_utils
+namespace behavior_velocity_planner::run_out_utils
 {
 
 class StateMachine
@@ -37,26 +34,25 @@ class StateMachine
 
   struct StateInput
   {
-    double current_velocity;
-    double dist_to_collision;
-    boost::optional<DynamicObstacle> current_obstacle;
+    double current_velocity{};
+    double dist_to_collision{};
+    std::optional<DynamicObstacle> current_obstacle;
   };
 
   explicit StateMachine(const StateParam & state_param) { state_param_ = state_param; }
   State getCurrentState() const { return state_; }
-  boost::optional<DynamicObstacle> getTargetObstacle() const { return target_obstacle_; }
+  std::optional<DynamicObstacle> getTargetObstacle() const { return target_obstacle_; }
   std::string toString(const State & state) const;
   void updateState(const StateInput & state_input, rclcpp::Clock & clock);
 
 private:
-  StateParam state_param_;
+  StateParam state_param_{};
   State state_{State::GO};
   rclcpp::Time stop_time_;
   rclcpp::Time prev_approach_time_;
-  boost::optional<DynamicObstacle> prev_obstacle_{};
-  boost::optional<DynamicObstacle> target_obstacle_{};
+  std::optional<DynamicObstacle> prev_obstacle_{};
+  std::optional<DynamicObstacle> target_obstacle_{};
 };
-}  // namespace run_out_utils
-}  // namespace behavior_velocity_planner
+}  // namespace behavior_velocity_planner::run_out_utils
 
 #endif  // STATE_MACHINE_HPP_

planning/behavior_velocity_run_out_module/src/utils.cpp

@@ -133,7 +133,7 @@ bool isSamePoint(const geometry_msgs::msg::Point & p1, const geometry_msgs::msg:
 }
 
 // if path points have the same point as target_point, return the index
-boost::optional<size_t> haveSamePoint(
+std::optional<size_t> haveSamePoint(
   const PathPointsWithLaneId & path_points, const geometry_msgs::msg::Point & target_point)
 {
   for (size_t i = 0; i < path_points.size(); i++) {
@@ -164,7 +164,7 @@ void insertPathVelocityFromIndex(
   }
 }
 
-boost::optional<size_t> findFirstStopPointIdx(PathPointsWithLaneId & path_points)
+std::optional<size_t> findFirstStopPointIdx(PathPointsWithLaneId & path_points)
 {
   for (size_t i = 0; i < path_points.size(); i++) {
     const auto vel = path_points.at(i).point.longitudinal_velocity_mps;
@@ -272,7 +272,7 @@ PathWithLaneId trimPathFromSelfPose(
 
 // create polygon for passing lines and deceleration line calculated by stopping jerk
 // note that this polygon is not closed
-boost::optional<std::vector<geometry_msgs::msg::Point>> createDetectionAreaPolygon(
+std::optional<std::vector<geometry_msgs::msg::Point>> createDetectionAreaPolygon(
   const std::vector<std::vector<geometry_msgs::msg::Point>> & passing_lines,
   const size_t deceleration_line_idx)
 {

planning/behavior_velocity_run_out_module/src/utils.hpp

@@ -209,7 +209,7 @@ std::vector<geometry_msgs::msg::Point> findLateralSameSidePoints(
 bool isSamePoint(const geometry_msgs::msg::Point & p1, const geometry_msgs::msg::Point & p2);
 
 // if path points have the same point as target_point, return the index
-boost::optional<size_t> haveSamePoint(
+std::optional<size_t> haveSamePoint(
   const PathPointsWithLaneId & path_points, const geometry_msgs::msg::Point & target_point);
 
 // insert path velocity which doesn't exceed original velocity
@@ -219,7 +219,7 @@ void insertPathVelocityFromIndexLimited(
 void insertPathVelocityFromIndex(
   const size_t & start_idx, const float velocity_mps, PathPointsWithLaneId & path_points);
 
-boost::optional<size_t> findFirstStopPointIdx(PathPointsWithLaneId & path_points);
+std::optional<size_t> findFirstStopPointIdx(PathPointsWithLaneId & path_points);
 
 LineString2d createLineString2d(const lanelet::BasicPolygon2d & poly);
 
@@ -237,7 +237,7 @@ PathWithLaneId trimPathFromSelfPose(
 
 // create polygon for passing lines and deceleration line calculated by stopping jerk
 // note that this polygon is not closed
-boost::optional<std::vector<geometry_msgs::msg::Point>> createDetectionAreaPolygon(
+std::optional<std::vector<geometry_msgs::msg::Point>> createDetectionAreaPolygon(
   const std::vector<std::vector<geometry_msgs::msg::Point>> & passing_lines,
   const size_t deceleration_line_idx);