feat(behavior_velocity_planner): remove virtual traffic light dependency from behavior_velocity_planner and behavior_velocity_planner_common (#9746)

* feat: remove-virtual-traffic-light-dependency-from-plugin-manager

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

* build passed

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

* fix test

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

* fix test

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

* fix

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

* fix typo

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

---------

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

Author: takayuki5168

planning/behavior_velocity_planner/autoware_behavior_velocity_planner/include/autoware/behavior_velocity_planner/node.hpp

@@ -81,10 +81,6 @@ class BehaviorVelocityPlannerNode : public rclcpp::Node
     autoware_perception_msgs::msg::TrafficLightGroupArray>
     sub_traffic_signals_{this, "~/input/traffic_signals"};
 
-  autoware::universe_utils::InterProcessPollingSubscriber<
-    tier4_v2x_msgs::msg::VirtualTrafficLightStateArray>
-    sub_virtual_traffic_light_states_{this, "~/input/virtual_traffic_light_states"};
-
   autoware::universe_utils::InterProcessPollingSubscriber<nav_msgs::msg::OccupancyGrid>
     sub_occupancy_grid_{this, "~/input/occupancy_grid"};
 

planning/behavior_velocity_planner/autoware_behavior_velocity_planner/package.xml

@@ -56,7 +56,6 @@
   <depend>tf2_geometry_msgs</depend>
   <depend>tf2_ros</depend>
   <depend>tier4_planning_msgs</depend>
-  <depend>tier4_v2x_msgs</depend>
   <depend>visualization_msgs</depend>
 
   <test_depend>ament_cmake_ros</test_depend>

planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/node.cpp

@@ -271,9 +271,6 @@ bool BehaviorVelocityPlannerNode::processData(rclcpp::Clock clock)
     planner_data_.route_handler_ = std::make_shared<route_handler::RouteHandler>(*map_data);
   }
 
-  // optional data
-  getData(planner_data_.virtual_traffic_light_states, sub_virtual_traffic_light_states_);
-
   // planner_data_.external_velocity_limit is std::optional type variable.
   const auto external_velocity_limit = sub_external_velocity_limit_.takeData();
   if (external_velocity_limit) {

planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/test_utils.cpp

@@ -113,8 +113,6 @@ void publishMandatoryTopics(
     test_target_node, "behavior_velocity_planner_node/input/traffic_signals");
   test_manager->publishMaxVelocity(
     test_target_node, "behavior_velocity_planner_node/input/external_velocity_limit_mps");
-  test_manager->publishVirtualTrafficLightState(
-    test_target_node, "behavior_velocity_planner_node/input/virtual_traffic_light_states");
   test_manager->publishOccupancyGrid(
     test_target_node, "behavior_velocity_planner_node/input/occupancy_grid");
 }

planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/planner_data.hpp

@@ -30,7 +30,6 @@
 #include <sensor_msgs/msg/point_cloud2.hpp>
 #include <std_msgs/msg/header.hpp>
 #include <tier4_planning_msgs/msg/velocity_limit.hpp>
-#include <tier4_v2x_msgs/msg/virtual_traffic_light_state_array.hpp>
 
 #include <pcl/point_cloud.h>
 #include <pcl/point_types.h>
@@ -64,7 +63,6 @@ struct PlannerData
   std::map<lanelet::Id, TrafficSignalStamped> traffic_light_id_map_raw_;
   std::map<lanelet::Id, TrafficSignalStamped> traffic_light_id_map_last_observed_;
   std::optional<tier4_planning_msgs::msg::VelocityLimit> external_velocity_limit;
-  tier4_v2x_msgs::msg::VirtualTrafficLightStateArray::ConstSharedPtr virtual_traffic_light_states;
 
   bool is_simulation = false;
 

planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/scene_module_interface.hpp

@@ -32,7 +32,6 @@
 #include <autoware_internal_debug_msgs/msg/float64_stamped.hpp>
 #include <autoware_planning_msgs/msg/path.hpp>
 #include <tier4_planning_msgs/msg/path_with_lane_id.hpp>
