Skip to content

Commit 4a15749

Browse files
committed
add visitor
Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>
1 parent 320eaa3 commit 4a15749

File tree

5 files changed

+212
-58
lines changed

5 files changed

+212
-58
lines changed

planning/behavior_velocity_blind_spot_module/CMakeLists.txt

+1
Original file line numberDiff line numberDiff line change
@@ -9,6 +9,7 @@ ament_auto_add_library(${PROJECT_NAME} SHARED
99
src/debug.cpp
1010
src/manager.cpp
1111
src/scene.cpp
12+
src/decisions.cpp
1213
)
1314

1415
ament_auto_package(INSTALL_TO_SHARE config)
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,154 @@
1+
// Copyright 2024 Tier IV, Inc.
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#include "scene.hpp"
16+
17+
#include <motion_utils/trajectory/trajectory.hpp>
18+
19+
namespace behavior_velocity_planner
20+
{
21+
22+
/*
23+
* for default
24+
*/
25+
template <typename T>
26+
void BlindSpotModule::setRTCStatusByDecision(
27+
const T &, const autoware_auto_planning_msgs::msg::PathWithLaneId & path)
28+
{
29+
static_assert("Unsupported type passed to setRTCStatus");
30+
return;
31+
}
32+
33+
template <typename T>
34+
void BlindSpotModule::reactRTCApprovalByDecision(
35+
[[maybe_unused]] const T & decision,
36+
[[maybe_unused]] autoware_auto_planning_msgs::msg::PathWithLaneId * path,
37+
[[maybe_unused]] StopReason * stop_reason)
38+
{
39+
static_assert("Unsupported type passed to reactRTCApprovalByDecision");
40+
}
41+
42+
/*
43+
* for InternalError
44+
*/
45+
template <>
46+
void BlindSpotModule::setRTCStatusByDecision(
47+
[[maybe_unused]] const InternalError & decision,
48+
[[maybe_unused]] const autoware_auto_planning_msgs::msg::PathWithLaneId & path)
49+
{
50+
return;
51+
}
52+
53+
template <>
54+
void BlindSpotModule::reactRTCApprovalByDecision(
55+
[[maybe_unused]] const InternalError & decision,
56+
[[maybe_unused]] autoware_auto_planning_msgs::msg::PathWithLaneId * path,
57+
[[maybe_unused]] StopReason * stop_reason)
58+
{
59+
return;
60+
}
61+
62+
/*
63+
* For OverPassJudge
64+
*/
65+
template <>
66+
void BlindSpotModule::setRTCStatusByDecision(
67+
[[maybe_unused]] const OverPassJudge & decision,
68+
[[maybe_unused]] const autoware_auto_planning_msgs::msg::PathWithLaneId & path)
69+
{
70+
return;
71+
}
72+
73+
template <>
74+
void BlindSpotModule::reactRTCApprovalByDecision(
75+
[[maybe_unused]] const OverPassJudge & decision,
76+
[[maybe_unused]] autoware_auto_planning_msgs::msg::PathWithLaneId * path,
77+
[[maybe_unused]] StopReason * stop_reason)
78+
{
79+
return;
80+
}
81+
82+
/*
83+
* for Unsafe
84+
*/
85+
template <>
86+
void BlindSpotModule::setRTCStatusByDecision(
87+
const Unsafe & decision, const autoware_auto_planning_msgs::msg::PathWithLaneId & path)
88+
{
89+
setSafe(false);
90+
const auto & current_pose = planner_data_->current_odometry->pose;
91+
setDistance(
92+
motion_utils::calcSignedArcLength(path.points, current_pose.position, decision.stop_line_idx));
93+
return;
94+
}
95+
96+
template <>
97+
void BlindSpotModule::reactRTCApprovalByDecision(
98+
const Unsafe & decision, autoware_auto_planning_msgs::msg::PathWithLaneId * path,
99+
StopReason * stop_reason)
100+
{
101+
if (!isActivated()) {
102+
constexpr double stop_vel = 0.0;
103+
planning_utils::setVelocityFromIndex(decision.stop_line_idx, stop_vel, path);
104+
debug_data_.virtual_wall_pose = planning_utils::getAheadPose(
105+
decision.stop_line_idx, planner_data_->vehicle_info_.max_longitudinal_offset_m, *path);
106+
107+
tier4_planning_msgs::msg::StopFactor stop_factor;
108+
const auto stop_pose = path->points.at(decision.stop_line_idx).point.pose;
109+
stop_factor.stop_pose = stop_pose;
110+
stop_factor.stop_factor_points = planning_utils::toRosPoints(debug_data_.conflicting_targets);
111+
planning_utils::appendStopReason(stop_factor, stop_reason);
112+
velocity_factor_.set(
113+
path->points, planner_data_->current_odometry->pose, stop_pose, VelocityFactor::UNKNOWN);
114+
}
115+
return;
116+
}
117+
118+
/*
119+
* for Safe
120+
*/
121+
template <>
122+
void BlindSpotModule::setRTCStatusByDecision(
123+
const Safe & decision, const autoware_auto_planning_msgs::msg::PathWithLaneId & path)
124+
{
125+
setSafe(true);
126+
const auto & current_pose = planner_data_->current_odometry->pose;
127+
setDistance(
128+
motion_utils::calcSignedArcLength(path.points, current_pose.position, decision.stop_line_idx));
129+
return;
130+
}
131+
132+
template <>
133+
void BlindSpotModule::reactRTCApprovalByDecision(
134+
const Safe & decision, autoware_auto_planning_msgs::msg::PathWithLaneId * path,
135+
StopReason * stop_reason)
136+
{
137+
if (!isActivated()) {
138+
constexpr double stop_vel = 0.0;
139+
planning_utils::setVelocityFromIndex(decision.stop_line_idx, stop_vel, path);
140+
debug_data_.virtual_wall_pose = planning_utils::getAheadPose(
141+
decision.stop_line_idx, planner_data_->vehicle_info_.max_longitudinal_offset_m, *path);
142+
143+
tier4_planning_msgs::msg::StopFactor stop_factor;
144+
const auto stop_pose = path->points.at(decision.stop_line_idx).point.pose;
145+
stop_factor.stop_pose = stop_pose;
146+
stop_factor.stop_factor_points = planning_utils::toRosPoints(debug_data_.conflicting_targets);
147+
planning_utils::appendStopReason(stop_factor, stop_reason);
148+
velocity_factor_.set(
149+
path->points, planner_data_->current_odometry->pose, stop_pose, VelocityFactor::UNKNOWN);
150+
}
151+
return;
152+
}
153+
154+
} // namespace behavior_velocity_planner

