Skip to content

Commit

Permalink
feat(behavior_path_planner): change planner data update method (autow…
Browse files Browse the repository at this point in the history 
…arefoundation#2510)

* feat(behavior_path_planner): change planner data update method

Signed-off-by: yutaka <purewater0901@gmail.com>

* remove unnecessary code

Signed-off-by: yutaka <purewater0901@gmail.com>

* update

Signed-off-by: yutaka <purewater0901@gmail.com>

* add mutex

Signed-off-by: yutaka <purewater0901@gmail.com>

* update

Signed-off-by: yutaka <purewater0901@gmail.com>

* fix comment

Signed-off-by: yutaka <purewater0901@gmail.com>

* add mutex

Signed-off-by: yutaka <purewater0901@gmail.com>

Signed-off-by: yutaka <purewater0901@gmail.com>
  • Loading branch information
@purewater0901
purewater0901 authored Dec 22, 2022
1 parent 98b1cf3 commit 8d4793f
Show file tree
Hide file tree
Showing 2 changed files with 57 additions and 32 deletions.
8 changes: 8 additions & 0 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 @@ -112,6 +112,11 @@ class BehaviorPathPlannerNode : public rclcpp::Node
tier4_autoware_utils::SelfPoseListener self_pose_listener_{this};
Scenario::SharedPtr current_scenario_{nullptr};

HADMapBin::ConstSharedPtr map_ptr_{nullptr};
LaneletRoute::ConstSharedPtr route_ptr_{nullptr};
bool has_received_map_{false};
bool has_received_route_{false};

TurnSignalDecider turn_signal_decider_;

std::mutex mutex_pd_; // mutex for planner_data_
Expand All @@ -120,6 +125,9 @@ class BehaviorPathPlannerNode : public rclcpp::Node
// setup
bool isDataReady();

// update planner data
std::shared_ptr<PlannerData> createLatestPlannerData();

// parameters
std::shared_ptr<AvoidanceParameters> avoidance_param_ptr;
std::shared_ptr<LaneChangeParameters> lane_change_param_ptr;
Expand Down
81 changes: 49 additions & 32 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 @@ -564,19 +564,21 @@ bool BehaviorPathPlannerNode::isDataReady()
{
const auto missing = [this](const auto & name) {
RCLCPP_INFO_SKIPFIRST_THROTTLE(get_logger(), *get_clock(), 5000, "waiting for %s", name);
mutex_pd_.unlock();
return false;
};

mutex_pd_.lock(); // for planner_data_
if (!current_scenario_) {
return missing("scenario_topic");
}

if (!planner_data_->route_handler->isHandlerReady()) {
if (!route_ptr_) {
return missing("route");
}

if (!map_ptr_) {
return missing("map");
}

if (!planner_data_->dynamic_object) {
return missing("dynamic_object");
}
Expand All @@ -594,10 +596,39 @@ bool BehaviorPathPlannerNode::isDataReady()
return missing("self_pose");
}

mutex_pd_.unlock();
return true;
}

std::shared_ptr<PlannerData> BehaviorPathPlannerNode::createLatestPlannerData()
{
const std::lock_guard<std::mutex> lock(mutex_pd_);

// update self
planner_data_->self_pose = self_pose_listener_.getCurrentPose();

// update map
if (has_received_map_) {
planner_data_->route_handler->setMap(*map_ptr_);
has_received_map_ = false;
}

// update route
const bool is_first_time = !(planner_data_->route_handler->isHandlerReady());
if (has_received_route_) {
planner_data_->route_handler->setRoute(*route_ptr_);
// Reset behavior tree when new route is received,
// so that the each modules do not have to care about the "route jump".
if (!is_first_time) {
RCLCPP_DEBUG(get_logger(), "new route is received. reset behavior tree.");
bt_manager_->resetBehaviorTree();
}

has_received_route_ = false;
}

return std::make_shared<PlannerData>(*planner_data_);
}

void BehaviorPathPlannerNode::run()
{
if (!isDataReady()) {
Expand All @@ -606,22 +637,15 @@ 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) {
mutex_bt_.unlock(); // for bt_manager_
mutex_pd_.unlock(); // for planner_data_
return;
}

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

const auto planner_data = std::make_shared<PlannerData>(*planner_data_);

// unlock planner data
mutex_pd_.unlock();
// create latest planner data
const auto planner_data = createLatestPlannerData();

// run behavior planner
const auto output = bt_manager_->run(planner_data);
Expand Down Expand Up @@ -856,34 +880,34 @@ bool BehaviorPathPlannerNode::keepInputPoints(

void BehaviorPathPlannerNode::onVelocity(const Odometry::ConstSharedPtr msg)
{
std::lock_guard<std::mutex> lock(mutex_pd_);
const std::lock_guard<std::mutex> lock(mutex_pd_);
planner_data_->self_odometry = msg;
}
void BehaviorPathPlannerNode::onAcceleration(const AccelWithCovarianceStamped::ConstSharedPtr msg)
{
std::lock_guard<std::mutex> lock(mutex_pd_);
const std::lock_guard<std::mutex> lock(mutex_pd_);
planner_data_->self_acceleration = msg;
}
void BehaviorPathPlannerNode::onPerception(const PredictedObjects::ConstSharedPtr msg)
{
std::lock_guard<std::mutex> lock(mutex_pd_);
const std::lock_guard<std::mutex> lock(mutex_pd_);
planner_data_->dynamic_object = msg;
}
void BehaviorPathPlannerNode::onOccupancyGrid(const OccupancyGrid::ConstSharedPtr msg)
{
std::lock_guard<std::mutex> lock(mutex_pd_);
const std::lock_guard<std::mutex> lock(mutex_pd_);
planner_data_->occupancy_grid = msg;
}
void BehaviorPathPlannerNode::onExternalApproval(const ApprovalMsg::ConstSharedPtr msg)
{
std::lock_guard<std::mutex> lock(mutex_pd_);
const 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_);
const std::lock_guard<std::mutex> lock(mutex_pd_);
auto getModuleName = [](PathChangeModuleId module) {
if (module.type == PathChangeModuleId::FORCE_LANE_CHANGE) {
return "ForceLaneChange";
Expand All @@ -896,22 +920,15 @@ 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);
const std::lock_guard<std::mutex> lock(mutex_pd_);
map_ptr_ = msg;
has_received_map_ = true;
}
void BehaviorPathPlannerNode::onRoute(const LaneletRoute::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);

// Reset behavior tree when new route is received,
// so that the each modules do not have to care about the "route jump".
if (!is_first_time) {
RCLCPP_DEBUG(get_logger(), "new route is received. reset behavior tree.");
bt_manager_->resetBehaviorTree();
}
const std::lock_guard<std::mutex> lock(mutex_pd_);
route_ptr_ = msg;
has_received_route_ = true;
}

SetParametersResult BehaviorPathPlannerNode::onSetParam(
Expand Down

0 comments on commit 8d4793f

Please sign in to comment.