Skip to content

Commit

Permalink
Merge branch 'main' into mrm-comfortable-stop-operator
Browse files Browse the repository at this point in the history
  • Loading branch information
@karishma1911
karishma1911 authored May 28, 2024
2 parents 89b45b3 + 39d406e commit d0927da
Show file tree
Hide file tree
Showing 4 changed files with 68 additions and 79 deletions.
12 changes: 4 additions & 8 deletions ...ng/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -501,11 +501,9 @@ using AvoidOutlines = std::vector<AvoidOutline>;
* avoidance state
*/
enum class AvoidanceState {
NOT_AVOID = 0,
AVOID_EXECUTE,
YIELD,
AVOID_PATH_READY,
AVOID_PATH_NOT_READY,
RUNNING = 0,
CANCEL,
SUCCEEDED,
};

/*
Expand All @@ -514,7 +512,7 @@ enum class AvoidanceState {
struct AvoidancePlanningData
{
// ego final state
AvoidanceState state{AvoidanceState::NOT_AVOID};
AvoidanceState state{AvoidanceState::RUNNING};

// un-shifted pose (for current lane detection)
Pose reference_pose;
Expand Down Expand Up @@ -566,8 +564,6 @@ struct AvoidancePlanningData

bool ready{false};

bool success{false};

bool comfortable{false};

bool avoid_required{false};
Expand Down
36 changes: 20 additions & 16 deletions planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -30,6 +30,7 @@
#include <memory>
#include <string>
#include <unordered_map>
#include <utility>
#include <vector>

namespace behavior_path_planner
Expand Down Expand Up @@ -64,7 +65,14 @@ class AvoidanceModule : public SceneModuleInterface
std::shared_ptr<AvoidanceDebugMsgArray> get_debug_msg_array() const;

private:
bool isSatisfiedSuccessCondition(const AvoidancePlanningData & data) const;
/**
* @brief return the result whether the module can stop path generation process.
* @param avoidance data.
* @return it will return AvoidanceState::RUNNING when there are obstacles ego should avoid.
* it will return AvoidanceState::CANCEL when all obstacles have gone.
* it will return AvoidanceState::SUCCEEDED when the ego avoid all obstacles.
*/
AvoidanceState getCurrentModuleState(const AvoidancePlanningData & data) const;

bool canTransitSuccessState() override;

Expand Down Expand Up @@ -188,14 +196,6 @@ class AvoidanceModule : public SceneModuleInterface
*/
void updateRTCData();

// ego state check

/**
* @brief update ego status based on avoidance path and surround condition.
* @param ego status. (NOT_AVOID, AVOID, YIELD, AVOID_EXECUTE, AVOID_PATH_NOT_READY)
*/
AvoidanceState updateEgoState(const AvoidancePlanningData & data) const;

// ego behavior update

