refactor(behavior_velocity_planner_common): boost::optional to std::o…

…ptional

Signed-off-by: Zulfaqar Azmi <[email protected]>

planning/behavior_velocity_occlusion_spot_module/src/risk_predictive_braking.cpp

@@ -64,7 +64,7 @@ void applySafeVelocityConsideringPossibleCollision(
     const auto & pose = possible_collision.collision_with_margin.pose;
     const auto & decel_pose =
       planning_utils::insertDecelPoint(pose.position, *inout_path, safe_velocity);
-    if (decel_pose) debug_poses.push_back(decel_pose.get());
+    if (decel_pose) debug_poses.push_back(decel_pose.value());
   }
 }
 

planning/behavior_velocity_planner/src/planner_manager.cpp

@@ -108,11 +108,11 @@ autoware_auto_planning_msgs::msg::PathWithLaneId BehaviorVelocityPlannerManager:
   for (const auto & plugin : scene_manager_plugins_) {
     plugin->updateSceneModuleInstances(planner_data, input_path_msg);
     plugin->plan(&output_path_msg);
-    boost::optional<int> firstStopPathPointIndex = plugin->getFirstStopPathPointIndex();
+    const auto firstStopPathPointIndex = plugin->getFirstStopPathPointIndex();
 
     if (firstStopPathPointIndex) {
-      if (firstStopPathPointIndex.get() < first_stop_path_point_index) {
-        first_stop_path_point_index = firstStopPathPointIndex.get();
+      if (firstStopPathPointIndex.value() < first_stop_path_point_index) {
+        first_stop_path_point_index = firstStopPathPointIndex.value();
         stop_reason_msg = plugin->getModuleName();
       }
     }

planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp

@@ -35,15 +35,14 @@
 #include <tier4_planning_msgs/msg/velocity_limit.hpp>
 #include <tier4_v2x_msgs/msg/virtual_traffic_light_state_array.hpp>
 
-#include <boost/optional.hpp>
-
 #include <pcl/point_cloud.h>
 #include <pcl/point_types.h>
 
 #include <algorithm>
 #include <deque>
 #include <map>
 #include <memory>
+#include <optional>
 #include <vector>
 
 namespace behavior_velocity_planner
@@ -78,7 +77,7 @@ struct PlannerData
 
   // other internal data
   std::map<int, TrafficSignalStamped> traffic_light_id_map;
-  boost::optional<tier4_planning_msgs::msg::VelocityLimit> external_velocity_limit;
+  std::optional<tier4_planning_msgs::msg::VelocityLimit> external_velocity_limit;
   tier4_v2x_msgs::msg::VirtualTrafficLightStateArray::ConstSharedPtr virtual_traffic_light_states;
 
   // velocity smoother

planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/plugin_interface.hpp

@@ -34,7 +34,7 @@ class PluginInterface
   virtual void updateSceneModuleInstances(
     const std::shared_ptr<const PlannerData> & planner_data,
     const autoware_auto_planning_msgs::msg::PathWithLaneId & path) = 0;
-  virtual boost::optional<int> getFirstStopPathPointIndex() = 0;
+  virtual std::optional<int> getFirstStopPathPointIndex() = 0;
   virtual const char * getModuleName() = 0;
 };
 

planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/plugin_wrapper.hpp

@@ -18,6 +18,7 @@
 #include <behavior_velocity_planner_common/plugin_interface.hpp>
 
 #include <memory>
+#include <optional>
 
 namespace behavior_velocity_planner
 {
@@ -37,7 +38,7 @@ class PluginWrapper : public PluginInterface
   {
     scene_manager_->updateSceneModuleInstances(planner_data, path);
   }
-  boost::optional<int> getFirstStopPathPointIndex() override
+  std::optional<int> getFirstStopPathPointIndex() override
   {
     return scene_manager_->getFirstStopPathPointIndex();
   }

planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/scene_module_interface.hpp

@@ -73,18 +73,18 @@ class SceneModuleInterface
     planner_data_ = planner_data;
   }
 
-  boost::optional<tier4_v2x_msgs::msg::InfrastructureCommand> getInfrastructureCommand()
+  std::optional<tier4_v2x_msgs::msg::InfrastructureCommand> getInfrastructureCommand()
   {
     return infrastructure_command_;
   }
 
   void setInfrastructureCommand(
-    const boost::optional<tier4_v2x_msgs::msg::InfrastructureCommand> & command)
+    const std::optional<tier4_v2x_msgs::msg::InfrastructureCommand> & command)
   {
     infrastructure_command_ = command;
   }
 
