Skip to content

Commit

Permalink
feat(behavior_path_planner): add detail text to virutal wall (#9600)
Browse files Browse the repository at this point in the history 
* feat(behavior_path_planner): add detail text to virutal wall

Signed-off-by: kosuke55 <[email protected]>

* goal is far

Signed-off-by: kosuke55 <[email protected]>

* pull over start pose is far

Signed-off-by: kosuke55 <[email protected]>

* fix lc build

Signed-off-by: kosuke55 <[email protected]>

* fix build

Signed-off-by: kosuke55 <[email protected]>

* Update planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp

Signed-off-by: kosuke55 <[email protected]>

---------

Signed-off-by: kosuke55 <[email protected]>
  • Loading branch information
@kosuke55
kosuke55 authored Dec 10, 2024
1 parent 9707caf commit 34f7c9b
Show file tree
Hide file tree
Showing 15 changed files with 123 additions and 136 deletions.
33 changes: 6 additions & 27 deletions ...planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -84,29 +84,6 @@ struct LastApprovalData
Pose pose{};
};

// store stop_pose_ pointer with reason string
struct PoseWithString
{
std::optional<Pose> * pose;
std::string string;

explicit PoseWithString(std::optional<Pose> * shared_pose) : pose(shared_pose), string("") {}

void set(const Pose & new_pose, const std::string & new_string)
{
*pose = new_pose;
string = new_string;
}

void set(const std::string & new_string) { string = new_string; }

void clear()
{
pose->reset();
string = "";
}
};

