refactor(behavior_velocity_planner): removed external input from beha…

…vior_velocity (autowarefoundation#3407)

removed external input from behavior_velocity

Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>

planning/behavior_velocity_planner/config/intersection.param.yaml

@@ -21,7 +21,7 @@
       use_stuck_stopline: false # stopline generate before the intersection lanelet when is_stuck
       assumed_front_car_decel: 1.0 # [m/ss] the expected deceleration of front car when front car as well as ego are turning
       enable_front_car_decel_prediction: false # By default this feature is disabled
-      stop_overshoot_margin: 0.5 # [m] overshoot margin for stuck, collsion detection
+      stop_overshoot_margin: 0.5 # [m] overshoot margin for stuck, collision detection
 
-    merge_from_private_road:
+    merge_from_private:
       stop_duration_sec: 1.0

planning/behavior_velocity_planner/config/traffic_light.param.yaml

@@ -3,6 +3,5 @@
     traffic_light:
       stop_margin: 0.0
       tl_state_timeout: 1.0
-      external_tl_state_timeout: 1.0
       yellow_lamp_period: 2.75
       enable_pass_judge: true

planning/behavior_velocity_planner/include/behavior_velocity_planner/node.hpp

@@ -62,12 +62,6 @@ class BehaviorVelocityPlannerNode : public rclcpp::Node
   rclcpp::Subscription<autoware_auto_mapping_msgs::msg::HADMapBin>::SharedPtr sub_lanelet_map_;
   rclcpp::Subscription<autoware_auto_perception_msgs::msg::TrafficSignalArray>::SharedPtr
     sub_traffic_signals_;
-  rclcpp::Subscription<tier4_api_msgs::msg::CrosswalkStatus>::SharedPtr
-    sub_external_crosswalk_states_;
-  rclcpp::Subscription<tier4_api_msgs::msg::IntersectionStatus>::SharedPtr
-    sub_external_intersection_states_;
-  rclcpp::Subscription<autoware_auto_perception_msgs::msg::TrafficSignalArray>::SharedPtr
-    sub_external_traffic_signals_;
   rclcpp::Subscription<tier4_v2x_msgs::msg::VirtualTrafficLightStateArray>::SharedPtr
     sub_virtual_traffic_light_states_;
   rclcpp::Subscription<nav_msgs::msg::OccupancyGrid>::SharedPtr sub_occupancy_grid_;
@@ -83,11 +77,6 @@ class BehaviorVelocityPlannerNode : public rclcpp::Node
   void onLaneletMap(const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr msg);
   void onTrafficSignals(
     const autoware_auto_perception_msgs::msg::TrafficSignalArray::ConstSharedPtr msg);
-  void onExternalTrafficSignals(
-    const autoware_auto_perception_msgs::msg::TrafficSignalArray::ConstSharedPtr msg);
-  void onExternalCrosswalkStates(const tier4_api_msgs::msg::CrosswalkStatus::ConstSharedPtr msg);
-  void onExternalIntersectionStates(
-    const tier4_api_msgs::msg::IntersectionStatus::ConstSharedPtr msg);
   void onVirtualTrafficLightStates(
     const tier4_v2x_msgs::msg::VirtualTrafficLightStateArray::ConstSharedPtr msg);
   void onOccupancyGrid(const nav_msgs::msg::OccupancyGrid::ConstSharedPtr msg);

planning/behavior_velocity_planner/include/behavior_velocity_planner/planner_data.hpp

@@ -85,11 +85,6 @@ struct PlannerData
 
   // other internal data
   std::map<int, autoware_auto_perception_msgs::msg::TrafficSignalStamped> traffic_light_id_map;
-  // external data
-  std::map<int, autoware_auto_perception_msgs::msg::TrafficSignalStamped>
-    external_traffic_light_id_map;
-  boost::optional<tier4_api_msgs::msg::CrosswalkStatus> external_crosswalk_status_input;
-  boost::optional<tier4_api_msgs::msg::IntersectionStatus> external_intersection_status_input;
   boost::optional<tier4_planning_msgs::msg::VelocityLimit> external_velocity_limit;
   tier4_v2x_msgs::msg::VirtualTrafficLightStateArray::ConstSharedPtr virtual_traffic_light_states;
 
@@ -147,16 +142,6 @@ struct PlannerData
     return std::make_shared<autoware_auto_perception_msgs::msg::TrafficSignalStamped>(
       traffic_light_id_map.at(id));
   }
