feat(behavior_path_planner): subscribe acceleration from localization…

… module

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

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