diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py
index 96c33ad813c21..0dcfaf8a36563 100644
--- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py
+++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py
@@ -142,6 +142,7 @@ def launch_setup(context, *args, **kwargs):
("~/input/vector_map", LaunchConfiguration("map_topic_name")),
("~/input/perception", "/perception/object_recognition/objects"),
("~/input/odometry", "/localization/kinematic_state"),
+ ("~/input/accel", "/localization/acceleration"),
("~/input/scenario", "/planning/scenario_planning/scenario"),
("~/output/path", "path_with_lane_id"),
("~/output/turn_indicators_cmd", "/planning/turn_indicators_cmd"),
@@ -329,6 +330,7 @@ def launch_setup(context, *args, **kwargs):
("~/input/path_with_lane_id", "path_with_lane_id"),
("~/input/vector_map", "/map/vector_map"),
("~/input/vehicle_odometry", "/localization/kinematic_state"),
+ ("~/input/accel", "/localization/acceleration"),
("~/input/dynamic_objects", "/perception/object_recognition/objects"),
(
"~/input/no_ground_pointcloud",
diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml
index 3a9c1e43d61d6..c4c7b3bab7597 100644
--- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml
+++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml
@@ -8,6 +8,7 @@
+
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp b/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp
index 2b062c5742fd5..58df76bc78baa 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp
@@ -86,6 +86,7 @@ class BehaviorPathPlannerNode : public rclcpp::Node
rclcpp::Subscription::SharedPtr route_subscriber_;
rclcpp::Subscription::SharedPtr vector_map_subscriber_;
rclcpp::Subscription::SharedPtr velocity_subscriber_;
+ rclcpp::Subscription::SharedPtr acceleration_subscriber_;
rclcpp::Subscription::SharedPtr scenario_subscriber_;
rclcpp::Subscription::SharedPtr perception_subscriber_;
rclcpp::Subscription::SharedPtr occupancy_grid_subscriber_;
@@ -123,6 +124,7 @@ class BehaviorPathPlannerNode : public rclcpp::Node
// callback
void onVelocity(const Odometry::ConstSharedPtr msg);
+ void onAcceleration(const AccelWithCovarianceStamped::ConstSharedPtr msg);
void onPerception(const PredictedObjects::ConstSharedPtr msg);
void onOccupancyGrid(const OccupancyGrid::ConstSharedPtr msg);
void onExternalApproval(const ApprovalMsg::ConstSharedPtr msg);
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp
index 282f8a19f246d..89f329508516f 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp
@@ -22,6 +22,7 @@
#include
#include
+#include
#include
#include
@@ -34,6 +35,7 @@ namespace behavior_path_planner
{
using autoware_auto_perception_msgs::msg::PredictedObjects;
using autoware_auto_planning_msgs::msg::PathWithLaneId;
+using geometry_msgs::msg::AccelWithCovarianceStamped;
using geometry_msgs::msg::PoseStamped;
using nav_msgs::msg::OccupancyGrid;
using nav_msgs::msg::Odometry;
@@ -61,13 +63,14 @@ struct PlannerData
{
PoseStamped::ConstSharedPtr self_pose{};
Odometry::ConstSharedPtr self_odometry{};
+ AccelWithCovarianceStamped::ConstSharedPtr self_acceleration{};
PredictedObjects::ConstSharedPtr dynamic_object{};
OccupancyGrid::ConstSharedPtr occupancy_grid{};
PathWithLaneId::SharedPtr reference_path{std::make_shared()};
PathWithLaneId::SharedPtr prev_output_path{std::make_shared()};
- BehaviorPathPlannerParameters parameters{};
lanelet::ConstLanelets current_lanes{};
std::shared_ptr route_handler{std::make_shared()};
+ BehaviorPathPlannerParameters parameters{};
Approval approval{};
};
diff --git a/planning/behavior_path_planner/launch/behavior_path_planner.launch.xml b/planning/behavior_path_planner/launch/behavior_path_planner.launch.xml
index 22d5283b1b0e3..a7df8ae9bf428 100644
--- a/planning/behavior_path_planner/launch/behavior_path_planner.launch.xml
+++ b/planning/behavior_path_planner/launch/behavior_path_planner.launch.xml
@@ -10,6 +10,7 @@
+
diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp
index 3be04884b6243..5fe8ea5c422ca 100644
--- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp
+++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp
@@ -77,6 +77,9 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod
velocity_subscriber_ = create_subscription(
"~/input/odometry", 1, std::bind(&BehaviorPathPlannerNode::onVelocity, this, _1),
createSubscriptionOptions(this));
+ acceleration_subscriber_ = create_subscription(
+ "~/input/accel", 1, std::bind(&BehaviorPathPlannerNode::onAcceleration, this, _1),
+ createSubscriptionOptions(this));
perception_subscriber_ = create_subscription(
"~/input/perception", 1, std::bind(&BehaviorPathPlannerNode::onPerception, this, _1),
createSubscriptionOptions(this));
@@ -528,6 +531,10 @@ bool BehaviorPathPlannerNode::isDataReady()
return missing("self_odometry");
}
+ if (!planner_data_->self_acceleration) {
+ return missing("self_acceleration");
+ }
+
planner_data_->self_pose = self_pose_listener_.getCurrentPose();
if (!planner_data_->self_pose) {
return missing("self_pose");
@@ -698,6 +705,11 @@ void BehaviorPathPlannerNode::onVelocity(const Odometry::ConstSharedPtr msg)
std::lock_guard lock(mutex_pd_);
planner_data_->self_odometry = msg;
}
+void BehaviorPathPlannerNode::onAcceleration(const AccelWithCovarianceStamped::ConstSharedPtr msg)
+{
+ std::lock_guard lock(mutex_pd_);
+ planner_data_->self_acceleration = msg;
+}
void BehaviorPathPlannerNode::onPerception(const PredictedObjects::ConstSharedPtr msg)
{
std::lock_guard lock(mutex_pd_);
diff --git a/planning/behavior_velocity_planner/include/behavior_velocity_planner/node.hpp b/planning/behavior_velocity_planner/include/behavior_velocity_planner/node.hpp
index 68e0e460dc4e3..40155ae90f4fb 100644
--- a/planning/behavior_velocity_planner/include/behavior_velocity_planner/node.hpp
+++ b/planning/behavior_velocity_planner/include/behavior_velocity_planner/node.hpp
@@ -58,6 +58,7 @@ class BehaviorVelocityPlannerNode : public rclcpp::Node
sub_predicted_objects_;
rclcpp::Subscription::SharedPtr sub_no_ground_pointcloud_;
rclcpp::Subscription::SharedPtr sub_vehicle_odometry_;
+ rclcpp::Subscription::SharedPtr sub_acceleration_;
rclcpp::Subscription::SharedPtr sub_lanelet_map_;
rclcpp::Subscription::SharedPtr
sub_traffic_signals_;
@@ -78,6 +79,7 @@ class BehaviorVelocityPlannerNode : public rclcpp::Node
const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr msg);
void onNoGroundPointCloud(const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg);
void onVehicleVelocity(const nav_msgs::msg::Odometry::ConstSharedPtr msg);
+ void onAcceleration(const geometry_msgs::msg::AccelWithCovarianceStamped::ConstSharedPtr msg);
void onLaneletMap(const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr msg);
void onTrafficSignals(
const autoware_auto_perception_msgs::msg::TrafficSignalArray::ConstSharedPtr msg);
diff --git a/planning/behavior_velocity_planner/include/behavior_velocity_planner/planner_data.hpp b/planning/behavior_velocity_planner/include/behavior_velocity_planner/planner_data.hpp
index fa736324c053e..9de2d422c1d2c 100644
--- a/planning/behavior_velocity_planner/include/behavior_velocity_planner/planner_data.hpp
+++ b/planning/behavior_velocity_planner/include/behavior_velocity_planner/planner_data.hpp
@@ -25,6 +25,7 @@
#include
#include
#include
+#include
#include
#include
#include
@@ -70,7 +71,7 @@ struct PlannerData
// msgs from callbacks that are used for data-ready
geometry_msgs::msg::TwistStamped::ConstSharedPtr current_velocity;
- boost::optional current_accel;
+ geometry_msgs::msg::AccelWithCovarianceStamped::ConstSharedPtr current_acceleration;
static constexpr double velocity_buffer_time_sec = 10.0;
std::deque velocity_buffer;
autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr predicted_objects;
@@ -156,44 +157,6 @@ struct PlannerData
return std::make_shared(
external_traffic_light_id_map.at(id));
}
-
-private:
- boost::optional prev_accel_;
- geometry_msgs::msg::TwistStamped::ConstSharedPtr prev_velocity_;
- double accel_lowpass_gain_;
-
- void updateCurrentAcc()
- {
- current_accel = {};
-
- if (!current_velocity) {
- return;
- }
-
- if (!prev_velocity_) {
- prev_velocity_ = current_velocity;
- return;
- }
-
- const double dv = current_velocity->twist.linear.x - prev_velocity_->twist.linear.x;
- const double dt = std::max(
- (rclcpp::Time(current_velocity->header.stamp) - rclcpp::Time(prev_velocity_->header.stamp))
- .seconds(),
- 1e-03);
-
- const double accel = dv / dt;
- prev_velocity_ = current_velocity;
-
- if (!prev_accel_) {
- prev_accel_ = accel;
- return;
- }
- // apply lowpass filter
- current_accel = accel_lowpass_gain_ * accel + (1.0 - accel_lowpass_gain_) * prev_accel_.get();
-
- prev_accel_ = current_accel;
- }
- friend BehaviorVelocityPlannerNode;
};
} // namespace behavior_velocity_planner
diff --git a/planning/behavior_velocity_planner/include/utilization/trajectory_utils.hpp b/planning/behavior_velocity_planner/include/utilization/trajectory_utils.hpp
index 3f6474ffbf895..26624913d3edc 100644
--- a/planning/behavior_velocity_planner/include/utilization/trajectory_utils.hpp
+++ b/planning/behavior_velocity_planner/include/utilization/trajectory_utils.hpp
@@ -92,7 +92,7 @@ inline bool smoothPath(
{
const geometry_msgs::msg::Pose current_pose = planner_data->current_pose.pose;
const double v0 = planner_data->current_velocity->twist.linear.x;
- const double a0 = planner_data->current_accel.get();
+ const double a0 = planner_data->current_acceleration->accel.accel.linear.x;
const auto & external_v_limit = planner_data->external_velocity_limit;
const auto & smoother = planner_data->velocity_smoother_;
diff --git a/planning/behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml b/planning/behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml
index a5c81187575eb..adbf844ce0376 100644
--- a/planning/behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml
+++ b/planning/behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml
@@ -41,6 +41,7 @@
+
diff --git a/planning/behavior_velocity_planner/src/node.cpp b/planning/behavior_velocity_planner/src/node.cpp
index 6d44723389831..3b8711543571e 100644
--- a/planning/behavior_velocity_planner/src/node.cpp
+++ b/planning/behavior_velocity_planner/src/node.cpp
@@ -113,6 +113,9 @@ BehaviorVelocityPlannerNode::BehaviorVelocityPlannerNode(const rclcpp::NodeOptio
"~/input/vehicle_odometry", 1,
std::bind(&BehaviorVelocityPlannerNode::onVehicleVelocity, this, _1),
createSubscriptionOptions(this));
+ sub_acceleration_ = this->create_subscription(
+ "~/input/accel", 1, std::bind(&BehaviorVelocityPlannerNode::onAcceleration, this, _1),
+ createSubscriptionOptions(this));
sub_lanelet_map_ = this->create_subscription(
"~/input/vector_map", rclcpp::QoS(10).transient_local(),
std::bind(&BehaviorVelocityPlannerNode::onLaneletMap, this, _1),
@@ -159,8 +162,6 @@ BehaviorVelocityPlannerNode::BehaviorVelocityPlannerNode(const rclcpp::NodeOptio
// Parameters
forward_path_length_ = this->declare_parameter("forward_path_length", 1000.0);
backward_path_length_ = this->declare_parameter("backward_path_length", 5.0);
- // TODO(yukkysaito): This will become unnecessary when acc output from localization is available.
- planner_data_.accel_lowpass_gain_ = this->declare_parameter("lowpass_gain", 0.5);
planner_data_.stop_line_extend_length = this->declare_parameter("stop_line_extend_length", 5.0);
// nearest search
@@ -225,7 +226,7 @@ bool BehaviorVelocityPlannerNode::isDataReady(
RCLCPP_INFO_THROTTLE(get_logger(), clock, 3000, "Waiting for current velocity");
return false;
}
- if (!d.current_accel) {
+ if (!d.current_acceleration) {
RCLCPP_INFO_THROTTLE(get_logger(), clock, 3000, "Waiting for current acceleration");
return false;
}
@@ -305,8 +306,6 @@ void BehaviorVelocityPlannerNode::onVehicleVelocity(
current_velocity->twist = msg->twist.twist;
planner_data_.current_velocity = current_velocity;
- planner_data_.updateCurrentAcc();
-
// Add velocity to buffer
planner_data_.velocity_buffer.push_front(*current_velocity);
const rclcpp::Time now = this->now();
@@ -329,6 +328,13 @@ void BehaviorVelocityPlannerNode::onVehicleVelocity(
}
}
+void BehaviorVelocityPlannerNode::onAcceleration(
+ const geometry_msgs::msg::AccelWithCovarianceStamped::ConstSharedPtr msg)
+{
+ std::lock_guard lock(mutex_);
+ planner_data_.current_acceleration = msg;
+}
+
void BehaviorVelocityPlannerNode::onParam()
{
planner_data_.velocity_smoother_ =
diff --git a/planning/behavior_velocity_planner/src/scene_module/crosswalk/scene_crosswalk.cpp b/planning/behavior_velocity_planner/src/scene_module/crosswalk/scene_crosswalk.cpp
index 3542b82c1bd92..99ef42304ada4 100644
--- a/planning/behavior_velocity_planner/src/scene_module/crosswalk/scene_crosswalk.cpp
+++ b/planning/behavior_velocity_planner/src/scene_module/crosswalk/scene_crosswalk.cpp
@@ -470,7 +470,7 @@ float CrosswalkModule::calcTargetVelocity(
return 0.0;
}
- const auto ego_acc = planner_data_->current_accel.get();
+ const auto ego_acc = planner_data_->current_acceleration->accel.accel.linear.x;
const auto dist_deceleration = calcSignedArcLength(ego_path.points, ego_pos, stop_point);
const auto feasible_velocity = planning_utils::calcDecelerationVelocityFromDistanceToTarget(
max_jerk, max_accel, ego_acc, ego_vel, dist_deceleration);
diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp
index c821d1044d72c..2acd007f7e759 100644
--- a/planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp
+++ b/planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp
@@ -123,7 +123,7 @@ bool generateStopLine(
{
/* set judge line dist */
const double current_vel = planner_data->current_velocity->twist.linear.x;
- const double current_acc = planner_data->current_accel.get();
+ const double current_acc = planner_data->current_acceleration->accel.accel.linear.x;
const double max_acc = planner_data->max_stop_acceleration_threshold;
const double max_jerk = planner_data->max_stop_jerk_threshold;
const double delay_response_time = planner_data->delay_response_time;
@@ -499,7 +499,7 @@ bool generateStopLineBeforeIntersection(
{
/* set judge line dist */
const double current_vel = planner_data->current_velocity->twist.linear.x;
- const double current_acc = planner_data->current_accel.get();
+ const double current_acc = planner_data->current_acceleration->accel.accel.linear.x;
const double max_acc = planner_data->max_stop_acceleration_threshold;
const double max_jerk = planner_data->max_stop_jerk_threshold;
const double delay_response_time = planner_data->delay_response_time;
diff --git a/planning/behavior_velocity_planner/src/scene_module/no_stopping_area/scene_no_stopping_area.cpp b/planning/behavior_velocity_planner/src/scene_module/no_stopping_area/scene_no_stopping_area.cpp
index 0472cc4aae720..8bae54a382edb 100644
--- a/planning/behavior_velocity_planner/src/scene_module/no_stopping_area/scene_no_stopping_area.cpp
+++ b/planning/behavior_velocity_planner/src/scene_module/no_stopping_area/scene_no_stopping_area.cpp
@@ -368,16 +368,10 @@ bool NoStoppingAreaModule::isStoppable(
{
// get vehicle info and compute pass_judge_line_distance
const auto current_velocity = planner_data_->current_velocity->twist.linear.x;
- const auto current_acceleration = planner_data_->current_accel.get();
+ const auto current_acceleration = planner_data_->current_acceleration->accel.accel.linear.x;
const double max_acc = planner_data_->max_stop_acceleration_threshold;
const double max_jerk = planner_data_->max_stop_jerk_threshold;
const double delay_response_time = planner_data_->delay_response_time;
- if (!planner_data_->current_accel) {
- RCLCPP_WARN_THROTTLE(
- logger_, *clock_, 1000,
- "[no stopping area] empty current acc! check current vel has been received.");
- return false;
- }
const double stoppable_distance = planning_utils::calcJudgeLineDistWithJerkLimit(
current_velocity, current_acceleration, max_acc, max_jerk, delay_response_time);
const double signed_arc_length =
diff --git a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot.cpp b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot.cpp
index c9c062110c1d1..983cdcde488a9 100644
--- a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot.cpp
+++ b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot.cpp
@@ -91,7 +91,7 @@ bool OcclusionSpotModule::modifyPathVelocity(
param_.v.max_stop_jerk = planner_data_->max_stop_jerk_threshold;
param_.v.max_stop_accel = planner_data_->max_stop_acceleration_threshold;
param_.v.v_ego = planner_data_->current_velocity->twist.linear.x;
- param_.v.a_ego = planner_data_->current_accel.get();
+ param_.v.a_ego = planner_data_->current_acceleration->accel.accel.linear.x;
param_.v.delay_time = planner_data_->system_delay;
param_.detection_area_max_length =
planning_utils::calcJudgeLineDistWithJerkLimit(
diff --git a/planning/behavior_velocity_planner/src/scene_module/run_out/scene.cpp b/planning/behavior_velocity_planner/src/scene_module/run_out/scene.cpp
index 25cae0b46802c..0b57d1f20bad5 100644
--- a/planning/behavior_velocity_planner/src/scene_module/run_out/scene.cpp
+++ b/planning/behavior_velocity_planner/src/scene_module/run_out/scene.cpp
@@ -52,7 +52,7 @@ bool RunOutModule::modifyPathVelocity(
// set planner data
const auto current_vel = planner_data_->current_velocity->twist.linear.x;
- const auto current_acc = planner_data_->current_accel.get();
+ const auto current_acc = planner_data_->current_acceleration->accel.accel.linear.x;
const auto & current_pose = planner_data_->current_pose.pose;
// set height of debug data
@@ -129,7 +129,7 @@ Polygons2d RunOutModule::createDetectionAreaPolygon(const PathWithLaneId & smoot
{
// calculate distance needed to stop with jerk and acc constraints
const float initial_vel = planner_data_->current_velocity->twist.linear.x;
- const float initial_acc = planner_data_->current_accel.get();
+ const float initial_acc = planner_data_->current_acceleration->accel.accel.linear.x;
const float target_vel = 0.0;
const float jerk_dec_max = planner_param_.smoother.start_jerk;
const float jerk_dec = planner_param_.run_out.specify_decel_jerk
diff --git a/planning/behavior_velocity_planner/src/scene_module/traffic_light/scene.cpp b/planning/behavior_velocity_planner/src/scene_module/traffic_light/scene.cpp
index a232d097ec5fe..dfeac305d8402 100644
--- a/planning/behavior_velocity_planner/src/scene_module/traffic_light/scene.cpp
+++ b/planning/behavior_velocity_planner/src/scene_module/traffic_light/scene.cpp
@@ -332,17 +332,12 @@ bool TrafficLightModule::isPassthrough(const double & signed_arc_length) const
const double reachable_distance =
planner_data_->current_velocity->twist.linear.x * planner_param_.yellow_lamp_period;
- if (!planner_data_->current_accel) {
- RCLCPP_WARN_THROTTLE(
- logger_, *clock_, 1000,
- "[traffic_light] empty current acc! check current vel has been received.");
- return false;
- }
// Calculate distance until ego vehicle decide not to stop,
// taking into account the jerk and acceleration.
const double pass_judge_line_distance = planning_utils::calcJudgeLineDistWithJerkLimit(
- planner_data_->current_velocity->twist.linear.x, planner_data_->current_accel.get(), max_acc,
- max_jerk, delay_response_time);
+ planner_data_->current_velocity->twist.linear.x,
+ planner_data_->current_acceleration->accel.accel.linear.x, max_acc, max_jerk,
+ delay_response_time);
const bool distance_stoppable = pass_judge_line_distance < signed_arc_length;
const bool slow_velocity = planner_data_->current_velocity->twist.linear.x < 2.0;