struct PullOverContextData
{
PullOverContextData() = delete;
Expand Down Expand Up @@ -371,7 +348,6 @@ class GoalPlannerModule : public SceneModuleInterface

// debug
mutable GoalPlannerDebugData debug_data_;
mutable PoseWithString debug_stop_pose_with_info_;

// goal seach
GoalCandidates generateGoalCandidates() const;
Expand All @@ -381,8 +357,10 @@ class GoalPlannerModule : public SceneModuleInterface
void decelerateForTurnSignal(const Pose & stop_pose, PathWithLaneId & path) const;
void decelerateBeforeSearchStart(
const Pose & search_start_offset_pose, PathWithLaneId & path) const;
PathWithLaneId generateStopPath(const PullOverContextData & context_data) const;
PathWithLaneId generateFeasibleStopPath(const PathWithLaneId & path) const;
PathWithLaneId generateStopPath(
const PullOverContextData & context_data, const std::string & detail) const;
PathWithLaneId generateFeasibleStopPath(
const PathWithLaneId & path, const std::string & detail) const;

void keepStoppedWithCurrentPath(
const PullOverContextData & ctx_data, PathWithLaneId & path) const;
Expand Down Expand Up @@ -420,7 +398,8 @@ class GoalPlannerModule : public SceneModuleInterface
// plan pull over path
BehaviorModuleOutput planPullOver(const PullOverContextData & context_data);
BehaviorModuleOutput planPullOverAsOutput(const PullOverContextData & context_data);
BehaviorModuleOutput planPullOverAsCandidate(const PullOverContextData & context_data);
BehaviorModuleOutput planPullOverAsCandidate(
const PullOverContextData & context_data, const std::string & detail);
std::optional<PullOverPath> selectPullOverPath(
const PullOverContextData & context_data,
const std::vector<PullOverPath> & pull_over_path_candidates,
Expand Down
93 changes: 40 additions & 53 deletions ...avior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -72,8 +72,7 @@ GoalPlannerModule::GoalPlannerModule(
vehicle_info_{autoware::vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo()},
thread_safe_data_{mutex_, clock_},
is_lane_parking_cb_running_{false},
is_freespace_parking_cb_running_{false},
debug_stop_pose_with_info_{&stop_pose_}
is_freespace_parking_cb_running_{false}
{
LaneDepartureChecker lane_departure_checker{};
lane_departure_checker.setVehicleInfo(vehicle_info_);
Expand Down Expand Up @@ -1237,7 +1236,8 @@ void GoalPlannerModule::setOutput(

if (!context_data.pull_over_path_opt) {
// situation : not safe against static objects use stop_path
output.path = generateStopPath(context_data);
output.path = generateStopPath(
context_data, (goal_candidates_.empty() ? "no goal candidate" : "no static safe path"));
RCLCPP_INFO_THROTTLE(
getLogger(), *clock_, 5000, "Not found safe pull_over path, generate stop path");
setDrivableAreaInfo(context_data, output);
Expand All @@ -1251,10 +1251,10 @@ void GoalPlannerModule::setOutput(
// situation : not safe against dynamic objects after approval
// insert stop point in current path if ego is able to stop with acceleration and jerk
// constraints
output.path = generateFeasibleStopPath(pull_over_path.getCurrentPath());
output.path =
generateFeasibleStopPath(pull_over_path.getCurrentPath(), "unsafe against dynamic objects");
RCLCPP_INFO_THROTTLE(
getLogger(), *clock_, 5000, "Not safe against dynamic objects, generate stop path");
debug_stop_pose_with_info_.set(std::string("feasible stop after approval"));
} else {
// situation : (safe against static and dynamic objects) or (safe against static objects and
// before approval) don't stop
Expand Down Expand Up @@ -1363,17 +1363,25 @@ BehaviorModuleOutput GoalPlannerModule::planPullOver(const PullOverContextData &
{
universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);

if (
path_decision_controller_.get_current_state().state !=
PathDecisionState::DecisionKind::DECIDED) {
return planPullOverAsCandidate(context_data);
const auto current_state = path_decision_controller_.get_current_state();
if (current_state.state != PathDecisionState::DecisionKind::DECIDED) {
const bool is_stable_safe = current_state.is_stable_safe;
// clang-format off
const std::string detail =
goal_candidates_.empty() ? "no goal candidate" :
context_data.pull_over_path_candidates.empty() ? "no path candidate" :
!thread_safe_data_.foundPullOverPath() ? "no static safe path" :
!is_stable_safe ? "unsafe against dynamic objects" :
"too far goal";
// clang-format on
return planPullOverAsCandidate(context_data, detail);
}

return planPullOverAsOutput(context_data);
}

BehaviorModuleOutput GoalPlannerModule::planPullOverAsCandidate(
const PullOverContextData & context_data)
const PullOverContextData & context_data, const std::string & detail)
{
universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);

Expand All @@ -1385,7 +1393,7 @@ BehaviorModuleOutput GoalPlannerModule::planPullOverAsCandidate(
BehaviorModuleOutput output{};
const BehaviorModuleOutput pull_over_output = planPullOverAsOutput(context_data);
output.modified_goal = pull_over_output.modified_goal;
output.path = generateStopPath(context_data);
output.path = generateStopPath(context_data, detail);
output.reference_path = getPreviousModuleOutput().reference_path;

const auto target_drivable_lanes = utils::getNonOverlappingExpandedLanes(
Expand Down Expand Up @@ -1560,7 +1568,7 @@ BehaviorModuleOutput GoalPlannerModule::planWaitingApproval()
"planner");
} else {
const auto & context_data = context_data_.value();
return planPullOverAsCandidate(context_data);
return planPullOverAsCandidate(context_data, "waiting approval");
}
}

Expand Down Expand Up @@ -1606,7 +1614,8 @@ void GoalPlannerModule::setParameters(const std::shared_ptr<GoalPlannerParameter
parameters_ = parameters;
}

PathWithLaneId GoalPlannerModule::generateStopPath(const PullOverContextData & context_data) const
PathWithLaneId GoalPlannerModule::generateStopPath(
const PullOverContextData & context_data, const std::string & detail) const
{
universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);

Expand Down Expand Up @@ -1656,28 +1665,20 @@ PathWithLaneId GoalPlannerModule::generateStopPath(const PullOverContextData & c
// (In the case of the curve lane, the position is not aligned due to the
// difference between the outer and inner sides)
// 4. feasible stop
const auto stop_pose_with_info =
std::invoke([&]() -> std::optional<std::pair<Pose, std::string>> {
if (context_data.pull_over_path_opt) {
return std::make_pair(
context_data.pull_over_path_opt.value().start_pose(), "stop at selected start pose");
}
if (thread_safe_data_.get_closest_start_pose()) {
return std::make_pair(
thread_safe_data_.get_closest_start_pose().value(), "stop at closest start pose");
}
if (!decel_pose) {
return std::nullopt;
}
return std::make_pair(decel_pose.value(), "stop at search start pose");
});
if (!stop_pose_with_info) {
const auto feasible_stop_path = generateFeasibleStopPath(getPreviousModuleOutput().path);
// override stop pose info debug string
debug_stop_pose_with_info_.set(std::string("feasible stop: not calculate stop pose"));
const auto stop_pose_opt = std::invoke([&]() -> std::optional<Pose> {
if (context_data.pull_over_path_opt)
return context_data.pull_over_path_opt.value().start_pose();
if (thread_safe_data_.get_closest_start_pose())
return thread_safe_data_.get_closest_start_pose().value();
if (!decel_pose) return std::nullopt;
return decel_pose.value();
});
if (!stop_pose_opt.has_value()) {
const auto feasible_stop_path =
generateFeasibleStopPath(getPreviousModuleOutput().path, detail);
return feasible_stop_path;
}
const Pose stop_pose = stop_pose_with_info->first;
const Pose stop_pose = stop_pose_opt.value();

// if stop pose is closer than min_stop_distance, stop as soon as possible
const double ego_to_stop_distance = calcSignedArcLengthFromEgo(extended_prev_path, stop_pose);
Expand All @@ -1687,16 +1688,15 @@ PathWithLaneId GoalPlannerModule::generateStopPath(const PullOverContextData & c
const bool is_stopped = std::abs(current_vel) < eps_vel;
const double buffer = is_stopped ? stop_distance_buffer_ : 0.0;
if (min_stop_distance && ego_to_stop_distance + buffer < *min_stop_distance) {
const auto feasible_stop_path = generateFeasibleStopPath(getPreviousModuleOutput().path);
debug_stop_pose_with_info_.set(
std::string("feasible stop: stop pose is closer than min_stop_distance"));
const auto feasible_stop_path =
generateFeasibleStopPath(getPreviousModuleOutput().path, detail);
return feasible_stop_path;
}

// slow down for turn signal, insert stop point to stop_pose
auto stop_path = extended_prev_path;
decelerateForTurnSignal(stop_pose, stop_path);
debug_stop_pose_with_info_.set(stop_pose, stop_pose_with_info->second);
stop_pose_ = PoseWithDetail(stop_pose, detail);

// slow down before the search area.
if (decel_pose) {
Expand All @@ -1719,7 +1719,8 @@ PathWithLaneId GoalPlannerModule::generateStopPath(const PullOverContextData & c
return stop_path;
}

PathWithLaneId GoalPlannerModule::generateFeasibleStopPath(const PathWithLaneId & path) const
PathWithLaneId GoalPlannerModule::generateFeasibleStopPath(
const PathWithLaneId & path, const std::string & detail) const
{
universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);

Expand All @@ -1736,7 +1737,7 @@ PathWithLaneId GoalPlannerModule::generateFeasibleStopPath(const PathWithLaneId
const auto stop_idx =
autoware::motion_utils::insertStopPoint(current_pose, *min_stop_distance, stop_path.points);
if (stop_idx) {
debug_stop_pose_with_info_.set(stop_path.points.at(*stop_idx).point.pose, "feasible stop");
stop_pose_ = PoseWithDetail(stop_path.points.at(*stop_idx).point.pose, detail);
}

return stop_path;
Expand Down Expand Up @@ -2564,20 +2565,6 @@ void GoalPlannerModule::setDebugData(const PullOverContextData & context_data)
planner_type_marker_array.markers.push_back(marker);
add(planner_type_marker_array);
}

// Visualize stop pose info
if (debug_stop_pose_with_info_.pose->has_value()) {
visualization_msgs::msg::MarkerArray stop_pose_marker_array{};
const auto color = createMarkerColor(1.0, 1.0, 1.0, 0.99);
auto marker = createDefaultMarker(
header.frame_id, header.stamp, "stop_pose_info", 0,
visualization_msgs::msg::Marker::TEXT_VIEW_FACING, createMarkerScale(0.0, 0.0, 0.5), color);
marker.pose = debug_stop_pose_with_info_.pose->value();
marker.text = debug_stop_pose_with_info_.string;
stop_pose_marker_array.markers.push_back(marker);
add(stop_pose_marker_array);
add(createPoseMarkerArray(marker.pose, "stop_pose", 1.0, 1.0, 1.0, 0.9));
}
}

void GoalPlannerModule::GoalPlannerData::initializeOccupancyGridMap(
Expand Down
8 changes: 6 additions & 2 deletions planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -111,7 +111,9 @@ BehaviorModuleOutput LaneChangeInterface::plan()
path_reference_ = std::make_shared<PathWithLaneId>(output.reference_path);
*prev_approved_path_ = getPreviousModuleOutput().path;

stop_pose_ = module_type_->getStopPose();
const auto stop_pose_opt = module_type_->getStopPose();
stop_pose_ = stop_pose_opt.has_value() ? PoseWithDetailOpt(PoseWithDetail(stop_pose_opt.value()))
: PoseWithDetailOpt();

const auto & lane_change_debug = module_type_->getDebugData();
for (const auto & [uuid, data] : lane_change_debug.collision_check_objects_after_approval) {
Expand Down Expand Up @@ -169,7 +171,9 @@ BehaviorModuleOutput LaneChangeInterface::planWaitingApproval()
}

path_reference_ = std::make_shared<PathWithLaneId>(out.reference_path);
stop_pose_ = module_type_->getStopPose();
const auto stop_pose_opt = module_type_->getStopPose();
stop_pose_ = stop_pose_opt.has_value() ? PoseWithDetailOpt(PoseWithDetail(stop_pose_opt.value()))
: PoseWithDetailOpt();

if (!module_type_->isValidPath()) {
path_candidate_ = std::make_shared<PathWithLaneId>();
Expand Down
11 changes: 11 additions & 0 deletions ...havior_path_planner_common/include/autoware/behavior_path_planner_common/data_manager.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -147,6 +147,17 @@ struct CandidateOutput
double finish_distance_to_path_change{std::numeric_limits<double>::lowest()};
};

/**
* @brief Adds detail text to stop/slow/dead_pose virtual walls.
*/
struct PoseWithDetail
{
Pose pose;
std::string detail;
explicit PoseWithDetail(const Pose & p, const std::string & d = "") : pose(p), detail(d) {}
};
using PoseWithDetailOpt = std::optional<PoseWithDetail>;

struct PlannerData
{
Odometry::ConstSharedPtr self_odometry{};
Expand Down
28 changes: 17 additions & 11 deletions ...common/include/autoware/behavior_path_planner_common/interface/scene_module_interface.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -270,34 +270,40 @@ class SceneModuleInterface

std::string name() const { return name_; }

std::optional<Pose> getStopPose() const
PoseWithDetailOpt getStopPose() const
{
if (!stop_pose_) {
return {};
}

const auto & base_link2front = planner_data_->parameters.base_link2front;
return calcOffsetPose(stop_pose_.value(), base_link2front, 0.0, 0.0);
return PoseWithDetail(
calcOffsetPose(stop_pose_.value().pose, base_link2front, 0.0, 0.0),
stop_pose_.value().detail);
}

std::optional<Pose> getSlowPose() const
PoseWithDetailOpt getSlowPose() const
{
if (!slow_pose_) {
return {};
}

const auto & base_link2front = planner_data_->parameters.base_link2front;
return calcOffsetPose(slow_pose_.value(), base_link2front, 0.0, 0.0);
return PoseWithDetail(
calcOffsetPose(slow_pose_.value().pose, base_link2front, 0.0, 0.0),
slow_pose_.value().detail);
}

std::optional<Pose> getDeadPose() const
PoseWithDetailOpt getDeadPose() const
{
if (!dead_pose_) {
return {};
}

const auto & base_link2front = planner_data_->parameters.base_link2front;
return calcOffsetPose(dead_pose_.value(), base_link2front, 0.0, 0.0);
return PoseWithDetail(
calcOffsetPose(dead_pose_.value().pose, base_link2front, 0.0, 0.0),
dead_pose_.value().detail);
}

void resetWallPoses() const
Expand Down Expand Up @@ -556,7 +562,7 @@ class SceneModuleInterface
{
if (stop_pose_.has_value()) {
velocity_factor_interface_.set(
path.points, getEgoPose(), stop_pose_.value(), VelocityFactor::APPROACHING, "stop");
path.points, getEgoPose(), stop_pose_.value().pose, VelocityFactor::APPROACHING, "stop");
return;
}

Expand All @@ -565,7 +571,7 @@ class SceneModuleInterface
}

velocity_factor_interface_.set(
path.points, getEgoPose(), slow_pose_.value(), VelocityFactor::APPROACHING, "slow down");
path.points, getEgoPose(), slow_pose_.value().pose, VelocityFactor::APPROACHING, "slow down");
}

void setDrivableLanes(const std::vector<DrivableLanes> & drivable_lanes);
Expand Down Expand Up @@ -625,11 +631,11 @@ class SceneModuleInterface

mutable VelocityFactorInterface velocity_factor_interface_;

mutable std::optional<Pose> stop_pose_{std::nullopt};
mutable PoseWithDetailOpt stop_pose_{std::nullopt};

mutable std::optional<Pose> slow_pose_{std::nullopt};
mutable PoseWithDetailOpt slow_pose_{std::nullopt};

mutable std::optional<Pose> dead_pose_{std::nullopt};
mutable PoseWithDetailOpt dead_pose_{std::nullopt};

mutable MarkerArray info_marker_;

Expand Down
Loading

0 comments on commit 34f7c9b

Please sign in to comment.