Skip to content

Commit

Permalink
refactor(goal_planner): remove unused header and divide ThreadSafeDat…
Browse files Browse the repository at this point in the history 
…a to another file (autowarefoundation#8990)

Signed-off-by: Mamoru Sobue <[email protected]>
  • Loading branch information
@soblin
soblin authored Oct 1, 2024
1 parent 1d4e949 commit 35c4f59
Show file tree
Hide file tree
Showing 14 changed files with 316 additions and 265 deletions.
87 changes: 87 additions & 0 deletions ...goal_planner_module/include/autoware/behavior_path_goal_planner_module/decision_state.hpp
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,87 @@
// Copyright 2024 Tier IV, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#ifndef AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__DECISION_STATE_HPP_
#define AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__DECISION_STATE_HPP_

#include "autoware/behavior_path_goal_planner_module/goal_searcher_base.hpp"
#include "autoware/behavior_path_goal_planner_module/pull_over_planner/pull_over_planner_base.hpp"
#include "autoware/behavior_path_planner_common/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp"

#include <memory>
#include <vector>

namespace autoware::behavior_path_planner
{

class PathDecisionState
{
public:
enum class DecisionKind {
NOT_DECIDED,
DECIDING,
DECIDED,
};

DecisionKind state{DecisionKind::NOT_DECIDED};
rclcpp::Time stamp{};
bool is_stable_safe{false};
std::optional<rclcpp::Time> safe_start_time{std::nullopt};
};

class PathDecisionStateController
{
public:
PathDecisionStateController() = default;

/**
* @brief update current state and save old current state to prev state
*/
void transit_state(
const bool found_pull_over_path, const rclcpp::Time & now,
const autoware_perception_msgs::msg::PredictedObjects & static_target_objects,
const autoware_perception_msgs::msg::PredictedObjects & dynamic_target_objects,
const std::optional<GoalCandidate> modified_goal_opt,
const std::shared_ptr<const PlannerData> planner_data,
const std::shared_ptr<OccupancyGridBasedCollisionDetector> occupancy_grid_map,
const bool is_current_safe, const GoalPlannerParameters & parameters,
const std::shared_ptr<GoalSearcherBase> goal_searcher, const bool is_activated,
const std::optional<PullOverPath> & pull_over_path,
std::vector<autoware::universe_utils::Polygon2d> & ego_polygons_expanded);

PathDecisionState get_current_state() const { return current_state_; }

PathDecisionState get_prev_state() const { return prev_state_; }

private:
PathDecisionState current_state_{};
PathDecisionState prev_state_{};

/**
* @brief update current state and save old current state to prev state
*/
PathDecisionState get_next_state(
const bool found_pull_over_path, const rclcpp::Time & now,
const PredictedObjects & static_target_objects, const PredictedObjects & dynamic_target_objects,
const std::optional<GoalCandidate> modified_goal_opt,
const std::shared_ptr<const PlannerData> planner_data,
const std::shared_ptr<OccupancyGridBasedCollisionDetector> occupancy_grid_map,
const bool is_current_safe, const GoalPlannerParameters & parameters,
const std::shared_ptr<GoalSearcherBase> goal_searcher, const bool is_activated,
const std::optional<PullOverPath> & pull_over_path_opt,
std::vector<autoware::universe_utils::Polygon2d> & ego_polygons_expanded) const;
};

} // namespace autoware::behavior_path_planner
#endif // AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__DECISION_STATE_HPP_
238 changes: 2 additions & 236 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 @@ -15,24 +15,15 @@
#ifndef AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__GOAL_PLANNER_MODULE_HPP_
#define AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__GOAL_PLANNER_MODULE_HPP_

#include "autoware/behavior_path_goal_planner_module/default_fixed_goal_planner.hpp"
#include "autoware/behavior_path_goal_planner_module/fixed_goal_planner_base.hpp"
#include "autoware/behavior_path_goal_planner_module/goal_planner_parameters.hpp"
#include "autoware/behavior_path_goal_planner_module/goal_searcher.hpp"
#include "autoware/behavior_path_goal_planner_module/pull_over_planner/freespace_pull_over.hpp"
#include "autoware/behavior_path_goal_planner_module/pull_over_planner/geometric_pull_over.hpp"
#include "autoware/behavior_path_goal_planner_module/pull_over_planner/shift_pull_over.hpp"
#include "autoware/behavior_path_goal_planner_module/thread_data.hpp"
#include "autoware/behavior_path_planner_common/interface/scene_module_interface.hpp"
#include "autoware/behavior_path_planner_common/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp"
#include "autoware/behavior_path_planner_common/utils/parking_departure/common_module_data.hpp"
#include "autoware/behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp"
#include "autoware/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp"
#include "autoware/behavior_path_planner_common/utils/utils.hpp"

#include <autoware/freespace_planning_algorithms/astar_search.hpp>
#include <autoware/freespace_planning_algorithms/rrtstar.hpp>
#include <autoware/lane_departure_checker/lane_departure_checker.hpp>
#include <autoware/motion_utils/distance/distance.hpp>
#include <autoware/universe_utils/geometry/boost_geometry.hpp>

#include <autoware_vehicle_msgs/msg/hazard_lights_command.hpp>
#include <tier4_planning_msgs/msg/path_with_lane_id.hpp>
Expand All @@ -41,7 +32,6 @@

#include <atomic>
#include <deque>
#include <limits>
#include <memory>
#include <string>
#include <unordered_map>
Expand All @@ -61,8 +51,6 @@ using autoware::freespace_planning_algorithms::AbstractPlanningAlgorithm;
using autoware::freespace_planning_algorithms::AstarParam;
using autoware::freespace_planning_algorithms::AstarSearch;
using autoware::freespace_planning_algorithms::PlannerCommonParam;
using autoware::freespace_planning_algorithms::RRTStar;
using autoware::freespace_planning_algorithms::RRTStarParam;

using autoware::behavior_path_planner::utils::path_safety_checker::EgoPredictedPathParams;
using autoware::behavior_path_planner::utils::path_safety_checker::ObjectsFilteringParams;
Expand All @@ -71,186 +59,6 @@ using autoware::behavior_path_planner::utils::path_safety_checker::SafetyCheckPa
using autoware::behavior_path_planner::utils::path_safety_checker::TargetObjectsOnLane;
using autoware::universe_utils::Polygon2d;

#define DEFINE_SETTER_WITH_MUTEX(TYPE, NAME) \
public: \
void set_##NAME(const TYPE & value) \
{ \
const std::lock_guard<std::recursive_mutex> lock(mutex_); \
NAME##_ = value; \
}

#define DEFINE_GETTER_WITH_MUTEX(TYPE, NAME) \
public: \
TYPE get_##NAME() const \
{ \
const std::lock_guard<std::recursive_mutex> lock(mutex_); \
return NAME##_; \
}

#define DEFINE_SETTER_GETTER_WITH_MUTEX(TYPE, NAME) \
DEFINE_SETTER_WITH_MUTEX(TYPE, NAME) \
DEFINE_GETTER_WITH_MUTEX(TYPE, NAME)

class PathDecisionState
{
public:
enum class DecisionKind {
NOT_DECIDED,
DECIDING,
DECIDED,
};

DecisionKind state{DecisionKind::NOT_DECIDED};
rclcpp::Time stamp{};
bool is_stable_safe{false};
std::optional<rclcpp::Time> safe_start_time{std::nullopt};
};

class ThreadSafeData
{
public:
ThreadSafeData(std::recursive_mutex & mutex, rclcpp::Clock::SharedPtr clock)
: mutex_(mutex), clock_(clock)
{
}

bool incrementPathIndex()
{
const std::lock_guard<std::recursive_mutex> lock(mutex_);
if (!pull_over_path_) {
return false;
}

if (pull_over_path_->incrementPathIndex()) {
last_path_idx_increment_time_ = clock_->now();
return true;
}
return false;
}

void set_pull_over_path(const PullOverPath & path)
{
const std::lock_guard<std::recursive_mutex> lock(mutex_);
set_pull_over_path_no_lock(path);
}

void set_pull_over_path(const std::shared_ptr<PullOverPath> & path)
{
const std::lock_guard<std::recursive_mutex> lock(mutex_);
set_pull_over_path_no_lock(path);
}

template <typename... Args>
void set(Args... args)
{
std::lock_guard<std::recursive_mutex> lock(mutex_);
(..., set_no_lock(args));
}

void clearPullOverPath()
{
const std::lock_guard<std::recursive_mutex> lock(mutex_);
pull_over_path_ = nullptr;
}

bool foundPullOverPath() const
{
const std::lock_guard<std::recursive_mutex> lock(mutex_);
if (!pull_over_path_) {
return false;
}

return true;
}

std::optional<PullOverPlannerType> getPullOverPlannerType() const
{
const std::lock_guard<std::recursive_mutex> lock(mutex_);
if (!pull_over_path_) {
return std::nullopt;
}
return pull_over_path_->type();
};

void reset()
{
const std::lock_guard<std::recursive_mutex> lock(mutex_);
pull_over_path_ = nullptr;
pull_over_path_candidates_.clear();
goal_candidates_.clear();
modified_goal_pose_ = std::nullopt;
last_path_update_time_ = std::nullopt;
last_path_idx_increment_time_ = std::nullopt;
closest_start_pose_ = std::nullopt;
last_previous_module_output_ = std::nullopt;
prev_data_ = PathDecisionState{};
}

DEFINE_GETTER_WITH_MUTEX(std::shared_ptr<PullOverPath>, pull_over_path)
DEFINE_GETTER_WITH_MUTEX(std::shared_ptr<PullOverPath>, lane_parking_pull_over_path)
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)

DEFINE_SETTER_GETTER_WITH_MUTEX(std::vector<PullOverPath>, pull_over_path_candidates)
DEFINE_SETTER_GETTER_WITH_MUTEX(GoalCandidates, goal_candidates)
DEFINE_SETTER_GETTER_WITH_MUTEX(std::optional<GoalCandidate>, modified_goal_pose)
DEFINE_SETTER_GETTER_WITH_MUTEX(std::optional<Pose>, closest_start_pose)
DEFINE_SETTER_GETTER_WITH_MUTEX(std::optional<BehaviorModuleOutput>, last_previous_module_output)
DEFINE_SETTER_GETTER_WITH_MUTEX(CollisionCheckDebugMap, collision_check)
DEFINE_SETTER_GETTER_WITH_MUTEX(PredictedObjects, static_target_objects)
DEFINE_SETTER_GETTER_WITH_MUTEX(PredictedObjects, dynamic_target_objects)
DEFINE_SETTER_GETTER_WITH_MUTEX(PathDecisionState, prev_data)

private:
void set_pull_over_path_no_lock(const PullOverPath & path)
{
pull_over_path_ = std::make_shared<PullOverPath>(path);
if (path.type() != PullOverPlannerType::FREESPACE) {
lane_parking_pull_over_path_ = std::make_shared<PullOverPath>(path);
}

last_path_update_time_ = clock_->now();
}

void set_pull_over_path_no_lock(const std::shared_ptr<PullOverPath> & path)
{
pull_over_path_ = path;
if (path->type() != PullOverPlannerType::FREESPACE) {
lane_parking_pull_over_path_ = path;
}
last_path_update_time_ = clock_->now();
}

void set_no_lock(const GoalCandidates & arg) { goal_candidates_ = arg; }
void set_no_lock(const std::vector<PullOverPath> & arg) { pull_over_path_candidates_ = arg; }
void set_no_lock(const std::shared_ptr<PullOverPath> & arg) { set_pull_over_path_no_lock(arg); }
void set_no_lock(const PullOverPath & arg) { set_pull_over_path_no_lock(arg); }
void set_no_lock(const GoalCandidate & arg) { modified_goal_pose_ = arg; }
void set_no_lock(const BehaviorModuleOutput & arg) { last_previous_module_output_ = arg; }
void set_no_lock(const CollisionCheckDebugMap & arg) { collision_check_ = arg; }

std::shared_ptr<PullOverPath> pull_over_path_{nullptr};
std::shared_ptr<PullOverPath> lane_parking_pull_over_path_{nullptr};
std::vector<PullOverPath> pull_over_path_candidates_;
GoalCandidates goal_candidates_{};
std::optional<GoalCandidate> modified_goal_pose_;
std::optional<rclcpp::Time> last_path_update_time_;
std::optional<rclcpp::Time> last_path_idx_increment_time_;
std::optional<Pose> closest_start_pose_{};
std::optional<BehaviorModuleOutput> last_previous_module_output_{};
CollisionCheckDebugMap collision_check_{};
PredictedObjects static_target_objects_{};
PredictedObjects dynamic_target_objects_{};
PathDecisionState prev_data_{};

std::recursive_mutex & mutex_;
rclcpp::Clock::SharedPtr clock_;
};

