Skip to content

Commit 37cbd8d

Browse files
takayuki5168takayuki5168
authored
refactor(behavior_velocity_crosswalk_module): use std::optional instead of boost::optional (#4311)
Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>
1 parent 61cde1d commit 37cbd8d

File tree

9 files changed

+71
-59
lines changed

9 files changed

+71
-59
lines changed

‎planning/behavior_velocity_crosswalk_module/src/debug.cpp

+2-2
Original file line numberDiff line numberDiff line change
@@ -83,15 +83,15 @@ visualization_msgs::msg::MarkerArray createCrosswalkMarkers(
8383
auto marker = createDefaultMarker(
8484
"map", now, "attention range near", uid, Marker::POINTS, createMarkerScale(0.25, 0.25, 0.0),
8585
createMarkerColor(0.0, 0.0, 1.0, 0.999));
86-
marker.points.push_back(debug_data.range_near_point.get());
86+
marker.points.push_back(*debug_data.range_near_point);
8787
msg.markers.push_back(marker);
8888
}
8989

9090
if (debug_data.range_far_point) {
9191
auto marker = createDefaultMarker(
9292
"map", now, "attention range far", uid, Marker::POINTS, createMarkerScale(0.25, 0.25, 0.0),
9393
createMarkerColor(1.0, 1.0, 1.0, 0.999));
94-
marker.points.push_back(debug_data.range_far_point.get());
94+
marker.points.push_back(*debug_data.range_far_point);
9595
msg.markers.push_back(marker);
9696
}
9797

‎planning/behavior_velocity_crosswalk_module/src/manager.cpp

+7-7
Original file line numberDiff line numberDiff line change
@@ -143,7 +143,7 @@ void CrosswalkModuleManager::launchNewModules(const PathWithLaneId & path)
143143
opt_use_regulatory_element_ = checkRegulatoryElementExistence(rh->getLaneletMapPtr());
144144
std::ostringstream string_stream;
145145
string_stream << "use crosswalk regulatory element: ";
146-
string_stream << std::boolalpha << opt_use_regulatory_element_.get();
146+
string_stream << std::boolalpha << *opt_use_regulatory_element_;
147147
RCLCPP_INFO_STREAM(logger_, string_stream.str());
148148
}
149149

@@ -157,12 +157,12 @@ void CrosswalkModuleManager::launchNewModules(const PathWithLaneId & path)
157157
const auto lanelet_map_ptr = planner_data_->route_handler_->getLaneletMapPtr();
158158

159159
registerModule(std::make_shared<CrosswalkModule>(
160-
id, lanelet_map_ptr, p, opt_use_regulatory_element_.get(), logger, clock_));
160+
id, lanelet_map_ptr, p, *opt_use_regulatory_element_, logger, clock_));
161161
generateUUID(id);
162162
updateRTCStatus(getUUID(id), true, std::numeric_limits<double>::lowest(), path.header.stamp);
163163
};
164164

