Skip to content

Commit

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

Signed-off-by: tomoya.kimura <[email protected]>

* fix mutex

Signed-off-by: tomoya.kimura <[email protected]>

* fix double unlock of mutex

Signed-off-by: tomoya.kimura <[email protected]>

* fix multi access to planner_data_

Signed-off-by: tomoya.kimura <[email protected]>

* rename mutex

Signed-off-by: tomoya.kimura <[email protected]>

* apply mutex to bt_manager_

Signed-off-by: tomoya.kimura <[email protected]>

* ci(pre-commit): autofix

Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
  • Loading branch information
@tkimura4 @pre-commit-ci
tkimura4 and pre-commit-ci[bot] authored Apr 28, 2022
1 parent c7c31de commit 827ff26
Show file tree
Hide file tree
Showing 2 changed files with 101 additions and 51 deletions.
19 changes: 11 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 @@ -53,6 +53,7 @@
#include <lanelet2_traffic_rules/TrafficRulesFactory.h>

#include <memory>
#include <mutex>
#include <string>
#include <vector>

Expand Down Expand Up @@ -109,6 +110,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 @@ -146,22 +150,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
133 changes: 90 additions & 43 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 @@ -30,6 +30,20 @@
#include <utility>
#include <vector>

namespace
{
rclcpp::SubscriptionOptions createSubscriptionOptions(rclcpp::Node * node_ptr)
{
rclcpp::CallbackGroup::SharedPtr callback_group =
node_ptr->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);

auto sub_opt = rclcpp::SubscriptionOptions();
sub_opt.callback_group = callback_group;

return sub_opt;
}
} // namespace

namespace behavior_path_planner
{
using tier4_planning_msgs::msg::PathChangeModuleId;
Expand All @@ -47,28 +61,6 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod
planner_data_->parameters = getCommonParam();
}

velocity_subscriber_ = create_subscription<Odometry>(
"~/input/odometry", 1, std::bind(&BehaviorPathPlannerNode::onVelocity, this, _1));
perception_subscriber_ = create_subscription<PredictedObjects>(
"~/input/perception", 1, std::bind(&BehaviorPathPlannerNode::onPerception, this, _1));
scenario_subscriber_ = create_subscription<Scenario>(
"~/input/scenario", 1, [this](const Scenario::ConstSharedPtr msg) {
current_scenario_ = std::make_shared<Scenario>(*msg);
});
external_approval_subscriber_ = create_subscription<ApprovalMsg>(
"~/input/external_approval", 1,
std::bind(&BehaviorPathPlannerNode::onExternalApproval, this, _1));
force_approval_subscriber_ = create_subscription<PathChangeModule>(
"~/input/force_approval", 1, std::bind(&BehaviorPathPlannerNode::onForceApproval, this, _1));

// route_handler
auto qos_transient_local = rclcpp::QoS{1}.transient_local();
vector_map_subscriber_ = create_subscription<HADMapBin>(
"~/input/vector_map", qos_transient_local,
std::bind(&BehaviorPathPlannerNode::onMap, this, _1));
route_subscriber_ = create_subscription<HADMapRoute>(
"~/input/route", qos_transient_local, std::bind(&BehaviorPathPlannerNode::onRoute, this, _1));

// publisher
path_publisher_ = create_publisher<PathWithLaneId>("~/output/path", 1);
path_candidate_publisher_ = create_publisher<Path>("~/output/path_candidate", 1);
Expand All @@ -94,8 +86,40 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod
create_publisher<MarkerArray>("~/drivable_area_boundary", 1);
}

// subscriber
velocity_subscriber_ = create_subscription<Odometry>(
"~/input/odometry", 1, std::bind(&BehaviorPathPlannerNode::onVelocity, this, _1),
createSubscriptionOptions(this));
perception_subscriber_ = create_subscription<PredictedObjects>(
"~/input/perception", 1, std::bind(&BehaviorPathPlannerNode::onPerception, this, _1),
createSubscriptionOptions(this));
scenario_subscriber_ = create_subscription<Scenario>(
"~/input/scenario", 1,
[this](const Scenario::ConstSharedPtr msg) {
current_scenario_ = std::make_shared<Scenario>(*msg);
},
createSubscriptionOptions(this));
external_approval_subscriber_ = create_subscription<ApprovalMsg>(
"~/input/external_approval", 1,
std::bind(&BehaviorPathPlannerNode::onExternalApproval, this, _1),
createSubscriptionOptions(this));
force_approval_subscriber_ = create_subscription<PathChangeModule>(
"~/input/force_approval", 1, std::bind(&BehaviorPathPlannerNode::onForceApproval, this, _1),
createSubscriptionOptions(this));

// route_handler
auto qos_transient_local = rclcpp::QoS{1}.transient_local();
vector_map_subscriber_ = create_subscription<HADMapBin>(
"~/input/vector_map", qos_transient_local, std::bind(&BehaviorPathPlannerNode::onMap, this, _1),
createSubscriptionOptions(this));
route_subscriber_ = create_subscription<HADMapRoute>(
"~/input/route", qos_transient_local, std::bind(&BehaviorPathPlannerNode::onRoute, this, _1),
createSubscriptionOptions(this));

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

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

auto side_shift_module =
Expand Down Expand Up @@ -129,6 +153,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 @@ -458,47 +484,64 @@ void BehaviorPathPlannerNode::waitForData()
rclcpp::Rate(100).sleep();
}

mutex_pd_.lock(); // for planner_data_
while (!planner_data_->route_handler->isHandlerReady() && rclcpp::ok()) {
mutex_pd_.unlock();
RCLCPP_INFO_SKIPFIRST_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()) {
if (planner_data_->dynamic_object && planner_data_->self_odometry) {
break;
}

mutex_pd_.unlock();
RCLCPP_INFO_SKIPFIRST_THROTTLE(
get_logger(), *get_clock(), 5000,
"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_

// behavior_path_planner runs only in LANE DRIVING scenario.
if (current_scenario_->current_scenario != Scenario::LANEDRIVING) {
return;
}

// 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 @@ -522,7 +565,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 @@ -533,37 +576,40 @@ void BehaviorPathPlannerNode::run()
}

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

publishDebugMarker(bt_manager_->getDebugMarkers());

if (planner_data_->parameters.visualize_drivable_area_for_shared_linestrings_lanelet) {
if (planner_data->parameters.visualize_drivable_area_for_shared_linestrings_lanelet) {
const auto drivable_area_lines = marker_utils::createFurthestLineStringMarkerArray(
util::getDrivableAreaForAllSharedLinestringLanelets(planner_data_));
util::getDrivableAreaForAllSharedLinestringLanelets(planner_data));
debug_drivable_area_lanelets_publisher_->publish(drivable_area_lines);
}

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 @@ -572,7 +618,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 @@ -604,7 +651,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 @@ -654,28 +701,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 @@ -688,10 +733,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 827ff26

Please sign in to comment.