refactor(behavior_velocity_crosswalk_module): use std::optional inste…

…ad of boost::optional (#4311) Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>
autowarefoundation · Jul 19, 2023 · 37cbd8d · 37cbd8d
1 parent 61cde1d
commit 37cbd8d
Show file tree

Hide file tree

Showing 9 changed files with 71 additions and 59 deletions.
diff --git a/planning/behavior_velocity_crosswalk_module/src/debug.cpp b/planning/behavior_velocity_crosswalk_module/src/debug.cpp
@@ -83,15 +83,15 @@ visualization_msgs::msg::MarkerArray createCrosswalkMarkers(
     auto marker = createDefaultMarker(
       "map", now, "attention range near", uid, Marker::POINTS, createMarkerScale(0.25, 0.25, 0.0),
       createMarkerColor(0.0, 0.0, 1.0, 0.999));
-    marker.points.push_back(debug_data.range_near_point.get());
+    marker.points.push_back(*debug_data.range_near_point);
     msg.markers.push_back(marker);
   }
 
   if (debug_data.range_far_point) {
     auto marker = createDefaultMarker(
       "map", now, "attention range far", uid, Marker::POINTS, createMarkerScale(0.25, 0.25, 0.0),
       createMarkerColor(1.0, 1.0, 1.0, 0.999));
-    marker.points.push_back(debug_data.range_far_point.get());
+    marker.points.push_back(*debug_data.range_far_point);
     msg.markers.push_back(marker);
   }
 

diff --git a/planning/behavior_velocity_crosswalk_module/src/manager.cpp b/planning/behavior_velocity_crosswalk_module/src/manager.cpp
@@ -143,7 +143,7 @@ void CrosswalkModuleManager::launchNewModules(const PathWithLaneId & path)
     opt_use_regulatory_element_ = checkRegulatoryElementExistence(rh->getLaneletMapPtr());
     std::ostringstream string_stream;
     string_stream << "use crosswalk regulatory element: ";
-    string_stream << std::boolalpha << opt_use_regulatory_element_.get();
+    string_stream << std::boolalpha << *opt_use_regulatory_element_;
     RCLCPP_INFO_STREAM(logger_, string_stream.str());
   }
 
@@ -157,12 +157,12 @@ void CrosswalkModuleManager::launchNewModules(const PathWithLaneId & path)
     const auto lanelet_map_ptr = planner_data_->route_handler_->getLaneletMapPtr();
 
     registerModule(std::make_shared<CrosswalkModule>(
-      id, lanelet_map_ptr, p, opt_use_regulatory_element_.get(), logger, clock_));
+      id, lanelet_map_ptr, p, *opt_use_regulatory_element_, logger, clock_));
     generateUUID(id);
     updateRTCStatus(getUUID(id), true, std::numeric_limits<double>::lowest(), path.header.stamp);
   };
 