165-
if (opt_use_regulatory_element_.get()) {
165+
if (*opt_use_regulatory_element_) {
166166
const auto crosswalk_leg_elem_map = planning_utils::getRegElemMapOnPath<Crosswalk>(
167167
path, rh->getLaneletMapPtr(), planner_data_->current_odometry->pose);
168168

@@ -187,7 +187,7 @@ CrosswalkModuleManager::getModuleExpiredFunction(const PathWithLaneId & path)
187187

188188
std::set<int64_t> crosswalk_id_set;
189189

190-
if (opt_use_regulatory_element_.get()) {
190+
if (*opt_use_regulatory_element_) {
191191
const auto crosswalk_leg_elem_map = planning_utils::getRegElemMapOnPath<Crosswalk>(
192192
path, rh->getLaneletMapPtr(), planner_data_->current_odometry->pose);
193193

@@ -239,10 +239,10 @@ void WalkwayModuleManager::launchNewModules(const PathWithLaneId & path)
239239
const auto lanelet_map_ptr = planner_data_->route_handler_->getLaneletMapPtr();
240240

241241
registerModule(std::make_shared<WalkwayModule>(
242-
lanelet.id(), lanelet_map_ptr, p, opt_use_regulatory_element_.get(), logger, clock_));
242+
lanelet.id(), lanelet_map_ptr, p, *opt_use_regulatory_element_, logger, clock_));
243243
};
244244

245-
if (opt_use_regulatory_element_.get()) {
245+
if (*opt_use_regulatory_element_) {
246246
const auto crosswalk_leg_elem_map = planning_utils::getRegElemMapOnPath<Crosswalk>(
247247
path, rh->getLaneletMapPtr(), planner_data_->current_odometry->pose);
248248

@@ -267,7 +267,7 @@ WalkwayModuleManager::getModuleExpiredFunction(const PathWithLaneId & path)
267267

268268
std::set<int64_t> walkway_id_set;
269269

270-
if (opt_use_regulatory_element_.get()) {
270+
if (*opt_use_regulatory_element_) {
271271
const auto crosswalk_leg_elem_map = planning_utils::getRegElemMapOnPath<Crosswalk>(
272272
path, rh->getLaneletMapPtr(), planner_data_->current_odometry->pose);
273273

‎planning/behavior_velocity_crosswalk_module/src/manager.hpp

+3-2
Original file line numberDiff line numberDiff line change
@@ -29,6 +29,7 @@
2929

3030
#include <functional>
3131
#include <memory>
32+
#include <optional>
3233
#include <set>
3334
#include <vector>
3435

@@ -52,7 +53,7 @@ class CrosswalkModuleManager : public SceneModuleManagerInterfaceWithRTC
5253
std::function<bool(const std::shared_ptr<SceneModuleInterface> &)> getModuleExpiredFunction(
5354
const PathWithLaneId & path) override;
5455

55-
boost::optional<bool> opt_use_regulatory_element_{boost::none};
56+
std::optional<bool> opt_use_regulatory_element_{std::nullopt};
5657
};
5758

5859
class WalkwayModuleManager : public SceneModuleManagerInterface
@@ -70,7 +71,7 @@ class WalkwayModuleManager : public SceneModuleManagerInterface
7071
std::function<bool(const std::shared_ptr<SceneModuleInterface> &)> getModuleExpiredFunction(
7172
const PathWithLaneId & path) override;
7273

73-
boost::optional<bool> opt_use_regulatory_element_{boost::none};
74+
std::optional<bool> opt_use_regulatory_element_{std::nullopt};
7475
};
7576

7677
class CrosswalkModulePlugin : public PluginWrapper<CrosswalkModuleManager>

‎planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp

+32-24
Original file line numberDiff line numberDiff line change
@@ -153,6 +153,15 @@ StopFactor createStopFactor(
153153
stop_factor.stop_pose = stop_pose;
154154
return stop_factor;
155155
}
156+
157+
std::optional<geometry_msgs::msg::Pose> toStdOptional(
158+
const boost::optional<geometry_msgs::msg::Pose> & boost_pose)
159+
{
160+
if (boost_pose) {
161+
return *boost_pose;
162+
}
163+
return std::nullopt;
164+
}
156165
} // namespace
157166

158167
CrosswalkModule::CrosswalkModule(
@@ -175,7 +184,7 @@ CrosswalkModule::CrosswalkModule(
175184
} else {
176185
const auto stop_line = getStopLineFromMap(module_id_, lanelet_map_ptr, "crosswalk_id");
177186
if (stop_line) {
178-
stop_lines_.push_back(stop_line.get());
187+
stop_lines_.push_back(*stop_line);
179188
}
180189
crosswalk_ = lanelet_map_ptr->laneletLayer.get(module_id);
181190
}
@@ -217,8 +226,8 @@ bool CrosswalkModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto
217226
// Calculate stop point with margin
218227
const auto p_stop_line = getStopPointWithMargin(*path, path_intersects);
219228
// TODO(murooka) add a guard of p_stop_line
220-
const auto default_stop_pose =
221-
calcLongitudinalOffsetPose(path->points, p_stop_line->first, p_stop_line->second);
229+
const auto default_stop_pose = toStdOptional(
230+
calcLongitudinalOffsetPose(path->points, p_stop_line->first, p_stop_line->second));
222231

223232
// Resample path sparsely for less computation cost
224233
constexpr double resample_interval = 4.0;
@@ -253,8 +262,7 @@ bool CrosswalkModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto
253262
}
254263

255264
// NOTE: The stop point will be the returned point with the margin.
256-
boost::optional<std::pair<geometry_msgs::msg::Point, double>>
257-
CrosswalkModule::getStopPointWithMargin(
265+
std::optional<std::pair<geometry_msgs::msg::Point, double>> CrosswalkModule::getStopPointWithMargin(
258266
const PathWithLaneId & ego_path,
259267
const std::vector<geometry_msgs::msg::Point> & path_intersects) const
260268
{
@@ -279,11 +287,11 @@ CrosswalkModule::getStopPointWithMargin(
279287
return {};
280288
}
281289

282-
boost::optional<StopFactor> CrosswalkModule::checkStopForCrosswalkUsers(
290+
std::optional<StopFactor> CrosswalkModule::checkStopForCrosswalkUsers(
283291
const PathWithLaneId & ego_path, const PathWithLaneId & sparse_resample_path,
284-
const boost::optional<std::pair<geometry_msgs::msg::Point, double>> & p_stop_line,
292+
const std::optional<std::pair<geometry_msgs::msg::Point, double>> & p_stop_line,
285293
const std::vector<geometry_msgs::msg::Point> & path_intersects,
286-
const boost::optional<geometry_msgs::msg::Pose> & default_stop_pose)
294+
const std::optional<geometry_msgs::msg::Pose> & default_stop_pose)
287295
{
288296
const auto & ego_pos = planner_data_->current_odometry->pose.position;
289297
const auto & objects_ptr = planner_data_->predicted_objects;
@@ -480,8 +488,8 @@ std::pair<double, double> CrosswalkModule::clampAttentionRangeByNeighborCrosswal
480488

481489
sortCrosswalksByDistance(ego_path, ego_pos, crosswalks);
482490

483-
boost::optional<lanelet::ConstLanelet> prev_crosswalk{boost::none};
484-
boost::optional<lanelet::ConstLanelet> next_crosswalk{boost::none};
491+
std::optional<lanelet::ConstLanelet> prev_crosswalk{std::nullopt};
492+
std::optional<lanelet::ConstLanelet> next_crosswalk{std::nullopt};
485493

486494
if (!crosswalks.empty()) {
487495
for (size_t i = 0; i < crosswalks.size() - 1; ++i) {
@@ -506,7 +514,7 @@ std::pair<double, double> CrosswalkModule::clampAttentionRangeByNeighborCrosswal
506514
std::reverse(reverse_ego_path.points.begin(), reverse_ego_path.points.end());
507515

508516
const auto prev_crosswalk_intersects = getPolygonIntersects(
509-
reverse_ego_path, prev_crosswalk.get().polygon2d().basicPolygon(), ego_pos, 2);
517+
reverse_ego_path, prev_crosswalk->polygon2d().basicPolygon(), ego_pos, 2);
510518
if (prev_crosswalk_intersects.empty()) {
511519
return near_attention_range;
512520
}
@@ -521,7 +529,7 @@ std::pair<double, double> CrosswalkModule::clampAttentionRangeByNeighborCrosswal
521529
return far_attention_range;
522530
}
523531
const auto next_crosswalk_intersects =
524-
getPolygonIntersects(ego_path, next_crosswalk.get().polygon2d().basicPolygon(), ego_pos, 2);
532+
getPolygonIntersects(ego_path, next_crosswalk->polygon2d().basicPolygon(), ego_pos, 2);
525533

526534
if (next_crosswalk_intersects.empty()) {
527535
return far_attention_range;
@@ -748,10 +756,10 @@ Polygon2d CrosswalkModule::getAttentionArea(
748756
return attention_area;
749757
}
750758

751-
boost::optional<StopFactor> CrosswalkModule::checkStopForStuckedVehicles(
759+
std::optional<StopFactor> CrosswalkModule::checkStopForStuckedVehicles(
752760
const PathWithLaneId & ego_path, const std::vector<PredictedObject> & objects,
753761
const std::vector<geometry_msgs::msg::Point> & path_intersects,
754-
const boost::optional<geometry_msgs::msg::Pose> & stop_pose) const
762+
const std::optional<geometry_msgs::msg::Pose> & stop_pose) const
755763
{
756764
if (path_intersects.size() < 2 || !stop_pose) {
757765
return {};
@@ -789,14 +797,14 @@ boost::optional<StopFactor> CrosswalkModule::checkStopForStuckedVehicles(
789797
return {};
790798
}
791799

792-
boost::optional<StopFactor> CrosswalkModule::getNearestStopFactor(
800+
std::optional<StopFactor> CrosswalkModule::getNearestStopFactor(
793801
const PathWithLaneId & ego_path,
794-
const boost::optional<StopFactor> & stop_factor_for_crosswalk_users,
795-
const boost::optional<StopFactor> & stop_factor_for_stucked_vehicles)
802+
const std::optional<StopFactor> & stop_factor_for_crosswalk_users,
803+
const std::optional<StopFactor> & stop_factor_for_stucked_vehicles)
796804
{
797-
const auto get_distance_to_stop = [&](const auto stop_factor) -> boost::optional<double> {
805+
const auto get_distance_to_stop = [&](const auto stop_factor) -> std::optional<double> {
798806
const auto & ego_pos = planner_data_->current_odometry->pose.position;
799-
if (!stop_factor) return boost::none;
807+
if (!stop_factor) return std::nullopt;
800808
return calcSignedArcLength(ego_path.points, ego_pos, stop_factor->stop_pose.position);
801809
};
802810
const auto dist_to_stop_for_crosswalk_users =
@@ -967,7 +975,7 @@ geometry_msgs::msg::Polygon CrosswalkModule::createVehiclePolygon(
967975
}
968976

969977
void CrosswalkModule::planGo(
970-
PathWithLaneId & ego_path, const boost::optional<StopFactor> & stop_factor)
978+
PathWithLaneId & ego_path, const std::optional<StopFactor> & stop_factor)
971979
{
972980
if (!stop_factor) {
973981
setDistance(std::numeric_limits<double>::lowest());
@@ -988,13 +996,13 @@ void CrosswalkModule::planGo(
988996
}
989997

990998
void CrosswalkModule::planStop(
991-
PathWithLaneId & ego_path, const boost::optional<StopFactor> & nearest_stop_factor,
992-
const boost::optional<geometry_msgs::msg::Pose> & default_stop_pose, StopReason * stop_reason)
999+
PathWithLaneId & ego_path, const std::optional<StopFactor> & nearest_stop_factor,
1000+
const std::optional<geometry_msgs::msg::Pose> & default_stop_pose, StopReason * stop_reason)
9931001
{
994-
const auto stop_factor = [&]() -> boost::optional<StopFactor> {
1002+
const auto stop_factor = [&]() -> std::optional<StopFactor> {
9951003
if (nearest_stop_factor) return *nearest_stop_factor;
9961004
if (default_stop_pose) return createStopFactor(*default_stop_pose);
997-
return boost::none;
1005+
return std::nullopt;
9981006
}();
9991007

10001008
if (!stop_factor) {

‎planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp

+15-14
Original file line numberDiff line numberDiff line change
@@ -38,6 +38,7 @@
3838
#include <pcl_conversions/pcl_conversions.h>
3939

4040
#include <memory>
41+
#include <optional>
4142
#include <string>
4243
#include <unordered_map>
4344
#include <utility>
@@ -98,7 +99,7 @@ class CrosswalkModule : public SceneModuleInterface
9899
// NOTE: FULLY_STOPPED means stopped object which can be ignored.
99100
enum class State { STOPPED = 0, FULLY_STOPPED, OTHER };
100101
State state{State::OTHER};
101-
boost::optional<rclcpp::Time> time_to_start_stopped{boost::none};
102+
std::optional<rclcpp::Time> time_to_start_stopped{std::nullopt};
102103

103104
void updateState(
104105
const rclcpp::Time & now, const double obj_vel, const bool is_ego_yielding,
@@ -123,7 +124,7 @@ class CrosswalkModule : public SceneModuleInterface
123124
state = State::STOPPED;
124125
}
125126
} else {
126-
time_to_start_stopped = boost::none;
127+
time_to_start_stopped = std::nullopt;
127128
state = State::OTHER;
128129
}
129130
}
@@ -182,35 +183,35 @@ class CrosswalkModule : public SceneModuleInterface
182183
void applySafetySlowDownSpeed(
183184
PathWithLaneId & output, const std::vector<geometry_msgs::msg::Point> & path_intersects);
184185

185-
boost::optional<std::pair<geometry_msgs::msg::Point, double>> getStopPointWithMargin(
186+
std::optional<std::pair<geometry_msgs::msg::Point, double>> getStopPointWithMargin(
186187
const PathWithLaneId & ego_path,
187188
const std::vector<geometry_msgs::msg::Point> & path_intersects) const;
188189

189-
boost::optional<StopFactor> checkStopForCrosswalkUsers(
190+
std::optional<StopFactor> checkStopForCrosswalkUsers(
190191
const PathWithLaneId & ego_path, const PathWithLaneId & sparse_resample_path,
191-
const boost::optional<std::pair<geometry_msgs::msg::Point, double>> & p_stop_line,
192+
const std::optional<std::pair<geometry_msgs::msg::Point, double>> & p_stop_line,
192193
const std::vector<geometry_msgs::msg::Point> & path_intersects,
193-
const boost::optional<geometry_msgs::msg::Pose> & default_stop_pose);
194+
const std::optional<geometry_msgs::msg::Pose> & default_stop_pose);
194195

195-
boost::optional<StopFactor> checkStopForStuckedVehicles(
196+
std::optional<StopFactor> checkStopForStuckedVehicles(
196197
const PathWithLaneId & ego_path, const std::vector<PredictedObject> & objects,
197198
const std::vector<geometry_msgs::msg::Point> & path_intersects,
198-
const boost::optional<geometry_msgs::msg::Pose> & stop_pose) const;
199+
const std::optional<geometry_msgs::msg::Pose> & stop_pose) const;
199200

200201
std::vector<CollisionPoint> getCollisionPoints(
201202
const PathWithLaneId & ego_path, const PredictedObject & object,
202203
const Polygon2d & attention_area, const std::pair<double, double> & crosswalk_attention_range);
203204

204-
boost::optional<StopFactor> getNearestStopFactor(
205+
std::optional<StopFactor> getNearestStopFactor(
205206
const PathWithLaneId & ego_path,
206-
const boost::optional<StopFactor> & stop_factor_for_crosswalk_users,
207-
const boost::optional<StopFactor> & stop_factor_for_stucked_vehicles);
207+
const std::optional<StopFactor> & stop_factor_for_crosswalk_users,
208+
const std::optional<StopFactor> & stop_factor_for_stucked_vehicles);
208209

209-
void planGo(PathWithLaneId & ego_path, const boost::optional<StopFactor> & stop_factor);
210+
void planGo(PathWithLaneId & ego_path, const std::optional<StopFactor> & stop_factor);
210211

211212
void planStop(
212-
PathWithLaneId & ego_path, const boost::optional<StopFactor> & nearest_stop_factor,
213-
const boost::optional<geometry_msgs::msg::Pose> & default_stop_pose, StopReason * stop_reason);
213+
PathWithLaneId & ego_path, const std::optional<StopFactor> & nearest_stop_factor,
214+
const std::optional<geometry_msgs::msg::Pose> & default_stop_pose, StopReason * stop_reason);
214215

215216
// minor functions
216217
std::pair<double, double> getAttentionRange(

‎planning/behavior_velocity_crosswalk_module/src/scene_walkway.cpp

+5-5
Original file line numberDiff line numberDiff line change
@@ -48,13 +48,13 @@ WalkwayModule::WalkwayModule(
4848
} else {
4949
const auto stop_line = getStopLineFromMap(module_id_, lanelet_map_ptr, "crosswalk_id");
5050
if (!!stop_line) {
51-
stop_lines_.push_back(stop_line.get());
51+
stop_lines_.push_back(*stop_line);
5252
}
5353
walkway_ = lanelet_map_ptr->laneletLayer.get(module_id);
5454
}
5555
}
5656

57-
boost::optional<std::pair<double, geometry_msgs::msg::Point>> WalkwayModule::getStopLine(
57+
std::optional<std::pair<double, geometry_msgs::msg::Point>> WalkwayModule::getStopLine(
5858
const PathWithLaneId & ego_path, bool & exist_stopline_in_map,
5959
const std::vector<geometry_msgs::msg::Point> & path_intersects) const
6060
{
@@ -111,7 +111,7 @@ bool WalkwayModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop_
111111
return false;
112112
}
113113

114-
const auto & p_stop = p_stop_line.get().second;
114+
const auto & p_stop = p_stop_line->second;
115115
const auto stop_line_distance = exist_stopline_in_map ? 0.0 : planner_param_.stop_line_distance;
116116
const auto margin = stop_line_distance + base_link2front;
117117
const auto stop_pose = calcLongitudinalOffsetPose(input.points, p_stop, -margin);
@@ -120,7 +120,7 @@ bool WalkwayModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop_
120120
return false;
121121
}
122122

123-
const auto inserted_pose = planning_utils::insertStopPoint(stop_pose.get().position, *path);
123+
const auto inserted_pose = planning_utils::insertStopPoint(stop_pose->position, *path);
124124
if (inserted_pose) {
125125
debug_data_.stop_poses.push_back(inserted_pose.get());
126126
}
@@ -136,7 +136,7 @@ bool WalkwayModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop_
136136

137137
// use arc length to identify if ego vehicle is in front of walkway stop or not.
138138
const double signed_arc_dist_to_stop_point =
139-
calcSignedArcLength(input.points, ego_pos, stop_pose.get().position);
139+
calcSignedArcLength(input.points, ego_pos, stop_pose->position);
140140

141141
const double distance_threshold = 1.0;
142142
debug_data_.stop_judge_range = distance_threshold;

‎planning/behavior_velocity_crosswalk_module/src/scene_walkway.hpp

+2-1
Original file line numberDiff line numberDiff line change
@@ -30,6 +30,7 @@
3030
#include <lanelet2_routing/RoutingGraphContainer.h>
3131

3232
#include <memory>
33+
#include <optional>
3334
#include <utility>
3435
#include <vector>
3536

@@ -57,7 +58,7 @@ class WalkwayModule : public SceneModuleInterface
5758
private:
5859
const int64_t module_id_;
5960

60-
[[nodiscard]] boost::optional<std::pair<double, geometry_msgs::msg::Point>> getStopLine(
61+
[[nodiscard]] std::optional<std::pair<double, geometry_msgs::msg::Point>> getStopLine(
6162
const PathWithLaneId & ego_path, bool & exist_stopline_in_map,
6263
const std::vector<geometry_msgs::msg::Point> & path_intersects) const;
6364

‎planning/behavior_velocity_crosswalk_module/src/util.cpp

+1-1
Original file line numberDiff line numberDiff line change
@@ -146,7 +146,7 @@ std::vector<geometry_msgs::msg::Point> getLinestringIntersects(
146146
return geometry_points;
147147
}
148148

149-
lanelet::Optional<lanelet::ConstLineString3d> getStopLineFromMap(
149+
std::optional<lanelet::ConstLineString3d> getStopLineFromMap(
150150
const int lane_id, const lanelet::LaneletMapPtr & lanelet_map_ptr,
151151
const std::string & attribute_name)
152152
{

0 commit comments

Comments
 (0)