/**
Expand Down Expand Up @@ -359,7 +359,7 @@ class AvoidanceModule : public SceneModuleInterface
* @brief reset registered shift lines.
* @details reset only when the base offset is zero. Otherwise, sudden steering will be caused;
*/
void removeRegisteredShiftLines()
void removeRegisteredShiftLines(const uint8_t state)
{
constexpr double threshold = 0.1;
if (std::abs(path_shifter_.getBaseOffset()) > threshold) {
Expand All @@ -370,15 +370,19 @@ class AvoidanceModule : public SceneModuleInterface
unlockNewModuleLaunch();

for (const auto & left_shift : left_shift_array_) {
rtc_interface_ptr_map_.at("left")->updateCooperateStatus(
left_shift.uuid, true, State::FAILED, std::numeric_limits<double>::lowest(),
std::numeric_limits<double>::lowest(), clock_->now());
if (rtc_interface_ptr_map_.at("left")->isRegistered(left_shift.uuid)) {
rtc_interface_ptr_map_.at("left")->updateCooperateStatus(
left_shift.uuid, true, state, std::numeric_limits<double>::lowest(),
std::numeric_limits<double>::lowest(), clock_->now());
}
}

for (const auto & right_shift : right_shift_array_) {
rtc_interface_ptr_map_.at("right")->updateCooperateStatus(
right_shift.uuid, true, State::FAILED, std::numeric_limits<double>::lowest(),
std::numeric_limits<double>::lowest(), clock_->now());
if (rtc_interface_ptr_map_.at("right")->isRegistered(right_shift.uuid)) {
rtc_interface_ptr_map_.at("right")->updateCooperateStatus(
right_shift.uuid, true, state, std::numeric_limits<double>::lowest(),
std::numeric_limits<double>::lowest(), clock_->now());
}
}

if (!path_shifter_.getShiftLines().empty()) {
Expand Down
90 changes: 38 additions & 52 deletions planning/behavior_path_avoidance_module/src/scene.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -115,14 +115,14 @@ bool AvoidanceModule::isExecutionReady() const
return avoid_data_.safe && avoid_data_.comfortable && avoid_data_.valid && avoid_data_.ready;
}

bool AvoidanceModule::isSatisfiedSuccessCondition(const AvoidancePlanningData & data) const
AvoidanceState AvoidanceModule::getCurrentModuleState(const AvoidancePlanningData & data) const
{
const bool has_avoidance_target = std::any_of(
data.target_objects.begin(), data.target_objects.end(),
[this](const auto & o) { return !helper_->isAbsolutelyNotAvoidable(o); });

if (has_avoidance_target) {
return false;
return AvoidanceState::RUNNING;
}

// If the ego is on the shift line, keep RUNNING.
Expand All @@ -133,7 +133,7 @@ bool AvoidanceModule::isSatisfiedSuccessCondition(const AvoidancePlanningData &
};
for (const auto & shift_line : path_shifter_.getShiftLines()) {
if (within(shift_line, idx)) {
return false;
return AvoidanceState::RUNNING;
}
}
}
Expand All @@ -142,17 +142,21 @@ bool AvoidanceModule::isSatisfiedSuccessCondition(const AvoidancePlanningData &
const bool has_base_offset =
std::abs(path_shifter_.getBaseOffset()) > parameters_->lateral_execution_threshold;

if (has_base_offset) {
return AvoidanceState::RUNNING;
}

// Nothing to do. -> EXIT.
if (!has_shift_point && !has_base_offset) {
return true;
if (!has_shift_point) {
return AvoidanceState::SUCCEEDED;
}

// Be able to canceling avoidance path. -> EXIT.
if (!helper_->isShifted() && parameters_->enable_cancel_maneuver) {
return true;
return AvoidanceState::CANCEL;
}

return false;
return AvoidanceState::RUNNING;
}

bool AvoidanceModule::canTransitSuccessState()
Expand Down Expand Up @@ -183,7 +187,7 @@ bool AvoidanceModule::canTransitSuccessState()
}
}

return data.success;
return data.state == AvoidanceState::CANCEL || data.state == AvoidanceState::SUCCEEDED;
}

