diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/package.xml
index e051374ed3dda..7aecbcceff027 100644
--- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/package.xml
+++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/package.xml
@@ -57,7 +57,6 @@
tf2_geometry_msgs
tf2_ros
tier4_planning_msgs
- tier4_v2x_msgs
visualization_msgs
rosidl_default_runtime
diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/node.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/node.cpp
index d78bc883e6b35..7dadb4fe54708 100644
--- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/node.cpp
+++ b/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(*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) {
diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/node.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/node.hpp
index 4efb58e38b74f..0c4fa0f4c0748 100644
--- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/node.hpp
+++ b/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
sub_occupancy_grid_{this, "~/input/occupancy_grid"};
diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/planner_data.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/planner_data.hpp
index 983aaec2acf4f..f8b37999cb6bf 100644
--- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/planner_data.hpp
+++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/planner_data.hpp
@@ -30,7 +30,6 @@
#include
#include
#include
-#include
#include
#include
@@ -64,7 +63,6 @@ struct PlannerData
std::map traffic_light_id_map_raw_;
std::map traffic_light_id_map_last_observed_;
std::optional external_velocity_limit;
- tier4_v2x_msgs::msg::VirtualTrafficLightStateArray::ConstSharedPtr virtual_traffic_light_states;
bool is_simulation = false;
diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/manager.cpp
index a51cf9211c21f..ca43c78941aea 100644
--- a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/manager.cpp
+++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/manager.cpp
@@ -53,6 +53,9 @@ VirtualTrafficLightModuleManager::VirtualTrafficLightModuleManager(rclcpp::Node
p.check_timeout_after_stop_line =
getOrDeclareParameter(node, ns + ".check_timeout_after_stop_line");
}
+
+ sub_virtual_traffic_light_states_ = autoware::universe_utils::InterProcessPollingSubscriber<
+ std_msgs::msg::String>::create_subscription(&node, "~/input/virtual_traffic_light_states");
}
void VirtualTrafficLightModuleManager::launchNewModules(
@@ -84,7 +87,8 @@ void VirtualTrafficLightModuleManager::launchNewModules(
ego_path_linestring, lanelet::utils::to2D(stop_line_opt.value()).basicLineString())) {
registerModule(std::make_shared(
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_));
}
}
}
diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/manager.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/manager.hpp
index ba5a4b23a6fe0..feda1ee6804f5 100644
--- a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/manager.hpp
+++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/manager.hpp
@@ -23,6 +23,7 @@
#include
#include
+#include
#include
#include
@@ -42,6 +43,10 @@ class VirtualTrafficLightModuleManager : public SceneModuleManagerInterface
std::function &)> 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
diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.cpp
index 206fb14b6d41c..515dfe86f56c5 100644
--- a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.cpp
+++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.cpp
@@ -38,12 +38,14 @@ 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 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 +336,12 @@ std::optional
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;
}
diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.hpp
index b031ba5b2bb2b..1e17617ba449d 100644
--- a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.hpp
+++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.hpp
@@ -24,6 +24,8 @@
#include
#include
+#include
+
#include
#include
@@ -77,7 +79,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 +98,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();