Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

feat(goal_planner): keep margin against objects as possible #5569

Merged
merged 2 commits into from
Nov 16, 2023
Merged

feat(goal_planner): keep margin against objects as possible #5569

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
bbe5225
refactor(goal_planner): add prev_data instead of status
kosuke55 Nov 6, 2023
5c5173b
feat(goal_planner): keep margin against objects as possible
kosuke55 Nov 6, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
86 changes: 24 additions & 62 deletions ...h_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -68,64 +68,6 @@
using behavior_path_planner::utils::path_safety_checker::TargetObjectsOnLane;
using tier4_autoware_utils::Polygon2d;

#define DEFINE_SETTER(TYPE, NAME) \
public: \
void set_##NAME(const TYPE & value) \
{ \
NAME##_ = value; \
}

#define DEFINE_GETTER(TYPE, NAME) \
public: \
TYPE get_##NAME() const \
{ \
return NAME##_; \
}

#define DEFINE_SETTER_GETTER(TYPE, NAME) \
DEFINE_SETTER(TYPE, NAME) \
DEFINE_GETTER(TYPE, NAME)

class PullOverStatus
{
public:
// Reset all data members to their initial states
void reset()
{
lane_parking_pull_over_path_ = nullptr;
prev_stop_path_ = nullptr;
prev_stop_path_after_approval_ = nullptr;

is_safe_ = false;
prev_found_path_ = false;
prev_is_safe_ = false;
}

DEFINE_SETTER_GETTER(std::shared_ptr<PullOverPath>, lane_parking_pull_over_path)
DEFINE_SETTER_GETTER(std::shared_ptr<PathWithLaneId>, prev_stop_path)
DEFINE_SETTER_GETTER(std::shared_ptr<PathWithLaneId>, prev_stop_path_after_approval)
DEFINE_SETTER_GETTER(bool, is_safe)
DEFINE_SETTER_GETTER(bool, prev_found_path)
DEFINE_SETTER_GETTER(bool, prev_is_safe)
DEFINE_SETTER_GETTER(Pose, refined_goal_pose)
DEFINE_SETTER_GETTER(Pose, closest_goal_candidate_pose)

private:
std::shared_ptr<PullOverPath> lane_parking_pull_over_path_{nullptr};
std::shared_ptr<PathWithLaneId> prev_stop_path_{nullptr};
std::shared_ptr<PathWithLaneId> prev_stop_path_after_approval_{nullptr};
bool is_safe_{false};
bool prev_found_path_{false};
bool prev_is_safe_{false};

Pose refined_goal_pose_{};
Pose closest_goal_candidate_pose_{};
};

#undef DEFINE_SETTER
#undef DEFINE_GETTER
#undef DEFINE_SETTER_GETTER

#define DEFINE_SETTER_WITH_MUTEX(TYPE, NAME) \
public: \
void set_##NAME(const TYPE & value) \
Expand Down Expand Up @@ -168,18 +110,18 @@
return false;
}

void set_pull_over_path(const PullOverPath & path)

Check warning on line 113 in planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp#L113 

Added line #L113 was not covered by tests
{
const std::lock_guard<std::recursive_mutex> lock(mutex_);
pull_over_path_ = std::make_shared<PullOverPath>(path);

Check warning on line 116 in planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp#L116 

Added line #L116 was not covered by tests
if (path.type != PullOverPlannerType::NONE && path.type != PullOverPlannerType::FREESPACE) {
lane_parking_pull_over_path_ = std::make_shared<PullOverPath>(path);

Check warning on line 118 in planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp#L118 

Added line #L118 was not covered by tests
}

last_path_update_time_ = clock_->now();
}

void set_pull_over_path(const std::shared_ptr<PullOverPath> & path)

Check warning on line 124 in planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp#L124 

Added line #L124 was not covered by tests
{
const std::lock_guard<std::recursive_mutex> lock(mutex_);
pull_over_path_ = path;
Expand All @@ -190,16 +132,16 @@
}

template <typename... Args>
void set(Args... args)

Check warning on line 135 in planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp#L135 

Added line #L135 was not covered by tests
{
std::lock_guard<std::recursive_mutex> lock(mutex_);

Check warning on line 137 in planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp#L137 

Added line #L137 was not covered by tests
(..., set(args));
}

Check warning on line 139 in planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp#L139 