#undef DEFINE_SETTER_WITH_MUTEX
#undef DEFINE_GETTER_WITH_MUTEX
#undef DEFINE_SETTER_GETTER_WITH_MUTEX

struct FreespacePlannerDebugData
{
bool is_planning{false};
Expand Down Expand Up @@ -313,48 +121,6 @@ struct PullOverContextData
const PredictedObjects dynamic_target_objects;
};

class PathDecisionStateController
{
public:
PathDecisionStateController() = default;

/**
* @brief update current state and save old current state to prev state
*/
void transit_state(
const bool found_pull_over_path, const rclcpp::Time & now,
const PredictedObjects & static_target_objects, const PredictedObjects & dynamic_target_objects,
const std::optional<GoalCandidate> modified_goal_opt,
const std::shared_ptr<const PlannerData> planner_data,
const std::shared_ptr<OccupancyGridBasedCollisionDetector> occupancy_grid_map,
const bool is_current_safe, const GoalPlannerParameters & parameters,
const std::shared_ptr<GoalSearcherBase> goal_searcher, const bool is_activated,
const std::optional<PullOverPath> & pull_over_path,
std::vector<Polygon2d> & ego_polygons_expanded);

PathDecisionState get_current_state() const { return current_state_; }

PathDecisionState get_prev_state() const { return prev_state_; }

private:
PathDecisionState current_state_{};
PathDecisionState prev_state_{};

/**
* @brief update current state and save old current state to prev state
*/
PathDecisionState get_next_state(
const bool found_pull_over_path, const rclcpp::Time & now,
const PredictedObjects & static_target_objects, const PredictedObjects & dynamic_target_objects,
const std::optional<GoalCandidate> modified_goal_opt,
const std::shared_ptr<const PlannerData> planner_data,
const std::shared_ptr<OccupancyGridBasedCollisionDetector> occupancy_grid_map,
const bool is_current_safe, const GoalPlannerParameters & parameters,
const std::shared_ptr<GoalSearcherBase> goal_searcher, const bool is_activated,
const std::optional<PullOverPath> & pull_over_path_opt,
std::vector<Polygon2d> & ego_polygons_expanded) const;
};

