Skip to content

Commit 49f91a2

Browse files
soblinsoblin
authored
refactor(blind_spot): devide scene into Categories and act accordingly (#7110)
Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>
1 parent 65c2c51 commit 49f91a2

File tree

6 files changed

+400
-268
lines changed

6 files changed

+400
-268
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)

‎planning/behavior_velocity_blind_spot_module/src/debug.cpp

+2-52
Original file line numberDiff line numberDiff line change
@@ -71,55 +71,16 @@ visualization_msgs::msg::MarkerArray createLaneletPolygonsMarkerArray(
7171
return msg;
7272
}
7373

74-
visualization_msgs::msg::MarkerArray createPoseMarkerArray(
75-
const geometry_msgs::msg::Pose & pose, const StateMachine::State & state, const std::string & ns,
76-
const int64_t id, const double r, const double g, const double b)
77-
{
78-
visualization_msgs::msg::MarkerArray msg;
79-
80-
if (state == StateMachine::State::STOP) {
81-
visualization_msgs::msg::Marker marker_line{};
82-
marker_line.header.frame_id = "map";
83-
marker_line.ns = ns + "_line";
84-
marker_line.id = id;
85-
marker_line.lifetime = rclcpp::Duration::from_seconds(0.3);
86-
marker_line.type = visualization_msgs::msg::Marker::LINE_STRIP;
87-
marker_line.action = visualization_msgs::msg::Marker::ADD;
88-
marker_line.pose.orientation = createMarkerOrientation(0, 0, 0, 1.0);
89-
marker_line.scale = createMarkerScale(0.1, 0.0, 0.0);
90-
marker_line.color = createMarkerColor(r, g, b, 0.999);
91-
92-
const double yaw = tf2::getYaw(pose.orientation);
93-
94-
const double a = 3.0;
95-
geometry_msgs::msg::Point p0;
96-
p0.x = pose.position.x - a * std::sin(yaw);
97-
p0.y = pose.position.y + a * std::cos(yaw);
98-
p0.z = pose.position.z;
99-
marker_line.points.push_back(p0);
100-
101-
geometry_msgs::msg::Point p1;
102-
p1.x = pose.position.x + a * std::sin(yaw);
103-
p1.y = pose.position.y - a * std::cos(yaw);
104-
p1.z = pose.position.z;
105-
marker_line.points.push_back(p1);
106-
107-
msg.markers.push_back(marker_line);
108-
}
109-
110-
return msg;
111-
}
112-
11374
} // namespace
11475

11576
motion_utils::VirtualWalls BlindSpotModule::createVirtualWalls()
11677
{
11778
motion_utils::VirtualWalls virtual_walls;
11879

119-
if (!isActivated() && !is_over_pass_judge_line_) {
80+
if (debug_data_.virtual_wall_pose) {
12081
motion_utils::VirtualWall wall;
12182
wall.text = "blind_spot";
122-
wall.pose = debug_data_.virtual_wall_pose;
83+
wall.pose = debug_data_.virtual_wall_pose.value();
12384
wall.ns = std::to_string(module_id_) + "_";
12485
virtual_walls.push_back(wall);
12586
}
@@ -130,19 +91,8 @@ visualization_msgs::msg::MarkerArray BlindSpotModule::createDebugMarkerArray()
13091
{
13192
visualization_msgs::msg::MarkerArray debug_marker_array;
13293

133-
const auto state = state_machine_.getState();
13494
const auto now = this->clock_->now();
13595

136-
appendMarkerArray(
137-
createPoseMarkerArray(
138-
debug_data_.stop_point_pose, state, "stop_point_pose", module_id_, 1.0, 0.0, 0.0),
139-
&debug_marker_array, now);
140-
141-
appendMarkerArray(
142-
createPoseMarkerArray(
143-
debug_data_.judge_point_pose, state, "judge_point_pose", module_id_, 1.0, 1.0, 0.5),
144-
&debug_marker_array, now);
145-
14696
appendMarkerArray(
14797
createLaneletPolygonsMarkerArray(
14898
debug_data_.conflict_areas, "conflict_area", module_id_, 0.0, 0.5, 0.5),

‎planning/behavior_velocity_blind_spot_module/src/decisions.cpp

+154
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

+7-4
Original file line numberDiff line numberDiff line change
@@ -63,14 +63,17 @@ void BlindSpotModuleManager::launchNewModules(
6363
}
6464

6565
// Is turning lane?
66-
const std::string turn_direction = ll.attributeOr("turn_direction", "else");
67-
if (turn_direction != "left" && turn_direction != "right") {
66+
const std::string turn_direction_str = ll.attributeOr("turn_direction", "else");
67+
if (turn_direction_str != "left" && turn_direction_str != "right") {
6868
continue;
6969
}
70+
const auto turn_direction = turn_direction_str == "left"
71+
? BlindSpotModule::TurnDirection::LEFT
72+
: BlindSpotModule::TurnDirection::RIGHT;
7073

7174
registerModule(std::make_shared<BlindSpotModule>(
72-
module_id, lane_id, planner_data_, planner_param_, logger_.get_child("blind_spot_module"),
73-
clock_));
75+
module_id, lane_id, turn_direction, planner_data_, planner_param_,
76+
logger_.get_child("blind_spot_module"), clock_));
7477
generateUUID(module_id);
7578
updateRTCStatus(
7679
getUUID(module_id), true, State::WAITING_FOR_EXECUTION, std::numeric_limits<double>::lowest(),

0 commit comments

Comments
 (0)