diff --git a/planning/behavior_velocity_crosswalk_module/src/debug.cpp b/planning/behavior_velocity_crosswalk_module/src/debug.cpp index 62248e21ac4fc..aa6acdc8829d5 100644 --- a/planning/behavior_velocity_crosswalk_module/src/debug.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/debug.cpp @@ -83,7 +83,7 @@ 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); } @@ -91,7 +91,7 @@ visualization_msgs::msg::MarkerArray createCrosswalkMarkers( 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 index 2e4e7247df646..0cf87cade7212 100644 --- 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( - 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::lowest(), path.header.stamp); }; - if (opt_use_regulatory_element_.get()) { + if (*opt_use_regulatory_element_) { const auto crosswalk_leg_elem_map = planning_utils::getRegElemMapOnPath( path, rh->getLaneletMapPtr(), planner_data_->current_odometry->pose); @@ -187,7 +187,7 @@ CrosswalkModuleManager::getModuleExpiredFunction(const PathWithLaneId & path) std::set crosswalk_id_set; - if (opt_use_regulatory_element_.get()) { + if (*opt_use_regulatory_element_) { const auto crosswalk_leg_elem_map = planning_utils::getRegElemMapOnPath( 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( - 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( path, rh->getLaneletMapPtr(), planner_data_->current_odometry->pose); @@ -267,7 +267,7 @@ WalkwayModuleManager::getModuleExpiredFunction(const PathWithLaneId & path) std::set walkway_id_set; - if (opt_use_regulatory_element_.get()) { + if (*opt_use_regulatory_element_) { const auto crosswalk_leg_elem_map = planning_utils::getRegElemMapOnPath( 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 index 024bb4582eb1a..ed342400808f1 100644 --- a/planning/behavior_velocity_crosswalk_module/src/manager.hpp +++ b/planning/behavior_velocity_crosswalk_module/src/manager.hpp @@ -29,6 +29,7 @@ #include #include +#include #include #include @@ -52,7 +53,7 @@ class CrosswalkModuleManager : public SceneModuleManagerInterfaceWithRTC std::function &)> getModuleExpiredFunction( const PathWithLaneId & path) override; - boost::optional opt_use_regulatory_element_{boost::none}; + std::optional opt_use_regulatory_element_{std::nullopt}; }; class WalkwayModuleManager : public SceneModuleManagerInterface @@ -70,7 +71,7 @@ class WalkwayModuleManager : public SceneModuleManagerInterface std::function &)> getModuleExpiredFunction( const PathWithLaneId & path) override; - boost::optional opt_use_regulatory_element_{boost::none}; + std::optional opt_use_regulatory_element_{std::nullopt}; }; class CrosswalkModulePlugin : public PluginWrapper diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp index 3ee1c3d16e152..9b2bf6a8c761d 100644 --- 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 toStdOptional( + const boost::optional & 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> -CrosswalkModule::getStopPointWithMargin( +std::optional> CrosswalkModule::getStopPointWithMargin( const PathWithLaneId & ego_path, const std::vector & path_intersects) const { @@ -279,11 +287,11 @@ CrosswalkModule::getStopPointWithMargin( return {}; } -boost::optional CrosswalkModule::checkStopForCrosswalkUsers( +std::optional CrosswalkModule::checkStopForCrosswalkUsers( const PathWithLaneId & ego_path, const PathWithLaneId & sparse_resample_path, - const boost::optional> & p_stop_line, + const std::optional> & p_stop_line, const std::vector & path_intersects, - const boost::optional & default_stop_pose) + const std::optional & 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 CrosswalkModule::clampAttentionRangeByNeighborCrosswal sortCrosswalksByDistance(ego_path, ego_pos, crosswalks); - boost::optional prev_crosswalk{boost::none}; - boost::optional next_crosswalk{boost::none}; + std::optional prev_crosswalk{std::nullopt}; + std::optional next_crosswalk{std::nullopt}; if (!crosswalks.empty()) { for (size_t i = 0; i < crosswalks.size() - 1; ++i) { @@ -506,7 +514,7 @@ std::pair 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 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 CrosswalkModule::checkStopForStuckedVehicles( +std::optional CrosswalkModule::checkStopForStuckedVehicles( const PathWithLaneId & ego_path, const std::vector & objects, const std::vector & path_intersects, - const boost::optional & stop_pose) const + const std::optional & stop_pose) const { if (path_intersects.size() < 2 || !stop_pose) { return {}; @@ -789,14 +797,14 @@ boost::optional CrosswalkModule::checkStopForStuckedVehicles( return {}; } -boost::optional CrosswalkModule::getNearestStopFactor( +std::optional CrosswalkModule::getNearestStopFactor( const PathWithLaneId & ego_path, - const boost::optional & stop_factor_for_crosswalk_users, - const boost::optional & stop_factor_for_stucked_vehicles) + const std::optional & stop_factor_for_crosswalk_users, + const std::optional & stop_factor_for_stucked_vehicles) { - const auto get_distance_to_stop = [&](const auto stop_factor) -> boost::optional { + const auto get_distance_to_stop = [&](const auto stop_factor) -> std::optional { 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 & stop_factor) + PathWithLaneId & ego_path, const std::optional & stop_factor) { if (!stop_factor) { setDistance(std::numeric_limits::lowest()); @@ -988,13 +996,13 @@ void CrosswalkModule::planGo( } void CrosswalkModule::planStop( - PathWithLaneId & ego_path, const boost::optional & nearest_stop_factor, - const boost::optional & default_stop_pose, StopReason * stop_reason) + PathWithLaneId & ego_path, const std::optional & nearest_stop_factor, + const std::optional & default_stop_pose, StopReason * stop_reason) { - const auto stop_factor = [&]() -> boost::optional { + const auto stop_factor = [&]() -> std::optional { 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 index f81ed44f83bac..327ec693cb264 100644 --- 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 #include +#include #include #include #include @@ -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 time_to_start_stopped{boost::none}; + std::optional 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 & path_intersects); - boost::optional> getStopPointWithMargin( + std::optional> getStopPointWithMargin( const PathWithLaneId & ego_path, const std::vector & path_intersects) const; - boost::optional checkStopForCrosswalkUsers( + std::optional checkStopForCrosswalkUsers( const PathWithLaneId & ego_path, const PathWithLaneId & sparse_resample_path, - const boost::optional> & p_stop_line, + const std::optional> & p_stop_line, const std::vector & path_intersects, - const boost::optional & default_stop_pose); + const std::optional & default_stop_pose); - boost::optional checkStopForStuckedVehicles( + std::optional checkStopForStuckedVehicles( const PathWithLaneId & ego_path, const std::vector & objects, const std::vector & path_intersects, - const boost::optional & stop_pose) const; + const std::optional & stop_pose) const; std::vector getCollisionPoints( const PathWithLaneId & ego_path, const PredictedObject & object, const Polygon2d & attention_area, const std::pair & crosswalk_attention_range); - boost::optional getNearestStopFactor( + std::optional getNearestStopFactor( const PathWithLaneId & ego_path, - const boost::optional & stop_factor_for_crosswalk_users, - const boost::optional & stop_factor_for_stucked_vehicles); + const std::optional & stop_factor_for_crosswalk_users, + const std::optional & stop_factor_for_stucked_vehicles); - void planGo(PathWithLaneId & ego_path, const boost::optional & stop_factor); + void planGo(PathWithLaneId & ego_path, const std::optional & stop_factor); void planStop( - PathWithLaneId & ego_path, const boost::optional & nearest_stop_factor, - const boost::optional & default_stop_pose, StopReason * stop_reason); + PathWithLaneId & ego_path, const std::optional & nearest_stop_factor, + const std::optional & default_stop_pose, StopReason * stop_reason); // minor functions std::pair getAttentionRange( diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_walkway.cpp b/planning/behavior_velocity_crosswalk_module/src/scene_walkway.cpp index f08d67c74c653..7979ff88bd52a 100644 --- 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> WalkwayModule::getStopLine( +std::optional> WalkwayModule::getStopLine( const PathWithLaneId & ego_path, bool & exist_stopline_in_map, const std::vector & 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 index 761139d7e1106..8aebcbf2f347f 100644 --- 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 #include +#include #include #include @@ -57,7 +58,7 @@ class WalkwayModule : public SceneModuleInterface private: const int64_t module_id_; - [[nodiscard]] boost::optional> getStopLine( + [[nodiscard]] std::optional> getStopLine( const PathWithLaneId & ego_path, bool & exist_stopline_in_map, const std::vector & path_intersects) const; diff --git a/planning/behavior_velocity_crosswalk_module/src/util.cpp b/planning/behavior_velocity_crosswalk_module/src/util.cpp index 3674f5e415b1a..4868a06dc909d 100644 --- a/planning/behavior_velocity_crosswalk_module/src/util.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/util.cpp @@ -146,7 +146,7 @@ std::vector getLinestringIntersects( return geometry_points; } -lanelet::Optional getStopLineFromMap( +std::optional getStopLineFromMap( const int lane_id, const lanelet::LaneletMapPtr & lanelet_map_ptr, const std::string & attribute_name) { diff --git a/planning/behavior_velocity_crosswalk_module/src/util.hpp b/planning/behavior_velocity_crosswalk_module/src/util.hpp index 2839dedaa5f66..299eb79906fc2 100644 --- a/planning/behavior_velocity_crosswalk_module/src/util.hpp +++ b/planning/behavior_velocity_crosswalk_module/src/util.hpp @@ -23,6 +23,7 @@ #include #include +#include #include #include #include @@ -68,8 +69,8 @@ struct DebugData geometry_msgs::msg::Pose first_stop_pose; geometry_msgs::msg::Point nearest_collision_point; - boost::optional range_near_point{boost::none}; - boost::optional range_far_point{boost::none}; + std::optional range_near_point{std::nullopt}; + std::optional range_far_point{std::nullopt}; std::vector> collision_points; @@ -89,7 +90,7 @@ std::vector getLinestringIntersects( const PathWithLaneId & ego_path, const lanelet::BasicLineString2d & linestring, const geometry_msgs::msg::Point & ego_pos, const size_t max_num); -lanelet::Optional getStopLineFromMap( +std::optional getStopLineFromMap( const int lane_id, const lanelet::LaneletMapPtr & lanelet_map_ptr, const std::string & attribute_name); } // namespace behavior_velocity_planner