Skip to content

Commit

Permalink
feat(behavior_path_planner): replace polling subscribers
Browse files Browse the repository at this point in the history 
Signed-off-by: Mamoru Sobue <[email protected]>
  • Loading branch information
@soblin
soblin committed Jun 12, 2024
1 parent cdb3d4a commit cd9064f
Show file tree
Hide file tree
Showing 2 changed files with 131 additions and 152 deletions.
56 changes: 35 additions & 21 deletions ...havior_path_planner/include/autoware_behavior_path_planner/behavior_path_planner_node.hpp
View file
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
Original file line number Diff line number Diff line change
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 @@ -78,54 +64,6 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod

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 @@ BehaviorPathPlannerParameters BehaviorPathPlannerNode::getCommonParam()
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 @@ bool BehaviorPathPlannerNode::isDataReady()
}

{
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 @@ bool BehaviorPathPlannerNode::isDataReady()

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

if (!isDataReady()) {
return;
}
Expand All @@ -341,7 +371,6 @@ void BehaviorPathPlannerNode::run()
// 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 @@ void BehaviorPathPlannerNode::run()
// 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 @@ bool BehaviorPathPlannerNode::keepInputPoints(
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 @@ void BehaviorPathPlannerNode::onTrafficSignals(const TrafficLightGroupArray::Con
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

0 comments on commit cd9064f

Please sign in to comment.