Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

feat(behavior_path_planner): replace polling subscribers #7369

Closed
wants to merge 2 commits into from
+131 −152
Closed

feat(behavior_path_planner): replace polling subscribers #7369

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
cd9064f
feat(behavior_path_planner): replace polling subscribers
soblin Jun 6, 2024
7cf4cfe
Merge branch 'main' into feat/behavior_path_polling_sub
soblin Jun 12, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
56 changes: 35 additions & 21 deletions ...havior_path_planner/include/autoware_behavior_path_planner/behavior_path_planner_node.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -21,6 +21,7 @@
#include "autoware_behavior_path_planner_common/interface/steering_factor_interface.hpp"
#include "tier4_autoware_utils/ros/logger_level_configure.hpp"

#include <tier4_autoware_utils/ros/polling_subscriber.hpp>
#include <tier4_autoware_utils/ros/published_time_publisher.hpp>

#include <autoware_adapi_v1_msgs/msg/operation_mode_state.hpp>
Expand Down Expand Up @@ -86,49 +87,62 @@ class BehaviorPathPlannerNode : public rclcpp::Node
std::vector<std::string> getRunningModules();

private:
rclcpp::Subscription<LaneletRoute>::SharedPtr route_subscriber_;
rclcpp::Subscription<LaneletMapBin>::SharedPtr vector_map_subscriber_;
rclcpp::Subscription<Odometry>::SharedPtr velocity_subscriber_;
rclcpp::Subscription<AccelWithCovarianceStamped>::SharedPtr acceleration_subscriber_;
rclcpp::Subscription<Scenario>::SharedPtr scenario_subscriber_;
rclcpp::Subscription<PredictedObjects>::SharedPtr perception_subscriber_;
rclcpp::Subscription<OccupancyGrid>::SharedPtr occupancy_grid_subscriber_;
rclcpp::Subscription<OccupancyGrid>::SharedPtr costmap_subscriber_;
rclcpp::Subscription<TrafficLightGroupArray>::SharedPtr traffic_signals_subscriber_;
rclcpp::Subscription<LateralOffset>::SharedPtr lateral_offset_subscriber_;
rclcpp::Subscription<OperationModeState>::SharedPtr operation_mode_subscriber_;
// subscriber
tier4_autoware_utils::InterProcessPollingSubscriber<LaneletRoute> route_subscriber_{
this, "~/input/route", rclcpp::QoS{1}.transient_local()};
tier4_autoware_utils::InterProcessPollingSubscriber<LaneletMapBin> vector_map_subscriber_{
this, "~/input/vector_map", rclcpp::QoS{1}.transient_local()};
tier4_autoware_utils::InterProcessPollingSubscriber<Odometry> velocity_subscriber_{
this, "~/input/odometry"};
tier4_autoware_utils::InterProcessPollingSubscriber<AccelWithCovarianceStamped>
acceleration_subscriber_{this, "~/input/accel"};
tier4_autoware_utils::InterProcessPollingSubscriber<Scenario> scenario_subscriber_{
this, "~/input/scenario"};
tier4_autoware_utils::InterProcessPollingSubscriber<PredictedObjects> perception_subscriber_{
this, "~/input/perception"};
tier4_autoware_utils::InterProcessPollingSubscriber<OccupancyGrid> occupancy_grid_subscriber_{
this, "~/input/occupancy_grid_map"};
tier4_autoware_utils::InterProcessPollingSubscriber<OccupancyGrid> costmap_subscriber_{
this, "~/input/costmap"};
tier4_autoware_utils::InterProcessPollingSubscriber<TrafficLightGroupArray>
traffic_signals_subscriber_{this, "~/input/traffic_signals"};
tier4_autoware_utils::InterProcessPollingSubscriber<LateralOffset> lateral_offset_subscriber_{
this, "~/input/lateral_offset"};
tier4_autoware_utils::InterProcessPollingSubscriber<OperationModeState>
operation_mode_subscriber_{
this, "/system/operation_mode/state", rclcpp::QoS{1}.transient_local()};
tier4_autoware_utils::InterProcessPollingSubscriber<tier4_planning_msgs::msg::VelocityLimit>
external_limit_max_velocity_subscriber_{this, "/planning/scenario_planning/max_velocity"};

