feat(behavior_path_planner): stop lane_driving planners in non-lane-d…

…riving scenario (tier4#668)

* stop lanedriving in parking scenario

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* use skip_first

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* add scenario remap in launch

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* replace warn to info

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py

@@ -138,6 +138,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/scenario", "/planning/scenario_planning/scenario"),
             (
                 "~/input/external_approval",
                 "/planning/scenario_planning/lane_driving/behavior_planning/"

planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp

@@ -45,6 +45,7 @@
 #include <tier4_planning_msgs/msg/path_change_module.hpp>
 #include <tier4_planning_msgs/msg/path_change_module_array.hpp>
 #include <tier4_planning_msgs/msg/path_change_module_id.hpp>
+#include <tier4_planning_msgs/msg/scenario.hpp>
 #include <tier4_planning_msgs/msg/stop_reason_array.hpp>
 
 #include <lanelet2_core/LaneletMap.h>
@@ -74,6 +75,7 @@ using tier4_planning_msgs::msg::AvoidanceDebugMsg;
 using tier4_planning_msgs::msg::AvoidanceDebugMsgArray;
 using tier4_planning_msgs::msg::PathChangeModule;
 using tier4_planning_msgs::msg::PathChangeModuleArray;
+using tier4_planning_msgs::msg::Scenario;
 using visualization_msgs::msg::MarkerArray;
 
 class BehaviorPathPlannerNode : public rclcpp::Node
@@ -85,6 +87,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<Scenario>::SharedPtr scenario_subscriber_;
   rclcpp::Subscription<PredictedObjects>::SharedPtr perception_subscriber_;
   rclcpp::Subscription<ApprovalMsg>::SharedPtr external_approval_subscriber_;
   rclcpp::Subscription<PathChangeModule>::SharedPtr force_approval_subscriber_;
@@ -100,6 +103,7 @@ class BehaviorPathPlannerNode : public rclcpp::Node
   std::shared_ptr<PlannerData> planner_data_;
   std::shared_ptr<BehaviorTreeManager> bt_manager_;
   tier4_autoware_utils::SelfPoseListener self_pose_listener_{this};
+  Scenario::SharedPtr current_scenario_{nullptr};
 
   std::string prev_ready_module_name_ = "NONE";
 

planning/behavior_path_planner/launch/behavior_path_planner.launch.xml

@@ -11,6 +11,7 @@
     <remap from="~/input/perception" to="/perception/object_recognition/objects"/>
     <remap from="~/input/odometry" to="/localization/kinematic_state"/>
     <remap from="~/output/path" to="path_with_lane_id"/>
+    <remap from="~/input/scenario" to="/planning/scenario_planning/scenario"/>
 
     <param from="$(find-pkg-share behavior_path_planner)/config/side_shift/side_shift.param.yaml"/>
     <param name="bt_tree_config_path" value="$(find-pkg-share behavior_path_planner)/config/behavior_path_planner_tree.xml"/>

planning/behavior_path_planner/src/behavior_path_planner_node.cpp

@@ -51,6 +51,8 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod
     "~/input/odometry", 1, std::bind(&BehaviorPathPlannerNode::onVelocity, this, _1));
   perception_subscriber_ = create_subscription<PredictedObjects>(
     "~/input/perception", 1, std::bind(&BehaviorPathPlannerNode::onPerception, this, _1));
+  scenario_subscriber_ = create_subscription<Scenario>(
+    "~/input/scenario", 1, [this](const Scenario::SharedPtr msg) { current_scenario_ = msg; });
   external_approval_subscriber_ = create_subscription<ApprovalMsg>(
     "~/input/external_approval", 1,
     std::bind(&BehaviorPathPlannerNode::onExternalApproval, this, _1));
@@ -445,8 +447,15 @@ BehaviorTreeManagerParam BehaviorPathPlannerNode::getBehaviorTreeManagerParam()
 void BehaviorPathPlannerNode::waitForData()
 {
   // wait until mandatory data is ready
+  while (!current_scenario_ && rclcpp::ok()) {
+    RCLCPP_INFO_SKIPFIRST_THROTTLE(get_logger(), *get_clock(), 5000, "waiting for scenario topic");
+    rclcpp::spin_some(this->get_node_base_interface());
+    rclcpp::Rate(100).sleep();
+  }
+
   while (!planner_data_->route_handler->isHandlerReady() && rclcpp::ok()) {
-    RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 5000, "waiting for route to be ready");
+    RCLCPP_INFO_SKIPFIRST_THROTTLE(
+      get_logger(), *get_clock(), 5000, "waiting for route to be ready");
     rclcpp::spin_some(this->get_node_base_interface());
     rclcpp::Rate(100).sleep();
   }
@@ -455,7 +464,7 @@ void BehaviorPathPlannerNode::waitForData()
     if (planner_data_->dynamic_object && planner_data_->self_odometry) {
       break;
     }
-    RCLCPP_WARN_THROTTLE(
+    RCLCPP_INFO_SKIPFIRST_THROTTLE(
       get_logger(), *get_clock(), 5000,
       "waiting for vehicle pose, vehicle_velocity, and obstacles");
     rclcpp::spin_some(this->get_node_base_interface());
@@ -470,6 +479,11 @@ void BehaviorPathPlannerNode::run()
 {
   RCLCPP_DEBUG(get_logger(), "----- BehaviorPathPlannerNode start -----");
 
+  // behavior_path_planner runs only in LANE DRIVING scenario.
+  if (current_scenario_->current_scenario != Scenario::LANEDRIVING) {
+    return;
+  }
+
   // update planner data
   updateCurrentPose();