void AvoidanceModule::fillFundamentalData(AvoidancePlanningData & data, DebugData & debug)
Expand Down Expand Up @@ -502,7 +506,7 @@ void AvoidanceModule::fillShiftLine(AvoidancePlanningData & data, DebugData & de
void AvoidanceModule::fillEgoStatus(
AvoidancePlanningData & data, [[maybe_unused]] DebugData & debug) const
{
data.success = isSatisfiedSuccessCondition(data);
data.state = getCurrentModuleState(data);

/**
* Find the nearest object that should be avoid. When the ego follows reference path,
Expand Down Expand Up @@ -633,27 +637,6 @@ void AvoidanceModule::fillDebugData(
}
}

AvoidanceState AvoidanceModule::updateEgoState(const AvoidancePlanningData & data) const
{
if (data.yield_required) {
return AvoidanceState::YIELD;
}

if (!data.avoid_required) {
return AvoidanceState::NOT_AVOID;
}

if (!data.found_avoidance_path) {
return AvoidanceState::AVOID_PATH_NOT_READY;
}

if (isWaitingApproval() && path_shifter_.getShiftLines().empty()) {
return AvoidanceState::AVOID_PATH_READY;
}

return AvoidanceState::AVOID_EXECUTE;
}

void AvoidanceModule::updateEgoBehavior(const AvoidancePlanningData & data, ShiftedPath & path)
{
if (parameters_->disable_path_update) {
Expand All @@ -663,29 +646,30 @@ void AvoidanceModule::updateEgoBehavior(const AvoidancePlanningData & data, Shif
insertPrepareVelocity(path);
insertAvoidanceVelocity(path);

switch (data.state) {
case AvoidanceState::NOT_AVOID: {
break;
}
case AvoidanceState::YIELD: {
const auto insert_velocity = [this, &data, &path]() {
if (data.yield_required) {
insertWaitPoint(isBestEffort(parameters_->policy_deceleration), path);
break;
return;
}
case AvoidanceState::AVOID_PATH_NOT_READY: {
insertWaitPoint(isBestEffort(parameters_->policy_deceleration), path);
break;

if (!data.avoid_required) {
return;
}
case AvoidanceState::AVOID_PATH_READY: {

if (!data.found_avoidance_path) {
insertWaitPoint(isBestEffort(parameters_->policy_deceleration), path);
break;
return;
}
case AvoidanceState::AVOID_EXECUTE: {
insertStopPoint(isBestEffort(parameters_->policy_deceleration), path);
break;

if (isWaitingApproval() && path_shifter_.getShiftLines().empty()) {
insertWaitPoint(isBestEffort(parameters_->policy_deceleration), path);
return;
}
default:
throw std::domain_error("invalid behavior");
}

insertStopPoint(isBestEffort(parameters_->policy_deceleration), path);
};

insert_velocity();

insertReturnDeadLine(isBestEffort(parameters_->policy_deceleration), path);

Expand Down Expand Up @@ -869,12 +853,16 @@ BehaviorModuleOutput AvoidanceModule::plan()

updatePathShifter(data.safe_shift_line);

if (data.success) {
removeRegisteredShiftLines();
if (data.state == AvoidanceState::SUCCEEDED) {
removeRegisteredShiftLines(State::SUCCEEDED);
}

if (data.state == AvoidanceState::CANCEL) {
removeRegisteredShiftLines(State::FAILED);
}

if (data.yield_required) {
removeRegisteredShiftLines();
removeRegisteredShiftLines(State::FAILED);
}

// generate path with shift points that have been inserted.
Expand Down Expand Up @@ -941,8 +929,6 @@ BehaviorModuleOutput AvoidanceModule::plan()
spline_shift_path.path, parameters_->resample_interval_for_output);
}

avoid_data_.state = updateEgoState(data);

// update output data
{
updateEgoBehavior(data, spline_shift_path);
Expand Down
9 changes: 6 additions & 3 deletions ..._planner_common/include/behavior_path_planner_common/interface/scene_module_interface.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -508,9 +508,12 @@ class SceneModuleInterface
{
for (const auto & [module_name, ptr] : rtc_interface_ptr_map_) {
if (ptr) {
ptr->updateCooperateStatus(
uuid_map_.at(module_name), true, State::SUCCEEDED, std::numeric_limits<double>::lowest(),
std::numeric_limits<double>::lowest(), clock_->now());
if (ptr->isRegistered(uuid_map_.at(module_name))) {
ptr->updateCooperateStatus(
uuid_map_.at(module_name), true, State::SUCCEEDED,
std::numeric_limits<double>::lowest(), std::numeric_limits<double>::lowest(),
clock_->now());
}
}
}
}
Expand Down

0 comments on commit d0927da

Please sign in to comment.