planning/behavior_velocity_blind_spot_module/src/manager.cpp

+2-2
Original file line numberDiff line numberDiff line change
@@ -72,8 +72,8 @@ void BlindSpotModuleManager::launchNewModules(
7272
: BlindSpotModule::TurnDirection::RIGHT;
7373

7474
registerModule(std::make_shared<BlindSpotModule>(
75-
module_id, lane_id, turn_direction, planner_param_, logger_.get_child("blind_spot_module"),
76-
clock_));
75+
module_id, lane_id, turn_direction, planner_data_, planner_param_,
76+
logger_.get_child("blind_spot_module"), clock_));
7777
generateUUID(module_id);
7878
updateRTCStatus(
7979
getUUID(module_id), true, State::WAITING_FOR_EXECUTION, std::numeric_limits<double>::lowest(),

planning/behavior_velocity_blind_spot_module/src/scene.cpp

+39-47
Original file line numberDiff line numberDiff line change
@@ -44,36 +44,18 @@ namespace behavior_velocity_planner
4444
{
4545
namespace bg = boost::geometry;
4646

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-
6547
BlindSpotModule::BlindSpotModule(
6648
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)
6951
: SceneModuleInterface(module_id, logger, clock),
7052
lane_id_(lane_id),
7153
planner_param_{planner_param},
7254
turn_direction_(turn_direction),
7355
is_over_pass_judge_line_(false)
7456
{
7557
velocity_factor_.init(PlanningBehavior::REAR_CHECK);
76-
sibling_straight_lanelet_ = getSiblingStraightLanelet();
58+
sibling_straight_lanelet_ = getSiblingStraightLanelet(planner_data);
7759
}
7860

7961
void BlindSpotModule::initializeRTCStatus()
@@ -130,33 +112,39 @@ BlindSpotDecision BlindSpotModule::modifyPathVelocityDetail(
130112
collision_obstacle.has_value() ? StateMachine::State::STOP : StateMachine::State::GO,
131113
logger_.get_child("state_machine"), *clock_);
132114

133-
if (state_machine_.getState() != StateMachine::State::STOP) {
115+
if (state_machine_.getState() == StateMachine::State::STOP) {
134116
return Unsafe{stop_line_idx, collision_obstacle};
135117
}
136118

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+
}
151121

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);
160148
}
161149