-#include <tier4_v2x_msgs/msg/infrastructure_command_array.hpp>
 #include <unique_identifier_msgs/msg/uuid.hpp>
 
 #include <memory>
@@ -95,17 +94,6 @@ class SceneModuleInterface
     planner_data_ = planner_data;
   }
 
-  std::optional<tier4_v2x_msgs::msg::InfrastructureCommand> getInfrastructureCommand()
-  {
-    return infrastructure_command_;
-  }
-
-  void setInfrastructureCommand(
-    const std::optional<tier4_v2x_msgs::msg::InfrastructureCommand> & command)
-  {
-    infrastructure_command_ = command;
-  }
-
   void setTimeKeeper(const std::shared_ptr<universe_utils::TimeKeeper> & time_keeper)
   {
     time_keeper_ = time_keeper;
@@ -123,7 +111,6 @@ class SceneModuleInterface
   rclcpp::Logger logger_;
   rclcpp::Clock::SharedPtr clock_;
   std::shared_ptr<const PlannerData> planner_data_;
-  std::optional<tier4_v2x_msgs::msg::InfrastructureCommand> infrastructure_command_;
   autoware::motion_utils::VelocityFactorInterface velocity_factor_;
   std::vector<ObjectOfInterest> objects_of_interest_;
   mutable std::shared_ptr<universe_utils::TimeKeeper> time_keeper_;
@@ -161,9 +148,6 @@ class SceneModuleManagerInterface
       std::string("~/virtual_wall/") + module_name, 5);
     pub_velocity_factor_ = node.create_publisher<autoware_adapi_v1_msgs::msg::VelocityFactorArray>(
       std::string("/planning/velocity_factors/") + module_name, 1);
-    pub_infrastructure_commands_ =
-      node.create_publisher<tier4_v2x_msgs::msg::InfrastructureCommandArray>(
-        "~/output/infrastructure_commands", 1);
 
     processing_time_publisher_ = std::make_shared<DebugPublisher>(&node, "~/debug");
 
@@ -201,9 +185,6 @@ class SceneModuleManagerInterface
     velocity_factor_array.header.frame_id = "map";
     velocity_factor_array.header.stamp = clock_->now();
 
-    tier4_v2x_msgs::msg::InfrastructureCommandArray infrastructure_command_array;
-    infrastructure_command_array.stamp = clock_->now();
-
     for (const auto & scene_module : scene_modules_) {
       scene_module->resetVelocityFactor();
       scene_module->setPlannerData(planner_data_);
@@ -215,10 +196,6 @@ class SceneModuleManagerInterface
         velocity_factor_array.factors.emplace_back(velocity_factor);
       }
 
-      if (const auto command = scene_module->getInfrastructureCommand()) {
-        infrastructure_command_array.commands.push_back(*command);
-      }
-
       for (const auto & marker : scene_module->createDebugMarkerArray().markers) {
         debug_marker_array.markers.push_back(marker);
       }
@@ -227,7 +204,6 @@ class SceneModuleManagerInterface
     }
 
     pub_velocity_factor_->publish(velocity_factor_array);
