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

Signed-off-by: Takayuki Murooka <[email protected]>

planning/behavior_velocity_planner/autoware_behavior_velocity_planner/package.xml

@@ -57,7 +57,6 @@
   <depend>tf2_geometry_msgs</depend>
   <depend>tf2_ros</depend>
   <depend>tier4_planning_msgs</depend>
-  <depend>tier4_v2x_msgs</depend>
   <depend>visualization_msgs</depend>
 
   <exec_depend>rosidl_default_runtime</exec_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/node.hpp

@@ -84,10 +84,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_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_virtual_traffic_light_module/src/manager.cpp

@@ -53,6 +53,10 @@ 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");
 }
 
 void VirtualTrafficLightModuleManager::launchNewModules(
@@ -84,7 +88,8 @@ void VirtualTrafficLightModuleManager::launchNewModules(
         ego_path_linestring, lanelet::utils::to2D(stop_line_opt.value()).basicLineString())) {
       registerModule(std::make_shared<VirtualTrafficLightModule>(
         module_id, lane_id, *m.first, m.second, planner_param_,
-        logger_.get_child("virtual_traffic_light_module"), clock_));
+        logger_.get_child("virtual_traffic_light_module"), clock_,
+        sub_virtual_traffic_light_states_));
     }
   }
 }

planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/manager.hpp

@@ -20,9 +20,11 @@
 #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/virtual_traffic_light_state_array.hpp>
 
 #include <functional>
 #include <memory>
@@ -42,6 +44,10 @@ class VirtualTrafficLightModuleManager : public SceneModuleManagerInterface
 
   std::function<bool(const std::shared_ptr<SceneModuleInterface> &)> getModuleExpiredFunction(
     const tier4_planning_msgs::msg::PathWithLaneId & path) override;
+
+  autoware::universe_utils::InterProcessPollingSubscriber<
+    tier4_v2x_msgs::msg::VirtualTrafficLightStateArray>::SharedPtr
+    sub_virtual_traffic_light_states_;
 };
 
 class VirtualTrafficLightModulePlugin : public PluginWrapper<VirtualTrafficLightModuleManager>

planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.cpp

@@ -38,12 +38,15 @@ VirtualTrafficLightModule::VirtualTrafficLightModule(
   const int64_t module_id, const int64_t lane_id,
   const lanelet::autoware::VirtualTrafficLight & reg_elem, lanelet::ConstLanelet lane,
   const PlannerParam & planner_param, const rclcpp::Logger logger,
-  const rclcpp::Clock::SharedPtr clock)
+  const rclcpp::Clock::SharedPtr clock,
+  const autoware::universe_utils::InterProcessPollingSubscriber<
+    tier4_v2x_msgs::msg::VirtualTrafficLightStateArray>::SharedPtr sub_virtual_traffic_light_states)
 : SceneModuleInterface(module_id, logger, clock),
   lane_id_(lane_id),
   reg_elem_(reg_elem),
   lane_(lane),
-  planner_param_(planner_param)
+  planner_param_(planner_param),
+  sub_virtual_traffic_light_states_(sub_virtual_traffic_light_states)
 {
   velocity_factor_.init(PlanningBehavior::VIRTUAL_TRAFFIC_LIGHT);
 
@@ -334,11 +337,12 @@ std::optional<tier4_v2x_msgs::msg::VirtualTrafficLightState>
 VirtualTrafficLightModule::findCorrespondingState()
 {
   // No message
-  if (!planner_data_->virtual_traffic_light_states) {
+  const auto virtual_traffic_light_states = sub_virtual_traffic_light_states_->takeData();
+  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;
     }

planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.hpp

@@ -17,13 +17,16 @@
 
 #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/virtual_traffic_light_state_array.hpp>
+
 #include <lanelet2_core/LaneletMap.h>
 #include <lanelet2_routing/RoutingGraph.h>
 
@@ -77,7 +80,10 @@ class VirtualTrafficLightModule : public SceneModuleInterface
     const int64_t module_id, const int64_t lane_id,
     const lanelet::autoware::VirtualTrafficLight & reg_elem, lanelet::ConstLanelet lane,
     const PlannerParam & planner_param, const rclcpp::Logger logger,
-    const rclcpp::Clock::SharedPtr clock);
+    const rclcpp::Clock::SharedPtr clock,
+    const autoware::universe_utils::InterProcessPollingSubscriber<
+      tier4_v2x_msgs::msg::VirtualTrafficLightStateArray>::SharedPtr
+      sub_virtual_traffic_light_states);
 
   bool modifyPathVelocity(PathWithLaneId * path) override;
 
@@ -93,6 +99,9 @@ class VirtualTrafficLightModule : public SceneModuleInterface
   tier4_v2x_msgs::msg::InfrastructureCommand command_;
   MapData map_data_;
   ModuleData module_data_;
+  autoware::universe_utils::InterProcessPollingSubscriber<
+    tier4_v2x_msgs::msg::VirtualTrafficLightStateArray>::SharedPtr
+    sub_virtual_traffic_light_states_;
 
   void updateInfrastructureCommand();