@@ -44,36 +44,18 @@ namespace behavior_velocity_planner
44
44
{
45
45
namespace bg = boost::geometry;
46
46
47
- namespace
48
- {
49
- [[maybe_unused]] geometry_msgs::msg::Polygon toGeomPoly (const lanelet::CompoundPolygon3d & poly)
50
- {
51
- geometry_msgs::msg::Polygon geom_poly;
52
-
53
- for (const auto & p : poly) {
54
- geometry_msgs::msg::Point32 geom_point;
55
- geom_point.x = p.x ();
56
- geom_point.y = p.y ();
57
- geom_point.z = p.z ();
58
- geom_poly.points .push_back (geom_point);
59
- }
60
-
61
- return geom_poly;
62
- }
63
- } // namespace
64
-
65
47
BlindSpotModule::BlindSpotModule (
66
48
const int64_t module_id, const int64_t lane_id, const TurnDirection turn_direction,
67
- const PlannerParam & planner_param , const rclcpp::Logger logger ,
68
- const rclcpp::Clock::SharedPtr clock)
49
+ const std::shared_ptr< const PlannerData> planner_data , const PlannerParam & planner_param ,
50
+ const rclcpp::Logger logger, const rclcpp:: Clock::SharedPtr clock)
69
51
: SceneModuleInterface(module_id, logger, clock),
70
52
lane_id_ (lane_id),
71
53
planner_param_{planner_param},
72
54
turn_direction_ (turn_direction),
73
55
is_over_pass_judge_line_(false )
74
56
{
75
57
velocity_factor_.init (PlanningBehavior::REAR_CHECK);
76
- sibling_straight_lanelet_ = getSiblingStraightLanelet ();
58
+ sibling_straight_lanelet_ = getSiblingStraightLanelet (planner_data );
77
59
}
78
60
79
61
void BlindSpotModule::initializeRTCStatus ()
@@ -130,33 +112,39 @@ BlindSpotDecision BlindSpotModule::modifyPathVelocityDetail(
130
112
collision_obstacle.has_value () ? StateMachine::State::STOP : StateMachine::State::GO,
131
113
logger_.get_child (" state_machine" ), *clock_);
132
114
133
- if (state_machine_.getState () ! = StateMachine::State::STOP) {
115
+ if (state_machine_.getState () = = StateMachine::State::STOP) {
134
116
return Unsafe{stop_line_idx, collision_obstacle};
135
117
}
136
118
137
- return Safe{};
138
- /* set stop speed */
139
- /*
140
- setSafe(state_machine_.getState() != StateMachine::State::STOP);
141
- setDistance(motion_utils::calcSignedArcLength(
142
- input_path.points, current_pose.position,
143
- input_path.points.at(stop_line_idx).point.pose.position));
144
- */
145
- /*
146
- if (!isActivated()) {
147
- constexpr double stop_vel = 0.0;
148
- planning_utils::setVelocityFromIndex(stop_line_idx, stop_vel, path);
149
- debug_data_.virtual_wall_pose = planning_utils::getAheadPose(
150
- stop_line_idx, planner_data_->vehicle_info_.max_longitudinal_offset_m, input_path);
119
+ return Safe{stop_line_idx};
120
+ }
151
121
152
- tier4_planning_msgs::msg::StopFactor stop_factor;
153
- stop_factor.stop_pose = debug_data_.stop_point_pose; // stop_point_poseを消したので直す
154
- stop_factor.stop_factor_points = planning_utils::toRosPoints(debug_data_.conflicting_targets);
155
- planning_utils::appendStopReason(stop_factor, stop_reason);
156
- velocity_factor_.set(
157
- input_path.points, planner_data_->current_odometry->pose, stop_pose, VelocityFactor::UNKNOWN);
158
- }
159
- */
122
+ // template-specification based visitor pattern
123
+ // https://en.cppreference.com/w/cpp/utility/variant/visit
124
+ template <class ... Ts>
125
+ struct VisitorSwitch : Ts...
126
+ {
127
+ using Ts::operator ()...;
128
+ };
129
+ template <class ... Ts>
130
+ VisitorSwitch (Ts...) -> VisitorSwitch<Ts...>;
131
+
132
+ void BlindSpotModule::setRTCStatus (
133
+ const BlindSpotDecision & decision, const autoware_auto_planning_msgs::msg::PathWithLaneId & path)
134
+ {
135
+ std::visit (
136
+ VisitorSwitch{[&](const auto & sub_decision) { setRTCStatusByDecision (sub_decision, path); }},
137
+ decision);
138
+ }
139
+
140
+ void BlindSpotModule::reactRTCApproval (
141
+ const BlindSpotDecision & decision, PathWithLaneId * path, StopReason * stop_reason)
142
+ {
143
+ std::visit (
144
+ VisitorSwitch{[&](const auto & sub_decision) {
145
+ reactRTCApprovalByDecision (sub_decision, path, stop_reason);
146
+ }},
147
+ decision);
160
148
}
161
149
162
150
bool BlindSpotModule::modifyPathVelocity (PathWithLaneId * path, StopReason * stop_reason)
@@ -166,6 +154,9 @@ bool BlindSpotModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto
166
154
167
155
initializeRTCStatus ();
168
156
const auto decision = modifyPathVelocityDetail (path, stop_reason);
157
+ const auto & input_path = *path;
158
+ setRTCStatus (decision, input_path);
159
+ reactRTCApproval (decision, path, stop_reason);
169
160
170
161
return true ;
171
162
}
@@ -223,10 +214,11 @@ std::optional<InterpolatedPathInfo> BlindSpotModule::generateInterpolatedPathInf
223
214
return interpolated_path_info;
224
215
}
225
216
226
- std::optional<lanelet::ConstLanelet> BlindSpotModule::getSiblingStraightLanelet () const
217
+ std::optional<lanelet::ConstLanelet> BlindSpotModule::getSiblingStraightLanelet (
218
+ const std::shared_ptr<const PlannerData> planner_data) const
227
219
{
228
- const auto lanelet_map_ptr = planner_data_ ->route_handler_ ->getLaneletMapPtr ();
229
- const auto routing_graph_ptr = planner_data_ ->route_handler_ ->getRoutingGraphPtr ();
220
+ const auto lanelet_map_ptr = planner_data ->route_handler_ ->getLaneletMapPtr ();
221
+ const auto routing_graph_ptr = planner_data ->route_handler_ ->getRoutingGraphPtr ();
230
222
231
223
const auto assigned_lane = lanelet_map_ptr->laneletLayer .get (lane_id_);
232
224
for (const auto & prev : routing_graph_ptr->previous (assigned_lane)) {
0 commit comments