// publisher
rclcpp::Publisher<PathWithLaneId>::SharedPtr path_publisher_;
rclcpp::Publisher<TurnIndicatorsCommand>::SharedPtr turn_signal_publisher_;
rclcpp::Publisher<HazardLightsCommand>::SharedPtr hazard_signal_publisher_;
rclcpp::Publisher<MarkerArray>::SharedPtr bound_publisher_;
rclcpp::Publisher<PoseWithUuidStamped>::SharedPtr modified_goal_publisher_;
rclcpp::Publisher<StopReasonArray>::SharedPtr stop_reason_publisher_;
rclcpp::Publisher<RerouteAvailability>::SharedPtr reroute_availability_publisher_;
rclcpp::Subscription<tier4_planning_msgs::msg::VelocityLimit>::SharedPtr
external_limit_max_velocity_subscriber_;
rclcpp::TimerBase::SharedPtr timer_;

std::map<std::string, rclcpp::Publisher<Path>::SharedPtr> path_candidate_publishers_;
std::map<std::string, rclcpp::Publisher<Path>::SharedPtr> path_reference_publishers_;

std::shared_ptr<PlannerData> planner_data_;

std::shared_ptr<PlannerManager> planner_manager_;

std::unique_ptr<SteeringFactorInterface> steering_factor_interface_ptr_;
Scenario::SharedPtr current_scenario_{nullptr};

Scenario::ConstSharedPtr current_scenario_{nullptr};
LaneletMapBin::ConstSharedPtr map_ptr_{nullptr};
LaneletRoute::ConstSharedPtr route_ptr_{nullptr};
bool has_received_map_{false};
bool has_received_route_{false};

std::shared_ptr<PlannerManager> planner_manager_;

std::unique_ptr<SteeringFactorInterface> steering_factor_interface_ptr_;

std::mutex mutex_pd_; // mutex for planner_data_
std::mutex mutex_manager_; // mutex for bt_manager_ or planner_manager_
std::mutex mutex_map_; // mutex for has_received_map_ and map_ptr_
std::mutex mutex_route_; // mutex for has_received_route_ and route_ptr_

// setup
void takeData();
bool isDataReady();

// parameters
Expand Down
227 changes: 96 additions & 131 deletions planning/autoware_behavior_path_planner/src/behavior_path_planner_node.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
// Copyright 2021-2023 Tier IV, Inc.

Check warning on line 1 in planning/autoware_behavior_path_planner/src/behavior_path_planner_node.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ New issue: Overall Code Complexity 