class GoalPlannerModule : public SceneModuleInterface
{
public:
Expand Down
1 change: 0 additions & 1 deletion ..._goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_searcher.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -16,7 +16,6 @@
#define AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__GOAL_SEARCHER_HPP_

#include "autoware/behavior_path_goal_planner_module/goal_searcher_base.hpp"
#include "autoware/behavior_path_planner_common/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp"

#include <memory>
#include <vector>
Expand Down
9 changes: 2 additions & 7 deletions ...r_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/manager.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,7 +15,7 @@
#ifndef AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__MANAGER_HPP_
#define AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__MANAGER_HPP_

#include "autoware/behavior_path_goal_planner_module/goal_planner_module.hpp"
#include "autoware/behavior_path_goal_planner_module/goal_planner_parameters.hpp"
#include "autoware/behavior_path_planner_common/interface/scene_module_manager_interface.hpp"

#include <rclcpp/rclcpp.hpp>
Expand All @@ -38,12 +38,7 @@ class GoalPlannerModuleManager : public SceneModuleManagerInterface

void init(rclcpp::Node * node) override;

std::unique_ptr<SceneModuleInterface> createNewSceneModuleInstance() override
{
return std::make_unique<GoalPlannerModule>(
name_, *node_, parameters_, rtc_interface_ptr_map_,
objects_of_interest_marker_interface_ptr_map_, steering_factor_interface_ptr_);
}
std::unique_ptr<SceneModuleInterface> createNewSceneModuleInstance() override;

void updateModuleParams(const std::vector<rclcpp::Parameter> & parameters) override;

Expand Down
Loading

0 comments on commit 35c4f59

Please sign in to comment.