Skip to content

Commit

Permalink
feat(behavior_path): use multi thread (#45)
Browse files Browse the repository at this point in the history 
* feat(behavior_path): use multi thread

Signed-off-by: tomoya.kimura <tomoya.kimura@tier4.jp>

* fix double unlock of mutex

Signed-off-by: tomoya.kimura <tomoya.kimura@tier4.jp>

* fix multi access to planner_data_

Signed-off-by: tomoya.kimura <tomoya.kimura@tier4.jp>

* rename mutex

Signed-off-by: tomoya.kimura <tomoya.kimura@tier4.jp>

* apply mutex to bt_manager_

Signed-off-by: tomoya.kimura <tomoya.kimura@tier4.jp>
  • Loading branch information
@tkimura4
tkimura4 authored May 19, 2022
1 parent a1c975a commit 172550a
Show file tree
Hide file tree
Showing 2 changed files with 51 additions and 27 deletions.
18 changes: 10 additions & 8 deletions planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -98,6 +98,9 @@ class BehaviorPathPlannerNode : public rclcpp::Node

TurnSignalDecider turn_signal_decider_;

std::mutex mutex_pd_; // mutex for planner_data_
std::mutex mutex_bt_; // mutex for bt_manager_

// setup
void waitForData();

Expand Down Expand Up @@ -135,22 +138,21 @@ class BehaviorPathPlannerNode : public rclcpp::Node
/**
* @brief extract path from behavior tree output
*/
PathWithLaneId::SharedPtr getPath(const BehaviorModuleOutput & bt_out);
PathWithLaneId::SharedPtr getPath(
const BehaviorModuleOutput & bt_out, const std::shared_ptr<PlannerData> planner_data);

/**
* @brief extract path candidate from behavior tree output
*/
PathWithLaneId::SharedPtr getPathCandidate(const BehaviorModuleOutput & bt_out);
PathWithLaneId::SharedPtr getPathCandidate(
const BehaviorModuleOutput & bt_out, const std::shared_ptr<PlannerData> planner_data);

/**
* @brief publish behavior module status mainly for the user interface
*/
void publishModuleStatus(const std::vector<std::shared_ptr<SceneModuleStatus>> & statuses);

/**
* @brief update current pose on the planner_data_
*/
void updateCurrentPose();
void publishModuleStatus(
const std::vector<std::shared_ptr<SceneModuleStatus>> & statuses,
const std::shared_ptr<PlannerData> planner_data);

// debug

Expand Down
60 changes: 41 additions & 19 deletions planning/behavior_path_planner/src/behavior_path_planner_node.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -84,6 +84,8 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod

// behavior tree manager
{
mutex_bt_.lock();

bt_manager_ = std::make_shared<BehaviorTreeManager>(*this, getBehaviorTreeManagerParam());

auto side_shift_module =
Expand Down Expand Up @@ -117,6 +119,8 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod
bt_manager_->registerSceneModule(pull_out_module);

bt_manager_->createBehaviorTree();

mutex_bt_.unlock();
}

// turn signal decider
Expand Down Expand Up @@ -427,10 +431,13 @@ BehaviorTreeManagerParam BehaviorPathPlannerNode::getBehaviorTreeManagerParam()
void BehaviorPathPlannerNode::waitForData()
{
// wait until mandatory data is ready
mutex_pd_.lock(); // for planner_data_
while (!planner_data_->route_handler->isHandlerReady() && rclcpp::ok()) {
mutex_pd_.unlock();
RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 5000, "waiting for route to be ready");
rclcpp::spin_some(this->get_node_base_interface());
rclcpp::Rate(100).sleep();
mutex_pd_.lock();
}

while (rclcpp::ok()) {
Expand All @@ -442,26 +449,38 @@ void BehaviorPathPlannerNode::waitForData()
"waiting for vehicle pose, vehicle_velocity, and obstacles");
rclcpp::spin_some(this->get_node_base_interface());
rclcpp::Rate(100).sleep();
mutex_pd_.lock();
}

self_pose_listener_.waitForFirstPose();
planner_data_->self_pose = self_pose_listener_.getCurrentPose();
mutex_pd_.unlock();
}

void BehaviorPathPlannerNode::run()
{
RCLCPP_DEBUG(get_logger(), "----- BehaviorPathPlannerNode start -----");
mutex_bt_.lock(); // for bt_manager_
mutex_pd_.lock(); // for planner_data_

// update planner data
updateCurrentPose();
planner_data_->self_pose = self_pose_listener_.getCurrentPose();

// NOTE: planner_data must not be referenced for multithreading
const auto planner_data = planner_data_;
mutex_pd_.unlock();

// run behavior planner
const auto output = bt_manager_->run(planner_data_);
const auto output = bt_manager_->run(planner_data);

// path handling
const auto path = getPath(output);
const auto path_candidate = getPathCandidate(output);
const auto path = getPath(output, planner_data);
const auto path_candidate = getPathCandidate(output, planner_data);

// update planner data
mutex_pd_.lock(); // for planner_data_
planner_data_->prev_output_path = path;
mutex_pd_.unlock();

auto clipped_path = modifyPathForSmoothGoalConnection(*path);
clipPathLength(clipped_path);
Expand All @@ -485,7 +504,7 @@ void BehaviorPathPlannerNode::run()
hazard_signal.command = output.turn_signal_info.hazard_signal.command;
} else {
turn_signal = turn_signal_decider_.getTurnSignal(
*path, planner_data_->self_pose->pose, *(planner_data_->route_handler),
*path, planner_data->self_pose->pose, *(planner_data->route_handler),
output.turn_signal_info.turn_signal, output.turn_signal_info.signal_distance);
hazard_signal.command = HazardLightsCommand::DISABLE;
}
Expand All @@ -496,31 +515,33 @@ void BehaviorPathPlannerNode::run()
}