-  if (opt_use_regulatory_element_.get()) {
+  if (*opt_use_regulatory_element_) {
     const auto crosswalk_leg_elem_map = planning_utils::getRegElemMapOnPath<Crosswalk>(
       path, rh->getLaneletMapPtr(), planner_data_->current_odometry->pose);
 
@@ -187,7 +187,7 @@ CrosswalkModuleManager::getModuleExpiredFunction(const PathWithLaneId & path)
 
   std::set<int64_t> crosswalk_id_set;
 
-  if (opt_use_regulatory_element_.get()) {
+  if (*opt_use_regulatory_element_) {
     const auto crosswalk_leg_elem_map = planning_utils::getRegElemMapOnPath<Crosswalk>(
       path, rh->getLaneletMapPtr(), planner_data_->current_odometry->pose);
 
@@ -239,10 +239,10 @@ void WalkwayModuleManager::launchNewModules(const PathWithLaneId & path)
     const auto lanelet_map_ptr = planner_data_->route_handler_->getLaneletMapPtr();
 
     registerModule(std::make_shared<WalkwayModule>(
-      lanelet.id(), lanelet_map_ptr, p, opt_use_regulatory_element_.get(), logger, clock_));
+      lanelet.id(), lanelet_map_ptr, p, *opt_use_regulatory_element_, logger, clock_));
   };
 
-  if (opt_use_regulatory_element_.get()) {
+  if (*opt_use_regulatory_element_) {
     const auto crosswalk_leg_elem_map = planning_utils::getRegElemMapOnPath<Crosswalk>(
       path, rh->getLaneletMapPtr(), planner_data_->current_odometry->pose);
 
@@ -267,7 +267,7 @@ WalkwayModuleManager::getModuleExpiredFunction(const PathWithLaneId & path)
 
   std::set<int64_t> walkway_id_set;
 
-  if (opt_use_regulatory_element_.get()) {
+  if (*opt_use_regulatory_element_) {
     const auto crosswalk_leg_elem_map = planning_utils::getRegElemMapOnPath<Crosswalk>(
       path, rh->getLaneletMapPtr(), planner_data_->current_odometry->pose);
 

diff --git a/planning/behavior_velocity_crosswalk_module/src/manager.hpp b/planning/behavior_velocity_crosswalk_module/src/manager.hpp
@@ -29,6 +29,7 @@
 
 #include <functional>
 #include <memory>
+#include <optional>
 #include <set>
 #include <vector>
 
@@ -52,7 +53,7 @@ class CrosswalkModuleManager : public SceneModuleManagerInterfaceWithRTC
   std::function<bool(const std::shared_ptr<SceneModuleInterface> &)> getModuleExpiredFunction(
     const PathWithLaneId & path) override;
 
-  boost::optional<bool> opt_use_regulatory_element_{boost::none};
+  std::optional<bool> opt_use_regulatory_element_{std::nullopt};
 };
 
 class WalkwayModuleManager : public SceneModuleManagerInterface
@@ -70,7 +71,7 @@ class WalkwayModuleManager : public SceneModuleManagerInterface
   std::function<bool(const std::shared_ptr<SceneModuleInterface> &)> getModuleExpiredFunction(
     const PathWithLaneId & path) override;
 
-  boost::optional<bool> opt_use_regulatory_element_{boost::none};
+  std::optional<bool> opt_use_regulatory_element_{std::nullopt};
 };
 
 class CrosswalkModulePlugin : public PluginWrapper<CrosswalkModuleManager>

diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp
@@ -153,6 +153,15 @@ StopFactor createStopFactor(
   stop_factor.stop_pose = stop_pose;
   return stop_factor;
 }
+
+std::optional<geometry_msgs::msg::Pose> toStdOptional(
+  const boost::optional<geometry_msgs::msg::Pose> & boost_pose)
+{
+  if (boost_pose) {
+    return *boost_pose;
+  }
+  return std::nullopt;
+}
 }  // namespace
 
 CrosswalkModule::CrosswalkModule(
@@ -175,7 +184,7 @@ CrosswalkModule::CrosswalkModule(
   } else {
     const auto stop_line = getStopLineFromMap(module_id_, lanelet_map_ptr, "crosswalk_id");
     if (stop_line) {
-      stop_lines_.push_back(stop_line.get());
+      stop_lines_.push_back(*stop_line);
     }
     crosswalk_ = lanelet_map_ptr->laneletLayer.get(module_id);
   }
@@ -217,8 +226,8 @@ bool CrosswalkModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto
   // Calculate stop point with margin
   const auto p_stop_line = getStopPointWithMargin(*path, path_intersects);
   // TODO(murooka) add a guard of p_stop_line
-  const auto default_stop_pose =
-    calcLongitudinalOffsetPose(path->points, p_stop_line->first, p_stop_line->second);
+  const auto default_stop_pose = toStdOptional(
+    calcLongitudinalOffsetPose(path->points, p_stop_line->first, p_stop_line->second));
 
   // Resample path sparsely for less computation cost
   constexpr double resample_interval = 4.0;
@@ -253,8 +262,7 @@ bool CrosswalkModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto
 }
 
 // NOTE: The stop point will be the returned point with the margin.
-boost::optional<std::pair<geometry_msgs::msg::Point, double>>
-CrosswalkModule::getStopPointWithMargin(
+std::optional<std::pair<geometry_msgs::msg::Point, double>> CrosswalkModule::getStopPointWithMargin(
   const PathWithLaneId & ego_path,
   const std::vector<geometry_msgs::msg::Point> & path_intersects) const
 {
@@ -279,11 +287,11 @@ CrosswalkModule::getStopPointWithMargin(
   return {};
 }
 
-boost::optional<StopFactor> CrosswalkModule::checkStopForCrosswalkUsers(
+std::optional<StopFactor> CrosswalkModule::checkStopForCrosswalkUsers(
   const PathWithLaneId & ego_path, const PathWithLaneId & sparse_resample_path,
-  const boost::optional<std::pair<geometry_msgs::msg::Point, double>> & p_stop_line,
+  const std::optional<std::pair<geometry_msgs::msg::Point, double>> & p_stop_line,
   const std::vector<geometry_msgs::msg::Point> & path_intersects,
-  const boost::optional<geometry_msgs::msg::Pose> & default_stop_pose)
+  const std::optional<geometry_msgs::msg::Pose> & default_stop_pose)
 {
   const auto & ego_pos = planner_data_->current_odometry->pose.position;
   const auto & objects_ptr = planner_data_->predicted_objects;
@@ -480,8 +488,8 @@ std::pair<double, double> CrosswalkModule::clampAttentionRangeByNeighborCrosswal
 
   sortCrosswalksByDistance(ego_path, ego_pos, crosswalks);
 
-  boost::optional<lanelet::ConstLanelet> prev_crosswalk{boost::none};
-  boost::optional<lanelet::ConstLanelet> next_crosswalk{boost::none};
+  std::optional<lanelet::ConstLanelet> prev_crosswalk{std::nullopt};
+  std::optional<lanelet::ConstLanelet> next_crosswalk{std::nullopt};
 
   if (!crosswalks.empty()) {
     for (size_t i = 0; i < crosswalks.size() - 1; ++i) {
@@ -506,7 +514,7 @@ std::pair<double, double> CrosswalkModule::clampAttentionRangeByNeighborCrosswal
     std::reverse(reverse_ego_path.points.begin(), reverse_ego_path.points.end());
 
     const auto prev_crosswalk_intersects = getPolygonIntersects(
-      reverse_ego_path, prev_crosswalk.get().polygon2d().basicPolygon(), ego_pos, 2);
+      reverse_ego_path, prev_crosswalk->polygon2d().basicPolygon(), ego_pos, 2);
     if (prev_crosswalk_intersects.empty()) {
       return near_attention_range;
     }
@@ -521,7 +529,7 @@ std::pair<double, double> CrosswalkModule::clampAttentionRangeByNeighborCrosswal
       return far_attention_range;
     }
     const auto next_crosswalk_intersects =
-      getPolygonIntersects(ego_path, next_crosswalk.get().polygon2d().basicPolygon(), ego_pos, 2);
+      getPolygonIntersects(ego_path, next_crosswalk->polygon2d().basicPolygon(), ego_pos, 2);
 
     if (next_crosswalk_intersects.empty()) {
       return far_attention_range;
@@ -748,10 +756,10 @@ Polygon2d CrosswalkModule::getAttentionArea(
   return attention_area;
 }
 
-boost::optional<StopFactor> CrosswalkModule::checkStopForStuckedVehicles(
+std::optional<StopFactor> CrosswalkModule::checkStopForStuckedVehicles(
   const PathWithLaneId & ego_path, const std::vector<PredictedObject> & objects,
   const std::vector<geometry_msgs::msg::Point> & path_intersects,
-  const boost::optional<geometry_msgs::msg::Pose> & stop_pose) const
+  const std::optional<geometry_msgs::msg::Pose> & stop_pose) const
 {
   if (path_intersects.size() < 2 || !stop_pose) {
     return {};
@@ -789,14 +797,14 @@ boost::optional<StopFactor> CrosswalkModule::checkStopForStuckedVehicles(
   return {};
 }
 
-boost::optional<StopFactor> CrosswalkModule::getNearestStopFactor(
+std::optional<StopFactor> CrosswalkModule::getNearestStopFactor(
   const PathWithLaneId & ego_path,
-  const boost::optional<StopFactor> & stop_factor_for_crosswalk_users,
-  const boost::optional<StopFactor> & stop_factor_for_stucked_vehicles)
+  const std::optional<StopFactor> & stop_factor_for_crosswalk_users,
+  const std::optional<StopFactor> & stop_factor_for_stucked_vehicles)
 {
-  const auto get_distance_to_stop = [&](const auto stop_factor) -> boost::optional<double> {
+  const auto get_distance_to_stop = [&](const auto stop_factor) -> std::optional<double> {
     const auto & ego_pos = planner_data_->current_odometry->pose.position;
-    if (!stop_factor) return boost::none;
+    if (!stop_factor) return std::nullopt;
     return calcSignedArcLength(ego_path.points, ego_pos, stop_factor->stop_pose.position);
   };
   const auto dist_to_stop_for_crosswalk_users =
@@ -967,7 +975,7 @@ geometry_msgs::msg::Polygon CrosswalkModule::createVehiclePolygon(
 }
 
 void CrosswalkModule::planGo(
-  PathWithLaneId & ego_path, const boost::optional<StopFactor> & stop_factor)
+  PathWithLaneId & ego_path, const std::optional<StopFactor> & stop_factor)
 {
   if (!stop_factor) {
     setDistance(std::numeric_limits<double>::lowest());
@@ -988,13 +996,13 @@ void CrosswalkModule::planGo(
 }
 
 void CrosswalkModule::planStop(
-  PathWithLaneId & ego_path, const boost::optional<StopFactor> & nearest_stop_factor,
-  const boost::optional<geometry_msgs::msg::Pose> & default_stop_pose, StopReason * stop_reason)
+  PathWithLaneId & ego_path, const std::optional<StopFactor> & nearest_stop_factor,
+  const std::optional<geometry_msgs::msg::Pose> & default_stop_pose, StopReason * stop_reason)
 {
-  const auto stop_factor = [&]() -> boost::optional<StopFactor> {
+  const auto stop_factor = [&]() -> std::optional<StopFactor> {
     if (nearest_stop_factor) return *nearest_stop_factor;
     if (default_stop_pose) return createStopFactor(*default_stop_pose);
-    return boost::none;
+    return std::nullopt;
   }();
 
   if (!stop_factor) {

diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp
@@ -38,6 +38,7 @@
 #include <pcl_conversions/pcl_conversions.h>
 
 #include <memory>
+#include <optional>
 #include <string>
 #include <unordered_map>
 #include <utility>
@@ -98,7 +99,7 @@ class CrosswalkModule : public SceneModuleInterface
     // NOTE: FULLY_STOPPED means stopped object which can be ignored.
     enum class State { STOPPED = 0, FULLY_STOPPED, OTHER };
     State state{State::OTHER};
-    boost::optional<rclcpp::Time> time_to_start_stopped{boost::none};
+    std::optional<rclcpp::Time> time_to_start_stopped{std::nullopt};
 
     void updateState(
       const rclcpp::Time & now, const double obj_vel, const bool is_ego_yielding,
@@ -123,7 +124,7 @@ class CrosswalkModule : public SceneModuleInterface
           state = State::STOPPED;
         }
       } else {
-        time_to_start_stopped = boost::none;
+        time_to_start_stopped = std::nullopt;
         state = State::OTHER;
       }
     }
@@ -182,35 +183,35 @@ class CrosswalkModule : public SceneModuleInterface
   void applySafetySlowDownSpeed(
     PathWithLaneId & output, const std::vector<geometry_msgs::msg::Point> & path_intersects);
 
-  boost::optional<std::pair<geometry_msgs::msg::Point, double>> getStopPointWithMargin(
+  std::optional<std::pair<geometry_msgs::msg::Point, double>> getStopPointWithMargin(
     const PathWithLaneId & ego_path,
     const std::vector<geometry_msgs::msg::Point> & path_intersects) const;
 
-  boost::optional<StopFactor> checkStopForCrosswalkUsers(
+  std::optional<StopFactor> checkStopForCrosswalkUsers(
     const PathWithLaneId & ego_path, const PathWithLaneId & sparse_resample_path,
-    const boost::optional<std::pair<geometry_msgs::msg::Point, double>> & p_stop_line,
+    const std::optional<std::pair<geometry_msgs::msg::Point, double>> & p_stop_line,
     const std::vector<geometry_msgs::msg::Point> & path_intersects,
-    const boost::optional<geometry_msgs::msg::Pose> & default_stop_pose);
+    const std::optional<geometry_msgs::msg::Pose> & default_stop_pose);
 
-  boost::optional<StopFactor> checkStopForStuckedVehicles(
+  std::optional<StopFactor> checkStopForStuckedVehicles(
     const PathWithLaneId & ego_path, const std::vector<PredictedObject> & objects,
     const std::vector<geometry_msgs::msg::Point> & path_intersects,
-    const boost::optional<geometry_msgs::msg::Pose> & stop_pose) const;
+    const std::optional<geometry_msgs::msg::Pose> & stop_pose) const;
 
   std::vector<CollisionPoint> getCollisionPoints(
     const PathWithLaneId & ego_path, const PredictedObject & object,
     const Polygon2d & attention_area, const std::pair<double, double> & crosswalk_attention_range);
 
-  boost::optional<StopFactor> getNearestStopFactor(
+  std::optional<StopFactor> getNearestStopFactor(
     const PathWithLaneId & ego_path,
-    const boost::optional<StopFactor> & stop_factor_for_crosswalk_users,
-    const boost::optional<StopFactor> & stop_factor_for_stucked_vehicles);
+    const std::optional<StopFactor> & stop_factor_for_crosswalk_users,
+    const std::optional<StopFactor> & stop_factor_for_stucked_vehicles);
 
-  void planGo(PathWithLaneId & ego_path, const boost::optional<StopFactor> & stop_factor);
+  void planGo(PathWithLaneId & ego_path, const std::optional<StopFactor> & stop_factor);
 
   void planStop(
-    PathWithLaneId & ego_path, const boost::optional<StopFactor> & nearest_stop_factor,
-    const boost::optional<geometry_msgs::msg::Pose> & default_stop_pose, StopReason * stop_reason);
+    PathWithLaneId & ego_path, const std::optional<StopFactor> & nearest_stop_factor,
+    const std::optional<geometry_msgs::msg::Pose> & default_stop_pose, StopReason * stop_reason);
 
   // minor functions
   std::pair<double, double> getAttentionRange(

diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_walkway.cpp b/planning/behavior_velocity_crosswalk_module/src/scene_walkway.cpp
@@ -48,13 +48,13 @@ WalkwayModule::WalkwayModule(
   } else {
     const auto stop_line = getStopLineFromMap(module_id_, lanelet_map_ptr, "crosswalk_id");
     if (!!stop_line) {
-      stop_lines_.push_back(stop_line.get());
+      stop_lines_.push_back(*stop_line);
     }
     walkway_ = lanelet_map_ptr->laneletLayer.get(module_id);
   }
 }
 
-boost::optional<std::pair<double, geometry_msgs::msg::Point>> WalkwayModule::getStopLine(
+std::optional<std::pair<double, geometry_msgs::msg::Point>> WalkwayModule::getStopLine(
   const PathWithLaneId & ego_path, bool & exist_stopline_in_map,
   const std::vector<geometry_msgs::msg::Point> & path_intersects) const
 {
@@ -111,7 +111,7 @@ bool WalkwayModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop_
       return false;
     }
 
-    const auto & p_stop = p_stop_line.get().second;
+    const auto & p_stop = p_stop_line->second;
     const auto stop_line_distance = exist_stopline_in_map ? 0.0 : planner_param_.stop_line_distance;
     const auto margin = stop_line_distance + base_link2front;
     const auto stop_pose = calcLongitudinalOffsetPose(input.points, p_stop, -margin);
@@ -120,7 +120,7 @@ bool WalkwayModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop_
       return false;
     }
 
-    const auto inserted_pose = planning_utils::insertStopPoint(stop_pose.get().position, *path);
+    const auto inserted_pose = planning_utils::insertStopPoint(stop_pose->position, *path);
     if (inserted_pose) {
       debug_data_.stop_poses.push_back(inserted_pose.get());
     }
@@ -136,7 +136,7 @@ bool WalkwayModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop_
 
     // use arc length to identify if ego vehicle is in front of walkway stop or not.
     const double signed_arc_dist_to_stop_point =
-      calcSignedArcLength(input.points, ego_pos, stop_pose.get().position);
+      calcSignedArcLength(input.points, ego_pos, stop_pose->position);
 
     const double distance_threshold = 1.0;
     debug_data_.stop_judge_range = distance_threshold;

diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_walkway.hpp b/planning/behavior_velocity_crosswalk_module/src/scene_walkway.hpp
@@ -30,6 +30,7 @@
 #include <lanelet2_routing/RoutingGraphContainer.h>
 
 #include <memory>
+#include <optional>
 #include <utility>
 #include <vector>
 
@@ -57,7 +58,7 @@ class WalkwayModule : public SceneModuleInterface
 private:
   const int64_t module_id_;
 
-  [[nodiscard]] boost::optional<std::pair<double, geometry_msgs::msg::Point>> getStopLine(
+  [[nodiscard]] std::optional<std::pair<double, geometry_msgs::msg::Point>> getStopLine(
     const PathWithLaneId & ego_path, bool & exist_stopline_in_map,
     const std::vector<geometry_msgs::msg::Point> & path_intersects) const;
 

diff --git a/planning/behavior_velocity_crosswalk_module/src/util.cpp b/planning/behavior_velocity_crosswalk_module/src/util.cpp
@@ -146,7 +146,7 @@ std::vector<geometry_msgs::msg::Point> getLinestringIntersects(
   return geometry_points;
 }
 
-lanelet::Optional<lanelet::ConstLineString3d> getStopLineFromMap(
+std::optional<lanelet::ConstLineString3d> getStopLineFromMap(
   const int lane_id, const lanelet::LaneletMapPtr & lanelet_map_ptr,
   const std::string & attribute_name)
 {