-    pub_infrastructure_commands_->publish(infrastructure_command_array);
     pub_debug_->publish(debug_marker_array);
     if (is_publish_debug_path_) {
       tier4_planning_msgs::msg::PathWithLaneId debug_path;
@@ -299,8 +275,6 @@ class SceneModuleManagerInterface
   rclcpp::Publisher<tier4_planning_msgs::msg::PathWithLaneId>::SharedPtr pub_debug_path_;
   rclcpp::Publisher<autoware_adapi_v1_msgs::msg::VelocityFactorArray>::SharedPtr
     pub_velocity_factor_;
-  rclcpp::Publisher<tier4_v2x_msgs::msg::InfrastructureCommandArray>::SharedPtr
-    pub_infrastructure_commands_;
 
   std::shared_ptr<DebugPublisher> processing_time_publisher_;
 

planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/package.xml

@@ -45,7 +45,6 @@
   <depend>tf2_geometry_msgs</depend>
   <depend>tf2_ros</depend>
   <depend>tier4_planning_msgs</depend>
-  <depend>tier4_v2x_msgs</depend>
   <depend>visualization_msgs</depend>
 
   <test_depend>ament_cmake_ros</test_depend>

planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/manager.cpp

@@ -21,6 +21,8 @@
 #include <autoware/universe_utils/ros/parameter.hpp>
 #include <autoware_lanelet2_extension/utility/utilities.hpp>
 
+#include <tier4_v2x_msgs/msg/infrastructure_command_array.hpp>
+
 #include <boost/geometry/algorithms/intersects.hpp>
 
 #include <lanelet2_core/geometry/LineString.h>
@@ -53,6 +55,14 @@ VirtualTrafficLightModuleManager::VirtualTrafficLightModuleManager(rclcpp::Node
     p.check_timeout_after_stop_line =
       getOrDeclareParameter<bool>(node, ns + ".check_timeout_after_stop_line");
   }
+
+  sub_virtual_traffic_light_states_ = autoware::universe_utils::InterProcessPollingSubscriber<
+    tier4_v2x_msgs::msg::VirtualTrafficLightStateArray>::
+    create_subscription(&node, "~/input/virtual_traffic_light_states");
+
+  pub_infrastructure_commands_ =
+    node.create_publisher<tier4_v2x_msgs::msg::InfrastructureCommandArray>(
+      "~/output/infrastructure_commands", 1);
 }
 
 void VirtualTrafficLightModuleManager::launchNewModules(
@@ -89,17 +99,43 @@ void VirtualTrafficLightModuleManager::launchNewModules(
   }
 }
 
-std::function<bool(const std::shared_ptr<SceneModuleInterface> &)>
+std::function<bool(const std::shared_ptr<VirtualTrafficLightModule> &)>
 VirtualTrafficLightModuleManager::getModuleExpiredFunction(
   const tier4_planning_msgs::msg::PathWithLaneId & path)
 {
   const auto id_set = planning_utils::getLaneletIdSetOnPath<VirtualTrafficLight>(
     path, planner_data_->route_handler_->getLaneletMapPtr(), planner_data_->current_odometry->pose);
 
-  return [id_set](const std::shared_ptr<SceneModuleInterface> & scene_module) {
+  return [id_set](const std::shared_ptr<VirtualTrafficLightModule> & scene_module) {
     return id_set.count(scene_module->getModuleId()) == 0;
   };
 }
+
+void VirtualTrafficLightModuleManager::modifyPathVelocity(
+  tier4_planning_msgs::msg::PathWithLaneId * path)
+{
+  // NOTE: virtual traffic light specific implementation
+  //       Since the argument of modifyPathVelocity cannot be changed, the specific information
+  //       of virtual traffic light states is set here.
+  const auto virtual_traffic_light_states = sub_virtual_traffic_light_states_->takeData();
+  for (const auto & scene_module : scene_modules_) {
+    scene_module->setVirtualTrafficLightStates(virtual_traffic_light_states);
+  }
+
+  SceneModuleManagerInterface<VirtualTrafficLightModule>::modifyPathVelocity(path);
+
+  // NOTE: virtual traffic light specific implementation
+  //       publish infrastructure_command_array
+  tier4_v2x_msgs::msg::InfrastructureCommandArray infrastructure_command_array;
+  infrastructure_command_array.stamp = clock_->now();
+
+  for (const auto & scene_module : scene_modules_) {
+    if (const auto command = scene_module->getInfrastructureCommand()) {
+      infrastructure_command_array.commands.push_back(*command);
+    }
+  }
+  pub_infrastructure_commands_->publish(infrastructure_command_array);
+}
 }  // namespace autoware::behavior_velocity_planner
 
 #include <pluginlib/class_list_macros.hpp>

planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/manager.hpp

