40
40
41
41
namespace
42
42
{
43
- rclcpp::SubscriptionOptions createSubscriptionOptions (rclcpp::Node * node_ptr)
43
+ rclcpp::SubscriptionOptions create_subscription_options (rclcpp::Node * node_ptr)
44
44
{
45
45
rclcpp::CallbackGroup::SharedPtr callback_group =
46
46
node_ptr->create_callback_group (rclcpp::CallbackGroupType::MutuallyExclusive);
@@ -83,44 +83,44 @@ BehaviorVelocityPlannerNode::BehaviorVelocityPlannerNode(const rclcpp::NodeOptio
83
83
trigger_sub_path_with_lane_id_ =
84
84
this ->create_subscription <autoware_auto_planning_msgs::msg::PathWithLaneId>(
85
85
" ~/input/path_with_lane_id" , 1 , std::bind (&BehaviorVelocityPlannerNode::on_trigger, this , _1),
86
- createSubscriptionOptions (this ));
86
+ create_subscription_options (this ));
87
87
88
88
// Subscribers
89
89
sub_predicted_objects_ =
90
90
this ->create_subscription <autoware_auto_perception_msgs::msg::PredictedObjects>(
91
91
" ~/input/dynamic_objects" , 1 ,
92
92
std::bind (&BehaviorVelocityPlannerNode::on_predicted_objects, this , _1),
93
- createSubscriptionOptions (this ));
93
+ create_subscription_options (this ));
94
94
sub_no_ground_pointcloud_ = this ->create_subscription <sensor_msgs::msg::PointCloud2>(
95
95
" ~/input/no_ground_pointcloud" , rclcpp::SensorDataQoS (),
96
96
std::bind (&BehaviorVelocityPlannerNode::on_no_ground_point_cloud, this , _1),
97
- createSubscriptionOptions (this ));
97
+ create_subscription_options (this ));
98
98
sub_vehicle_odometry_ = this ->create_subscription <nav_msgs::msg::Odometry>(
99
- " ~/input/vehicle_odometry" , 1 , std::bind (&BehaviorVelocityPlannerNode::onOdometry , this , _1),
100
- createSubscriptionOptions (this ));
99
+ " ~/input/vehicle_odometry" , 1 , std::bind (&BehaviorVelocityPlannerNode::on_odometry , this , _1),
100
+ create_subscription_options (this ));
101
101
sub_acceleration_ = this ->create_subscription <geometry_msgs::msg::AccelWithCovarianceStamped>(
102
- " ~/input/accel" , 1 , std::bind (&BehaviorVelocityPlannerNode::onAcceleration , this , _1),
103
- createSubscriptionOptions (this ));
102
+ " ~/input/accel" , 1 , std::bind (&BehaviorVelocityPlannerNode::on_acceleration , this , _1),
103
+ create_subscription_options (this ));
104
104
sub_lanelet_map_ = this ->create_subscription <autoware_auto_mapping_msgs::msg::HADMapBin>(
105
105
" ~/input/vector_map" , rclcpp::QoS (10 ).transient_local (),
106
- std::bind (&BehaviorVelocityPlannerNode::onLaneletMap , this , _1),
107
- createSubscriptionOptions (this ));
106
+ std::bind (&BehaviorVelocityPlannerNode::on_lanelet_map , this , _1),
107
+ create_subscription_options (this ));
108
108
sub_traffic_signals_ =
109
109
this ->create_subscription <autoware_perception_msgs::msg::TrafficSignalArray>(
110
110
" ~/input/traffic_signals" , 1 ,
111
111
std::bind (&BehaviorVelocityPlannerNode::onTrafficSignals, this , _1),
112
- createSubscriptionOptions (this ));
112
+ create_subscription_options (this ));
113
113
sub_external_velocity_limit_ = this ->create_subscription <VelocityLimit>(
114
114
" ~/input/external_velocity_limit_mps" , rclcpp::QoS{1 }.transient_local (),
115
115
std::bind (&BehaviorVelocityPlannerNode::onExternalVelocityLimit, this , _1));
116
116
sub_virtual_traffic_light_states_ =
117
117
this ->create_subscription <tier4_v2x_msgs::msg::VirtualTrafficLightStateArray>(
118
118
" ~/input/virtual_traffic_light_states" , 1 ,
119
119
std::bind (&BehaviorVelocityPlannerNode::on_virtual_traffic_light_states, this , _1),
120
- createSubscriptionOptions (this ));
120
+ create_subscription_options (this ));
121
121
sub_occupancy_grid_ = this ->create_subscription <nav_msgs::msg::OccupancyGrid>(
122
- " ~/input/occupancy_grid" , 1 , std::bind (&BehaviorVelocityPlannerNode::onOccupancyGrid , this , _1),
123
- createSubscriptionOptions (this ));
122
+ " ~/input/occupancy_grid" , 1 , std::bind (&BehaviorVelocityPlannerNode::on_occupancy_grid , this , _1),
123
+ create_subscription_options (this ));
124
124
125
125
srv_load_plugin_ = create_service<LoadPlugin>(
126
126
" ~/service/load_plugin" , std::bind (&BehaviorVelocityPlannerNode::onLoadPlugin, this , _1, _2));
@@ -181,7 +181,7 @@ void BehaviorVelocityPlannerNode::onUnloadPlugin(
181
181
}
182
182
183
183
// NOTE: argument planner_data must not be referenced for multithreading
184
- bool BehaviorVelocityPlannerNode::isDataReady (
184
+ bool BehaviorVelocityPlannerNode::is_data_ready (
185
185
const PlannerData & planner_data, rclcpp::Clock clock) const
186
186
{
187
187
const auto & d = planner_data;
@@ -225,7 +225,7 @@ bool BehaviorVelocityPlannerNode::isDataReady(
225
225
return true ;
226
226
}
227
227
228
- void BehaviorVelocityPlannerNode::onOccupancyGrid (
228
+ void BehaviorVelocityPlannerNode::on_occupancy_grid (
229
229
const nav_msgs::msg::OccupancyGrid::ConstSharedPtr msg)
230
230
{
231
231
std::lock_guard<std::mutex> lock (mutex_);
@@ -266,7 +266,7 @@ void BehaviorVelocityPlannerNode::on_no_ground_point_cloud(
266
266
}
267
267
}
268
268
269
- void BehaviorVelocityPlannerNode::onOdometry (const nav_msgs::msg::Odometry::ConstSharedPtr msg)
269
+ void BehaviorVelocityPlannerNode::on_odometry (const nav_msgs::msg::Odometry::ConstSharedPtr msg)
270
270
{
271
271
std::lock_guard<std::mutex> lock (mutex_);
272
272
@@ -299,7 +299,7 @@ void BehaviorVelocityPlannerNode::onOdometry(const nav_msgs::msg::Odometry::Cons
299
299
}
300
300
}
301
301
302
- void BehaviorVelocityPlannerNode::onAcceleration (
302
+ void BehaviorVelocityPlannerNode::on_acceleration (
303
303
const geometry_msgs::msg::AccelWithCovarianceStamped::ConstSharedPtr msg)
304
304
{
305
305
std::lock_guard<std::mutex> lock (mutex_);
@@ -315,7 +315,7 @@ void BehaviorVelocityPlannerNode::onParam()
315
315
std::make_unique<motion_velocity_smoother::AnalyticalJerkConstrainedSmoother>(*this );
316
316
}
317
317
318
- void BehaviorVelocityPlannerNode::onLaneletMap (
318
+ void BehaviorVelocityPlannerNode::on_lanelet_map (
319
319
const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr msg)
320
320
{
321
321
std::lock_guard<std::mutex> lock (mutex_);
@@ -377,7 +377,7 @@ void BehaviorVelocityPlannerNode::on_trigger(
377
377
{
378
378
std::unique_lock<std::mutex> lk (mutex_);
379
379
380
- if (!isDataReady (planner_data_, *get_clock ())) {
380
+ if (!is_data_ready (planner_data_, *get_clock ())) {
381
381
return ;
382
382
}
383
383
0 commit comments