diff --git a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp index 2c839f582be12..30827945b7f9e 100644 --- a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp +++ b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp @@ -37,6 +37,7 @@ #include #include +#include #include #include #include @@ -290,6 +291,35 @@ class GoalPlannerModule : public SceneModuleInterface std::unordered_map> & objects_of_interest_marker_interface_ptr_map); + ~GoalPlannerModule() + { + if (lane_parking_timer_) { + lane_parking_timer_->cancel(); + } + if (freespace_parking_timer_) { + freespace_parking_timer_->cancel(); + } + + while (is_lane_parking_cb_running_.load() || is_freespace_parking_cb_running_.load()) { + const std::string running_callbacks = std::invoke([&]() { + if (is_lane_parking_cb_running_ && is_freespace_parking_cb_running_) { + return "lane parking and freespace parking"; + } + if (is_lane_parking_cb_running_) { + return "lane parking"; + } + return "freespace parking"; + }); + RCLCPP_INFO_THROTTLE( + getLogger(), *clock_, 1000, "Waiting for %s callback to finish...", + running_callbacks.c_str()); + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + + RCLCPP_INFO_THROTTLE( + getLogger(), *clock_, 1000, "lane parking and freespace parking callbacks finished"); + } + void updateModuleParams(const std::any & parameters) override { parameters_ = std::any_cast>(parameters); @@ -330,6 +360,18 @@ class GoalPlannerModule : public SceneModuleInterface CandidateOutput planCandidate() const override { return CandidateOutput{}; } private: + // Flag class for managing whether a certain callback is running in multi-threading + class ScopedFlag + { + public: + explicit ScopedFlag(std::atomic & flag) : flag_(flag) { flag_.store(true); } + + ~ScopedFlag() { flag_.store(false); } + + private: + std::atomic & flag_; + }; + /* * state transitions and plan function used in each state * @@ -404,10 +446,12 @@ class GoalPlannerModule : public SceneModuleInterface // pre-generate lane parking paths in a separate thread rclcpp::TimerBase::SharedPtr lane_parking_timer_; rclcpp::CallbackGroup::SharedPtr lane_parking_timer_cb_group_; + std::atomic is_lane_parking_cb_running_; // generate freespace parking paths in a separate thread rclcpp::TimerBase::SharedPtr freespace_parking_timer_; rclcpp::CallbackGroup::SharedPtr freespace_parking_timer_cb_group_; + std::atomic is_freespace_parking_cb_running_; // debug mutable GoalPlannerDebugData debug_data_; @@ -493,6 +537,7 @@ class GoalPlannerModule : public SceneModuleInterface std::optional last_previous_module_output_{}; bool hasPreviousModulePathShapeChanged() const; bool hasDeviatedFromLastPreviousModulePath() const; + bool hasDeviatedFromCurrentPreviousModulePath() const; // timer for generating pull over path candidates in a separate thread void onTimer(); diff --git a/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp b/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp index 00810432a6f82..b6f8597752f7a 100644 --- a/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp +++ b/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp @@ -62,6 +62,8 @@ GoalPlannerModule::GoalPlannerModule( parameters_{parameters}, vehicle_info_{vehicle_info_util::VehicleInfoUtil(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_} { LaneDepartureChecker lane_departure_checker{}; @@ -161,9 +163,18 @@ bool GoalPlannerModule::hasDeviatedFromLastPreviousModulePath() const planner_data_->self_odometry->pose.pose.position)) > 0.3; } +bool GoalPlannerModule::hasDeviatedFromCurrentPreviousModulePath() const +{ + return std::abs(motion_utils::calcLateralOffset( + getPreviousModuleOutput().path.points, + planner_data_->self_odometry->pose.pose.position)) > 0.3; +} + // generate pull over candidate paths void GoalPlannerModule::onTimer() { + const ScopedFlag flag(is_lane_parking_cb_running_); + if (getCurrentStatus() == ModuleStatus::IDLE) { return; } @@ -179,15 +190,19 @@ void GoalPlannerModule::onTimer() // check if new pull over path candidates are needed to be generated const bool need_update = std::invoke([&]() { + if (hasDeviatedFromCurrentPreviousModulePath()) { + RCLCPP_DEBUG(getLogger(), "has deviated from current previous module path"); + return false; + } if (thread_safe_data_.get_pull_over_path_candidates().empty()) { return true; } if (hasPreviousModulePathShapeChanged()) { - RCLCPP_ERROR(getLogger(), "has previous module path shape changed"); + RCLCPP_DEBUG(getLogger(), "has previous module path shape changed"); return true; } if (hasDeviatedFromLastPreviousModulePath() && !hasDecidedPath()) { - RCLCPP_ERROR(getLogger(), "has deviated from last previous module path"); + RCLCPP_DEBUG(getLogger(), "has deviated from last previous module path"); return true; } return false; @@ -276,6 +291,8 @@ void GoalPlannerModule::onTimer() void GoalPlannerModule::onFreespaceParkingTimer() { + const ScopedFlag flag(is_freespace_parking_cb_running_); + if (getCurrentStatus() == ModuleStatus::IDLE) { return; } @@ -1554,27 +1571,9 @@ bool GoalPlannerModule::checkObjectsCollision( } /* Expand ego collision check polygon - * - `collision_check_margin` in all directions - * - `extra_stopping_margin` in the moving direction - * - `extra_lateral_margin` in external direction of path curve - * - * - * ^ moving direction - * x - * x - * x - * +----------------------+------x--+ - * | | x | - * | +---------------+ | xx | - * | | | | xx | - * | | ego footprint |xxxxx | - * | | | | | - * | +---------------+ | extra_stopping_margin - * | margin | | - * +----------------------+ | - * | extra_lateral_margin | - * +--------------------------------+ - * + * - `collision_check_margin` is added in all directions. + * - `extra_stopping_margin` adds stopping margin under deceleration constraints forward. + * - `extra_lateral_margin` adds the lateral margin on curves. */ std::vector ego_polygons_expanded{}; const auto curvatures = motion_utils::calcCurvature(path.points); @@ -1585,19 +1584,19 @@ bool GoalPlannerModule::checkObjectsCollision( std::pow(p.point.longitudinal_velocity_mps, 2) * 0.5 / parameters_->maximum_deceleration, parameters_->object_recognition_collision_check_max_extra_stopping_margin); - double extra_lateral_margin = (-1) * curvatures.at(i) * p.point.longitudinal_velocity_mps * - std::abs(p.point.longitudinal_velocity_mps); - extra_lateral_margin = - std::clamp(extra_lateral_margin, -extra_stopping_margin, extra_stopping_margin); + // The square is meant to imply centrifugal force, but it is not a very well-founded formula. + // TODO(kosuke55): It is needed to consider better way because there is an inherently different + // conception of the inside and outside margins. + const double extra_lateral_margin = std::min( + extra_stopping_margin, + std::abs(curvatures.at(i) * std::pow(p.point.longitudinal_velocity_mps, 2))); - const auto lateral_offset_pose = - tier4_autoware_utils::calcOffsetPose(p.point.pose, 0.0, extra_lateral_margin / 2.0, 0.0); const auto ego_polygon = tier4_autoware_utils::toFootprint( - lateral_offset_pose, + p.point.pose, planner_data_->parameters.base_link2front + collision_check_margin + extra_stopping_margin, planner_data_->parameters.base_link2rear + collision_check_margin, planner_data_->parameters.vehicle_width + collision_check_margin * 2.0 + - std::abs(extra_lateral_margin)); + extra_lateral_margin * 2.0); ego_polygons_expanded.push_back(ego_polygon); } diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp index 6ec23c8a557bf..925ed65ee665b 100644 --- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp +++ b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp @@ -35,6 +35,7 @@ #include +#include #include #include #include @@ -83,6 +84,21 @@ class StartPlannerModule : public SceneModuleInterface std::unordered_map> & objects_of_interest_marker_interface_ptr_map); + ~StartPlannerModule() + { + if (freespace_planner_timer_) { + freespace_planner_timer_->cancel(); + } + + while (is_freespace_planner_cb_running_.load()) { + RCLCPP_INFO_THROTTLE( + getLogger(), *clock_, 1000, "Waiting for freespace planner callback to finish..."); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + + RCLCPP_INFO_THROTTLE(getLogger(), *clock_, 1000, "freespace planner callback finished"); + } + void updateModuleParams(const std::any & parameters) override { parameters_ = std::any_cast>(parameters); @@ -135,6 +151,18 @@ class StartPlannerModule : public SceneModuleInterface bool isFreespacePlanning() const { return status_.planner_type == PlannerType::FREESPACE; } private: + // Flag class for managing whether a certain callback is running in multi-threading + class ScopedFlag + { + public: + explicit ScopedFlag(std::atomic & flag) : flag_(flag) { flag_.store(true); } + + ~ScopedFlag() { flag_.store(false); } + + private: + std::atomic & flag_; + }; + bool canTransitSuccessState() override; bool canTransitFailureState() override { return false; } @@ -202,6 +230,8 @@ class StartPlannerModule : public SceneModuleInterface std::unique_ptr freespace_planner_; rclcpp::TimerBase::SharedPtr freespace_planner_timer_; rclcpp::CallbackGroup::SharedPtr freespace_planner_timer_cb_group_; + std::atomic is_freespace_planner_cb_running_; + // TODO(kosuke55) // Currently, we only do lock when updating a member of status_. // However, we need to ensure that the value does not change when referring to it. diff --git a/planning/behavior_path_start_planner_module/src/start_planner_module.cpp b/planning/behavior_path_start_planner_module/src/start_planner_module.cpp index 6eda7a95bb4cb..8b133bcaca619 100644 --- a/planning/behavior_path_start_planner_module/src/start_planner_module.cpp +++ b/planning/behavior_path_start_planner_module/src/start_planner_module.cpp @@ -54,7 +54,8 @@ StartPlannerModule::StartPlannerModule( objects_of_interest_marker_interface_ptr_map) : SceneModuleInterface{name, node, rtc_interface_ptr_map, objects_of_interest_marker_interface_ptr_map}, // NOLINT parameters_{parameters}, - vehicle_info_{vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo()} + vehicle_info_{vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo()}, + is_freespace_planner_cb_running_{false} { lane_departure_checker_ = std::make_shared(); lane_departure_checker_->setVehicleInfo(vehicle_info_); @@ -85,6 +86,8 @@ StartPlannerModule::StartPlannerModule( void StartPlannerModule::onFreespacePlannerTimer() { + const ScopedFlag flag(is_freespace_planner_cb_running_); + if (getCurrentStatus() == ModuleStatus::IDLE) { return; }