feat(behavior_planning): use acceleration from localization module (a…

…utowarefoundation#1859)

* feat(behavior_path_planner): subscribe acceleration from localization module

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

* feat(behavior_velocity_planner): subscribe acceleration from localization module

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

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",

launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml

@@ -8,6 +8,7 @@
     <remap from="~/input/vector_map" to="$(var map_topic_name)"/>
     <remap from="~/input/perception" to="/perception/object_recognition/objects"/>
     <remap from="~/input/odometry" to="/localization/kinematic_state"/>
+    <remap from="~/input/accel" to="/localization/acceleration"/>
     <remap from="~/output/path" to="path_with_lane_id"/>
     <remap from="~/output/turn_indicators_cmd" to="/planning/turn_signal_cmd"/>
     <remap from="~/output/hazard_lights_cmd" to="/planning/hazard_signal_cmd"/>

planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp

@@ -86,6 +86,7 @@ class BehaviorPathPlannerNode : public rclcpp::Node
   rclcpp::Subscription<HADMapRoute>::SharedPtr route_subscriber_;
   rclcpp::Subscription<HADMapBin>::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_;
@@ -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);

planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp

@@ -22,6 +22,7 @@
 
 #include <autoware_auto_perception_msgs/msg/predicted_objects.hpp>
 #include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
+#include <geometry_msgs/msg/accel_with_covariance_stamped.hpp>
 #include <geometry_msgs/msg/pose_stamped.hpp>
 #include <nav_msgs/msg/odometry.hpp>
 
@@ -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>()};
   PathWithLaneId::SharedPtr prev_output_path{std::make_shared<PathWithLaneId>()};
-  BehaviorPathPlannerParameters parameters{};
   lanelet::ConstLanelets current_lanes{};
   std::shared_ptr<RouteHandler> route_handler{std::make_shared<RouteHandler>()};
+  BehaviorPathPlannerParameters parameters{};
   Approval approval{};
 };
 

planning/behavior_path_planner/launch/behavior_path_planner.launch.xml

@@ -10,6 +10,7 @@
     <remap from="~/input/vector_map" to="$(var map_topic_name)"/>
     <remap from="~/input/perception" to="/perception/object_recognition/objects"/>
     <remap from="~/input/odometry" to="/localization/kinematic_state"/>
+    <remap from="~/input/accel" to="/localization/acceleration"/>
     <remap from="~/output/path" to="path_with_lane_id"/>
     <remap from="~/input/scenario" to="/planning/scenario_planning/scenario"/>
 

planning/behavior_path_planner/src/behavior_path_planner_node.cpp

@@ -77,6 +77,9 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod
   velocity_subscriber_ = create_subscription<Odometry>(
     "~/input/odometry", 1, std::bind(&BehaviorPathPlannerNode::onVelocity, 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));
@@ -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<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_);
+  planner_data_->self_acceleration = msg;
+}
 void BehaviorPathPlannerNode::onPerception(const PredictedObjects::ConstSharedPtr msg)
 {
   std::lock_guard<std::mutex> lock(mutex_pd_);

planning/behavior_velocity_planner/include/behavior_velocity_planner/node.hpp

@@ -58,6 +58,7 @@ class BehaviorVelocityPlannerNode : public rclcpp::Node
     sub_predicted_objects_;
   rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr sub_no_ground_pointcloud_;
   rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr sub_vehicle_odometry_;
+  rclcpp::Subscription<geometry_msgs::msg::AccelWithCovarianceStamped>::SharedPtr sub_acceleration_;
   rclcpp::Subscription<autoware_auto_mapping_msgs::msg::HADMapBin>::SharedPtr sub_lanelet_map_;
   rclcpp::Subscription<autoware_auto_perception_msgs::msg::TrafficSignalArray>::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);

planning/behavior_velocity_planner/include/behavior_velocity_planner/planner_data.hpp

@@ -25,6 +25,7 @@
 #include <autoware_auto_perception_msgs/msg/predicted_objects.hpp>
 #include <autoware_auto_perception_msgs/msg/traffic_signal_array.hpp>
 #include <autoware_auto_perception_msgs/msg/traffic_signal_stamped.hpp>
+#include <geometry_msgs/msg/accel_with_covariance_stamped.hpp>
 #include <geometry_msgs/msg/pose_stamped.hpp>
 #include <geometry_msgs/msg/twist_stamped.hpp>
 #include <nav_msgs/msg/occupancy_grid.hpp>
@@ -70,7 +71,7 @@ struct PlannerData
 
   // msgs from callbacks that are used for data-ready
   geometry_msgs::msg::TwistStamped::ConstSharedPtr current_velocity;
-  boost::optional<double> current_accel;
+  geometry_msgs::msg::AccelWithCovarianceStamped::ConstSharedPtr current_acceleration;
   static constexpr double velocity_buffer_time_sec = 10.0;
   std::deque<geometry_msgs::msg::TwistStamped> velocity_buffer;
   autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr predicted_objects;
@@ -156,44 +157,6 @@ struct PlannerData
     return std::make_shared<autoware_auto_perception_msgs::msg::TrafficSignalStamped>(
       external_traffic_light_id_map.at(id));
   }
-
-private:
-  boost::optional<double> 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
 

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_;
 

planning/behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml

@@ -41,6 +41,7 @@
     <remap from="~/input/path_with_lane_id" to="path_with_lane_id"/>
     <remap from="~/input/vector_map" to="$(var map_topic_name)"/>
     <remap from="~/input/vehicle_odometry" to="/localization/kinematic_state"/>
+    <remap from="~/input/accel" to="/localization/acceleration"/>
     <remap from="~/input/dynamic_objects" to="/perception/object_recognition/objects"/>
     <remap from="~/input/no_ground_pointcloud" to="/perception/obstacle_segmentation/pointcloud"/>
     <remap from="~/input/traffic_signals" to="$(var input_traffic_light_topic_name)"/>

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<geometry_msgs::msg::AccelWithCovarianceStamped>(
+    "~/input/accel", 1, std::bind(&BehaviorVelocityPlannerNode::onAcceleration, this, _1),
+    createSubscriptionOptions(this));
   sub_lanelet_map_ = this->create_subscription<autoware_auto_mapping_msgs::msg::HADMapBin>(
     "~/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<std::mutex> lock(mutex_);
+  planner_data_.current_acceleration = msg;
+}
+
 void BehaviorVelocityPlannerNode::onParam()
 {
   planner_data_.velocity_smoother_ =

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);

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;

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 =

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(

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

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;