Added line #L139 was not covered by tests
void set(const GoalCandidates & arg) { set_goal_candidates(arg); }
void set(const std::vector<PullOverPath> & arg) { set_pull_over_path_candidates(arg); }
void set(const std::shared_ptr<PullOverPath> & arg) { set_pull_over_path(arg); }
void set(const PullOverPath & arg) { set_pull_over_path(arg); }
void set(const GoalCandidate & arg) { set_modified_goal_pose(arg); }

Check warning on line 144 in planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp#L144 

Added line #L144 was not covered by tests

void clearPullOverPath()
{
Expand Down Expand Up @@ -240,7 +182,7 @@
}

DEFINE_GETTER_WITH_MUTEX(std::shared_ptr<PullOverPath>, pull_over_path)
DEFINE_GETTER_WITH_MUTEX(std::shared_ptr<PullOverPath>, lane_parking_pull_over_path)

Check warning on line 185 in planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp#L185 

Added line #L185 was not covered by tests
DEFINE_GETTER_WITH_MUTEX(std::optional<rclcpp::Time>, last_path_update_time)
DEFINE_GETTER_WITH_MUTEX(std::optional<rclcpp::Time>, last_path_idx_increment_time)

Expand Down Expand Up @@ -288,6 +230,22 @@
Pose pose{};
};

struct PreviousPullOverData
{
void reset()
{
stop_path = nullptr;
stop_path_after_approval = nullptr;
found_path = false;
is_safe = false;
}

std::shared_ptr<PathWithLaneId> stop_path{nullptr};
std::shared_ptr<PathWithLaneId> stop_path_after_approval{nullptr};
bool found_path{false};
bool is_safe{false};
};

class GoalPlannerModule : public SceneModuleInterface
{
public:
Expand Down Expand Up @@ -380,10 +338,10 @@
tier4_autoware_utils::LinearRing2d vehicle_footprint_;

std::recursive_mutex mutex_;
PullOverStatus status_;
ThreadSafeData thread_safe_data_;

std::unique_ptr<LastApprovalData> last_approval_data_{nullptr};
PreviousPullOverData prev_data_{nullptr};

// approximate distance from the start point to the end point of pull_over.
// this is used as an assumed value to decelerate, etc., before generating the actual path.
Expand All @@ -408,11 +366,14 @@
// collision check
void initializeOccupancyGridMap();
void updateOccupancyGrid();
bool checkCollision(const PathWithLaneId & path) const;
bool checkOccupancyGridCollision(const PathWithLaneId & path) const;
bool checkObjectsCollision(
const PathWithLaneId & path, const double collision_check_margin,
const bool update_debug_data = false) const;

// goal seach
Pose calcRefinedGoal(const Pose & goal_pose) const;
void generateGoalCandidates();
GoalCandidates generateGoalCandidates() const;

// stop or decelerate
void deceleratePath(PullOverPath & pull_over_path) const;
Expand Down Expand Up @@ -458,7 +419,7 @@
BehaviorModuleOutput planPullOverAsCandidate();
std::optional<std::pair<PullOverPath, GoalCandidate>> selectPullOverPath(
const std::vector<PullOverPath> & pull_over_path_candidates,
const GoalCandidates & goal_candidates) const;
const GoalCandidates & goal_candidates, const double collision_check_margin) const;
std::vector<PullOverPath> sortPullOverPathCandidatesByGoalPriority(
const std::vector<PullOverPath> & pull_over_path_candidates,
const GoalCandidates & goal_candidates) const;
Expand All @@ -470,6 +431,7 @@
// output setter
void setOutput(BehaviorModuleOutput & output) const;
void setStopPath(BehaviorModuleOutput & output) const;
void updatePreviousData(const BehaviorModuleOutput & output);

/**
* @brief Sets a stop path in the current path based on safety conditions and previous paths.
Expand Down
1 change: 1 addition & 0 deletions ..._path_planner/include/behavior_path_planner/utils/goal_planner/pull_over_planner_base.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -53,6 +53,7 @@ struct PullOverPath
Pose end_pose{};
std::vector<Pose> debug_poses{};
size_t goal_id{};
size_t id{};
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

id for path_candidates?

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

yes

bool decided_velocity{false};

PathWithLaneId getFullPath() const
Expand Down
Loading
Loading