Skip to content

Commit e7a9153

Browse files
shmpwkshmpwk
authored
Merge pull request #1459 from tier4/feat/force_execution_cancel
feat(lane_change, avoidance): add force execution and cancel
2 parents d0348de + d6b8c9c commit e7a9153

File tree

10 files changed

+203
-7
lines changed

10 files changed

+203
-7
lines changed

‎planning/autoware_rtc_interface/include/autoware/rtc_interface/rtc_interface.hpp

+2
Original file line numberDiff line numberDiff line change
@@ -59,6 +59,8 @@ class RTCInterface
5959
void removeExpiredCooperateStatus();
6060
void clearCooperateStatus();
6161
bool isActivated(const UUID & uuid) const;
62+
bool isForceActivated(const UUID & uuid) const;
63+
bool isForceDeactivated(const UUID & UUID) const;
6264
bool isRegistered(const UUID & uuid) const;
6365
bool isRTCEnabled(const UUID & uuid) const;
6466
bool isTerminated(const UUID & uuid) const;

‎planning/autoware_rtc_interface/src/rtc_interface.cpp

+48
Original file line numberDiff line numberDiff line change
@@ -345,6 +345,54 @@ bool RTCInterface::isActivated(const UUID & uuid) const
345345
return false;
346346
}
347347

348+
bool RTCInterface::isForceActivated(const UUID & uuid) const
349+
{
350+
std::lock_guard<std::mutex> lock(mutex_);
351+
const auto itr = std::find_if(
352+
registered_status_.statuses.begin(), registered_status_.statuses.end(),
353+
[uuid](const auto & s) { return s.uuid == uuid; });
354+
355+
if (itr != registered_status_.statuses.end()) {
356+
if (itr->state.type != State::WAITING_FOR_EXECUTION && itr->state.type != State::RUNNING) {
357+
return false;
358+
}
359+
if (itr->command_status.type == Command::ACTIVATE && !itr->safe) {
360+
return true;
361+
} else {
362+
return false;
363+
}
364+
}
365+
366+
RCLCPP_WARN_STREAM(
367+
getLogger(),
368+
"[isForceActivated] uuid : " << uuid_to_string(uuid) << " is not found" << std::endl);
369+
return false;
370+
}
371+
372+
bool RTCInterface::isForceDeactivated(const UUID & uuid) const
373+
{
374+
std::lock_guard<std::mutex> lock(mutex_);
375+
const auto itr = std::find_if(
376+
registered_status_.statuses.begin(), registered_status_.statuses.end(),
377+
[uuid](const auto & s) { return s.uuid == uuid; });
378+
379+
if (itr != registered_status_.statuses.end()) {
380+
if (itr->state.type != State::RUNNING) {
381+
return false;
382+
}
383+
if (itr->command_status.type == Command::DEACTIVATE && itr->safe && !itr->auto_mode) {
384+
return true;
385+
} else {
386+
return false;
387+
}
388+
}
389+
390+
RCLCPP_WARN_STREAM(
391+
getLogger(),
392+
"[isForceDeactivated] uuid : " << uuid_to_string(uuid) << " is not found" << std::endl);
393+
return false;
394+
}
395+
348396
bool RTCInterface::isRegistered(const UUID & uuid) const
349397
{
350398
std::lock_guard<std::mutex> lock(mutex_);

‎planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp

+34-3
Original file line numberDiff line numberDiff line change
@@ -124,9 +124,18 @@ BehaviorModuleOutput LaneChangeInterface::plan()
124124
} else {
125125
const auto path =
126126
assignToCandidate(module_type_->getLaneChangePath(), module_type_->getEgoPosition());
127-
updateRTCStatus(
128-
path.start_distance_to_path_change, path.finish_distance_to_path_change, true,
129-
State::RUNNING);
127+
const auto force_activated = std::any_of(
128+
rtc_interface_ptr_map_.begin(), rtc_interface_ptr_map_.end(),
129+
[&](const auto & rtc) { return rtc.second->isForceActivated(uuid_map_.at(rtc.first)); });
130+
if (!force_activated) {
131+
updateRTCStatus(
132+
path.start_distance_to_path_change, path.finish_distance_to_path_change, true,
133+
State::RUNNING);
134+
} else {
135+
updateRTCStatus(
136+
path.start_distance_to_path_change, path.finish_distance_to_path_change, false,
137+
State::RUNNING);
138+
}
130139
}
131140