// for remote operation
publishModuleStatus(bt_manager_->getModulesStatus());
publishModuleStatus(bt_manager_->getModulesStatus(), planner_data);

publishDebugMarker(bt_manager_->getDebugMarkers());

mutex_bt_.unlock();
RCLCPP_DEBUG(get_logger(), "----- behavior path planner end -----\n\n");
}

PathWithLaneId::SharedPtr BehaviorPathPlannerNode::getPath(const BehaviorModuleOutput & bt_output)
PathWithLaneId::SharedPtr BehaviorPathPlannerNode::getPath(
const BehaviorModuleOutput & bt_output, const std::shared_ptr<PlannerData> planner_data)
{
// TODO(Horibe) do some error handling when path is not available.

auto path = bt_output.path ? bt_output.path : planner_data_->prev_output_path;
path->header = planner_data_->route_handler->getRouteHeader();
auto path = bt_output.path ? bt_output.path : planner_data->prev_output_path;
path->header = planner_data->route_handler->getRouteHeader();
path->header.stamp = this->now();
RCLCPP_DEBUG(
get_logger(), "BehaviorTreeManager: output is %s.", bt_output.path ? "FOUND" : "NOT FOUND");
return path;
}

PathWithLaneId::SharedPtr BehaviorPathPlannerNode::getPathCandidate(
const BehaviorModuleOutput & bt_output)
const BehaviorModuleOutput & bt_output, const std::shared_ptr<PlannerData> planner_data)
{
auto path_candidate =
bt_output.path_candidate ? bt_output.path_candidate : std::make_shared<PathWithLaneId>();
path_candidate->header = planner_data_->route_handler->getRouteHeader();
path_candidate->header = planner_data->route_handler->getRouteHeader();
path_candidate->header.stamp = this->now();
RCLCPP_DEBUG(
get_logger(), "BehaviorTreeManager: path candidate is %s.",
Expand All @@ -529,7 +550,8 @@ PathWithLaneId::SharedPtr BehaviorPathPlannerNode::getPathCandidate(
}

void BehaviorPathPlannerNode::publishModuleStatus(
const std::vector<std::shared_ptr<SceneModuleStatus>> & statuses)
const std::vector<std::shared_ptr<SceneModuleStatus>> & statuses,
const std::shared_ptr<PlannerData> planner_data)
{
auto getModuleType = [](std::string name) {
if (name == "LaneChange") {
Expand Down Expand Up @@ -561,7 +583,7 @@ void BehaviorPathPlannerNode::publishModuleStatus(
running_modules.modules.push_back(module);
}
if (status->module_name == "LaneChange") {
const auto force_approval = planner_data_->approval.is_force_approved;
const auto force_approval = planner_data->approval.is_force_approved;
if (
force_approval.module_name == "ForceLaneChange" &&
(now - force_approval.stamp).seconds() < 0.5) {
Expand Down Expand Up @@ -611,28 +633,26 @@ void BehaviorPathPlannerNode::publishDebugMarker(const std::vector<MarkerArray>
debug_marker_publisher_->publish(msg);
}

void BehaviorPathPlannerNode::updateCurrentPose()
{
auto self_pose = self_pose_listener_.getCurrentPose();
planner_data_->self_pose = self_pose;
}

void BehaviorPathPlannerNode::onVelocity(const Odometry::ConstSharedPtr msg)
{
std::lock_guard<std::mutex> lock(mutex_pd_);
planner_data_->self_odometry = msg;
}
void BehaviorPathPlannerNode::onPerception(const PredictedObjects::ConstSharedPtr msg)
{
std::lock_guard<std::mutex> lock(mutex_pd_);
planner_data_->dynamic_object = msg;
}
void BehaviorPathPlannerNode::onExternalApproval(const ApprovalMsg::ConstSharedPtr msg)
{
std::lock_guard<std::mutex> lock(mutex_pd_);
planner_data_->approval.is_approved.data = msg->approval;
// TODO(wep21): Replace msg stamp after {stamp: now} is implemented in ros2 topic pub
planner_data_->approval.is_approved.stamp = this->now();
}
void BehaviorPathPlannerNode::onForceApproval(const PathChangeModule::ConstSharedPtr msg)
{
std::lock_guard<std::mutex> lock(mutex_pd_);
auto getModuleName = [](PathChangeModuleId module) {
if (module.type == PathChangeModuleId::FORCE_LANE_CHANGE) {
return "ForceLaneChange";
Expand All @@ -645,10 +665,12 @@ void BehaviorPathPlannerNode::onForceApproval(const PathChangeModule::ConstShare
}
void BehaviorPathPlannerNode::onMap(const HADMapBin::ConstSharedPtr msg)
{
std::lock_guard<std::mutex> lock(mutex_pd_);
planner_data_->route_handler->setMap(*msg);
}
void BehaviorPathPlannerNode::onRoute(const HADMapRoute::ConstSharedPtr msg)
{
std::lock_guard<std::mutex> lock(mutex_pd_);
const bool is_first_time = !(planner_data_->route_handler->isHandlerReady());

planner_data_->route_handler->setRoute(*msg);
Expand Down

0 comments on commit 172550a

Please sign in to comment.