diff --git a/common/tier4_traffic_light_rviz_plugin/README.md b/common/tier4_traffic_light_rviz_plugin/README.md
index 953412a917432..91fb5bc124cb7 100644
--- a/common/tier4_traffic_light_rviz_plugin/README.md
+++ b/common/tier4_traffic_light_rviz_plugin/README.md
@@ -8,9 +8,9 @@ This plugin panel publishes dummy traffic light signals.
### Output
-| Name | Type | Description |
-| ------------------------------------------------------- | -------------------------------------------------------- | ----------------------------- |
-| `/perception/traffic_light_recognition/traffic_signals` | `autoware_auto_perception_msgs::msg::TrafficSignalArray` | Publish traffic light signals |
+| Name | Type | Description |
+| ------------------------------------------------------- | --------------------------------------------------- | ----------------------------- |
+| `/perception/traffic_light_recognition/traffic_signals` | `autoware_perception_msgs::msg::TrafficSignalArray` | Publish traffic light signals |
## HowToUse
diff --git a/common/tier4_traffic_light_rviz_plugin/package.xml b/common/tier4_traffic_light_rviz_plugin/package.xml
index 21d8bae8f6cff..2b118b429f1af 100644
--- a/common/tier4_traffic_light_rviz_plugin/package.xml
+++ b/common/tier4_traffic_light_rviz_plugin/package.xml
@@ -11,7 +11,7 @@
autoware_cmake
autoware_auto_mapping_msgs
- autoware_auto_perception_msgs
+ autoware_perception_msgs
lanelet2_extension
libqt5-core
libqt5-gui
diff --git a/common/tier4_traffic_light_rviz_plugin/src/traffic_light_publish_panel.cpp b/common/tier4_traffic_light_rviz_plugin/src/traffic_light_publish_panel.cpp
index e4fb0095b8d0a..ec5917ee65b57 100644
--- a/common/tier4_traffic_light_rviz_plugin/src/traffic_light_publish_panel.cpp
+++ b/common/tier4_traffic_light_rviz_plugin/src/traffic_light_publish_panel.cpp
@@ -34,6 +34,7 @@
#include
#include
+#undef signals
namespace rviz_plugins
{
TrafficLightPublishPanel::TrafficLightPublishPanel(QWidget * parent) : rviz_common::Panel(parent)
@@ -138,55 +139,55 @@ void TrafficLightPublishPanel::onSetTrafficLightState()
const auto shape = light_shape_combo_->currentText();
const auto status = light_status_combo_->currentText();
- TrafficLight traffic_light;
+ TrafficLightElement traffic_light;
traffic_light.confidence = traffic_light_confidence_input_->value();
if (color == "RED") {
- traffic_light.color = TrafficLight::RED;
+ traffic_light.color = TrafficLightElement::RED;
} else if (color == "AMBER") {
- traffic_light.color = TrafficLight::AMBER;
+ traffic_light.color = TrafficLightElement::AMBER;
} else if (color == "GREEN") {
- traffic_light.color = TrafficLight::GREEN;
+ traffic_light.color = TrafficLightElement::GREEN;
} else if (color == "WHITE") {
- traffic_light.color = TrafficLight::WHITE;
+ traffic_light.color = TrafficLightElement::WHITE;
} else if (color == "UNKNOWN") {
- traffic_light.color = TrafficLight::UNKNOWN;
+ traffic_light.color = TrafficLightElement::UNKNOWN;
}
if (shape == "CIRCLE") {
- traffic_light.shape = TrafficLight::CIRCLE;
+ traffic_light.shape = TrafficLightElement::CIRCLE;
} else if (shape == "LEFT_ARROW") {
- traffic_light.shape = TrafficLight::LEFT_ARROW;
+ traffic_light.shape = TrafficLightElement::LEFT_ARROW;
} else if (shape == "RIGHT_ARROW") {
- traffic_light.shape = TrafficLight::RIGHT_ARROW;
+ traffic_light.shape = TrafficLightElement::RIGHT_ARROW;
} else if (shape == "UP_ARROW") {
- traffic_light.shape = TrafficLight::UP_ARROW;
+ traffic_light.shape = TrafficLightElement::UP_ARROW;
} else if (shape == "DOWN_ARROW") {
- traffic_light.shape = TrafficLight::DOWN_ARROW;
+ traffic_light.shape = TrafficLightElement::DOWN_ARROW;
} else if (shape == "DOWN_LEFT_ARROW") {
- traffic_light.shape = TrafficLight::DOWN_LEFT_ARROW;
+ traffic_light.shape = TrafficLightElement::DOWN_LEFT_ARROW;
} else if (shape == "DOWN_RIGHT_ARROW") {
- traffic_light.shape = TrafficLight::DOWN_RIGHT_ARROW;
+ traffic_light.shape = TrafficLightElement::DOWN_RIGHT_ARROW;
} else if (shape == "UNKNOWN") {
- traffic_light.shape = TrafficLight::UNKNOWN;
+ traffic_light.shape = TrafficLightElement::UNKNOWN;
}
if (status == "SOLID_OFF") {
- traffic_light.status = TrafficLight::SOLID_OFF;
+ traffic_light.status = TrafficLightElement::SOLID_OFF;
} else if (status == "SOLID_ON") {
- traffic_light.status = TrafficLight::SOLID_ON;
+ traffic_light.status = TrafficLightElement::SOLID_ON;
} else if (status == "FLASHING") {
- traffic_light.status = TrafficLight::FLASHING;
+ traffic_light.status = TrafficLightElement::FLASHING;
} else if (status == "UNKNOWN") {
- traffic_light.status = TrafficLight::UNKNOWN;
+ traffic_light.status = TrafficLightElement::UNKNOWN;
}
TrafficSignal traffic_signal;
- traffic_signal.lights.push_back(traffic_light);
- traffic_signal.map_primitive_id = traffic_light_id;
+ traffic_signal.elements.push_back(traffic_light);
+ traffic_signal.traffic_signal_id = traffic_light_id;
for (auto & signal : extra_traffic_signals_.signals) {
- if (signal.map_primitive_id == traffic_light_id) {
+ if (signal.traffic_signal_id == traffic_light_id) {
signal = traffic_signal;
return;
}
@@ -247,7 +248,7 @@ void TrafficLightPublishPanel::createWallTimer()
void TrafficLightPublishPanel::onTimer()
{
if (enable_publish_) {
- extra_traffic_signals_.header.stamp = rclcpp::Clock().now();
+ extra_traffic_signals_.stamp = rclcpp::Clock().now();
pub_traffic_signals_->publish(extra_traffic_signals_);
}
@@ -260,35 +261,35 @@ void TrafficLightPublishPanel::onTimer()
for (size_t i = 0; i < extra_traffic_signals_.signals.size(); ++i) {
const auto & signal = extra_traffic_signals_.signals.at(i);
- if (signal.lights.empty()) {
+ if (signal.elements.empty()) {
continue;
}
- auto id_label = new QLabel(QString::number(signal.map_primitive_id));
+ auto id_label = new QLabel(QString::number(signal.traffic_signal_id));
id_label->setAlignment(Qt::AlignCenter);
auto color_label = new QLabel();
color_label->setAlignment(Qt::AlignCenter);
- const auto & light = signal.lights.front();
+ const auto & light = signal.elements.front();
switch (light.color) {
- case TrafficLight::RED:
+ case TrafficLightElement::RED:
color_label->setText("RED");
color_label->setStyleSheet("background-color: #FF0000;");
break;
- case TrafficLight::AMBER:
+ case TrafficLightElement::AMBER:
color_label->setText("AMBER");
color_label->setStyleSheet("background-color: #FFBF00;");
break;
- case TrafficLight::GREEN:
+ case TrafficLightElement::GREEN:
color_label->setText("GREEN");
color_label->setStyleSheet("background-color: #7CFC00;");
break;
- case TrafficLight::WHITE:
+ case TrafficLightElement::WHITE:
color_label->setText("WHITE");
color_label->setStyleSheet("background-color: #FFFFFF;");
break;
- case TrafficLight::UNKNOWN:
+ case TrafficLightElement::UNKNOWN:
color_label->setText("UNKNOWN");
color_label->setStyleSheet("background-color: #808080;");
break;
@@ -300,31 +301,28 @@ void TrafficLightPublishPanel::onTimer()
shape_label->setAlignment(Qt::AlignCenter);
switch (light.shape) {
- case TrafficLight::CIRCLE:
+ case TrafficLightElement::CIRCLE:
shape_label->setText("CIRCLE");
break;
- case TrafficLight::LEFT_ARROW:
+ case TrafficLightElement::LEFT_ARROW:
shape_label->setText("LEFT_ARROW");
break;
- case TrafficLight::RIGHT_ARROW:
+ case TrafficLightElement::RIGHT_ARROW:
shape_label->setText("RIGHT_ARROW");
break;
- case TrafficLight::UP_ARROW:
+ case TrafficLightElement::UP_ARROW:
shape_label->setText("UP_ARROW");
break;
- case TrafficLight::DOWN_ARROW:
+ case TrafficLightElement::DOWN_ARROW:
shape_label->setText("DOWN_ARROW");
break;
- case TrafficLight::DOWN_LEFT_ARROW:
+ case TrafficLightElement::DOWN_LEFT_ARROW:
shape_label->setText("DOWN_LEFT_ARROW");
break;
- case TrafficLight::DOWN_RIGHT_ARROW:
+ case TrafficLightElement::DOWN_RIGHT_ARROW:
shape_label->setText("DOWN_RIGHT_ARROW");
break;
- case TrafficLight::FLASHING:
- shape_label->setText("FLASHING");
- break;
- case TrafficLight::UNKNOWN:
+ case TrafficLightElement::UNKNOWN:
shape_label->setText("UNKNOWN");
break;
default:
@@ -335,16 +333,16 @@ void TrafficLightPublishPanel::onTimer()
status_label->setAlignment(Qt::AlignCenter);
switch (light.status) {
- case TrafficLight::SOLID_OFF:
+ case TrafficLightElement::SOLID_OFF:
status_label->setText("SOLID_OFF");
break;
- case TrafficLight::SOLID_ON:
+ case TrafficLightElement::SOLID_ON:
status_label->setText("SOLID_ON");
break;
- case TrafficLight::FLASHING:
+ case TrafficLightElement::FLASHING:
status_label->setText("FLASHING");
break;
- case TrafficLight::UNKNOWN:
+ case TrafficLightElement::UNKNOWN:
status_label->setText("UNKNOWN");
break;
default:
diff --git a/common/tier4_traffic_light_rviz_plugin/src/traffic_light_publish_panel.hpp b/common/tier4_traffic_light_rviz_plugin/src/traffic_light_publish_panel.hpp
index 75e6405417084..9981a194ccedc 100644
--- a/common/tier4_traffic_light_rviz_plugin/src/traffic_light_publish_panel.hpp
+++ b/common/tier4_traffic_light_rviz_plugin/src/traffic_light_publish_panel.hpp
@@ -27,7 +27,7 @@
#include
#include
-#include
+#include
#endif
#include
@@ -36,10 +36,9 @@ namespace rviz_plugins
{
using autoware_auto_mapping_msgs::msg::HADMapBin;
-using autoware_auto_perception_msgs::msg::TrafficLight;
-using autoware_auto_perception_msgs::msg::TrafficSignal;
-using autoware_auto_perception_msgs::msg::TrafficSignalArray;
-
+using autoware_perception_msgs::msg::TrafficLightElement;
+using autoware_perception_msgs::msg::TrafficSignal;
+using autoware_perception_msgs::msg::TrafficSignalArray;
class TrafficLightPublishPanel : public rviz_common::Panel
{
Q_OBJECT
diff --git a/perception/crosswalk_traffic_light_estimator/include/crosswalk_traffic_light_estimator/node.hpp b/perception/crosswalk_traffic_light_estimator/include/crosswalk_traffic_light_estimator/node.hpp
index 852e99ff7ba54..6aa391b8dfdfc 100644
--- a/perception/crosswalk_traffic_light_estimator/include/crosswalk_traffic_light_estimator/node.hpp
+++ b/perception/crosswalk_traffic_light_estimator/include/crosswalk_traffic_light_estimator/node.hpp
@@ -22,9 +22,7 @@
#include
#include
-#include
-#include
-#include
+#include
#include
#include
@@ -43,9 +41,9 @@ namespace traffic_light
{
using autoware_auto_mapping_msgs::msg::HADMapBin;
-using autoware_auto_perception_msgs::msg::TrafficLight;
-using autoware_auto_perception_msgs::msg::TrafficSignal;
-using autoware_auto_perception_msgs::msg::TrafficSignalArray;
+using autoware_perception_msgs::msg::TrafficLightElement;
+using autoware_perception_msgs::msg::TrafficSignal;
+using autoware_perception_msgs::msg::TrafficSignalArray;
using autoware_planning_msgs::msg::LaneletRoute;
using tier4_autoware_utils::DebugPublisher;
using tier4_autoware_utils::StopWatch;
diff --git a/perception/crosswalk_traffic_light_estimator/package.xml b/perception/crosswalk_traffic_light_estimator/package.xml
index 1a3b90f617f4e..8db8d2a3f8ee5 100644
--- a/perception/crosswalk_traffic_light_estimator/package.xml
+++ b/perception/crosswalk_traffic_light_estimator/package.xml
@@ -11,8 +11,8 @@
autoware_cmake
autoware_auto_mapping_msgs
- autoware_auto_perception_msgs
autoware_auto_planning_msgs
+ autoware_perception_msgs
autoware_planning_msgs
lanelet2_extension
rclcpp
diff --git a/perception/crosswalk_traffic_light_estimator/src/node.cpp b/perception/crosswalk_traffic_light_estimator/src/node.cpp
index 8400a1708e6af..87ced0e766e51 100644
--- a/perception/crosswalk_traffic_light_estimator/src/node.cpp
+++ b/perception/crosswalk_traffic_light_estimator/src/node.cpp
@@ -160,7 +160,7 @@ void CrosswalkTrafficLightEstimatorNode::onTrafficLightArray(
TrafficLightIdMap traffic_light_id_map;
for (const auto & traffic_signal : msg->signals) {
- traffic_light_id_map[traffic_signal.map_primitive_id] =
+ traffic_light_id_map[traffic_signal.traffic_signal_id] =
std::pair(traffic_signal, get_clock()->now());
}
@@ -185,17 +185,17 @@ void CrosswalkTrafficLightEstimatorNode::updateLastDetectedSignal(
const TrafficLightIdMap & traffic_light_id_map)
{
for (const auto & input_traffic_signal : traffic_light_id_map) {
- const auto & lights = input_traffic_signal.second.first.lights;
+ const auto & lights = input_traffic_signal.second.first.elements;
if (lights.empty()) {
continue;
}
- if (lights.front().color == TrafficLight::UNKNOWN) {
+ if (lights.front().color == TrafficLightElement::UNKNOWN) {
continue;
}
- const auto & id = input_traffic_signal.second.first.map_primitive_id;
+ const auto & id = input_traffic_signal.second.first.traffic_signal_id;
if (last_detect_color_.count(id) == 0) {
last_detect_color_.insert(std::make_pair(id, input_traffic_signal.second));
@@ -207,7 +207,7 @@ void CrosswalkTrafficLightEstimatorNode::updateLastDetectedSignal(
std::vector erase_id_list;
for (auto & last_traffic_signal : last_detect_color_) {
- const auto & id = last_traffic_signal.second.first.map_primitive_id;
+ const auto & id = last_traffic_signal.second.first.traffic_signal_id;
if (traffic_light_id_map.count(id) == 0) {
// hold signal recognition results for [last_detect_color_hold_time_] seconds.
@@ -233,11 +233,11 @@ void CrosswalkTrafficLightEstimatorNode::setCrosswalkTrafficSignal(
const auto ll_traffic_light = static_cast(traffic_light);
TrafficSignal output_traffic_signal;
- TrafficLight output_traffic_light;
+ TrafficLightElement output_traffic_light;
output_traffic_light.color = color;
output_traffic_light.confidence = 1.0;
- output_traffic_signal.lights.push_back(output_traffic_light);
- output_traffic_signal.map_primitive_id = ll_traffic_light.id();
+ output_traffic_signal.elements.push_back(output_traffic_light);
+ output_traffic_signal.traffic_signal_id = ll_traffic_light.id();
msg.signals.push_back(output_traffic_signal);
}
}
@@ -265,13 +265,14 @@ lanelet::ConstLanelets CrosswalkTrafficLightEstimatorNode::getNonRedLanelets(
continue;
}
- const auto current_is_not_red = current_detected_signal
- ? current_detected_signal.get() == TrafficLight::GREEN ||
- current_detected_signal.get() == TrafficLight::AMBER
- : true;
+ const auto current_is_not_red =
+ current_detected_signal ? current_detected_signal.get() == TrafficLightElement::GREEN ||
+ current_detected_signal.get() == TrafficLightElement::AMBER
+ : true;
const auto current_is_unknown_or_none =
- current_detected_signal ? current_detected_signal.get() == TrafficLight::UNKNOWN : true;
+ current_detected_signal ? current_detected_signal.get() == TrafficLightElement::UNKNOWN
+ : true;
const auto last_detected_signal =
getHighestConfidenceTrafficSignal(traffic_lights_for_vehicle, last_detect_color_);
@@ -281,8 +282,8 @@ lanelet::ConstLanelets CrosswalkTrafficLightEstimatorNode::getNonRedLanelets(
}
const auto was_not_red = current_is_unknown_or_none &&
- (last_detected_signal.get() == TrafficLight::GREEN ||
- last_detected_signal.get() == TrafficLight::AMBER) &&
+ (last_detected_signal.get() == TrafficLightElement::GREEN ||
+ last_detected_signal.get() == TrafficLightElement::AMBER) &&
use_last_detect_color_;
if (!current_is_not_red && !was_not_red) {
@@ -323,12 +324,13 @@ uint8_t CrosswalkTrafficLightEstimatorNode::estimateCrosswalkTrafficSignal(
}
if (has_straight_non_red_lane || has_related_non_red_tl) {
- return TrafficLight::RED;
+ return TrafficLightElement::RED;
}
const auto has_merge_lane = hasMergeLane(non_red_lanelets, routing_graph_ptr_);
- return !has_merge_lane && has_left_non_red_lane && has_right_non_red_lane ? TrafficLight::RED
- : TrafficLight::UNKNOWN;
+ return !has_merge_lane && has_left_non_red_lane && has_right_non_red_lane
+ ? TrafficLightElement::RED
+ : TrafficLightElement::UNKNOWN;
}
boost::optional CrosswalkTrafficLightEstimatorNode::getHighestConfidenceTrafficSignal(
@@ -348,7 +350,7 @@ boost::optional CrosswalkTrafficLightEstimatorNode::getHighestConfidenc
continue;
}
- const auto & lights = traffic_light_id_map.at(id).first.lights;
+ const auto & lights = traffic_light_id_map.at(id).first.elements;
if (lights.empty()) {
continue;
}
diff --git a/planning/behavior_velocity_crosswalk_module/package.xml b/planning/behavior_velocity_crosswalk_module/package.xml
index e9b38f86a00ee..d8d624558389e 100644
--- a/planning/behavior_velocity_crosswalk_module/package.xml
+++ b/planning/behavior_velocity_crosswalk_module/package.xml
@@ -17,6 +17,7 @@
autoware_cmake
eigen3_cmake_module
+ autoware_perception_msgs
autoware_auto_perception_msgs
autoware_auto_planning_msgs
autoware_auto_tf2
diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp
index 7047fc7b56f2e..5ad46deb3502a 100644
--- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp
+++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp
@@ -878,16 +878,16 @@ bool CrosswalkModule::isRedSignalForPedestrians() const
if (
planner_param_.tl_state_timeout <
- (clock_->now() - traffic_signal_stamped->header.stamp).seconds()) {
+ (clock_->now() - traffic_signal_stamped->stamp).seconds()) {
continue;
}
- const auto & lights = traffic_signal_stamped->signal.lights;
+ const auto & lights = traffic_signal_stamped->signal.elements;
if (lights.empty()) {
continue;
}
- if (lights.front().color == TrafficLight::RED) {
+ if (lights.front().color == TrafficLightElement::RED) {
return true;
}
}
diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp
index c382d09e9ec6e..61f624311170a 100644
--- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp
+++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp
@@ -51,8 +51,8 @@ namespace behavior_velocity_planner
using autoware_auto_perception_msgs::msg::ObjectClassification;
using autoware_auto_perception_msgs::msg::PredictedObject;
using autoware_auto_perception_msgs::msg::PredictedObjects;
-using autoware_auto_perception_msgs::msg::TrafficLight;
using autoware_auto_planning_msgs::msg::PathWithLaneId;
+using autoware_perception_msgs::msg::TrafficLightElement;
using lanelet::autoware::Crosswalk;
using tier4_api_msgs::msg::CrosswalkStatus;
using tier4_autoware_utils::StopWatch;
diff --git a/planning/behavior_velocity_intersection_module/package.xml b/planning/behavior_velocity_intersection_module/package.xml
index 25df9f2385d74..65ed11fcbc6e8 100644
--- a/planning/behavior_velocity_intersection_module/package.xml
+++ b/planning/behavior_velocity_intersection_module/package.xml
@@ -17,6 +17,7 @@
ament_cmake_auto
autoware_cmake
+ autoware_perception_msgs
autoware_auto_perception_msgs
autoware_auto_planning_msgs
behavior_velocity_planner_common
diff --git a/planning/behavior_velocity_intersection_module/src/util.cpp b/planning/behavior_velocity_intersection_module/src/util.cpp
index 2dcc6a0ef761d..44a25564e9fb9 100644
--- a/planning/behavior_velocity_intersection_module/src/util.cpp
+++ b/planning/behavior_velocity_intersection_module/src/util.cpp
@@ -668,9 +668,10 @@ bool hasAssociatedTrafficLight(lanelet::ConstLanelet lane)
}
bool isTrafficLightArrowActivated(
- lanelet::ConstLanelet lane,
- const std::map & tl_infos)
+ lanelet::ConstLanelet lane, const std::map & tl_infos)
{
+ using TrafficLightElement = autoware_perception_msgs::msg::TrafficLightElement;
+
const auto & turn_direction = lane.attributeOr("turn_direction", "else");
std::optional tl_id = std::nullopt;
for (auto && tl_reg_elem : lane.regulatoryElementsAs()) {
@@ -687,16 +688,13 @@ bool isTrafficLightArrowActivated(
return false;
}
const auto & tl_info = tl_info_it->second;
- for (auto && tl_light : tl_info.signal.lights) {
- if (tl_light.color != autoware_auto_perception_msgs::msg::TrafficLight::GREEN) continue;
- if (tl_light.status != autoware_auto_perception_msgs::msg::TrafficLight::SOLID_ON) continue;
- if (
- turn_direction == std::string("left") &&
- tl_light.shape == autoware_auto_perception_msgs::msg::TrafficLight::LEFT_ARROW)
+ for (auto && tl_light : tl_info.signal.elements) {
+ if (tl_light.color != TrafficLightElement::GREEN) continue;
+ if (tl_light.status != TrafficLightElement::SOLID_ON) continue;
+ if (turn_direction == std::string("left") && tl_light.shape == TrafficLightElement::LEFT_ARROW)
return true;
if (
- turn_direction == std::string("right") &&
- tl_light.shape == autoware_auto_perception_msgs::msg::TrafficLight::RIGHT_ARROW)
+ turn_direction == std::string("right") && tl_light.shape == TrafficLightElement::RIGHT_ARROW)
return true;
}
return false;
diff --git a/planning/behavior_velocity_intersection_module/src/util.hpp b/planning/behavior_velocity_intersection_module/src/util.hpp
index c65a4e6e886a0..65ac1a35bf7cc 100644
--- a/planning/behavior_velocity_intersection_module/src/util.hpp
+++ b/planning/behavior_velocity_intersection_module/src/util.hpp
@@ -112,8 +112,7 @@ std::optional getIntersectionArea(
bool hasAssociatedTrafficLight(lanelet::ConstLanelet lane);
bool isTrafficLightArrowActivated(
- lanelet::ConstLanelet lane,
- const std::map & tl_infos);
+ lanelet::ConstLanelet lane, const std::map & tl_infos);
std::vector generateDetectionLaneDivisions(
lanelet::ConstLanelets detection_lanelets,
diff --git a/planning/behavior_velocity_planner/README.md b/planning/behavior_velocity_planner/README.md
index 25b22a2616ede..7b7e6436bf02b 100644
--- a/planning/behavior_velocity_planner/README.md
+++ b/planning/behavior_velocity_planner/README.md
@@ -28,15 +28,15 @@ So for example, in order to stop at a stop line with the vehicles' front on the
## Input topics
-| Name | Type | Description |
-| ----------------------------------------- | ------------------------------------------------------ | ------------------------------------------------------------------------------------------------------------------------------- |
-| `~input/path_with_lane_id` | autoware_auto_planning_msgs::msg::PathWithLaneId | path with lane_id |
-| `~input/vector_map` | autoware_auto_mapping_msgs::msg::HADMapBin | vector map |
-| `~input/vehicle_odometry` | nav_msgs::msg::Odometry | vehicle velocity |
-| `~input/dynamic_objects` | autoware_auto_perception_msgs::msg::PredictedObjects | dynamic objects |
-| `~input/no_ground_pointcloud` | sensor_msgs::msg::PointCloud2 | obstacle pointcloud |
-| `~/input/compare_map_filtered_pointcloud` | sensor_msgs::msg::PointCloud2 | obstacle pointcloud filtered by compare map. Note that this is used only when the detection method of run out module is Points. |
-| `~input/traffic_signals` | autoware_auto_perception_msgs::msg::TrafficSignalArray | traffic light states |
+| Name | Type | Description |
+| ----------------------------------------- | ---------------------------------------------------- | ------------------------------------------------------------------------------------------------------------------------------- |
+| `~input/path_with_lane_id` | autoware_auto_planning_msgs::msg::PathWithLaneId | path with lane_id |
+| `~input/vector_map` | autoware_auto_mapping_msgs::msg::HADMapBin | vector map |
+| `~input/vehicle_odometry` | nav_msgs::msg::Odometry | vehicle velocity |
+| `~input/dynamic_objects` | autoware_auto_perception_msgs::msg::PredictedObjects | dynamic objects |
+| `~input/no_ground_pointcloud` | sensor_msgs::msg::PointCloud2 | obstacle pointcloud |
+| `~/input/compare_map_filtered_pointcloud` | sensor_msgs::msg::PointCloud2 | obstacle pointcloud filtered by compare map. Note that this is used only when the detection method of run out module is Points. |
+| `~input/traffic_signals` | autoware_perception_msgs::msg::TrafficSignalArray | traffic light states |
## Output topics
diff --git a/planning/behavior_velocity_planner/src/node.cpp b/planning/behavior_velocity_planner/src/node.cpp
index 12503f743b0f2..0aed3015fbf40 100644
--- a/planning/behavior_velocity_planner/src/node.cpp
+++ b/planning/behavior_velocity_planner/src/node.cpp
@@ -99,7 +99,7 @@ BehaviorVelocityPlannerNode::BehaviorVelocityPlannerNode(const rclcpp::NodeOptio
std::bind(&BehaviorVelocityPlannerNode::onLaneletMap, this, _1),
createSubscriptionOptions(this));
sub_traffic_signals_ =
- this->create_subscription(
+ this->create_subscription(
"~/input/traffic_signals", 1,
std::bind(&BehaviorVelocityPlannerNode::onTrafficSignals, this, _1),
createSubscriptionOptions(this));
@@ -287,15 +287,15 @@ void BehaviorVelocityPlannerNode::onLaneletMap(
}
void BehaviorVelocityPlannerNode::onTrafficSignals(
- const autoware_auto_perception_msgs::msg::TrafficSignalArray::ConstSharedPtr msg)
+ const autoware_perception_msgs::msg::TrafficSignalArray::ConstSharedPtr msg)
{
std::lock_guard lock(mutex_);
for (const auto & signal : msg->signals) {
- autoware_auto_perception_msgs::msg::TrafficSignalStamped traffic_signal;
- traffic_signal.header = msg->header;
+ TrafficSignalStamped traffic_signal;
+ traffic_signal.stamp = msg->stamp;
traffic_signal.signal = signal;
- planner_data_.traffic_light_id_map[signal.map_primitive_id] = traffic_signal;
+ planner_data_.traffic_light_id_map[signal.traffic_signal_id] = traffic_signal;
}
}
diff --git a/planning/behavior_velocity_planner/src/node.hpp b/planning/behavior_velocity_planner/src/node.hpp
index 91172fdd77da8..98cdaeb3e0cdb 100644
--- a/planning/behavior_velocity_planner/src/node.hpp
+++ b/planning/behavior_velocity_planner/src/node.hpp
@@ -61,7 +61,7 @@ class BehaviorVelocityPlannerNode : public rclcpp::Node
rclcpp::Subscription::SharedPtr sub_vehicle_odometry_;
rclcpp::Subscription::SharedPtr sub_acceleration_;
rclcpp::Subscription::SharedPtr sub_lanelet_map_;
- rclcpp::Subscription::SharedPtr
+ rclcpp::Subscription::SharedPtr
sub_traffic_signals_;
rclcpp::Subscription::SharedPtr
sub_virtual_traffic_light_states_;
@@ -77,7 +77,7 @@ class BehaviorVelocityPlannerNode : public rclcpp::Node
void onAcceleration(const geometry_msgs::msg::AccelWithCovarianceStamped::ConstSharedPtr msg);
void onLaneletMap(const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr msg);
void onTrafficSignals(
- const autoware_auto_perception_msgs::msg::TrafficSignalArray::ConstSharedPtr msg);
+ const autoware_perception_msgs::msg::TrafficSignalArray::ConstSharedPtr msg);
void onVirtualTrafficLightStates(
const tier4_v2x_msgs::msg::VirtualTrafficLightStateArray::ConstSharedPtr msg);
void onOccupancyGrid(const nav_msgs::msg::OccupancyGrid::ConstSharedPtr msg);
diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp
index 44d3184a5f6b7..2668d83b0f510 100644
--- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp
+++ b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp
@@ -17,14 +17,14 @@
#include "route_handler/route_handler.hpp"
+#include
#include
#include
#include
#include
#include
-#include
-#include
+#include
#include
#include
#include
@@ -83,7 +83,7 @@ struct PlannerData
double ego_nearest_yaw_threshold;
// other internal data
- std::map traffic_light_id_map;
+ std::map traffic_light_id_map;
boost::optional external_velocity_limit;
tier4_v2x_msgs::msg::VirtualTrafficLightStateArray::ConstSharedPtr virtual_traffic_light_states;
@@ -132,14 +132,12 @@ struct PlannerData
return true;
}
- std::shared_ptr getTrafficSignal(
- const int id) const
+ std::shared_ptr getTrafficSignal(const int id) const
{
if (traffic_light_id_map.count(id) == 0) {
return {};
}
- return std::make_shared(
- traffic_light_id_map.at(id));
+ return std::make_shared(traffic_light_id_map.at(id));
}
};
} // namespace behavior_velocity_planner
diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/util.hpp b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/util.hpp
index 14b009ceb0748..14c80b59acf5e 100644
--- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/util.hpp
+++ b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/util.hpp
@@ -27,6 +27,7 @@
#include
#include
#include
+#include
#include
#include
#include
@@ -78,6 +79,12 @@ struct PointWithSearchRangeIndex
SearchRangeIndex index;
};
+struct TrafficSignalStamped
+{
+ builtin_interfaces::msg::Time stamp;
+ autoware_perception_msgs::msg::TrafficSignal signal;
+};
+
using geometry_msgs::msg::Pose;
using BasicPolygons2d = std::vector;
using Polygons2d = std::vector;
diff --git a/planning/behavior_velocity_planner_common/package.xml b/planning/behavior_velocity_planner_common/package.xml
index 293eee96dffdf..edc9f46a64186 100644
--- a/planning/behavior_velocity_planner_common/package.xml
+++ b/planning/behavior_velocity_planner_common/package.xml
@@ -18,8 +18,8 @@
eigen3_cmake_module
autoware_adapi_v1_msgs
+ autoware_perception_msgs
autoware_auto_mapping_msgs
- autoware_auto_perception_msgs
autoware_auto_planning_msgs
autoware_auto_tf2
cv_bridge
diff --git a/planning/behavior_velocity_traffic_light_module/package.xml b/planning/behavior_velocity_traffic_light_module/package.xml
index a8733c5dcf5be..1d4fa98e14235 100644
--- a/planning/behavior_velocity_traffic_light_module/package.xml
+++ b/planning/behavior_velocity_traffic_light_module/package.xml
@@ -18,7 +18,7 @@
eigen3_cmake_module
autoware_adapi_v1_msgs
- autoware_auto_perception_msgs
+ autoware_perception_msgs
autoware_auto_planning_msgs
behavior_velocity_planner_common
eigen
diff --git a/planning/behavior_velocity_traffic_light_module/src/manager.cpp b/planning/behavior_velocity_traffic_light_module/src/manager.cpp
index 4227c0eb4b3c3..8e954b20ea4eb 100644
--- a/planning/behavior_velocity_traffic_light_module/src/manager.cpp
+++ b/planning/behavior_velocity_traffic_light_module/src/manager.cpp
@@ -35,7 +35,7 @@ TrafficLightModuleManager::TrafficLightModuleManager(rclcpp::Node & node)
planner_param_.tl_state_timeout = node.declare_parameter(ns + ".tl_state_timeout");
planner_param_.enable_pass_judge = node.declare_parameter(ns + ".enable_pass_judge");
planner_param_.yellow_lamp_period = node.declare_parameter(ns + ".yellow_lamp_period");
- pub_tl_state_ = node.create_publisher(
+ pub_tl_state_ = node.create_publisher(
"~/output/traffic_signal", 1);
}
@@ -45,9 +45,7 @@ void TrafficLightModuleManager::modifyPathVelocity(
visualization_msgs::msg::MarkerArray debug_marker_array;
visualization_msgs::msg::MarkerArray virtual_wall_marker_array;
- autoware_auto_perception_msgs::msg::LookingTrafficSignal tl_state;
- tl_state.header.stamp = path->header.stamp;
- tl_state.is_module_running = false;
+ autoware_perception_msgs::msg::TrafficSignal tl_state;
autoware_adapi_v1_msgs::msg::VelocityFactorArray velocity_factor_array;
velocity_factor_array.header.frame_id = "map";
diff --git a/planning/behavior_velocity_traffic_light_module/src/manager.hpp b/planning/behavior_velocity_traffic_light_module/src/manager.hpp
index 450cba09e1e08..c36c6af1128ce 100644
--- a/planning/behavior_velocity_traffic_light_module/src/manager.hpp
+++ b/planning/behavior_velocity_traffic_light_module/src/manager.hpp
@@ -55,8 +55,7 @@ class TrafficLightModuleManager : public SceneModuleManagerInterfaceWithRTC
const lanelet::TrafficLightConstPtr registered_element) const;
// Debug
- rclcpp::Publisher::SharedPtr
- pub_tl_state_;
+ rclcpp::Publisher::SharedPtr pub_tl_state_;
boost::optional first_ref_stop_path_point_index_;
};
diff --git a/planning/behavior_velocity_traffic_light_module/src/scene.cpp b/planning/behavior_velocity_traffic_light_module/src/scene.cpp
index 925dfb0470b98..81a0dc6c2e691 100644
--- a/planning/behavior_velocity_traffic_light_module/src/scene.cpp
+++ b/planning/behavior_velocity_traffic_light_module/src/scene.cpp
@@ -175,16 +175,6 @@ geometry_msgs::msg::Point getTrafficLightPosition(
}
return tl_center;
}
-autoware_auto_perception_msgs::msg::LookingTrafficSignal initializeTrafficSignal(
- const rclcpp::Time stamp)
-{
- autoware_auto_perception_msgs::msg::LookingTrafficSignal state;
- state.header.stamp = stamp;
- state.is_module_running = true;
- state.perception.has_state = false;
- state.result.has_state = false;
- return state;
-}
} // namespace
TrafficLightModule::TrafficLightModule(
@@ -197,7 +187,6 @@ TrafficLightModule::TrafficLightModule(
traffic_light_reg_elem_(traffic_light_reg_elem),
lane_(lane),
state_(State::APPROACH),
- input_(Input::PERCEPTION),
is_prev_state_stop_(false)
{
velocity_factor_.init(VelocityFactor::TRAFFIC_SIGNAL);
@@ -206,7 +195,6 @@ TrafficLightModule::TrafficLightModule(
bool TrafficLightModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason)
{
- looking_tl_state_ = initializeTrafficSignal(path->header.stamp);
debug_data_ = DebugData();
debug_data_.base_link2front = planner_data_->vehicle_info_.max_longitudinal_offset_m;
first_stop_path_point_index_ = static_cast(path->points.size()) - 1;
@@ -289,27 +277,22 @@ bool TrafficLightModule::isStopSignal(const lanelet::ConstLineStringsOrPolygons3
return false;
}
- return looking_tl_state_.result.judge ==
- autoware_auto_perception_msgs::msg::TrafficSignalWithJudge::STOP;
+ return isTrafficSignalStop(looking_tl_state_);
}
bool TrafficLightModule::updateTrafficSignal(
const lanelet::ConstLineStringsOrPolygons3d & traffic_lights)
{
- autoware_auto_perception_msgs::msg::TrafficSignalStamped tl_state_perception;
- bool found_perception = getHighestConfidenceTrafficSignal(traffic_lights, tl_state_perception);
+ TrafficSignal signal;
+ bool found_signal = getHighestConfidenceTrafficSignal(traffic_lights, signal);
- if (!found_perception) {
+ if (!found_signal) {
// Don't stop when UNKNOWN or TIMEOUT as discussed at #508
- input_ = Input::NONE;
return false;
}
- if (found_perception) {
- looking_tl_state_.perception = generateTlStateWithJudgeFromTlState(tl_state_perception.signal);
- looking_tl_state_.result = looking_tl_state_.perception;
- input_ = Input::PERCEPTION;
- }
+ // Found signal associated with the lanelet
+ looking_tl_state_ = signal;
return true;
}
@@ -337,7 +320,7 @@ bool TrafficLightModule::isPassthrough(const double & signed_arc_length) const
const auto & enable_pass_judge = planner_param_.enable_pass_judge;
- if (enable_pass_judge && !stoppable && !is_prev_state_stop_ && input_ == Input::PERCEPTION) {
+ if (enable_pass_judge && !stoppable && !is_prev_state_stop_) {
// Cannot stop under acceleration and jerk limits.
// However, ego vehicle can't enter the intersection while the light is yellow.
// It is called dilemma zone. Make a stop decision to be safe.
@@ -359,10 +342,9 @@ bool TrafficLightModule::isPassthrough(const double & signed_arc_length) const
}
bool TrafficLightModule::isTrafficSignalStop(
- const autoware_auto_perception_msgs::msg::TrafficSignal & tl_state) const
+ const autoware_perception_msgs::msg::TrafficSignal & tl_state) const
{
- if (hasTrafficLightCircleColor(
- tl_state, autoware_auto_perception_msgs::msg::TrafficLight::GREEN)) {
+ if (hasTrafficLightCircleColor(tl_state, TrafficLightElement::GREEN)) {
return false;
}
@@ -372,18 +354,14 @@ bool TrafficLightModule::isTrafficSignalStop(
return true;
}
if (
- turn_direction == "right" &&
- hasTrafficLightShape(tl_state, autoware_auto_perception_msgs::msg::TrafficLight::RIGHT_ARROW)) {
+ turn_direction == "right" && hasTrafficLightShape(tl_state, TrafficLightElement::RIGHT_ARROW)) {
return false;
}
- if (
- turn_direction == "left" &&
- hasTrafficLightShape(tl_state, autoware_auto_perception_msgs::msg::TrafficLight::LEFT_ARROW)) {
+ if (turn_direction == "left" && hasTrafficLightShape(tl_state, TrafficLightElement::LEFT_ARROW)) {
return false;
}
if (
- turn_direction == "straight" &&
- hasTrafficLightShape(tl_state, autoware_auto_perception_msgs::msg::TrafficLight::UP_ARROW)) {
+ turn_direction == "straight" && hasTrafficLightShape(tl_state, TrafficLightElement::UP_ARROW)) {
return false;
}
@@ -392,7 +370,7 @@ bool TrafficLightModule::isTrafficSignalStop(
bool TrafficLightModule::getHighestConfidenceTrafficSignal(
const lanelet::ConstLineStringsOrPolygons3d & traffic_lights,
- autoware_auto_perception_msgs::msg::TrafficSignalStamped & highest_confidence_tl_state)
+ autoware_perception_msgs::msg::TrafficSignal & highest_confidence_tl_state)
{
// search traffic light state
bool found = false;
@@ -413,23 +391,23 @@ bool TrafficLightModule::getHighestConfidenceTrafficSignal(
continue;
}
- const auto header = tl_state_stamped->header;
+ const auto stamp = tl_state_stamped->stamp;
const auto tl_state = tl_state_stamped->signal;
- if (!((clock_->now() - header.stamp).seconds() < planner_param_.tl_state_timeout)) {
+ if (!((clock_->now() - stamp).seconds() < planner_param_.tl_state_timeout)) {
reason = "TimeOut";
continue;
}
if (
- tl_state.lights.empty() ||
- tl_state.lights.front().color == autoware_auto_perception_msgs::msg::TrafficLight::UNKNOWN) {
+ tl_state.elements.empty() ||
+ tl_state.elements.front().color == TrafficLightElement::UNKNOWN) {
reason = "TrafficLightUnknown";
continue;
}
- if (highest_confidence < tl_state.lights.front().confidence) {
- highest_confidence = tl_state.lights.front().confidence;
- highest_confidence_tl_state = *tl_state_stamped;
+ if (highest_confidence < tl_state.elements.front().confidence) {
+ highest_confidence = tl_state.elements.front().confidence;
+ highest_confidence_tl_state = tl_state_stamped->signal;
try {
auto tl_position = getTrafficLightPosition(traffic_light);
debug_data_.traffic_light_points.push_back(tl_position);
@@ -490,40 +468,24 @@ autoware_auto_planning_msgs::msg::PathWithLaneId TrafficLightModule::insertStopP
}
bool TrafficLightModule::hasTrafficLightCircleColor(
- const autoware_auto_perception_msgs::msg::TrafficSignal & tl_state,
- const uint8_t & lamp_color) const
+ const autoware_perception_msgs::msg::TrafficSignal & tl_state, const uint8_t & lamp_color) const
{
- using autoware_auto_perception_msgs::msg::TrafficLight;
-
const auto it_lamp =
- std::find_if(tl_state.lights.begin(), tl_state.lights.end(), [&lamp_color](const auto & x) {
- return x.shape == TrafficLight::CIRCLE && x.color == lamp_color;
+ std::find_if(tl_state.elements.begin(), tl_state.elements.end(), [&lamp_color](const auto & x) {
+ return x.shape == TrafficLightElement::CIRCLE && x.color == lamp_color;
});
- return it_lamp != tl_state.lights.end();
+ return it_lamp != tl_state.elements.end();
}
bool TrafficLightModule::hasTrafficLightShape(
- const autoware_auto_perception_msgs::msg::TrafficSignal & tl_state,
- const uint8_t & lamp_shape) const
+ const autoware_perception_msgs::msg::TrafficSignal & tl_state, const uint8_t & lamp_shape) const
{
const auto it_lamp = std::find_if(
- tl_state.lights.begin(), tl_state.lights.end(),
+ tl_state.elements.begin(), tl_state.elements.end(),
[&lamp_shape](const auto & x) { return x.shape == lamp_shape; });
- return it_lamp != tl_state.lights.end();
+ return it_lamp != tl_state.elements.end();
}
-autoware_auto_perception_msgs::msg::TrafficSignalWithJudge
-TrafficLightModule::generateTlStateWithJudgeFromTlState(
- const autoware_auto_perception_msgs::msg::TrafficSignal tl_state) const
-{
- autoware_auto_perception_msgs::msg::TrafficSignalWithJudge tl_state_with_judge;
- tl_state_with_judge.signal = tl_state;
- tl_state_with_judge.has_state = true;
- tl_state_with_judge.judge = isTrafficSignalStop(tl_state)
- ? autoware_auto_perception_msgs::msg::TrafficSignalWithJudge::STOP
- : autoware_auto_perception_msgs::msg::TrafficSignalWithJudge::GO;
- return tl_state_with_judge;
-}
} // namespace behavior_velocity_planner
diff --git a/planning/behavior_velocity_traffic_light_module/src/scene.hpp b/planning/behavior_velocity_traffic_light_module/src/scene.hpp
index 99ece6cbca09d..10f785ec4be3c 100644
--- a/planning/behavior_velocity_traffic_light_module/src/scene.hpp
+++ b/planning/behavior_velocity_traffic_light_module/src/scene.hpp
@@ -29,8 +29,6 @@
#include
#include
-#include
-
#include
#include
@@ -39,15 +37,15 @@ namespace behavior_velocity_planner
class TrafficLightModule : public SceneModuleInterface
{
public:
+ using TrafficSignal = autoware_perception_msgs::msg::TrafficSignal;
+ using TrafficLightElement = autoware_perception_msgs::msg::TrafficLightElement;
enum class State { APPROACH, GO_OUT };
- enum class Input { PERCEPTION, NONE }; // EXTERNAL: FOA, V2X, etc.
struct DebugData
{
double base_link2front;
std::vector,
- autoware_auto_perception_msgs::msg::TrafficSignal>>
+ std::shared_ptr, autoware_perception_msgs::msg::TrafficSignal>>
tl_state;
std::vector stop_poses;
geometry_msgs::msg::Pose first_stop_pose;
@@ -77,10 +75,7 @@ class TrafficLightModule : public SceneModuleInterface
visualization_msgs::msg::MarkerArray createDebugMarkerArray() override;
motion_utils::VirtualWalls createVirtualWalls() override;
- inline autoware_auto_perception_msgs::msg::LookingTrafficSignal getTrafficSignal() const
- {
- return looking_tl_state_;
- }
+ inline TrafficSignal getTrafficSignal() const { return looking_tl_state_; }
inline State getTrafficLightModuleState() const { return state_; }
@@ -92,8 +87,7 @@ class TrafficLightModule : public SceneModuleInterface
private:
bool isStopSignal(const lanelet::ConstLineStringsOrPolygons3d & traffic_lights);
- bool isTrafficSignalStop(
- const autoware_auto_perception_msgs::msg::TrafficSignal & tl_state) const;
+ bool isTrafficSignalStop(const TrafficSignal & tl_state) const;
autoware_auto_planning_msgs::msg::PathWithLaneId insertStopPose(
const autoware_auto_planning_msgs::msg::PathWithLaneId & input,
@@ -102,23 +96,16 @@ class TrafficLightModule : public SceneModuleInterface
bool isPassthrough(const double & signed_arc_length) const;
- bool hasTrafficLightCircleColor(
- const autoware_auto_perception_msgs::msg::TrafficSignal & tl_state,
- const uint8_t & lamp_color) const;
+ bool hasTrafficLightCircleColor(const TrafficSignal & tl_state, const uint8_t & lamp_color) const;
- bool hasTrafficLightShape(
- const autoware_auto_perception_msgs::msg::TrafficSignal & tl_state,
- const uint8_t & lamp_shape) const;
+ bool hasTrafficLightShape(const TrafficSignal & tl_state, const uint8_t & lamp_shape) const;
bool getHighestConfidenceTrafficSignal(
const lanelet::ConstLineStringsOrPolygons3d & traffic_lights,
- autoware_auto_perception_msgs::msg::TrafficSignalStamped & highest_confidence_tl_state);
+ TrafficSignal & highest_confidence_tl_state);
bool updateTrafficSignal(const lanelet::ConstLineStringsOrPolygons3d & traffic_lights);
- autoware_auto_perception_msgs::msg::TrafficSignalWithJudge generateTlStateWithJudgeFromTlState(
- const autoware_auto_perception_msgs::msg::TrafficSignal tl_state) const;
-
// Lane id
const int64_t lane_id_;
@@ -129,9 +116,6 @@ class TrafficLightModule : public SceneModuleInterface
// State
State state_;
- // Input
- Input input_;
-
// Parameter
PlannerParam planner_param_;
@@ -144,7 +128,7 @@ class TrafficLightModule : public SceneModuleInterface
boost::optional first_ref_stop_path_point_index_;
// Traffic Light State
- autoware_auto_perception_msgs::msg::LookingTrafficSignal looking_tl_state_;
+ TrafficSignal looking_tl_state_;
};
} // namespace behavior_velocity_planner
diff --git a/planning/planning_debug_tools/scripts/perception_reproducer.py b/planning/planning_debug_tools/scripts/perception_reproducer.py
index 5fc367665e256..4abca84ce2008 100755
--- a/planning/planning_debug_tools/scripts/perception_reproducer.py
+++ b/planning/planning_debug_tools/scripts/perception_reproducer.py
@@ -25,7 +25,7 @@
from autoware_auto_perception_msgs.msg import DetectedObjects
from autoware_auto_perception_msgs.msg import PredictedObjects
from autoware_auto_perception_msgs.msg import TrackedObjects
-from autoware_auto_perception_msgs.msg import TrafficSignalArray
+from autoware_perception_msgs.msg import TrafficSignalArray
from geometry_msgs.msg import Quaternion
from nav_msgs.msg import Odometry
import numpy as np
@@ -238,11 +238,11 @@ def on_timer(self):
self.objects_pub.publish(objects_msg)
if traffic_signals_msg:
- traffic_signals_msg.header.stamp = timestamp
+ traffic_signals_msg.stamp = timestamp
self.traffic_signals_pub.publish(traffic_signals_msg)
self.prev_traffic_signals_msg = traffic_signals_msg
elif self.prev_traffic_signals_msg:
- self.prev_traffic_signals_msg.header.stamp = timestamp
+ self.prev_traffic_signals_msg.stamp = timestamp
self.traffic_signals_pub.publish(self.prev_traffic_signals_msg)
else:
print("No ego pose found.")
diff --git a/planning/planning_test_utils/include/planning_interface_test_manager/planning_interface_test_manager.hpp b/planning/planning_test_utils/include/planning_interface_test_manager/planning_interface_test_manager.hpp
index ddc304c99ea6a..a5f7c120b4385 100644
--- a/planning/planning_test_utils/include/planning_interface_test_manager/planning_interface_test_manager.hpp
+++ b/planning/planning_test_utils/include/planning_interface_test_manager/planning_interface_test_manager.hpp
@@ -38,7 +38,7 @@
#include
#include
#include
-#include
+#include
#include
#include
#include
@@ -77,7 +77,7 @@ namespace planning_test_utils
using autoware_adapi_v1_msgs::msg::OperationModeState;
using autoware_auto_mapping_msgs::msg::HADMapBin;
using autoware_auto_perception_msgs::msg::PredictedObjects;
-using autoware_auto_perception_msgs::msg::TrafficSignalArray;
+using autoware_perception_msgs::msg::TrafficSignalArray;
using autoware_auto_planning_msgs::msg::Path;
using autoware_auto_planning_msgs::msg::PathWithLaneId;
using autoware_auto_planning_msgs::msg::Trajectory;