Skip to content

Commit

Permalink
Merge pull request #1128 from tier4/fix/threads_v0.20.1
Browse files Browse the repository at this point in the history 
fix(goal_planner): fix multithread and magin
  • Loading branch information
@shmpwk
shmpwk authored Feb 6, 2024
2 parents b632aeb + 8c0de1c commit a352835
Show file tree
Hide file tree
Showing 4 changed files with 109 additions and 32 deletions.
45 changes: 45 additions & 0 deletions ...ath_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -37,6 +37,7 @@
#include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
#include <autoware_auto_vehicle_msgs/msg/hazard_lights_command.hpp>

#include <atomic>
#include <deque>
#include <limits>
#include <memory>
Expand Down Expand Up @@ -290,6 +291,35 @@ class GoalPlannerModule : public SceneModuleInterface
std::unordered_map<std::string, std::shared_ptr<ObjectsOfInterestMarkerInterface>> &
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<std::shared_ptr<GoalPlannerParameters>>(parameters);
Expand Down Expand Up @@ -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<bool> & flag) : flag_(flag) { flag_.store(true); }

~ScopedFlag() { flag_.store(false); }

private:
std::atomic<bool> & flag_;
};

/*
* state transitions and plan function used in each state
*
Expand Down Expand Up @@ -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<bool> 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<bool> is_freespace_parking_cb_running_;

// debug
mutable GoalPlannerDebugData debug_data_;
Expand Down Expand Up @@ -493,6 +537,7 @@ class GoalPlannerModule : public SceneModuleInterface
std::optional<BehaviorModuleOutput> 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();
Expand Down
61 changes: 30 additions & 31 deletions planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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{};
Expand Down Expand Up @@ -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;
}
Expand All @@ -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;
Expand Down Expand Up @@ -276,6 +291,8 @@ void GoalPlannerModule::onTimer()

void GoalPlannerModule::onFreespaceParkingTimer()
{
const ScopedFlag flag(is_freespace_parking_cb_running_);

if (getCurrentStatus() == ModuleStatus::IDLE) {
return;
}
Expand Down Expand Up @@ -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<Polygon2d> ego_polygons_expanded{};
const auto curvatures = motion_utils::calcCurvature(path.points);
Expand All @@ -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);
}

Expand Down
30 changes: 30 additions & 0 deletions ..._start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -35,6 +35,7 @@

#include <tf2/utils.h>

#include <atomic>
#include <deque>
#include <memory>
#include <string>
Expand Down Expand Up @@ -83,6 +84,21 @@ class StartPlannerModule : public SceneModuleInterface
std::unordered_map<std::string, std::shared_ptr<ObjectsOfInterestMarkerInterface>> &
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<std::shared_ptr<StartPlannerParameters>>(parameters);
Expand Down Expand Up @@ -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<bool> & flag) : flag_(flag) { flag_.store(true); }

~ScopedFlag() { flag_.store(false); }

private:
std::atomic<bool> & flag_;
};

bool canTransitSuccessState() override;

bool canTransitFailureState() override { return false; }
Expand Down Expand Up @@ -202,6 +230,8 @@ class StartPlannerModule : public SceneModuleInterface
std::unique_ptr<PullOutPlannerBase> freespace_planner_;
rclcpp::TimerBase::SharedPtr freespace_planner_timer_;
rclcpp::CallbackGroup::SharedPtr freespace_planner_timer_cb_group_;
std::atomic<bool> 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.
Expand Down
5 changes: 4 additions & 1 deletion planning/behavior_path_start_planner_module/src/start_planner_module.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<LaneDepartureChecker>();
lane_departure_checker_->setVehicleInfo(vehicle_info_);
Expand Down Expand Up @@ -85,6 +86,8 @@ StartPlannerModule::StartPlannerModule(

void StartPlannerModule::onFreespacePlannerTimer()
{
const ScopedFlag flag(is_freespace_planner_cb_running_);

if (getCurrentStatus() == ModuleStatus::IDLE) {
return;
}
Expand Down

0 comments on commit a352835

Please sign in to comment.