@@ -20,16 +20,20 @@
 #include <autoware/behavior_velocity_planner_common/plugin_interface.hpp>
 #include <autoware/behavior_velocity_planner_common/plugin_wrapper.hpp>
 #include <autoware/behavior_velocity_planner_common/scene_module_interface.hpp>
+#include <autoware/universe_utils/ros/polling_subscriber.hpp>
 #include <rclcpp/rclcpp.hpp>
 
 #include <tier4_planning_msgs/msg/path_with_lane_id.hpp>
+#include <tier4_v2x_msgs/msg/infrastructure_command_array.hpp>
+#include <tier4_v2x_msgs/msg/virtual_traffic_light_state_array.hpp>
 
 #include <functional>
 #include <memory>
 
 namespace autoware::behavior_velocity_planner
 {
-class VirtualTrafficLightModuleManager : public SceneModuleManagerInterface<>
+class VirtualTrafficLightModuleManager
+: public SceneModuleManagerInterface<VirtualTrafficLightModule>
 {
 public:
   explicit VirtualTrafficLightModuleManager(rclcpp::Node & node);
@@ -38,10 +42,20 @@ class VirtualTrafficLightModuleManager : public SceneModuleManagerInterface<>
 
 private:
   VirtualTrafficLightModule::PlannerParam planner_param_;
+
+  void modifyPathVelocity(tier4_planning_msgs::msg::PathWithLaneId * path) override;
+
   void launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) override;
 
-  std::function<bool(const std::shared_ptr<SceneModuleInterface> &)> getModuleExpiredFunction(
+  std::function<bool(const std::shared_ptr<VirtualTrafficLightModule> &)> getModuleExpiredFunction(
     const tier4_planning_msgs::msg::PathWithLaneId & path) override;
+
+  autoware::universe_utils::InterProcessPollingSubscriber<
+    tier4_v2x_msgs::msg::VirtualTrafficLightStateArray>::SharedPtr
+    sub_virtual_traffic_light_states_;
+
+  rclcpp::Publisher<tier4_v2x_msgs::msg::InfrastructureCommandArray>::SharedPtr
+    pub_infrastructure_commands_;
 };
 
 class VirtualTrafficLightModulePlugin : public PluginWrapper<VirtualTrafficLightModuleManager>

planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.cpp

@@ -333,12 +333,12 @@ bool VirtualTrafficLightModule::isNearAnyEndLine(const size_t end_line_idx)
 std::optional<tier4_v2x_msgs::msg::VirtualTrafficLightState>
 VirtualTrafficLightModule::findCorrespondingState()
 {
-  // No message
-  if (!planner_data_->virtual_traffic_light_states) {
+  // Note: This variable is set by virtual traffic light's manager.
+  if (!virtual_traffic_light_states_) {
     return {};
   }
 
-  for (const auto & state : planner_data_->virtual_traffic_light_states->states) {
+  for (const auto & state : virtual_traffic_light_states_->states) {
     if (state.id == map_data_.instrument_id) {
       return state;
     }
@@ -457,4 +457,23 @@ void VirtualTrafficLightModule::insertStopVelocityAtEndLine(
   module_data_.stop_head_pose_at_end_line =
     calcHeadPose(stop_pose, planner_data_->vehicle_info_.max_longitudinal_offset_m);
 }
+
+std::optional<tier4_v2x_msgs::msg::InfrastructureCommand>
+VirtualTrafficLightModule::getInfrastructureCommand() const
+{
+  return infrastructure_command_;
+}
+
+void VirtualTrafficLightModule::setInfrastructureCommand(
+  const std::optional<tier4_v2x_msgs::msg::InfrastructureCommand> & command)
+{
+  infrastructure_command_ = command;
+}
+
+void VirtualTrafficLightModule::setVirtualTrafficLightStates(
+  const tier4_v2x_msgs::msg::VirtualTrafficLightStateArray::ConstSharedPtr
+    virtual_traffic_light_states)
+{
+  virtual_traffic_light_states_ = virtual_traffic_light_states;
+}
 }  // namespace autoware::behavior_velocity_planner

planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.hpp

@@ -17,13 +17,17 @@
 
 #include <autoware/behavior_velocity_planner_common/scene_module_interface.hpp>
 #include <autoware/universe_utils/geometry/boost_geometry.hpp>
+#include <autoware/universe_utils/ros/polling_subscriber.hpp>
 #include <autoware_lanelet2_extension/regulatory_elements/virtual_traffic_light.hpp>
 #include <autoware_lanelet2_extension/utility/query.hpp>
 #include <autoware_vehicle_info_utils/vehicle_info.hpp>
 #include <nlohmann/json.hpp>
 #include <rclcpp/clock.hpp>
 #include <rclcpp/logger.hpp>
 
+#include <tier4_v2x_msgs/msg/infrastructure_command_array.hpp>
+#include <tier4_v2x_msgs/msg/virtual_traffic_light_state_array.hpp>
+
 #include <lanelet2_core/LaneletMap.h>
 #include <lanelet2_routing/RoutingGraph.h>
 
@@ -84,13 +88,23 @@ class VirtualTrafficLightModule : public SceneModuleInterface
   visualization_msgs::msg::MarkerArray createDebugMarkerArray() override;
   autoware::motion_utils::VirtualWalls createVirtualWalls() override;
 
+  std::optional<tier4_v2x_msgs::msg::InfrastructureCommand> getInfrastructureCommand() const;
+  void setInfrastructureCommand(
+    const std::optional<tier4_v2x_msgs::msg::InfrastructureCommand> & command);
+
+  void setVirtualTrafficLightStates(
+    const tier4_v2x_msgs::msg::VirtualTrafficLightStateArray::ConstSharedPtr
+      virtual_traffic_light_states);
+
 private:
   const int64_t lane_id_;
   const lanelet::autoware::VirtualTrafficLight & reg_elem_;
   const lanelet::ConstLanelet lane_;
   const PlannerParam planner_param_;
+  tier4_v2x_msgs::msg::VirtualTrafficLightStateArray::ConstSharedPtr virtual_traffic_light_states_;
   State state_{State::NONE};
   tier4_v2x_msgs::msg::InfrastructureCommand command_;
+  std::optional<tier4_v2x_msgs::msg::InfrastructureCommand> infrastructure_command_;
   MapData map_data_;
   ModuleData module_data_;
 

planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/test/test_node_interface.cpp

@@ -26,10 +26,17 @@ namespace autoware::behavior_velocity_planner
 TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionPathWithLaneID)
 {
   rclcpp::init(0, nullptr);
+
+  const auto plugin_info_vec = {autoware::behavior_velocity_planner::PluginInfo{
+    "virtual_traffic_light",
+    "autoware::behavior_velocity_planner::VirtualTrafficLightModulePlugin"}};
+
   auto test_manager = autoware::behavior_velocity_planner::generateTestManager();
-  auto test_target_node = autoware::behavior_velocity_planner::generateNode({});
+  auto test_target_node = autoware::behavior_velocity_planner::generateNode(plugin_info_vec);
 
   autoware::behavior_velocity_planner::publishMandatoryTopics(test_manager, test_target_node);
+  test_manager->publishVirtualTrafficLightState(
+    test_target_node, "behavior_velocity_planner_node/input/virtual_traffic_light_states");
 
   // test with nominal path_with_lane_id
   ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPathWithLaneId(test_target_node));
@@ -51,6 +58,8 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose)
   auto test_manager = autoware::behavior_velocity_planner::generateTestManager();
   auto test_target_node = autoware::behavior_velocity_planner::generateNode(plugin_info_vec);
   autoware::behavior_velocity_planner::publishMandatoryTopics(test_manager, test_target_node);
+  test_manager->publishVirtualTrafficLightState(
+    test_target_node, "behavior_velocity_planner_node/input/virtual_traffic_light_states");
 
   // test for normal trajectory
   ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPathWithLaneId(test_target_node));