132141
return output;
@@ -227,6 +236,15 @@ bool LaneChangeInterface::canTransitFailureState()
227236
updateDebugMarker();
228237
log_debug_throttled(__func__);
229238

239+
const auto force_activated = std::any_of(
240+
rtc_interface_ptr_map_.begin(), rtc_interface_ptr_map_.end(),
241+
[&](const auto & rtc) { return rtc.second->isForceActivated(uuid_map_.at(rtc.first)); });
242+
243+
if (force_activated) {
244+
RCLCPP_WARN_THROTTLE(getLogger(), *clock_, 5000, "unsafe but force executed");
245+
return false;
246+
}
247+
230248
if (module_type_->isAbortState() && !module_type_->hasFinishedAbort()) {
231249
log_debug_throttled("Abort process has on going.");
232250
return false;
@@ -266,6 +284,19 @@ bool LaneChangeInterface::canTransitFailureState()
266284
return true;
267285
}
268286

287+
const auto force_deactivated = std::any_of(
288+
rtc_interface_ptr_map_.begin(), rtc_interface_ptr_map_.end(),
289+
[&](const auto & rtc) { return rtc.second->isForceDeactivated(uuid_map_.at(rtc.first)); });
290+
291+
if (force_deactivated && module_type_->isAbleToReturnCurrentLane()) {
292+
log_debug_throttled("Cancel lane change due to force deactivation");
293+
module_type_->toCancelState();
294+
updateRTCStatus(
295+
std::numeric_limits<double>::lowest(), std::numeric_limits<double>::lowest(), true,
296+
State::FAILED);
297+
return true;
298+
}
299+
269300
if (post_process_safety_status_.is_safe) {
270301
log_debug_throttled("Can't transit to failure state. Ego is on prepare, and it's safe.");
271302
return false;

‎planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/config/static_obstacle_avoidance.param.yaml

+2
Original file line numberDiff line numberDiff line change
@@ -243,6 +243,8 @@
243243
# For cancel maneuver
244244
cancel:
245245
enable: true # [-]
246+
force:
247+
duration_time: 2.0 # [s]
246248

247249
# For yield maneuver
248250
yield:

‎planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/data_structs.hpp

+4
Original file line numberDiff line numberDiff line change
@@ -107,6 +107,8 @@ struct AvoidanceParameters
107107
// if this param is true, it reverts avoidance path when the path is no longer needed.
108108
bool enable_cancel_maneuver{false};
109109

110+
double force_deactivate_duration_time{0.0};
111+
110112
// enable avoidance for all parking vehicle
111113
std::string policy_ambiguous_vehicle{"ignore"};
112114

@@ -581,6 +583,8 @@ struct AvoidancePlanningData
581583

582584
bool found_avoidance_path{false};
583585

586+
bool force_deactivated{false};
587+
584588
double to_stop_line{std::numeric_limits<double>::max()};
585589

586590
double to_start_point{std::numeric_limits<double>::lowest()};

‎planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/parameter_helper.hpp

+2
Original file line numberDiff line numberDiff line change
@@ -301,6 +301,8 @@ AvoidanceParameters getParameter(rclcpp::Node * node)
301301
{
302302
const std::string ns = "avoidance.cancel.";
303303
p.enable_cancel_maneuver = getOrDeclareParameter<bool>(*node, ns + "enable");
304+
p.force_deactivate_duration_time =
305+
getOrDeclareParameter<double>(*node, ns + "force.duration_time");
304306
}
305307

306308
// yield

‎planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/scene.hpp

+22-4
Original file line numberDiff line numberDiff line change
@@ -118,8 +118,16 @@ class StaticObstacleAvoidanceModule : public SceneModuleInterface
118118
const double start_distance = autoware::motion_utils::calcSignedArcLength(
119119
path.points, ego_idx, left_shift.start_pose.position);
120120
const double finish_distance = start_distance + left_shift.relative_longitudinal;
121-
rtc_interface_ptr_map_.at("left")->updateCooperateStatus(
122-
left_shift.uuid, true, State::RUNNING, start_distance, finish_distance, clock_->now());
121+
122+
// If force activated keep safety to false
123+
if (rtc_interface_ptr_map_.at("left")->isForceActivated(left_shift.uuid)) {
124+
rtc_interface_ptr_map_.at("left")->updateCooperateStatus(
125+
left_shift.uuid, false, State::RUNNING, start_distance, finish_distance, clock_->now());
126+
} else {
127+
rtc_interface_ptr_map_.at("left")->updateCooperateStatus(
128+
left_shift.uuid, true, State::RUNNING, start_distance, finish_distance, clock_->now());
129+
}
130+
123131
if (finish_distance > -1.0e-03) {
124132
steering_factor_interface_ptr_->updateSteeringFactor(
125133
{left_shift.start_pose, left_shift.finish_pose}, {start_distance, finish_distance},
@@ -131,8 +139,15 @@ class StaticObstacleAvoidanceModule : public SceneModuleInterface
131139
const double start_distance = autoware::motion_utils::calcSignedArcLength(
132140
path.points, ego_idx, right_shift.start_pose.position);
133141
const double finish_distance = start_distance + right_shift.relative_longitudinal;
134-
rtc_interface_ptr_map_.at("right")->updateCooperateStatus(
135-
right_shift.uuid, true, State::RUNNING, start_distance, finish_distance, clock_->now());
142+
143+
if (rtc_interface_ptr_map_.at("right")->isForceActivated(right_shift.uuid)) {
144+
rtc_interface_ptr_map_.at("right")->updateCooperateStatus(
145+
right_shift.uuid, false, State::RUNNING, start_distance, finish_distance, clock_->now());
146+
} else {
147+
rtc_interface_ptr_map_.at("right")->updateCooperateStatus(
148+
right_shift.uuid, true, State::RUNNING, start_distance, finish_distance, clock_->now());
149+
}
150+
136151
if (finish_distance > -1.0e-03) {
137152
steering_factor_interface_ptr_->updateSteeringFactor(
138153
{right_shift.start_pose, right_shift.finish_pose}, {start_distance, finish_distance},
@@ -462,6 +477,9 @@ class StaticObstacleAvoidanceModule : public SceneModuleInterface
462477
mutable std::vector<AvoidanceDebugMsg> debug_avoidance_initializer_for_shift_line_;
463478

464479
mutable rclcpp::Time debug_avoidance_initializer_for_shift_line_time_;
480+
481+
bool force_deactivated_{false};
482+
rclcpp::Time last_deactivation_triggered_time_;
465483
};
466484

467485
} // namespace autoware::behavior_path_planner

‎planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/schema/static_obstacle_avoidance.schema.json

+10
Original file line numberDiff line numberDiff line change
@@ -1263,6 +1263,16 @@
12631263
"type": "boolean",
12641264
"description": "Flag to enable cancel maneuver.",
12651265
"default": "true"
1266+
},
1267+
"force": {
1268+
"type": "object",
1269+
"properties": {
1270+
"duration_time": {
1271+
"type": "number",
1272+
"description": "force deactivate duration time",
1273+
"default": 2.0
1274+
}
1275+
}
12661276
}
12671277
},
12681278
"required": ["enable"],

‎planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/manager.cpp

+5
Original file line numberDiff line numberDiff line change
@@ -182,6 +182,11 @@ void StaticObstacleAvoidanceModuleManager::updateModuleParams(
182182
updateParam<bool>(parameters, ns + "consider_rear_overhang", p->consider_rear_overhang);
183183
}
184184

185+
{
186+
const std::string ns = "avoidance.cancel.";
187+
updateParam<double>(parameters, ns + "force.duration_time", p->force_deactivate_duration_time);
188+
}
189+
185190
{
186191
const std::string ns = "avoidance.stop.";
187192
updateParam<double>(parameters, ns + "max_distance", p->stop_max_distance);

‎planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp

+74
Original file line numberDiff line numberDiff line change
@@ -570,6 +570,24 @@ void StaticObstacleAvoidanceModule::fillEgoStatus(
570570
return;
571571
}
572572

573+
const auto registered_sl_force_deactivated =
574+
[&](const std::string & direction, const RegisteredShiftLineArray shift_line_array) {
575+
return std::any_of(
576+
shift_line_array.begin(), shift_line_array.end(), [&](const auto & shift_line) {
577+
return rtc_interface_ptr_map_.at(direction)->isForceDeactivated(shift_line.uuid);
578+
});
579+
};
580+
581+
const auto is_force_deactivated = registered_sl_force_deactivated("left", left_shift_array_) ||
582+
registered_sl_force_deactivated("right", right_shift_array_);
583+
if (is_force_deactivated && can_yield_maneuver) {
584+
data.yield_required = true;
585+
data.safe_shift_line = data.new_shift_line;
586+
data.force_deactivated = true;
587+
RCLCPP_INFO(getLogger(), "this module is force deactivated. wait until reactivation");
588+
return;
589+
}
590+
573591
/**
574592
* If the avoidance path is safe, use unapproved_new_sl for avoidance path generation.
575593
*/
@@ -579,6 +597,45 @@ void StaticObstacleAvoidanceModule::fillEgoStatus(
579597
return;
580598
}
581599

600+
auto candidate_sl_force_activated = [&](const std::string & direction) {
601+
// If statement to avoid unnecessary warning occurring from isForceActivated function
602+
if (candidate_uuid_ == uuid_map_.at(direction)) {
603+
if (rtc_interface_ptr_map_.at(direction)->isForceActivated(candidate_uuid_)) {
604+
return true;
605+
}
606+
}
607+
return false;
608+
};
609+
610+
auto registered_sl_force_activated =
611+
[&](const std::string & direction, const RegisteredShiftLineArray shift_line_array) {
612+
return std::any_of(
613+
shift_line_array.begin(), shift_line_array.end(), [&](const auto & shift_line) {
614+
return rtc_interface_ptr_map_.at(direction)->isForceActivated(shift_line.uuid);
615+
});
616+
};
617+
618+
/**
619+
* Check if the candidate avoidance path is force activated
620+
*/
621+
if (candidate_sl_force_activated("left") || candidate_sl_force_activated("right")) {
622+
data.yield_required = false;
623+
data.safe_shift_line = data.new_shift_line;
624+
return;
625+
}
626+
627+
/**
628+
* Check if any registered shift line is force activated
629+
*/
630+
if (
631+
registered_sl_force_activated("left", left_shift_array_) ||
632+
registered_sl_force_activated("right", right_shift_array_)) {
633+
data.yield_required = false;
634+
data.safe_shift_line = data.new_shift_line;
635+
RCLCPP_WARN_THROTTLE(getLogger(), *clock_, 5000, "unsafe but force executed");
636+
return;
637+
}
638+
582639
/**
583640
* If the yield maneuver is disabled, use unapproved_new_sl for avoidance path generation even if
584641
* the shift line is unsafe.
@@ -716,6 +773,10 @@ bool StaticObstacleAvoidanceModule::isSafePath(
716773
universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
717774
const auto & p = planner_data_->parameters;
718775

776+
if (force_deactivated_) {
777+
return false;
778+
}
779+
719780
if (!parameters_->enable_safety_check) {
720781
return true; // if safety check is disabled, it always return safe.
721782
}
@@ -1327,6 +1388,19 @@ void StaticObstacleAvoidanceModule::updateData()
13271388
}
13281389

13291390
safe_ = avoid_data_.safe;
1391+
1392+
if (!force_deactivated_) {
1393+
last_deactivation_triggered_time_ = clock_->now();
1394+
force_deactivated_ = avoid_data_.force_deactivated;
1395+
return;
1396+
}
1397+
1398+
if (
1399+
(clock_->now() - last_deactivation_triggered_time_).seconds() >
1400+
parameters_->force_deactivate_duration_time) {
1401+
RCLCPP_INFO(getLogger(), "The force deactivation is released");
1402+
force_deactivated_ = false;
1403+
}
13301404
}
13311405

13321406
void StaticObstacleAvoidanceModule::processOnEntry()

0 commit comments

Comments
 (0)