-
-  std::shared_ptr<autoware_auto_perception_msgs::msg::TrafficSignalStamped>
-  getExternalTrafficSignal(const int id) const
-  {
-    if (external_traffic_light_id_map.count(id) == 0) {
-      return {};
-    }
-    return std::make_shared<autoware_auto_perception_msgs::msg::TrafficSignalStamped>(
-      external_traffic_light_id_map.at(id));
-  }
 };
 }  // namespace behavior_velocity_planner
 

planning/behavior_velocity_planner/include/scene_module/crosswalk/scene_crosswalk.hpp

@@ -81,7 +81,6 @@ class CrosswalkModule : public SceneModuleInterface
     double max_yield_timeout;
     double ego_yield_query_stop_duration;
     // param for input data
-    double external_input_timeout;
     double tl_state_timeout;
     // param for target area & object
     double crosswalk_attention_range;
@@ -147,8 +146,6 @@ class CrosswalkModule : public SceneModuleInterface
 
   bool isTargetType(const PredictedObject & object) const;
 
-  bool isTargetExternalInputStatus(const int target_status) const;
-
   static geometry_msgs::msg::Polygon createObjectPolygon(
     const double width_m, const double length_m);
 

planning/behavior_velocity_planner/include/scene_module/crosswalk/scene_walkway.hpp

