From 0ab6a3be3835f97b87c524896aca9d9ab75cb92d Mon Sep 17 00:00:00 2001 From: Tomohito ANDO Date: Thu, 7 Sep 2023 00:14:47 +0900 Subject: [PATCH] feat(behavior_velocity): support new traffic signal interface (#4133) * feat(behavior_velocity): support new traffic signal interface Signed-off-by: Tomohito Ando * style(pre-commit): autofix * add missing dependency Signed-off-by: Tomohito Ando * style(pre-commit): autofix * remove the external signal input source in behavior_planning_launch.py Signed-off-by: Tomohito Ando * replace TrafficLightElement with TrafficSignalElement Signed-off-by: Tomohito Ando * style(pre-commit): autofix * use the regulatory element id instead of traffic light id Signed-off-by: Tomohito Ando * change the input of traffic signal to traffic light arbiter Signed-off-by: Tomohito Ando * style(pre-commit): autofix * do not return until the all regulatory elements are checked Signed-off-by: Tomohito Ando * change input topic of the traffic signals Signed-off-by: Tomohito Ando * fix the traffic signal type in perception reproducer Signed-off-by: Tomohito Ando * add debug log when the signal data is outdated Signed-off-by: Tomohito Ando --------- Signed-off-by: Tomohito Ando Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../tier4_traffic_light_rviz_plugin/README.md | 6 +- .../package.xml | 2 +- .../src/traffic_light_publish_panel.cpp | 96 +++++----- .../src/traffic_light_publish_panel.hpp | 9 +- .../behavior_planning.launch.py | 4 - .../package.xml | 1 + .../src/scene_crosswalk.cpp | 35 ++-- .../src/scene_crosswalk.hpp | 2 +- .../package.xml | 1 + .../src/util.cpp | 18 +- .../src/util.hpp | 3 +- planning/behavior_velocity_planner/README.md | 18 +- .../behavior_velocity_planner/src/node.cpp | 10 +- .../behavior_velocity_planner/src/node.hpp | 4 +- .../planner_data.hpp | 12 +- .../utilization/util.hpp | 7 + .../package.xml | 2 +- .../package.xml | 2 +- .../src/manager.cpp | 6 +- .../src/manager.hpp | 3 +- .../src/scene.cpp | 176 +++++------------- .../src/scene.hpp | 40 ++-- .../perception_replayer.py | 16 +- .../perception_replayer_common.py | 2 +- .../perception_reproducer.py | 16 +- .../planning_interface_test_manager.hpp | 4 +- planning/planning_test_utils/package.xml | 2 +- 27 files changed, 188 insertions(+), 309 deletions(-) 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..35c5a88a2f8a6 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; + TrafficSignalElement traffic_light; traffic_light.confidence = traffic_light_confidence_input_->value(); if (color == "RED") { - traffic_light.color = TrafficLight::RED; + traffic_light.color = TrafficSignalElement::RED; } else if (color == "AMBER") { - traffic_light.color = TrafficLight::AMBER; + traffic_light.color = TrafficSignalElement::AMBER; } else if (color == "GREEN") { - traffic_light.color = TrafficLight::GREEN; + traffic_light.color = TrafficSignalElement::GREEN; } else if (color == "WHITE") { - traffic_light.color = TrafficLight::WHITE; + traffic_light.color = TrafficSignalElement::WHITE; } else if (color == "UNKNOWN") { - traffic_light.color = TrafficLight::UNKNOWN; + traffic_light.color = TrafficSignalElement::UNKNOWN; } if (shape == "CIRCLE") { - traffic_light.shape = TrafficLight::CIRCLE; + traffic_light.shape = TrafficSignalElement::CIRCLE; } else if (shape == "LEFT_ARROW") { - traffic_light.shape = TrafficLight::LEFT_ARROW; + traffic_light.shape = TrafficSignalElement::LEFT_ARROW; } else if (shape == "RIGHT_ARROW") { - traffic_light.shape = TrafficLight::RIGHT_ARROW; + traffic_light.shape = TrafficSignalElement::RIGHT_ARROW; } else if (shape == "UP_ARROW") { - traffic_light.shape = TrafficLight::UP_ARROW; + traffic_light.shape = TrafficSignalElement::UP_ARROW; } else if (shape == "DOWN_ARROW") { - traffic_light.shape = TrafficLight::DOWN_ARROW; + traffic_light.shape = TrafficSignalElement::DOWN_ARROW; } else if (shape == "DOWN_LEFT_ARROW") { - traffic_light.shape = TrafficLight::DOWN_LEFT_ARROW; + traffic_light.shape = TrafficSignalElement::DOWN_LEFT_ARROW; } else if (shape == "DOWN_RIGHT_ARROW") { - traffic_light.shape = TrafficLight::DOWN_RIGHT_ARROW; + traffic_light.shape = TrafficSignalElement::DOWN_RIGHT_ARROW; } else if (shape == "UNKNOWN") { - traffic_light.shape = TrafficLight::UNKNOWN; + traffic_light.shape = TrafficSignalElement::UNKNOWN; } if (status == "SOLID_OFF") { - traffic_light.status = TrafficLight::SOLID_OFF; + traffic_light.status = TrafficSignalElement::SOLID_OFF; } else if (status == "SOLID_ON") { - traffic_light.status = TrafficLight::SOLID_ON; + traffic_light.status = TrafficSignalElement::SOLID_ON; } else if (status == "FLASHING") { - traffic_light.status = TrafficLight::FLASHING; + traffic_light.status = TrafficSignalElement::FLASHING; } else if (status == "UNKNOWN") { - traffic_light.status = TrafficLight::UNKNOWN; + traffic_light.status = TrafficSignalElement::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 TrafficSignalElement::RED: color_label->setText("RED"); color_label->setStyleSheet("background-color: #FF0000;"); break; - case TrafficLight::AMBER: + case TrafficSignalElement::AMBER: color_label->setText("AMBER"); color_label->setStyleSheet("background-color: #FFBF00;"); break; - case TrafficLight::GREEN: + case TrafficSignalElement::GREEN: color_label->setText("GREEN"); color_label->setStyleSheet("background-color: #7CFC00;"); break; - case TrafficLight::WHITE: + case TrafficSignalElement::WHITE: color_label->setText("WHITE"); color_label->setStyleSheet("background-color: #FFFFFF;"); break; - case TrafficLight::UNKNOWN: + case TrafficSignalElement::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 TrafficSignalElement::CIRCLE: shape_label->setText("CIRCLE"); break; - case TrafficLight::LEFT_ARROW: + case TrafficSignalElement::LEFT_ARROW: shape_label->setText("LEFT_ARROW"); break; - case TrafficLight::RIGHT_ARROW: + case TrafficSignalElement::RIGHT_ARROW: shape_label->setText("RIGHT_ARROW"); break; - case TrafficLight::UP_ARROW: + case TrafficSignalElement::UP_ARROW: shape_label->setText("UP_ARROW"); break; - case TrafficLight::DOWN_ARROW: + case TrafficSignalElement::DOWN_ARROW: shape_label->setText("DOWN_ARROW"); break; - case TrafficLight::DOWN_LEFT_ARROW: + case TrafficSignalElement::DOWN_LEFT_ARROW: shape_label->setText("DOWN_LEFT_ARROW"); break; - case TrafficLight::DOWN_RIGHT_ARROW: + case TrafficSignalElement::DOWN_RIGHT_ARROW: shape_label->setText("DOWN_RIGHT_ARROW"); break; - case TrafficLight::FLASHING: - shape_label->setText("FLASHING"); - break; - case TrafficLight::UNKNOWN: + case TrafficSignalElement::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 TrafficSignalElement::SOLID_OFF: status_label->setText("SOLID_OFF"); break; - case TrafficLight::SOLID_ON: + case TrafficSignalElement::SOLID_ON: status_label->setText("SOLID_ON"); break; - case TrafficLight::FLASHING: + case TrafficSignalElement::FLASHING: status_label->setText("FLASHING"); break; - case TrafficLight::UNKNOWN: + case TrafficSignalElement::UNKNOWN: status_label->setText("UNKNOWN"); break; default: @@ -375,11 +373,9 @@ void TrafficLightPublishPanel::onVectorMap(const HADMapBin::ConstSharedPtr msg) std::string info = "Fetching traffic lights :"; std::string delim = " "; for (auto && tl_reg_elem_ptr : tl_reg_elems) { - for (auto && traffic_light : tl_reg_elem_ptr->trafficLights()) { - auto id = static_cast(traffic_light.id()); - info += (std::exchange(delim, ", ") + std::to_string(id)); - traffic_light_ids_.insert(id); - } + auto id = static_cast(tl_reg_elem_ptr->id()); + info += (std::exchange(delim, ", ") + std::to_string(id)); + traffic_light_ids_.insert(id); } RCLCPP_INFO_STREAM(raw_node_->get_logger(), info); received_vector_map_ = true; 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..e54b6a301873b 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::TrafficSignal; +using autoware_perception_msgs::msg::TrafficSignalArray; +using autoware_perception_msgs::msg::TrafficSignalElement; class TrafficLightPublishPanel : public rviz_common::Panel { Q_OBJECT diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py index 59e8038e49188..62d4c5b7188ee 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py +++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py @@ -177,10 +177,6 @@ def launch_setup(context, *args, **kwargs): "~/input/traffic_signals", "/perception/traffic_light_recognition/traffic_signals", ), - ( - "~/input/external_traffic_signals", - "/external/traffic_light_recognition/traffic_signals", - ), ( "~/input/external_velocity_limit_mps", "/planning/scenario_planning/max_velocity_default", diff --git a/planning/behavior_velocity_crosswalk_module/package.xml b/planning/behavior_velocity_crosswalk_module/package.xml index 75f96c9194aef..66f326ed799af 100644 --- a/planning/behavior_velocity_crosswalk_module/package.xml +++ b/planning/behavior_velocity_crosswalk_module/package.xml @@ -21,6 +21,7 @@ autoware_auto_perception_msgs autoware_auto_planning_msgs autoware_auto_tf2 + autoware_perception_msgs behavior_velocity_planner_common eigen geometry_msgs diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp index 6d2a3110cbdb7..7a972891fd053 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp @@ -938,28 +938,25 @@ bool CrosswalkModule::isRedSignalForPedestrians() const crosswalk_.regulatoryElementsAs(); for (const auto & traffic_lights_reg_elem : traffic_lights_reg_elems) { - lanelet::ConstLineStringsOrPolygons3d traffic_lights = traffic_lights_reg_elem->trafficLights(); - for (const auto & traffic_light : traffic_lights) { - const auto ll_traffic_light = static_cast(traffic_light); - const auto traffic_signal_stamped = planner_data_->getTrafficSignal(ll_traffic_light.id()); - if (!traffic_signal_stamped) { - continue; - } + const auto traffic_signal_stamped = + planner_data_->getTrafficSignal(traffic_lights_reg_elem->id()); + if (!traffic_signal_stamped) { + continue; + } - if ( - planner_param_.traffic_light_state_timeout < - (clock_->now() - traffic_signal_stamped->header.stamp).seconds()) { - continue; - } + if ( + planner_param_.traffic_light_state_timeout < + (clock_->now() - traffic_signal_stamped->stamp).seconds()) { + continue; + } - const auto & lights = traffic_signal_stamped->signal.lights; - if (lights.empty()) { - continue; - } + const auto & lights = traffic_signal_stamped->signal.elements; + if (lights.empty()) { + continue; + } - if (lights.front().color == TrafficLight::RED) { - return true; - } + if (lights.front().color == TrafficSignalElement::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 e0851366d7f71..65f3772cd4d33 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp @@ -50,8 +50,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::TrafficSignalElement; using lanelet::autoware::Crosswalk; using tier4_api_msgs::msg::CrosswalkStatus; using tier4_autoware_utils::Polygon2d; diff --git a/planning/behavior_velocity_intersection_module/package.xml b/planning/behavior_velocity_intersection_module/package.xml index f22e9d788fc65..879b26209a05e 100644 --- a/planning/behavior_velocity_intersection_module/package.xml +++ b/planning/behavior_velocity_intersection_module/package.xml @@ -19,6 +19,7 @@ autoware_auto_perception_msgs autoware_auto_planning_msgs + autoware_perception_msgs behavior_velocity_planner_common geometry_msgs interpolation diff --git a/planning/behavior_velocity_intersection_module/src/util.cpp b/planning/behavior_velocity_intersection_module/src/util.cpp index e430f925ab2d6..d78b7252d0878 100644 --- a/planning/behavior_velocity_intersection_module/src/util.cpp +++ b/planning/behavior_velocity_intersection_module/src/util.cpp @@ -704,9 +704,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 TrafficSignalElement = autoware_perception_msgs::msg::TrafficSignalElement; + const auto & turn_direction = lane.attributeOr("turn_direction", "else"); std::optional tl_id = std::nullopt; for (auto && tl_reg_elem : lane.regulatoryElementsAs()) { @@ -723,16 +724,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 != TrafficSignalElement::GREEN) continue; + if (tl_light.status != TrafficSignalElement::SOLID_ON) continue; + if (turn_direction == std::string("left") && tl_light.shape == TrafficSignalElement::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 == TrafficSignalElement::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 4ae4e72cb4ac2..e766da6651417 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 25546c95074c3..519e68764b450 100644 --- a/planning/behavior_velocity_planner/README.md +++ b/planning/behavior_velocity_planner/README.md @@ -29,15 +29,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 6a7a6f698bfbc..aedbab65068fb 100644 --- a/planning/behavior_velocity_planner_common/package.xml +++ b/planning/behavior_velocity_planner_common/package.xml @@ -19,9 +19,9 @@ autoware_adapi_v1_msgs autoware_auto_mapping_msgs - autoware_auto_perception_msgs autoware_auto_planning_msgs autoware_auto_tf2 + autoware_perception_msgs diagnostic_msgs eigen geometry_msgs diff --git a/planning/behavior_velocity_traffic_light_module/package.xml b/planning/behavior_velocity_traffic_light_module/package.xml index a8733c5dcf5be..c658f0890b986 100644 --- a/planning/behavior_velocity_traffic_light_module/package.xml +++ b/planning/behavior_velocity_traffic_light_module/package.xml @@ -18,8 +18,8 @@ eigen3_cmake_module autoware_adapi_v1_msgs - autoware_auto_perception_msgs autoware_auto_planning_msgs + autoware_perception_msgs behavior_velocity_planner_common eigen geometry_msgs diff --git a/planning/behavior_velocity_traffic_light_module/src/manager.cpp b/planning/behavior_velocity_traffic_light_module/src/manager.cpp index 1b0646f4bee00..f035544b81592 100644 --- a/planning/behavior_velocity_traffic_light_module/src/manager.cpp +++ b/planning/behavior_velocity_traffic_light_module/src/manager.cpp @@ -41,7 +41,7 @@ TrafficLightModuleManager::TrafficLightModuleManager(rclcpp::Node & node) planner_param_.enable_pass_judge = getOrDeclareParameter(node, ns + ".enable_pass_judge"); planner_param_.yellow_lamp_period = getOrDeclareParameter(node, ns + ".yellow_lamp_period"); - pub_tl_state_ = node.create_publisher( + pub_tl_state_ = node.create_publisher( "~/output/traffic_signal", 1); } @@ -51,9 +51,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 679a2f0cb9320..e0633926e35df 100644 --- a/planning/behavior_velocity_traffic_light_module/src/scene.cpp +++ b/planning/behavior_velocity_traffic_light_module/src/scene.cpp @@ -159,32 +159,6 @@ bool calcStopPointAndInsertIndex( } return false; } - -geometry_msgs::msg::Point getTrafficLightPosition( - const lanelet::ConstLineStringOrPolygon3d & traffic_light) -{ - if (!traffic_light.lineString()) { - throw std::invalid_argument{"Traffic light is not LineString"}; - } - geometry_msgs::msg::Point tl_center; - auto traffic_light_ls = traffic_light.lineString().value(); - for (const auto & tl_point : traffic_light_ls) { - tl_center.x += tl_point.x() / traffic_light_ls.size(); - tl_center.y += tl_point.y() / traffic_light_ls.size(); - tl_center.z += tl_point.z() / traffic_light_ls.size(); - } - 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 +171,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 +179,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; @@ -217,9 +189,8 @@ bool TrafficLightModule::modifyPathVelocity(PathWithLaneId * path, StopReason * const auto & self_pose = planner_data_->current_odometry; - // Get lanelet2 traffic lights and stop lines. + // Get lanelet2 stop lines. lanelet::ConstLineString3d lanelet_stop_lines = *(traffic_light_reg_elem_.stopLine()); - lanelet::ConstLineStringsOrPolygons3d traffic_lights = traffic_light_reg_elem_.trafficLights(); // Calculate stop pose and insert index Eigen::Vector2d stop_line_point{}; @@ -255,7 +226,7 @@ bool TrafficLightModule::modifyPathVelocity(PathWithLaneId * path, StopReason * first_ref_stop_path_point_index_ = stop_line_point_idx; // Check if stop is coming. - setSafe(!isStopSignal(traffic_lights)); + setSafe(!isStopSignal()); if (isActivated()) { is_prev_state_stop_ = false; return true; @@ -283,33 +254,27 @@ bool TrafficLightModule::modifyPathVelocity(PathWithLaneId * path, StopReason * return false; } -bool TrafficLightModule::isStopSignal(const lanelet::ConstLineStringsOrPolygons3d & traffic_lights) +bool TrafficLightModule::isStopSignal() { - if (!updateTrafficSignal(traffic_lights)) { + if (!updateTrafficSignal()) { 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) +bool TrafficLightModule::updateTrafficSignal() { - autoware_auto_perception_msgs::msg::TrafficSignalStamped tl_state_perception; - bool found_perception = getHighestConfidenceTrafficSignal(traffic_lights, tl_state_perception); + TrafficSignal signal; + bool found_signal = findValidTrafficSignal(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 +302,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 +324,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, TrafficSignalElement::GREEN)) { return false; } @@ -373,86 +337,46 @@ bool TrafficLightModule::isTrafficSignalStop( } if ( turn_direction == "right" && - hasTrafficLightShape(tl_state, autoware_auto_perception_msgs::msg::TrafficLight::RIGHT_ARROW)) { + hasTrafficLightShape(tl_state, TrafficSignalElement::RIGHT_ARROW)) { return false; } if ( - turn_direction == "left" && - hasTrafficLightShape(tl_state, autoware_auto_perception_msgs::msg::TrafficLight::LEFT_ARROW)) { + turn_direction == "left" && hasTrafficLightShape(tl_state, TrafficSignalElement::LEFT_ARROW)) { return false; } if ( turn_direction == "straight" && - hasTrafficLightShape(tl_state, autoware_auto_perception_msgs::msg::TrafficLight::UP_ARROW)) { + hasTrafficLightShape(tl_state, TrafficSignalElement::UP_ARROW)) { return false; } return true; } -bool TrafficLightModule::getHighestConfidenceTrafficSignal( - const lanelet::ConstLineStringsOrPolygons3d & traffic_lights, - autoware_auto_perception_msgs::msg::TrafficSignalStamped & highest_confidence_tl_state) +bool TrafficLightModule::findValidTrafficSignal(TrafficSignal & valid_traffic_signal) { - // search traffic light state - bool found = false; - double highest_confidence = 0.0; - std::string reason; - for (const auto & traffic_light : traffic_lights) { - // traffic light must be linestrings - if (!traffic_light.isLineString()) { - reason = "NotLineString"; - continue; - } - - const int id = static_cast(traffic_light).id(); - RCLCPP_DEBUG(logger_, "traffic light id: %d (on route)", id); - const auto tl_state_stamped = planner_data_->getTrafficSignal(id); - if (!tl_state_stamped) { - reason = "TrafficSignalNotFound"; - continue; - } - - const auto header = tl_state_stamped->header; - const auto tl_state = tl_state_stamped->signal; - if (!((clock_->now() - header.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) { - 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; - try { - auto tl_position = getTrafficLightPosition(traffic_light); - debug_data_.traffic_light_points.push_back(tl_position); - debug_data_.highest_confidence_traffic_light_point = std::make_optional(tl_position); - } catch (const std::invalid_argument & ex) { - RCLCPP_WARN_STREAM(logger_, ex.what()); - continue; - } - found = true; - } + // get traffic signal associated with the regulatory element id + const auto traffic_signal_stamped = planner_data_->getTrafficSignal(traffic_light_reg_elem_.id()); + if (!traffic_signal_stamped) { + RCLCPP_WARN_THROTTLE( + logger_, *clock_, 5000 /* ms */, + "the traffic signal data associated with regulatory element id is not received"); + return false; } - if (!found) { - std::string not_found_traffic_ids{""}; - for (size_t i = 0; i < traffic_lights.size(); ++i) { - const int id = static_cast(traffic_lights.at(i)).id(); - not_found_traffic_ids += (i != 0 ? "," : "") + std::to_string(id); - } + // check if the traffic signal data is outdated + const auto is_traffic_signal_timeout = + (clock_->now() - traffic_signal_stamped->stamp).seconds() > planner_param_.tl_state_timeout; + if (is_traffic_signal_timeout) { RCLCPP_WARN_THROTTLE( - logger_, *clock_, 5000 /* ms */, "cannot find traffic light lamp state (%s) due to %s.", - not_found_traffic_ids.c_str(), reason.c_str()); + logger_, *clock_, 5000 /* ms */, "the received traffic signal data is outdated"); + RCLCPP_WARN_STREAM_THROTTLE( + logger_, *clock_, 5000 /* ms */, + "time diff: " << (clock_->now() - traffic_signal_stamped->stamp).seconds()); return false; } + + valid_traffic_signal = traffic_signal_stamped->signal; return true; } @@ -496,40 +420,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 == TrafficSignalElement::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..a24db2c440e91 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 TrafficSignalElement = autoware_perception_msgs::msg::TrafficSignalElement; 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_; } @@ -90,10 +85,9 @@ class TrafficLightModule : public SceneModuleInterface } private: - bool isStopSignal(const lanelet::ConstLineStringsOrPolygons3d & traffic_lights); + bool isStopSignal(); - 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,22 +96,13 @@ 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); + bool findValidTrafficSignal(TrafficSignal & valid_traffic_signal); - bool updateTrafficSignal(const lanelet::ConstLineStringsOrPolygons3d & traffic_lights); - - autoware_auto_perception_msgs::msg::TrafficSignalWithJudge generateTlStateWithJudgeFromTlState( - const autoware_auto_perception_msgs::msg::TrafficSignal tl_state) const; + bool updateTrafficSignal(); // Lane id const int64_t lane_id_; @@ -129,9 +114,6 @@ class TrafficLightModule : public SceneModuleInterface // State State state_; - // Input - Input input_; - // Parameter PlannerParam planner_param_; @@ -144,7 +126,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_replayer/perception_replayer.py b/planning/planning_debug_tools/scripts/perception_replayer/perception_replayer.py index 1299062638471..9d8fa4a32a93d 100755 --- a/planning/planning_debug_tools/scripts/perception_replayer/perception_replayer.py +++ b/planning/planning_debug_tools/scripts/perception_replayer/perception_replayer.py @@ -73,7 +73,7 @@ def on_timer(self): # extract message by the timestamp msgs = copy.deepcopy(self.find_topics_by_timestamp(self.bag_timestamp)) objects_msg = msgs[0] - # traffic_signals_msg = msgs[1] + traffic_signals_msg = msgs[1] # objects if objects_msg: @@ -92,13 +92,13 @@ def on_timer(self): self.objects_pub.publish(objects_msg) # traffic signals - # if traffic_signals_msg: - # traffic_signals_msg.header.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.traffic_signals_pub.publish(self.prev_traffic_signals_msg) + if traffic_signals_msg: + 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.stamp = timestamp + self.traffic_signals_pub.publish(self.prev_traffic_signals_msg) def onPushed(self, event): if self.widget.button.isChecked(): diff --git a/planning/planning_debug_tools/scripts/perception_replayer/perception_replayer_common.py b/planning/planning_debug_tools/scripts/perception_replayer/perception_replayer_common.py index a86a9ae9b2bb0..7774a8c82daf6 100644 --- a/planning/planning_debug_tools/scripts/perception_replayer/perception_replayer_common.py +++ b/planning/planning_debug_tools/scripts/perception_replayer/perception_replayer_common.py @@ -22,7 +22,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 nav_msgs.msg import Odometry import psutil from rclpy.node import Node diff --git a/planning/planning_debug_tools/scripts/perception_replayer/perception_reproducer.py b/planning/planning_debug_tools/scripts/perception_replayer/perception_reproducer.py index 8895f7d921626..5344e622be38e 100755 --- a/planning/planning_debug_tools/scripts/perception_replayer/perception_reproducer.py +++ b/planning/planning_debug_tools/scripts/perception_replayer/perception_reproducer.py @@ -56,7 +56,7 @@ def on_timer(self): # extract message by the nearest ego odom timestamp msgs = copy.deepcopy(self.find_topics_by_timestamp(pose_timestamp)) objects_msg = msgs[0] - # traffic_signals_msg = msgs[1] + traffic_signals_msg = msgs[1] # objects if objects_msg: @@ -66,13 +66,13 @@ def on_timer(self): self.objects_pub.publish(objects_msg) # traffic signals - # if traffic_signals_msg: - # traffic_signals_msg.header.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.traffic_signals_pub.publish(self.prev_traffic_signals_msg) + if traffic_signals_msg: + 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.stamp = timestamp + self.traffic_signals_pub.publish(self.prev_traffic_signals_msg) def find_nearest_ego_odom_by_observation(self, ego_pose): if self.ego_pose_idx: 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..c5a30be126661 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,11 +38,11 @@ #include #include #include -#include #include #include #include #include +#include #include #include #include @@ -77,10 +77,10 @@ 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_auto_planning_msgs::msg::Path; using autoware_auto_planning_msgs::msg::PathWithLaneId; using autoware_auto_planning_msgs::msg::Trajectory; +using autoware_perception_msgs::msg::TrafficSignalArray; using autoware_planning_msgs::msg::LaneletRoute; using geometry_msgs::msg::AccelWithCovarianceStamped; using geometry_msgs::msg::Point; diff --git a/planning/planning_test_utils/package.xml b/planning/planning_test_utils/package.xml index 72662f7ee7b76..23bfb2f44cb96 100644 --- a/planning/planning_test_utils/package.xml +++ b/planning/planning_test_utils/package.xml @@ -15,9 +15,9 @@ autoware_auto_control_msgs autoware_auto_mapping_msgs - autoware_auto_perception_msgs autoware_auto_planning_msgs autoware_auto_vehicle_msgs + autoware_perception_msgs autoware_planning_msgs component_interface_specs component_interface_utils