162150
bool BlindSpotModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason)
@@ -166,6 +154,9 @@ bool BlindSpotModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto
166154

167155
initializeRTCStatus();
168156
const auto decision = modifyPathVelocityDetail(path, stop_reason);
157+
const auto & input_path = *path;
158+
setRTCStatus(decision, input_path);
159+
reactRTCApproval(decision, path, stop_reason);
169160

170161
return true;
171162
}
@@ -223,10 +214,11 @@ std::optional<InterpolatedPathInfo> BlindSpotModule::generateInterpolatedPathInf
223214
return interpolated_path_info;
224215
}
225216

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
227219
{
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();
230222

231223
const auto assigned_lane = lanelet_map_ptr->laneletLayer.get(lane_id_);
232224
for (const auto & prev : routing_graph_ptr->previous(assigned_lane)) {

planning/behavior_velocity_blind_spot_module/src/scene.hpp

+16-9
Original file line numberDiff line numberDiff line change
@@ -78,9 +78,9 @@ struct Unsafe
7878
const std::optional<autoware_auto_perception_msgs::msg::PredictedObject> collision_obstacle;
7979
};
8080

81-
struct Safe : public std::monostate
81+
struct Safe
8282
{
83-
using std::monostate::monostate;
83+
const size_t stop_line_idx;
8484
};
8585

8686
using BlindSpotDecision = std::variant<InternalError, OverPassJudge, Unsafe, Safe>;
@@ -117,8 +117,8 @@ class BlindSpotModule : public SceneModuleInterface
117117

118118
BlindSpotModule(
119119
const int64_t module_id, const int64_t lane_id, const TurnDirection turn_direction,
120-
const PlannerParam & planner_param, const rclcpp::Logger logger,
121-
const rclcpp::Clock::SharedPtr clock);
120+
const std::shared_ptr<const PlannerData> planner_data, const PlannerParam & planner_param,
121+
const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock);
122122

123123
/**
124124
* @brief plan go-stop velocity at traffic crossing with collision check between reference path
@@ -144,18 +144,25 @@ class BlindSpotModule : public SceneModuleInterface
144144
void initializeRTCStatus();
145145
BlindSpotDecision modifyPathVelocityDetail(PathWithLaneId * path, StopReason * stop_reason);
146146
// setDafe(), setDistance()
147-
void setRTCStatus(const BlindSpotDecision & decision);
147+
void setRTCStatus(
148+
const BlindSpotDecision & decision,
149+
const autoware_auto_planning_msgs::msg::PathWithLaneId & path);
148150
template <typename Decision>
149-
void setRTCStatusByDecision(const Decision & decision);
151+
void setRTCStatusByDecision(
152+
const Decision & decision, const autoware_auto_planning_msgs::msg::PathWithLaneId & path);
150153
// stop/GO
151-
void reactRTCApproval(PathWithLaneId * path, StopReason * stop_reason) const;
154+
void reactRTCApproval(
155+
const BlindSpotDecision & decision, PathWithLaneId * path, StopReason * stop_reason);
152156
template <typename Decision>
153-
void reactRTCApprovalByDecision(const Decision & decision) const;
157+
void reactRTCApprovalByDecision(
158+
const Decision & decision, autoware_auto_planning_msgs::msg::PathWithLaneId * path,
159+
StopReason * stop_reason);
154160

155161
std::optional<InterpolatedPathInfo> generateInterpolatedPathInfo(
156162
const autoware_auto_planning_msgs::msg::PathWithLaneId & input_path) const;
157163

158-
std::optional<lanelet::ConstLanelet> getSiblingStraightLanelet() const;
164+
std::optional<lanelet::ConstLanelet> getSiblingStraightLanelet(
165+
const std::shared_ptr<const PlannerData> planner_data) const;
159166

160167
/**
161168
* @brief Generate a stop line and insert it into the path.

0 commit comments

Comments
 (0)