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

refactor(goal_planner): reduce call to isSafePath #8812

Merged
merged 8 commits into from
Sep 10, 2024
Merged

refactor(goal_planner): reduce call to isSafePath #8812

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
8 commits
Select commit Hold shift + click to select a range
23615eb
feat(map_loader): visualize BusStopArea
soblin Sep 4, 2024
958c2fd
feat(goal_planner): add getBusStopArea function
soblin Sep 6, 2024
c88e8ab
add bus_stop_area param
soblin Sep 6, 2024
2636737
add bus_stop_area param
soblin Sep 6, 2024
d42f6fd
save isSafePath to PullOverContextData
soblin Sep 9, 2024
bc83821
migrate
soblin Sep 10, 2024
9f51250
fix spell
soblin Sep 10, 2024
3f6db4b
reflect comment
soblin Sep 10, 2024
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
41 changes: 23 additions & 18 deletions ...planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -309,6 +309,17 @@ struct PoseWithString
}
};

struct PullOverContextData
{
PullOverContextData() = delete;
explicit PullOverContextData(const std::pair<bool, bool> & is_safe_path)
: is_safe_path_latched(is_safe_path.first), is_safe_path_instant(is_safe_path.second)
{
}
bool is_safe_path_latched{true};
bool is_safe_path_instant{true};
};

class GoalPlannerModule : public SceneModuleInterface
{
public:
Expand Down Expand Up @@ -505,6 +516,9 @@ class GoalPlannerModule : public SceneModuleInterface
// TODO(Mamoru Sobue): isSafePath() modifies ThreadSafeData::check_collision, avoid this mutable
mutable ThreadSafeData thread_safe_data_;

// TODO(soblin): organize part of thread_safe_data and previous data to PullOverContextData
Copy link
Contributor

Choose a reason for hiding this comment

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

agree!

struct PreviousPullOverData
{
  struct SafetyStatus
  {
    std::optional<rclcpp::Time> safe_start_time{};
    bool is_safe{false};
  };

  void reset()
  {
    safety_status = SafetyStatus{};
    deciding_path_status = DecidingPathStatusWithStamp{};
  }

  SafetyStatus safety_status{};
  DecidingPathStatusWithStamp deciding_path_status{};
};

is similar to ContextData

Copy link
Contributor Author

Choose a reason for hiding this comment

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

I will move PreviousPullOverData later in coming PRs.

// context_data_ is initialized in updateData(), used in plan() and refreshed in postProcess()
std::optional<PullOverContextData> context_data_{std::nullopt};
std::unique_ptr<LastApprovalData> last_approval_data_{nullptr};

// approximate distance from the start point to the end point of pull_over.
Expand Down Expand Up @@ -572,26 +586,17 @@ class GoalPlannerModule : public SceneModuleInterface
bool hasDecidedPath(
const std::shared_ptr<const PlannerData> planner_data,
const std::shared_ptr<OccupancyGridBasedCollisionDetector> occupancy_grid_map,
const GoalPlannerParameters & parameters,
const std::shared_ptr<EgoPredictedPathParams> & ego_predicted_path_params,
const std::shared_ptr<ObjectsFilteringParams> & objects_filtering_params,
const std::shared_ptr<SafetyCheckParams> & safety_check_params,
const PullOverContextData & context_data, const GoalPlannerParameters & parameters,
const std::shared_ptr<GoalSearcherBase> goal_searcher) const;
bool hasNotDecidedPath(
const std::shared_ptr<const PlannerData> planner_data,
const std::shared_ptr<OccupancyGridBasedCollisionDetector> occupancy_grid_map,
const GoalPlannerParameters & parameters,
const std::shared_ptr<EgoPredictedPathParams> & ego_predicted_path_params,
const std::shared_ptr<ObjectsFilteringParams> & objects_filtering_params,
const std::shared_ptr<SafetyCheckParams> & safety_check_params,
const PullOverContextData & context_data, const GoalPlannerParameters & parameters,
const std::shared_ptr<GoalSearcherBase> goal_searcher) const;
DecidingPathStatusWithStamp checkDecidingPathStatus(
const std::shared_ptr<const PlannerData> planner_data,
const std::shared_ptr<OccupancyGridBasedCollisionDetector> occupancy_grid_map,
const GoalPlannerParameters & parameters,
const std::shared_ptr<EgoPredictedPathParams> & ego_predicted_path_params,
const std::shared_ptr<ObjectsFilteringParams> & objects_filtering_params,
const std::shared_ptr<SafetyCheckParams> & safety_check_params,
const PullOverContextData & context_data, const GoalPlannerParameters & parameters,
const std::shared_ptr<GoalSearcherBase> goal_searcher) const;
void decideVelocity();
bool foundPullOverPath() const;
Expand All @@ -615,9 +620,9 @@ class GoalPlannerModule : public SceneModuleInterface
bool canReturnToLaneParking();

// plan pull over path
BehaviorModuleOutput planPullOver();
BehaviorModuleOutput planPullOverAsOutput();
BehaviorModuleOutput planPullOverAsCandidate();
BehaviorModuleOutput planPullOver(const PullOverContextData & context_data);
BehaviorModuleOutput planPullOverAsOutput(const PullOverContextData & context_data);
BehaviorModuleOutput planPullOverAsCandidate(const PullOverContextData & context_data);
std::optional<std::pair<PullOverPath, GoalCandidate>> selectPullOverPath(
const std::vector<PullOverPath> & pull_over_path_candidates,
const GoalCandidates & goal_candidates) const;
Expand All @@ -627,8 +632,8 @@ class GoalPlannerModule : public SceneModuleInterface
void setDrivableAreaInfo(BehaviorModuleOutput & output) const;

// output setter
void setOutput(BehaviorModuleOutput & output);
void updatePreviousData();
void setOutput(const PullOverContextData & context_data, BehaviorModuleOutput & output);
void updatePreviousData(const PullOverContextData & context_data);

void setModifiedGoal(BehaviorModuleOutput & output) const;
void setTurnSignalInfo(BehaviorModuleOutput & output);
Expand Down Expand Up @@ -676,7 +681,7 @@ class GoalPlannerModule : public SceneModuleInterface
const std::shared_ptr<SafetyCheckParams> & safety_check_params) const;

// debug
void setDebugData();
void setDebugData(const PullOverContextData & context_data);
void printParkingPositionError() const;
};
} // namespace autoware::behavior_path_planner
Expand Down
Loading
Loading