@@ -41,7 +41,6 @@ class WalkwayModule : public SceneModuleInterface
   {
     double stop_line_distance;
     double stop_duration_sec;
-    double external_input_timeout;
   };
   WalkwayModule(
     const int64_t module_id, lanelet::ConstLanelet walkway, const PlannerParam & planner_param,

planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp

@@ -84,8 +84,7 @@ class IntersectionModule : public SceneModuleInterface
     double detection_area_angle_thr;     //! threshold in checking the angle of detecting objects
     double min_predicted_path_confidence;
     //! minimum confidence value of predicted path to use for collision detection
-    double external_input_timeout;          //! used to disable external input
-    double minimum_ego_predicted_velocity;  //! used to calclate ego's future velocity profile
+    double minimum_ego_predicted_velocity;  //! used to calculate ego's future velocity profile
     double collision_start_margin_time;     //! start margin time to check collision
     double collision_end_margin_time;       //! end margin time to check collision
     bool use_stuck_stopline;  //! stopline generate before the intersection lanelet when is_stuck.
@@ -207,13 +206,6 @@ class IntersectionModule : public SceneModuleInterface
   bool isTargetStuckVehicleType(
     const autoware_auto_perception_msgs::msg::PredictedObject & object) const;
 
-  /**
-   * @brief Whether target tier4_api_msgs::Intersection::status is valid or not
-   * @param target_status target tier4_api_msgs::Intersection::status
-   * @return rue if the object has a target type
-   */
-  bool isTargetExternalInputStatus(const int target_status);
-
   /**
    * @brief Whether the given pose belongs to any target lanelet or not
    * @param pose pose to be checked

planning/behavior_velocity_planner/include/scene_module/traffic_light/scene.hpp

@@ -40,7 +40,7 @@ class TrafficLightModule : public SceneModuleInterface
 {
 public:
   enum class State { APPROACH, GO_OUT };
-  enum class Input { PERCEPTION, EXTERNAL, NONE };  // EXTERNAL: FOA, V2X, etc.
+  enum class Input { PERCEPTION, NONE };  // EXTERNAL: FOA, V2X, etc.
 
   struct DebugData
   {
@@ -61,7 +61,6 @@ class TrafficLightModule : public SceneModuleInterface
   {
     double stop_margin;
     double tl_state_timeout;
-    double external_tl_state_timeout;
     double yellow_lamp_period;
     bool enable_pass_judge;
   };
@@ -115,10 +114,6 @@ class TrafficLightModule : public SceneModuleInterface
     const lanelet::ConstLineStringsOrPolygons3d & traffic_lights,
     autoware_auto_perception_msgs::msg::TrafficSignalStamped & highest_confidence_tl_state);
 
-  bool getExternalTrafficSignal(
-    const lanelet::ConstLineStringsOrPolygons3d & traffic_lights,
-    autoware_auto_perception_msgs::msg::TrafficSignalStamped & external_tl_state);
-
   bool updateTrafficSignal(const lanelet::ConstLineStringsOrPolygons3d & traffic_lights);
 
   autoware_auto_perception_msgs::msg::TrafficSignalWithJudge generateTlStateWithJudgeFromTlState(

planning/behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml

@@ -5,7 +5,6 @@
 
   <arg name="input_route_topic_name" default="/planning/mission_planning/route"/>
   <arg name="input_traffic_light_topic_name" default="/perception/traffic_light_recognition/traffic_signals"/>
-  <arg name="external_traffic_light_topic_name" default="/external/traffic_light_recognition/traffic_signals"/>
   <arg name="virtual_traffic_light_topic_name" default="/awapi/tmp/virtual_traffic_light_states"/>
   <arg name="map_topic_name" default="/map/vector_map"/>
 
@@ -45,7 +44,6 @@
     <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)"/>
-    <remap from="~/input/external_traffic_signals" to="$(var external_traffic_light_topic_name)"/>
     <remap from="~/input/virtual_traffic_light_states" to="$(var virtual_traffic_light_topic_name)"/>
     <remap from="~/input/occupancy_grid" to="/sensing/lidar/occupancy_grid"/>
     <remap from="~/output/path" to="path"/>

planning/behavior_velocity_planner/src/node.cpp

@@ -125,22 +125,9 @@ BehaviorVelocityPlannerNode::BehaviorVelocityPlannerNode(const rclcpp::NodeOptio
       "~/input/traffic_signals", 1,
       std::bind(&BehaviorVelocityPlannerNode::onTrafficSignals, this, _1),
       createSubscriptionOptions(this));
-  sub_external_crosswalk_states_ = this->create_subscription<tier4_api_msgs::msg::CrosswalkStatus>(
-    "~/input/external_crosswalk_states", 1,
-    std::bind(&BehaviorVelocityPlannerNode::onExternalCrosswalkStates, this, _1),
-    createSubscriptionOptions(this));
-  sub_external_intersection_states_ =
-    this->create_subscription<tier4_api_msgs::msg::IntersectionStatus>(
-      "~/input/external_intersection_states", 1,
-      std::bind(&BehaviorVelocityPlannerNode::onExternalIntersectionStates, this, _1));
   sub_external_velocity_limit_ = this->create_subscription<VelocityLimit>(
     "~/input/external_velocity_limit_mps", rclcpp::QoS{1}.transient_local(),
     std::bind(&BehaviorVelocityPlannerNode::onExternalVelocityLimit, this, _1));
-  sub_external_traffic_signals_ =
-    this->create_subscription<autoware_auto_perception_msgs::msg::TrafficSignalArray>(
-      "~/input/external_traffic_signals", 1,
-      std::bind(&BehaviorVelocityPlannerNode::onExternalTrafficSignals, this, _1),
-      createSubscriptionOptions(this));
   sub_virtual_traffic_light_states_ =
     this->create_subscription<tier4_v2x_msgs::msg::VirtualTrafficLightStateArray>(
       "~/input/virtual_traffic_light_states", 1,
@@ -361,37 +348,11 @@ void BehaviorVelocityPlannerNode::onTrafficSignals(
   }
 }
 
-void BehaviorVelocityPlannerNode::onExternalCrosswalkStates(
-  const tier4_api_msgs::msg::CrosswalkStatus::ConstSharedPtr msg)
-{
-  std::lock_guard<std::mutex> lock(mutex_);
-  planner_data_.external_crosswalk_status_input = *msg;
-}
-
-void BehaviorVelocityPlannerNode::onExternalIntersectionStates(
-  const tier4_api_msgs::msg::IntersectionStatus::ConstSharedPtr msg)
-{
-  std::lock_guard<std::mutex> lock(mutex_);
-  planner_data_.external_intersection_status_input = *msg;
-}
-
 void BehaviorVelocityPlannerNode::onExternalVelocityLimit(const VelocityLimit::ConstSharedPtr msg)
 {
   planner_data_.external_velocity_limit = *msg;
 }
 
-void BehaviorVelocityPlannerNode::onExternalTrafficSignals(
-  const autoware_auto_perception_msgs::msg::TrafficSignalArray::ConstSharedPtr msg)
-{
-  std::lock_guard<std::mutex> lock(mutex_);
-  for (const auto & signal : msg->signals) {
-    autoware_auto_perception_msgs::msg::TrafficSignalStamped traffic_signal;
-    traffic_signal.header = msg->header;
-    traffic_signal.signal = signal;
-    planner_data_.external_traffic_light_id_map[signal.map_primitive_id] = traffic_signal;
-  }
-}
-
 void BehaviorVelocityPlannerNode::onVirtualTrafficLightStates(
   const tier4_v2x_msgs::msg::VirtualTrafficLightStateArray::ConstSharedPtr msg)
 {

planning/behavior_velocity_planner/src/scene_module/crosswalk/manager.cpp

@@ -114,7 +114,6 @@ CrosswalkModuleManager::CrosswalkModuleManager(rclcpp::Node & node)
     node.declare_parameter(ns + ".ego_yield_query_stop_duration", 0.1);
 
   // param for input data
-  cp.external_input_timeout = node.declare_parameter(ns + ".external_input_timeout", 1.0);
   cp.tl_state_timeout = node.declare_parameter(ns + ".tl_state_timeout", 1.0);
 
   // param for target area & object
@@ -164,7 +163,6 @@ WalkwayModuleManager::WalkwayModuleManager(rclcpp::Node & node)
   auto & wp = walkway_planner_param_;
   wp.stop_line_distance = node.declare_parameter(ns + ".stop_line_distance", 1.0);
   wp.stop_duration_sec = node.declare_parameter(ns + ".stop_duration_sec", 1.0);
-  wp.external_input_timeout = node.declare_parameter(ns + ".external_input_timeout", 1.0);
 }
 
 void WalkwayModuleManager::launchNewModules(

planning/behavior_velocity_planner/src/scene_module/crosswalk/scene_crosswalk.cpp

@@ -923,21 +923,6 @@ bool CrosswalkModule::isTargetType(const PredictedObject & object) const
   return false;
 }
 
-bool CrosswalkModule::isTargetExternalInputStatus(const int target_status) const
-{
-  const auto & ex_input = planner_data_->external_crosswalk_status_input;
-  if (!ex_input) {
-    return false;
-  }
-
-  const auto time_delta = (clock_->now() - ex_input.get().header.stamp).seconds();
-  if (planner_param_.external_input_timeout < time_delta) {
-    return false;
-  }
-
-  return ex_input.get().status == target_status;
-}
-
 geometry_msgs::msg::Polygon CrosswalkModule::createObjectPolygon(
   const double width_m, const double length_m)
 {

planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp

@@ -49,7 +49,6 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node)
     node.declare_parameter(ns + ".detection_area_angle_threshold", M_PI / 4.0);
   ip.min_predicted_path_confidence =
     node.declare_parameter(ns + ".min_predicted_path_confidence", 0.05);
-  ip.external_input_timeout = node.declare_parameter(ns + ".walkway.external_input_timeout", 1.0);
   ip.minimum_ego_predicted_velocity =
     node.declare_parameter(ns + ".minimum_ego_predicted_velocity", 1.388);
   ip.collision_start_margin_time = node.declare_parameter(ns + ".collision_start_margin_time", 5.0);
@@ -66,8 +65,7 @@ MergeFromPrivateModuleManager::MergeFromPrivateModuleManager(rclcpp::Node & node
 {
   const std::string ns(getModuleName());
   auto & mp = merge_from_private_area_param_;
-  mp.stop_duration_sec =
-    node.declare_parameter(ns + ".merge_from_private_area.stop_duration_sec", 1.0);
+  mp.stop_duration_sec = node.declare_parameter(ns + ".stop_duration_sec", 1.0);
   mp.detection_area_length = node.get_parameter("intersection.detection_area_length").as_double();
   mp.detection_area_right_margin =
     node.get_parameter("intersection.detection_area_right_margin").as_double();

planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp

@@ -75,13 +75,8 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason *
 {
   RCLCPP_DEBUG(logger_, "===== plan start =====");
 
-  const bool external_go = isTargetExternalInputStatus(tier4_api_msgs::msg::IntersectionStatus::GO);
-  const bool external_stop =
-    isTargetExternalInputStatus(tier4_api_msgs::msg::IntersectionStatus::STOP);
-  const StateMachine::State current_state = state_machine_.getState();
-
   debug_data_ = DebugData();
-
+  const StateMachine::State current_state = state_machine_.getState();
   *stop_reason = planning_utils::initializeStopReason(StopReason::INTERSECTION);
 
   RCLCPP_DEBUG(
@@ -221,15 +216,9 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason *
 
   /* calculate final stop lines */
   std::optional<size_t> stop_line_idx =
-    std::make_optional<size_t>(stop_lines_idx_opt.value().collision_stop_line);
-  if (external_go) {
-    is_entry_prohibited = false;
-  } else if (external_stop) {
-    is_entry_prohibited = true;
-    stop_line_idx = stop_lines_idx_opt
-                      ? std::make_optional<size_t>(stop_lines_idx_opt.value().collision_stop_line)
-                      : std::nullopt;
-  } else if (is_stuck && stuck_line_idx_opt) {
+    stop_lines_idx_opt ? std::make_optional<size_t>(stop_lines_idx_opt.value().collision_stop_line)
+                       : std::nullopt;
+  if (is_stuck && stuck_line_idx_opt) {
     is_entry_prohibited = true;
     const size_t stuck_line_idx = stuck_line_idx_opt.value();
     const double dist_stuck_stopline = motion_utils::calcSignedArcLength(
@@ -642,14 +631,6 @@ bool IntersectionModule::isTargetStuckVehicleType(
   return false;
 }
 
-bool IntersectionModule::isTargetExternalInputStatus(const int target_status)
-{
-  return planner_data_->external_intersection_status_input &&
-         planner_data_->external_intersection_status_input.get().status == target_status &&
-         (clock_->now() - planner_data_->external_intersection_status_input.get().header.stamp)
-             .seconds() < planner_param_.external_input_timeout;
-}
-
 bool IntersectionModule::checkAngleForTargetLanelets(
   const geometry_msgs::msg::Pose & pose, const lanelet::ConstLanelets & target_lanelets,
   const double margin)

planning/behavior_velocity_planner/src/scene_module/traffic_light/manager.cpp

@@ -32,8 +32,6 @@ TrafficLightModuleManager::TrafficLightModuleManager(rclcpp::Node & node)
   const std::string ns(getModuleName());
   planner_param_.stop_margin = node.declare_parameter(ns + ".stop_margin", 0.0);
   planner_param_.tl_state_timeout = node.declare_parameter(ns + ".tl_state_timeout", 1.0);
-  planner_param_.external_tl_state_timeout =
-    node.declare_parameter(ns + ".external_tl_state_timeout", 1.0);
   planner_param_.enable_pass_judge = node.declare_parameter(ns + ".enable_pass_judge", true);
   planner_param_.yellow_lamp_period = node.declare_parameter(ns + ".yellow_lamp_period", 2.75);
   pub_tl_state_ = node.create_publisher<autoware_auto_perception_msgs::msg::LookingTrafficSignal>(