This module has a mean cyclomatic complexity of 4.95 across 21 functions. The mean complexity threshold is 4. This file has many conditional statements (e.g. if, for, while) across its implementation, leading to lower code health. Avoid adding more conditionals.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
Expand Down Expand Up @@ -28,20 +28,6 @@
#include <string>
#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 autoware::behavior_path_planner
{
using autoware::vehicle_info_utils::VehicleInfoUtils;
Expand Down Expand Up @@ -77,55 +63,7 @@
debug_turn_signal_info_publisher_ = create_publisher<MarkerArray>("~/debug/turn_signal_info", 1);

bound_publisher_ = create_publisher<MarkerArray>("~/debug/bound", 1);

Check notice on line 66 in planning/autoware_behavior_path_planner/src/behavior_path_planner_node.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

✅ No longer an issue: Large Method 

BehaviorPathPlannerNode::BehaviorPathPlannerNode is no longer above the threshold for lines of code. Large functions with many lines of code are generally harder to understand and lower the code health. Avoid adding more lines to this function.
const auto qos_transient_local = rclcpp::QoS{1}.transient_local();
// subscriber
velocity_subscriber_ = create_subscription<Odometry>(
"~/input/odometry", 1, std::bind(&BehaviorPathPlannerNode::onOdometry, this, _1),
createSubscriptionOptions(this));
acceleration_subscriber_ = create_subscription<AccelWithCovarianceStamped>(
"~/input/accel", 1, std::bind(&BehaviorPathPlannerNode::onAcceleration, this, _1),
createSubscriptionOptions(this));
perception_subscriber_ = create_subscription<PredictedObjects>(
"~/input/perception", 1, std::bind(&BehaviorPathPlannerNode::onPerception, this, _1),
createSubscriptionOptions(this));
occupancy_grid_subscriber_ = create_subscription<OccupancyGrid>(
"~/input/occupancy_grid_map", 1, std::bind(&BehaviorPathPlannerNode::onOccupancyGrid, this, _1),
createSubscriptionOptions(this));
costmap_subscriber_ = create_subscription<OccupancyGrid>(
"~/input/costmap", 1, std::bind(&BehaviorPathPlannerNode::onCostMap, this, _1),
createSubscriptionOptions(this));
traffic_signals_subscriber_ =
this->create_subscription<autoware_perception_msgs::msg::TrafficLightGroupArray>(
"~/input/traffic_signals", 1, std::bind(&BehaviorPathPlannerNode::onTrafficSignals, this, _1),
createSubscriptionOptions(this));
lateral_offset_subscriber_ = this->create_subscription<LateralOffset>(
"~/input/lateral_offset", 1, std::bind(&BehaviorPathPlannerNode::onLateralOffset, this, _1),
createSubscriptionOptions(this));
operation_mode_subscriber_ = create_subscription<OperationModeState>(
"/system/operation_mode/state", qos_transient_local,
std::bind(&BehaviorPathPlannerNode::onOperationMode, 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_limit_max_velocity_subscriber_ =
create_subscription<tier4_planning_msgs::msg::VelocityLimit>(
"/planning/scenario_planning/max_velocity", 1,
std::bind(&BehaviorPathPlannerNode::on_external_velocity_limiter, this, _1),
createSubscriptionOptions(this));

// route_handler
vector_map_subscriber_ = create_subscription<LaneletMapBin>(
"~/input/vector_map", qos_transient_local, std::bind(&BehaviorPathPlannerNode::onMap, this, _1),
createSubscriptionOptions(this));
route_subscriber_ = create_subscription<LaneletRoute>(
"~/input/route", qos_transient_local, std::bind(&BehaviorPathPlannerNode::onRoute, this, _1),
createSubscriptionOptions(this));

{
const std::string path_candidate_name_space = "/planning/path_candidate/";
const std::string path_reference_name_space = "/planning/path_reference/";
Expand Down Expand Up @@ -274,6 +212,100 @@
return p;
}

void BehaviorPathPlannerNode::takeData()
{
// route
{
const auto msg = route_subscriber_.takeNewData();
if (msg) {
if (msg->segments.empty()) {
RCLCPP_ERROR(get_logger(), "input route is empty. ignored");
} else {
route_ptr_ = msg;
has_received_route_ = true;
}
}
}
// map
{
const auto msg = vector_map_subscriber_.takeNewData();
if (msg) {
map_ptr_ = msg;
has_received_map_ = true;
}
}
// velocity
{
const auto msg = velocity_subscriber_.takeData();
if (msg) {
planner_data_->self_odometry = msg;
}
}
// acceleration
{
const auto msg = acceleration_subscriber_.takeData();
if (msg) {
planner_data_->self_acceleration = msg;
}
}
// scenario
{
const auto msg = scenario_subscriber_.takeData();
if (msg) {
current_scenario_ = msg;
}
}
// perception
{
const auto msg = perception_subscriber_.takeData();
if (msg) {
planner_data_->dynamic_object = msg;
}
}
// occupancy_grid
{
const auto msg = occupancy_grid_subscriber_.takeData();
if (msg) {
planner_data_->occupancy_grid = msg;
}
}
// costmap
{
const auto msg = costmap_subscriber_.takeData();
if (msg) {
planner_data_->costmap = msg;
}
}
// traffic_signal
{
const auto msg = traffic_signals_subscriber_.takeData();
if (msg) {
onTrafficSignals(msg);
}
}
// lateral_offset
{
const auto msg = lateral_offset_subscriber_.takeData();
if (msg) {
onLateralOffset(msg);
}
}
// operation_mode
{
const auto msg = operation_mode_subscriber_.takeData();
if (msg) {
planner_data_->operation_mode = msg;
}
}
// external_velocity_limiter
{
const auto msg = external_limit_max_velocity_subscriber_.takeData();
if (msg) {
planner_data_->external_limit_max_velocity = msg;
}
}
}

Check warning on line 307 in planning/autoware_behavior_path_planner/src/behavior_path_planner_node.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ New issue: Complex Method 

BehaviorPathPlannerNode::takeData has a cyclomatic complexity of 14, threshold = 9. This function has many conditional statements (e.g. if, for, while), leading to lower code health. Avoid adding more conditionals and code to it without refactoring.

// wait until mandatory data is ready
bool BehaviorPathPlannerNode::isDataReady()
{
Expand All @@ -287,21 +319,17 @@
}

{
std::lock_guard<std::mutex> lk_route(mutex_route_);
if (!route_ptr_) {
return missing("route");
}
}

{
std::lock_guard<std::mutex> lk_map(mutex_map_);
if (!map_ptr_) {
return missing("map");
}
}

const std::lock_guard<std::mutex> lock(mutex_pd_); // for planner_data_

if (!planner_data_->dynamic_object) {
return missing("dynamic_object");
}
Expand All @@ -327,6 +355,8 @@

void BehaviorPathPlannerNode::run()
{
takeData();

if (!isDataReady()) {
return;
}
Expand All @@ -341,7 +371,6 @@
// check for map update
LaneletMapBin::ConstSharedPtr map_ptr{nullptr};
{
std::lock_guard<std::mutex> lk_map(mutex_map_); // for has_received_map_ and map_ptr_
if (has_received_map_) {
// Note: duplicating the shared_ptr prevents the data from being deleted by another thread!
map_ptr = map_ptr_;
Expand All @@ -352,7 +381,6 @@
// check for route update
LaneletRoute::ConstSharedPtr route_ptr{nullptr};
{
std::lock_guard<std::mutex> lk_route(mutex_route_); // for has_received_route_ and route_ptr_
if (has_received_route_) {
// Note: duplicating the shared_ptr prevents the data from being deleted by another thread!
route_ptr = route_ptr_;
Expand Down Expand Up @@ -765,35 +793,8 @@
return false;
}

void BehaviorPathPlannerNode::onOdometry(const Odometry::ConstSharedPtr msg)
{
const std::lock_guard<std::mutex> lock(mutex_pd_);
planner_data_->self_odometry = msg;
}
void BehaviorPathPlannerNode::onAcceleration(const AccelWithCovarianceStamped::ConstSharedPtr msg)
{
const std::lock_guard<std::mutex> lock(mutex_pd_);
planner_data_->self_acceleration = msg;
}
void BehaviorPathPlannerNode::onPerception(const PredictedObjects::ConstSharedPtr msg)
{
const std::lock_guard<std::mutex> lock(mutex_pd_);
planner_data_->dynamic_object = msg;
}
void BehaviorPathPlannerNode::onOccupancyGrid(const OccupancyGrid::ConstSharedPtr msg)
{
const std::lock_guard<std::mutex> lock(mutex_pd_);
planner_data_->occupancy_grid = msg;
}
void BehaviorPathPlannerNode::onCostMap(const OccupancyGrid::ConstSharedPtr msg)
{
const std::lock_guard<std::mutex> lock(mutex_pd_);
planner_data_->costmap = msg;
}
void BehaviorPathPlannerNode::onTrafficSignals(const TrafficLightGroupArray::ConstSharedPtr msg)
{
std::lock_guard<std::mutex> lock(mutex_pd_);

planner_data_->traffic_light_id_map.clear();
for (const auto & signal : msg->traffic_light_groups) {
TrafficSignalStamped traffic_signal;
Expand All @@ -802,45 +803,9 @@
planner_data_->traffic_light_id_map[signal.traffic_light_group_id] = traffic_signal;
}
}
void BehaviorPathPlannerNode::onMap(const LaneletMapBin::ConstSharedPtr msg)
{
const std::lock_guard<std::mutex> lock(mutex_map_);
map_ptr_ = msg;
has_received_map_ = true;
}
void BehaviorPathPlannerNode::onRoute(const LaneletRoute::ConstSharedPtr msg)
{
if (msg->segments.empty()) {
RCLCPP_ERROR(get_logger(), "input route is empty. ignored");
return;
}

const std::lock_guard<std::mutex> lock(mutex_route_);
route_ptr_ = msg;
has_received_route_ = true;
}
void BehaviorPathPlannerNode::onOperationMode(const OperationModeState::ConstSharedPtr msg)
{
const std::lock_guard<std::mutex> lock(mutex_pd_);
planner_data_->operation_mode = msg;
}

void BehaviorPathPlannerNode::on_external_velocity_limiter(
const tier4_planning_msgs::msg::VelocityLimit::ConstSharedPtr msg)
{
// Note: Using this parameter during path planning might cause unexpected deceleration or jerk.
// Therefore, do not use it for anything other than safety checks.
if (!msg) {
return;
}

const std::lock_guard<std::mutex> lock(mutex_pd_);
planner_data_->external_limit_max_velocity = msg;
}
void BehaviorPathPlannerNode::onLateralOffset(const LateralOffset::ConstSharedPtr msg)
{
std::lock_guard<std::mutex> lock(mutex_pd_);

if (!planner_data_->lateral_offset) {
planner_data_->lateral_offset = msg;
return;
Expand Down
Loading