Skip to content

Commit e62422b

Browse files
satoshi-otasatoshi-ota
authored
feat(crosswalk): support crosswalk regulatory element (autowarefoundation#3939)
* feat(crosswalk): use regulatory element Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com> * feat(map_loader): show crosswalk areas Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com> --------- Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>
1 parent 0eed93c commit e62422b

File tree

9 files changed

+232
-65
lines changed

9 files changed

+232
-65
lines changed

‎map/map_loader/src/lanelet2_map_loader/lanelet2_map_visualization_node.cpp

+8-2
Original file line numberDiff line numberDiff line change
@@ -111,6 +111,8 @@ void Lanelet2MapVisualizationNode::onMapBin(
111111
lanelet::utils::query::noStoppingAreas(all_lanelets);
112112
std::vector<lanelet::SpeedBumpConstPtr> sb_reg_elems =
113113
lanelet::utils::query::speedBumps(all_lanelets);
114+
std::vector<lanelet::CrosswalkConstPtr> cw_reg_elems =
115+
lanelet::utils::query::crosswalks(all_lanelets);
114116
lanelet::ConstLineStrings3d parking_spaces =
115117
lanelet::utils::query::getAllParkingSpaces(viz_lanelet_map);
116118
lanelet::ConstPolygons3d parking_lots = lanelet::utils::query::getAllParkingLots(viz_lanelet_map);
@@ -126,8 +128,8 @@ void Lanelet2MapVisualizationNode::onMapBin(
126128

127129
std_msgs::msg::ColorRGBA cl_road, cl_shoulder, cl_cross, cl_partitions, cl_pedestrian_markings,
128130
cl_ll_borders, cl_shoulder_borders, cl_stoplines, cl_trafficlights, cl_detection_areas,
129-
cl_speed_bumps, cl_parking_lots, cl_parking_spaces, cl_lanelet_id, cl_obstacle_polygons,
130-
cl_no_stopping_areas, cl_no_obstacle_segmentation_area,
131+
cl_speed_bumps, cl_crosswalks, cl_parking_lots, cl_parking_spaces, cl_lanelet_id,
132+
cl_obstacle_polygons, cl_no_stopping_areas, cl_no_obstacle_segmentation_area,
131133
cl_no_obstacle_segmentation_area_for_run_out, cl_hatched_road_markings_area,
132134
cl_hatched_road_markings_line;
133135
setColor(&cl_road, 0.27, 0.27, 0.27, 0.999);
@@ -142,6 +144,7 @@ void Lanelet2MapVisualizationNode::onMapBin(
142144
setColor(&cl_detection_areas, 0.27, 0.27, 0.37, 0.5);
143145
setColor(&cl_no_stopping_areas, 0.37, 0.37, 0.37, 0.5);
144146
setColor(&cl_speed_bumps, 0.56, 0.40, 0.27, 0.5);
147+
setColor(&cl_crosswalks, 0.80, 0.80, 0.0, 0.5);
145148
setColor(&cl_obstacle_polygons, 0.4, 0.27, 0.27, 0.5);
146149
setColor(&cl_parking_lots, 0.5, 0.5, 0.0, 0.3);
147150
setColor(&cl_parking_spaces, 1.0, 0.647, 0.0, 0.6);
@@ -185,6 +188,9 @@ void Lanelet2MapVisualizationNode::onMapBin(
185188
insertMarkerArray(
186189
&map_marker_array,
187190
lanelet::visualization::speedBumpsAsMarkerArray(sb_reg_elems, cl_speed_bumps));
191+
insertMarkerArray(
192+
&map_marker_array,
193+
lanelet::visualization::crosswalkAreasAsMarkerArray(cw_reg_elems, cl_crosswalks));
188194
insertMarkerArray(
189195
&map_marker_array,
190196
lanelet::visualization::parkingLotsAsMarkerArray(parking_lots, cl_parking_lots));

‎planning/behavior_velocity_crosswalk_module/src/manager.cpp

+115-29
Original file line numberDiff line numberDiff line change
@@ -74,10 +74,19 @@ std::set<int64_t> getCrosswalkIdSetOnPath(
7474

7575
return crosswalk_id_set;
7676
}
77+
78+
bool checkRegulatoryElementExistence(const lanelet::LaneletMapPtr & lanelet_map_ptr)
79+
{
80+
const auto all_lanelets = lanelet::utils::query::laneletLayer(lanelet_map_ptr);
81+
return !lanelet::utils::query::crosswalks(all_lanelets).empty();
82+
}
7783
} // namespace
7884

7985
namespace behavior_velocity_planner
8086
{
87+
88+
using lanelet::autoware::Crosswalk;
89+
8190
CrosswalkModuleManager::CrosswalkModuleManager(rclcpp::Node & node)
8291
: SceneModuleManagerInterfaceWithRTC(node, getModuleName())
8392
{
@@ -128,17 +137,43 @@ CrosswalkModuleManager::CrosswalkModuleManager(rclcpp::Node & node)
128137
void CrosswalkModuleManager::launchNewModules(const PathWithLaneId & path)
129138
{
130139
const auto rh = planner_data_->route_handler_;
131-
for (const auto & crosswalk : getCrosswalksOnPath(
132-
planner_data_->current_odometry->pose, path, rh->getLaneletMapPtr(),
133-
rh->getOverallGraphPtr())) {
134-
const auto module_id = crosswalk.id();
135-
if (!isModuleRegistered(module_id)) {
136-
registerModule(std::make_shared<CrosswalkModule>(
137-
module_id, crosswalk, crosswalk_planner_param_, logger_.get_child("crosswalk_module"),
138-
clock_));
139-
generateUUID(module_id);
140-
updateRTCStatus(
141-
getUUID(module_id), true, std::numeric_limits<double>::lowest(), path.header.stamp);
140+
if (!opt_use_regulatory_element_) {
141+
opt_use_regulatory_element_ = checkRegulatoryElementExistence(rh->getLaneletMapPtr());
142+
std::ostringstream string_stream;
143+
string_stream << "use crosswalk regulatory element: ";
144+
string_stream << std::boolalpha << opt_use_regulatory_element_.get();
145+
RCLCPP_INFO_STREAM(logger_, string_stream.str());
146+
}
147+
148+
const auto launch = [this, &path](const auto id) {
149+
if (isModuleRegistered(id)) {
150+
return;
151+
}
152+
153+
const auto & p = crosswalk_planner_param_;
154+
const auto logger = logger_.get_child("crosswalk_module");
155+
const auto lanelet_map_ptr = planner_data_->route_handler_->getLaneletMapPtr();
156+
157+
registerModule(std::make_shared<CrosswalkModule>(
158+
id, lanelet_map_ptr, p, opt_use_regulatory_element_.get(), logger, clock_));
159+
generateUUID(id);
160+
updateRTCStatus(getUUID(id), true, std::numeric_limits<double>::lowest(), path.header.stamp);
161+
};
162+
163+
if (opt_use_regulatory_element_.get()) {
164+
const auto crosswalk_leg_elem_map = planning_utils::getRegElemMapOnPath<Crosswalk>(
165+
path, rh->getLaneletMapPtr(), planner_data_->current_odometry->pose);
166+
167+
for (const auto & crosswalk : crosswalk_leg_elem_map) {
168+
launch(crosswalk.first->id());
169+
}
170+
} else {
171+
const auto crosswalk_lanelets = getCrosswalksOnPath(
172+
planner_data_->current_odometry->pose, path, rh->getLaneletMapPtr(),
173+
rh->getOverallGraphPtr());
174+
175+
for (const auto & crosswalk : crosswalk_lanelets) {
176+
launch(crosswalk.id());
142177
}
143178
}
144179
}
@@ -147,8 +182,21 @@ std::function<bool(const std::shared_ptr<SceneModuleInterface> &)>
147182
CrosswalkModuleManager::getModuleExpiredFunction(const PathWithLaneId & path)
148183
{
149184
const auto rh = planner_data_->route_handler_;
150-
const auto crosswalk_id_set = getCrosswalkIdSetOnPath(
151-
planner_data_->current_odometry->pose, path, rh->getLaneletMapPtr(), rh->getOverallGraphPtr());
185+
186+
std::set<int64_t> crosswalk_id_set;
187+
188+
if (opt_use_regulatory_element_.get()) {
189+
const auto crosswalk_leg_elem_map = planning_utils::getRegElemMapOnPath<Crosswalk>(
190+
path, rh->getLaneletMapPtr(), planner_data_->current_odometry->pose);
191+
192+
for (const auto & crosswalk : crosswalk_leg_elem_map) {
193+
crosswalk_id_set.insert(crosswalk.first->id());
194+
}
195+
} else {
196+
crosswalk_id_set = getCrosswalkIdSetOnPath(
197+
planner_data_->current_odometry->pose, path, rh->getLaneletMapPtr(),
198+
rh->getOverallGraphPtr());
199+
}
152200

153201
return [crosswalk_id_set](const std::shared_ptr<SceneModuleInterface> & scene_module) {
154202
return crosswalk_id_set.count(scene_module->getModuleId()) == 0;
@@ -166,31 +214,69 @@ WalkwayModuleManager::WalkwayModuleManager(rclcpp::Node & node)
166214
wp.stop_duration_sec = node.declare_parameter<double>(ns + ".stop_duration_sec");
167215
}
168216

169-
void WalkwayModuleManager::launchNewModules(
170-
const autoware_auto_planning_msgs::msg::PathWithLaneId & path)
217+
void WalkwayModuleManager::launchNewModules(const PathWithLaneId & path)
171218
{
172219
const auto rh = planner_data_->route_handler_;
173-
for (const auto & crosswalk : getCrosswalksOnPath(
174-
planner_data_->current_odometry->pose, path, rh->getLaneletMapPtr(),
175-
rh->getOverallGraphPtr())) {
176-
const auto module_id = crosswalk.id();
177-
if (
178-
!isModuleRegistered(module_id) &&
179-
crosswalk.attributeOr(lanelet::AttributeNamesString::Subtype, std::string("")) ==
180-
lanelet::AttributeValueString::Walkway) {
181-
registerModule(std::make_shared<WalkwayModule>(
182-
module_id, crosswalk, walkway_planner_param_, logger_.get_child("walkway_module"), clock_));
220+
if (!opt_use_regulatory_element_) {
221+
opt_use_regulatory_element_ = checkRegulatoryElementExistence(rh->getLaneletMapPtr());
222+
}
223+
224+
const auto launch = [this, &path](const auto & lanelet) {
225+
const auto attribute =
226+
lanelet.attributeOr(lanelet::AttributeNamesString::Subtype, std::string(""));
227+
if (attribute != lanelet::AttributeValueString::Walkway) {
228+
return;
229+
}
230+
231+
if (isModuleRegistered(lanelet.id())) {
232+
return;
233+
}
234+
235+
const auto & p = walkway_planner_param_;
236+
const auto logger = logger_.get_child("walkway_module");
237+
const auto lanelet_map_ptr = planner_data_->route_handler_->getLaneletMapPtr();
238+
239+
registerModule(std::make_shared<WalkwayModule>(
240+
lanelet.id(), lanelet_map_ptr, p, opt_use_regulatory_element_.get(), logger, clock_));
241+
};
242+
243+
if (opt_use_regulatory_element_.get()) {
244+
const auto crosswalk_leg_elem_map = planning_utils::getRegElemMapOnPath<Crosswalk>(
245+
path, rh->getLaneletMapPtr(), planner_data_->current_odometry->pose);
246+
247+
for (const auto & crosswalk : crosswalk_leg_elem_map) {
248+
launch(crosswalk.first->crosswalkLanelet());
249+
}
250+
} else {
251+
const auto crosswalk_lanelets = getCrosswalksOnPath(
252+
planner_data_->current_odometry->pose, path, rh->getLaneletMapPtr(),
253+
rh->getOverallGraphPtr());
254+
255+
for (const auto & crosswalk : crosswalk_lanelets) {
256+
launch(crosswalk);
183257
}
184258
}
185259
}
186260

187261
std::function<bool(const std::shared_ptr<SceneModuleInterface> &)>
188-
WalkwayModuleManager::getModuleExpiredFunction(
189-
const autoware_auto_planning_msgs::msg::PathWithLaneId & path)
262+
WalkwayModuleManager::getModuleExpiredFunction(const PathWithLaneId & path)
190263
{
191264
const auto rh = planner_data_->route_handler_;
192-
const auto walkway_id_set = getCrosswalkIdSetOnPath(
193-
planner_data_->current_odometry->pose, path, rh->getLaneletMapPtr(), rh->getOverallGraphPtr());
265+
266+
std::set<int64_t> walkway_id_set;
267+
268+
if (opt_use_regulatory_element_.get()) {
269+
const auto crosswalk_leg_elem_map = planning_utils::getRegElemMapOnPath<Crosswalk>(
270+
path, rh->getLaneletMapPtr(), planner_data_->current_odometry->pose);
271+
272+
for (const auto & crosswalk : crosswalk_leg_elem_map) {
273+
walkway_id_set.insert(crosswalk.first->id());
274+
}
275+
} else {
276+
walkway_id_set = getCrosswalkIdSetOnPath(
277+
planner_data_->current_odometry->pose, path, rh->getLaneletMapPtr(),
278+
rh->getOverallGraphPtr());
279+
}
194280

195281
return [walkway_id_set](const std::shared_ptr<SceneModuleInterface> & scene_module) {
196282
return walkway_id_set.count(scene_module->getModuleId()) == 0;

‎planning/behavior_velocity_crosswalk_module/src/manager.hpp

+12-4
Original file line numberDiff line numberDiff line change
@@ -21,6 +21,7 @@
2121
#include <behavior_velocity_planner_common/plugin_interface.hpp>
2222
#include <behavior_velocity_planner_common/plugin_wrapper.hpp>
2323
#include <behavior_velocity_planner_common/scene_module_interface.hpp>
24+
#include <lanelet2_extension/regulatory_elements/crosswalk.hpp>
2425
#include <rclcpp/rclcpp.hpp>
2526

2627
#include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
@@ -33,6 +34,9 @@
3334

3435
namespace behavior_velocity_planner
3536
{
37+
38+
using autoware_auto_planning_msgs::msg::PathWithLaneId;
39+
3640
class CrosswalkModuleManager : public SceneModuleManagerInterfaceWithRTC
3741
{
3842
public:
@@ -43,10 +47,12 @@ class CrosswalkModuleManager : public SceneModuleManagerInterfaceWithRTC
4347
private:
4448
CrosswalkModule::PlannerParam crosswalk_planner_param_{};
4549

46-
void launchNewModules(const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override;
50+
void launchNewModules(const PathWithLaneId & path) override;
4751

4852
std::function<bool(const std::shared_ptr<SceneModuleInterface> &)> getModuleExpiredFunction(
49-
const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override;
53+
const PathWithLaneId & path) override;
54+
55+
boost::optional<bool> opt_use_regulatory_element_{boost::none};
5056
};
5157

5258
class WalkwayModuleManager : public SceneModuleManagerInterface
@@ -59,10 +65,12 @@ class WalkwayModuleManager : public SceneModuleManagerInterface
5965
private:
6066
WalkwayModule::PlannerParam walkway_planner_param_{};
6167

62-
void launchNewModules(const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override;
68+
void launchNewModules(const PathWithLaneId & path) override;
6369

6470
std::function<bool(const std::shared_ptr<SceneModuleInterface> &)> getModuleExpiredFunction(
65-
const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override;
71+
const PathWithLaneId & path) override;
72+
73+
boost::optional<bool> opt_use_regulatory_element_{boost::none};
6674
};
6775

6876
class CrosswalkModulePlugin : public PluginWrapper<CrosswalkModuleManager>

‎planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp

+39-12
Original file line numberDiff line numberDiff line change
@@ -127,15 +127,29 @@ void sortCrosswalksByDistance(
127127
} // namespace
128128

129129
CrosswalkModule::CrosswalkModule(
130-
const int64_t module_id, lanelet::ConstLanelet crosswalk, const PlannerParam & planner_param,
130+
const int64_t module_id, const lanelet::LaneletMapPtr & lanelet_map_ptr,
131+
const PlannerParam & planner_param, const bool use_regulatory_element,
131132
const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock)
132133
: SceneModuleInterface(module_id, logger, clock),
133134
module_id_(module_id),
134-
crosswalk_(std::move(crosswalk)),
135-
planner_param_(planner_param)
135+
planner_param_(planner_param),
136+
use_regulatory_element_(use_regulatory_element)
136137
{
137138
velocity_factor_.init(VelocityFactor::CROSSWALK);
138139
passed_safety_slow_point_ = false;
140+
141+
if (use_regulatory_element_) {
142+
const auto reg_elem_ptr = std::dynamic_pointer_cast<const lanelet::autoware::Crosswalk>(
143+
lanelet_map_ptr->regulatoryElementLayer.get(module_id));
144+
stop_lines_ = reg_elem_ptr->stopLines();
145+
crosswalk_ = reg_elem_ptr->crosswalkLanelet();
146+
} else {
147+
const auto stop_line = getStopLineFromMap(module_id_, lanelet_map_ptr, "crosswalk_id");
148+
if (!!stop_line) {
149+
stop_lines_.push_back(stop_line.get());
150+
}
151+
crosswalk_ = lanelet_map_ptr->laneletLayer.get(module_id);
152+
}
139153
}
140154

141155
bool CrosswalkModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason)
@@ -239,20 +253,33 @@ boost::optional<std::pair<double, geometry_msgs::msg::Point>> CrosswalkModule::g
239253
{
240254
const auto & ego_pos = planner_data_->current_odometry->pose.position;
241255

242-
const auto stop_line = getStopLineFromMap(module_id_, planner_data_, "crosswalk_id");
243-
exist_stopline_in_map = static_cast<bool>(stop_line);
244-
if (stop_line) {
256+
const auto get_stop_point =
257+
[&](const auto & stop_line) -> boost::optional<geometry_msgs::msg::Point> {
245258
const auto intersects = getLinestringIntersects(
246-
ego_path, lanelet::utils::to2D(stop_line.get()).basicLineString(), ego_pos, 2);
247-
if (!intersects.empty()) {
248-
const auto p_stop_line =
249-
createPoint(intersects.front().x(), intersects.front().y(), ego_pos.z);
250-
const auto dist_ego_to_stop = calcSignedArcLength(ego_path.points, ego_pos, p_stop_line);
251-
return std::make_pair(dist_ego_to_stop, p_stop_line);
259+
ego_path, lanelet::utils::to2D(stop_line).basicLineString(), ego_pos, 2);
260+
261+
if (intersects.empty()) {
262+
return boost::none;
263+
}
264+
265+
return createPoint(intersects.front().x(), intersects.front().y(), ego_pos.z);
266+
};
267+
268+
for (const auto & stop_line : stop_lines_) {
269+
const auto p_stop_line = get_stop_point(stop_line);
270+
if (!p_stop_line) {
271+
continue;
252272
}
273+
274+
exist_stopline_in_map = true;
275+
276+
const auto dist_ego_to_stop = calcSignedArcLength(ego_path.points, ego_pos, p_stop_line.get());
277+
return std::make_pair(dist_ego_to_stop, p_stop_line.get());
253278
}
254279

255280
{
281+
exist_stopline_in_map = false;
282+
256283
if (!path_intersects_.empty()) {
257284
const auto p_stop_line = path_intersects_.front();
258285
const auto dist_ego_to_stop = calcSignedArcLength(ego_path.points, ego_pos, p_stop_line) -

‎planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp

+9-1
Original file line numberDiff line numberDiff line change
@@ -18,6 +18,7 @@
1818
#include "util.hpp"
1919

2020
#include <behavior_velocity_planner_common/scene_module_interface.hpp>
21+
#include <lanelet2_extension/regulatory_elements/crosswalk.hpp>
2122
#include <lanelet2_extension/utility/query.hpp>
2223
#include <lanelet2_extension/utility/utilities.hpp>
2324
#include <rclcpp/rclcpp.hpp>
@@ -52,6 +53,7 @@ using autoware_auto_perception_msgs::msg::PredictedObject;
5253
using autoware_auto_perception_msgs::msg::PredictedObjects;
5354
using autoware_auto_perception_msgs::msg::TrafficLight;
5455
using autoware_auto_planning_msgs::msg::PathWithLaneId;
56+
using lanelet::autoware::Crosswalk;
5557
using tier4_api_msgs::msg::CrosswalkStatus;
5658
using tier4_autoware_utils::StopWatch;
5759

@@ -93,7 +95,8 @@ class CrosswalkModule : public SceneModuleInterface
9395
};
9496

9597
CrosswalkModule(
96-
const int64_t module_id, lanelet::ConstLanelet crosswalk, const PlannerParam & planner_param,
98+
const int64_t module_id, const lanelet::LaneletMapPtr & lanelet_map_ptr,
99+
const PlannerParam & planner_param, const bool use_regulatory_element,
97100
const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock);
98101

99102
bool modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) override;
@@ -156,6 +159,8 @@ class CrosswalkModule : public SceneModuleInterface
156159

157160
lanelet::ConstLanelet crosswalk_;
158161

162+
lanelet::ConstLineStrings3d stop_lines_;
163+
159164
std::vector<geometry_msgs::msg::Point> path_intersects_;
160165

161166
// Parameter
@@ -173,6 +178,9 @@ class CrosswalkModule : public SceneModuleInterface
173178

174179
// whether ego passed safety_slow_point
175180
bool passed_safety_slow_point_;
181+
182+
// whether use regulatory element
183+
bool use_regulatory_element_;
176184
};
177185
} // namespace behavior_velocity_planner
178186

0 commit comments

Comments
 (0)