-  boost::optional<int> getFirstStopPathPointIndex() { return first_stop_path_point_index_; }
+  std::optional<int> getFirstStopPathPointIndex() { return first_stop_path_point_index_; }
 
   void setActivation(const bool activated) { activated_ = activated; }
   void setRTCEnabled(const bool enable_rtc) { rtc_enabled_ = enable_rtc; }
@@ -104,8 +104,8 @@ class SceneModuleInterface
   rclcpp::Logger logger_;
   rclcpp::Clock::SharedPtr clock_;
   std::shared_ptr<const PlannerData> planner_data_;
-  boost::optional<tier4_v2x_msgs::msg::InfrastructureCommand> infrastructure_command_;
-  boost::optional<int> first_stop_path_point_index_;
+  std::optional<tier4_v2x_msgs::msg::InfrastructureCommand> infrastructure_command_;
+  std::optional<int> first_stop_path_point_index_;
   VelocityFactorInterface velocity_factor_;
 
   void setSafe(const bool safe)
@@ -131,7 +131,7 @@ class SceneModuleManagerInterface
 
   virtual const char * getModuleName() = 0;
 
-  boost::optional<int> getFirstStopPathPointIndex() { return first_stop_path_point_index_; }
+  std::optional<int> getFirstStopPathPointIndex() { return first_stop_path_point_index_; }
 
   void updateSceneModuleInstances(
     const std::shared_ptr<const PlannerData> & planner_data,
@@ -170,7 +170,7 @@ class SceneModuleManagerInterface
   std::shared_ptr<const PlannerData> planner_data_;
   motion_utils::VirtualWallMarkerCreator virtual_wall_marker_creator_;
 
-  boost::optional<int> first_stop_path_point_index_;
+  std::optional<int> first_stop_path_point_index_;
   rclcpp::Node & node_;
   rclcpp::Clock::SharedPtr clock_;
   // Debug

planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/arc_lane_util.hpp

@@ -20,20 +20,15 @@
 
 #include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
 
-#include <boost/optional.hpp>
-
 #include <algorithm>
-#include <memory>
+#include <optional>
 #include <utility>
-#include <vector>
 
 #define EIGEN_MPL2_ONLY
 #include <Eigen/Core>
 
 namespace behavior_velocity_planner
 {
-namespace bg = boost::geometry;
-
 namespace
 {
 geometry_msgs::msg::Point convertToGeomPoint(const tier4_autoware_utils::Point2d & p)
@@ -59,12 +54,12 @@ double calcSignedDistance(
   const geometry_msgs::msg::Pose & p1, const geometry_msgs::msg::Point & p2);
 
 // calculate one collision point between the line (from p1 to p2) and the line (from p3 to p4)
-boost::optional<geometry_msgs::msg::Point> checkCollision(
+std::optional<geometry_msgs::msg::Point> checkCollision(
   const geometry_msgs::msg::Point & p1, const geometry_msgs::msg::Point & p2,
   const geometry_msgs::msg::Point & p3, const geometry_msgs::msg::Point & p4);
 
 template <class T>
-boost::optional<PathIndexWithPoint> findCollisionSegment(
+std::optional<PathIndexWithPoint> findCollisionSegment(
   const T & path, const geometry_msgs::msg::Point & stop_line_p1,
   const geometry_msgs::msg::Point & stop_line_p2, const size_t target_lane_id)
 {
@@ -88,15 +83,15 @@ boost::optional<PathIndexWithPoint> findCollisionSegment(
     const auto collision_point = checkCollision(p1, p2, stop_line_p1, stop_line_p2);
 
     if (collision_point) {
-      return std::make_pair(i, collision_point.get());
+      return std::make_pair(i, collision_point.value());
     }
   }
 
   return {};
 }
 
 template <class T>
-boost::optional<PathIndexWithPoint> findCollisionSegment(
+std::optional<PathIndexWithPoint> findCollisionSegment(
   const T & path, const LineString2d & stop_line, const size_t target_lane_id)
 {
   const auto stop_line_p1 = convertToGeomPoint(stop_line.at(0));
@@ -106,7 +101,7 @@ boost::optional<PathIndexWithPoint> findCollisionSegment(
 }
 
 template <class T>
-boost::optional<PathIndexWithOffset> findForwardOffsetSegment(
+std::optional<PathIndexWithOffset> findForwardOffsetSegment(
   const T & path, const size_t base_idx, const double offset_length)
 {
   double sum_length = 0.0;
@@ -124,7 +119,7 @@ boost::optional<PathIndexWithOffset> findForwardOffsetSegment(
 }
 
 template <class T>
-boost::optional<PathIndexWithOffset> findBackwardOffsetSegment(
+std::optional<PathIndexWithOffset> findBackwardOffsetSegment(
   const T & path, const size_t base_idx, const double offset_length)
 {
   double sum_length = 0.0;
@@ -144,7 +139,7 @@ boost::optional<PathIndexWithOffset> findBackwardOffsetSegment(
 }
 
 template <class T>
-boost::optional<PathIndexWithOffset> findOffsetSegment(
+std::optional<PathIndexWithOffset> findOffsetSegment(
   const T & path, const PathIndexWithPoint & collision_segment, const double offset_length)
 {
   const size_t collision_idx = collision_segment.first;
@@ -163,7 +158,7 @@ boost::optional<PathIndexWithOffset> findOffsetSegment(
       tier4_autoware_utils::calcDistance2d(path.points.at(collision_idx + 1), collision_point));
 }
 
-boost::optional<PathIndexWithOffset> findOffsetSegment(
+std::optional<PathIndexWithOffset> findOffsetSegment(
   const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t index,
   const double offset);
 
@@ -196,7 +191,7 @@ geometry_msgs::msg::Pose calcTargetPose(const T & path, const PathIndexWithOffse
   return target_pose;
 }
 
-boost::optional<PathIndexWithPose> createTargetPoint(
+std::optional<PathIndexWithPose> createTargetPoint(
   const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const LineString2d & stop_line,
   const size_t lane_id, const double margin, const double vehicle_offset);
 

planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/util.hpp

@@ -27,14 +27,13 @@
 #include <lanelet2_core/LaneletMap.h>
 #include <lanelet2_routing/Forward.h>
 
-#include <algorithm>
-#include <limits>
 #include <memory>
 #include <set>
 #include <string>
 #include <unordered_map>
 #include <utility>
 #include <vector>
+#include <optional>
 
 namespace behavior_velocity_planner
 {
@@ -133,7 +132,7 @@ std::vector<T> concatVector(const std::vector<T> & vec1, const std::vector<T> &
   return concat_vec;
 }
 
-boost::optional<int64_t> getNearestLaneId(
+std::optional<int64_t> getNearestLaneId(
   const PathWithLaneId & path, const lanelet::LaneletMapPtr lanelet_map,
   const geometry_msgs::msg::Pose & current_pose);
 
@@ -198,7 +197,7 @@ std::set<int64_t> getLaneletIdSetOnPath(
   return id_set;
 }
 
-boost::optional<geometry_msgs::msg::Pose> insertDecelPoint(
+std::optional<geometry_msgs::msg::Pose> insertDecelPoint(
   const geometry_msgs::msg::Point & stop_point, PathWithLaneId & output,
   const float target_velocity);
 
@@ -215,9 +214,9 @@ bool isOverLine(
   const geometry_msgs::msg::Pose & self_pose, const geometry_msgs::msg::Pose & line_pose,
   const double offset = 0.0);
 
-boost::optional<geometry_msgs::msg::Pose> insertStopPoint(
+std::optional<geometry_msgs::msg::Pose> insertStopPoint(
   const geometry_msgs::msg::Point & stop_point, PathWithLaneId & output);
-boost::optional<geometry_msgs::msg::Pose> insertStopPoint(
+std::optional<geometry_msgs::msg::Pose> insertStopPoint(
   const geometry_msgs::msg::Point & stop_point, const size_t stop_seg_idx, PathWithLaneId & output);
 
 /*

planning/behavior_velocity_planner_common/package.xml

@@ -29,7 +29,6 @@
   <depend>geometry_msgs</depend>
   <depend>interpolation</depend>
   <depend>lanelet2_extension</depend>
-  <depend>libboost-dev</depend>
   <depend>motion_utils</depend>
   <depend>motion_velocity_smoother</depend>
   <depend>nav_msgs</depend>

planning/behavior_velocity_planner_common/src/utilization/arc_lane_util.cpp

@@ -75,29 +75,29 @@ double calcSignedDistance(const geometry_msgs::msg::Pose & p1, const geometry_ms
 
 // calculate one collision point between the line (from p1 to p2) and the line (from p3 to p4)
 
-boost::optional<geometry_msgs::msg::Point> checkCollision(
+std::optional<geometry_msgs::msg::Point> checkCollision(
   const geometry_msgs::msg::Point & p1, const geometry_msgs::msg::Point & p2,
   const geometry_msgs::msg::Point & p3, const geometry_msgs::msg::Point & p4)
 {
   const double det = (p2.x - p1.x) * (p4.y - p3.y) - (p2.y - p1.y) * (p4.x - p3.x);
 
   if (det == 0.0) {
     // collision is not one point.
-    return boost::none;
+    return std::nullopt;
   }
 
   const double t1 = ((p4.y - p3.y) * (p4.x - p1.x) - (p4.x - p3.x) * (p4.y - p1.y)) / det;
   const double t2 = ((p2.x - p1.x) * (p4.y - p1.y) - (p2.y - p1.y) * (p4.x - p1.x)) / det;
 
   // check collision is outside the segment line
   if (t1 < 0.0 || 1.0 < t1 || t2 < 0.0 || 1.0 < t2) {
-    return boost::none;
+    return std::nullopt;
   }
 
   return p1 * (1.0 - t1) + p2 * t1;
 }
 
-boost::optional<PathIndexWithOffset> findOffsetSegment(
+std::optional<PathIndexWithOffset> findOffsetSegment(
   const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t index,
   const double offset)
 {
@@ -108,7 +108,7 @@ boost::optional<PathIndexWithOffset> findOffsetSegment(
   return findBackwardOffsetSegment(path, index, -offset);
 }
 
-boost::optional<PathIndexWithPose> createTargetPoint(
+std::optional<PathIndexWithPose> createTargetPoint(
   const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const LineString2d & stop_line,
   const size_t lane_id, const double margin, const double vehicle_offset)
 {

planning/behavior_velocity_planner_common/src/utilization/util.cpp

@@ -519,7 +519,7 @@ LineString2d extendLine(
     {(p1 - length * t).x(), (p1 - length * t).y()}, {(p2 + length * t).x(), (p2 + length * t).y()}};
 }
 
-boost::optional<int64_t> getNearestLaneId(
+std::optional<int64_t> getNearestLaneId(
   const PathWithLaneId & path, const lanelet::LaneletMapPtr lanelet_map,
   const geometry_msgs::msg::Pose & current_pose)
 {
@@ -533,7 +533,7 @@ boost::optional<int64_t> getNearestLaneId(
   if (lanelet::utils::query::getClosestLanelet(lanes, current_pose, &closest_lane)) {
     return closest_lane.id();
   }
-  return boost::none;
+  return std::nullopt;
 }
 
 std::vector<lanelet::ConstLanelet> getLaneletsOnPath(
@@ -614,7 +614,7 @@ bool isOverLine(
          0.0;
 }
 
-boost::optional<geometry_msgs::msg::Pose> insertDecelPoint(
+std::optional<geometry_msgs::msg::Pose> insertDecelPoint(
   const geometry_msgs::msg::Point & stop_point, PathWithLaneId & output,
   const float target_velocity)
 {
@@ -638,7 +638,7 @@ boost::optional<geometry_msgs::msg::Pose> insertDecelPoint(
 }
 
 // TODO(murooka): remove this function for u-turn and crossing-path
-boost::optional<geometry_msgs::msg::Pose> insertStopPoint(
+std::optional<geometry_msgs::msg::Pose> insertStopPoint(
   const geometry_msgs::msg::Point & stop_point, PathWithLaneId & output)
 {
   const size_t base_idx = motion_utils::findNearestSegmentIndex(output.points, stop_point);
@@ -651,7 +651,7 @@ boost::optional<geometry_msgs::msg::Pose> insertStopPoint(
   return tier4_autoware_utils::getPose(output.points.at(insert_idx.value()));
 }
 
-boost::optional<geometry_msgs::msg::Pose> insertStopPoint(
+std::optional<geometry_msgs::msg::Pose> insertStopPoint(
   const geometry_msgs::msg::Point & stop_point, const size_t stop_seg_idx, PathWithLaneId & output)
 {
   const auto insert_idx = motion_utils::insertStopPoint(stop_seg_idx, stop_point, output.points);