From 9ab9633ad4710612ebadb4109f04796f24fb82c8 Mon Sep 17 00:00:00 2001 From: KhalilSelyan Date: Mon, 15 Jan 2024 11:53:47 +0300 Subject: [PATCH] re-integrate traffic light subscriptions with new message type and topic from the adapter message converter --- .../include/signal_display.hpp | 11 ++- .../src/signal_display.cpp | 76 +++++++++---------- 2 files changed, 40 insertions(+), 47 deletions(-) diff --git a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/include/signal_display.hpp b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/include/signal_display.hpp index e50b56d8b8abe..702c0e20507e0 100644 --- a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/include/signal_display.hpp +++ b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/include/signal_display.hpp @@ -86,7 +86,7 @@ private Q_SLOTS: void topic_updated_speed_limit(); void topic_updated_turn_signals(); void topic_updated_hazard_lights(); - // void topic_updated_traffic(); + void topic_updated_traffic(); private: std::mutex mutex_; @@ -101,7 +101,7 @@ private Q_SLOTS: std::unique_ptr speed_topic_property_; std::unique_ptr turn_signals_topic_property_; std::unique_ptr hazard_lights_topic_property_; - // std::unique_ptr traffic_topic_property_; + std::unique_ptr traffic_topic_property_; std::unique_ptr speed_limit_topic_property_; void drawBackground(QPainter & painter, const QRectF & backgroundRect); @@ -121,8 +121,7 @@ private Q_SLOTS: turn_signals_sub_; rclcpp::Subscription::SharedPtr hazard_lights_sub_; - // rclcpp::Subscription::SharedPtr - // traffic_sub_; + rclcpp::Subscription::SharedPtr traffic_sub_; rclcpp::Subscription::SharedPtr speed_limit_sub_; std::mutex property_mutex_; @@ -136,8 +135,8 @@ private Q_SLOTS: void updateHazardLightsData( const autoware_auto_vehicle_msgs::msg::HazardLightsReport::ConstSharedPtr & msg); void updateSpeedLimitData(const tier4_planning_msgs::msg::VelocityLimit::ConstSharedPtr msg); - // void updateTrafficLightData( - // const autoware_perception_msgs::msg::TrafficSignalArray::ConstSharedPtr msg); + void updateTrafficLightData( + const rviz_2d_overlay_msgs::msg::TrafficSignalArrayUI::ConstSharedPtr msg); void drawWidget(QImage & hud); }; } // namespace awf_2d_overlay_vehicle diff --git a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/src/signal_display.cpp b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/src/signal_display.cpp index ebeaf462ad3c5..b8f7a666c3140 100644 --- a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/src/signal_display.cpp +++ b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/src/signal_display.cpp @@ -96,11 +96,11 @@ void SignalDisplay::onInitialize() SLOT(topic_updated_speed_limit())); speed_limit_topic_property_->initialize(rviz_ros_node); - // traffic_topic_property_ = std::make_unique( - // "Traffic Topic", "/perception/traffic_light", - // "autoware_perception_msgs/msg/TrafficSignalArray", "Topic for Traffic Light Data", this, - // SLOT(topic_updated_traffic())); - // traffic_topic_property_->initialize(rviz_ros_node); + traffic_topic_property_ = std::make_unique( + "Traffic Topic", "/perception/traffic_light_recognition/traffic_signals_plugin", + "rviz_2d_overlay_msgs/msg/TrafficSignalArrayUI", "Topic for Traffic Light Data", this, + SLOT(topic_updated_traffic())); + traffic_topic_property_->initialize(rviz_ros_node); } void SignalDisplay::setupRosSubscriptions() @@ -145,14 +145,12 @@ void SignalDisplay::setupRosSubscriptions() updateHazardLightsData(msg); }); - // traffic_sub_ = - // rviz_node_->create_subscription( - // traffic_topic_property_->getTopicStd(), - // rclcpp::QoS(rclcpp::KeepLast(10)).durability_volatile().reliable(), - // [this](const autoware_perception_msgs::msg::TrafficSignalArray::SharedPtr msg) { - // std::cout << "Traffic Light Data Received" << std::endl; - // updateTrafficLightData(msg); - // }); + traffic_sub_ = rviz_node_->create_subscription( + traffic_topic_property_->getTopicStd(), + rclcpp::QoS(rclcpp::KeepLast(10)).durability_volatile().reliable(), + [this](const rviz_2d_overlay_msgs::msg::TrafficSignalArrayUI::SharedPtr msg) { + updateTrafficLightData(msg); + }); speed_limit_sub_ = rviz_node_->create_subscription( speed_limit_topic_property_->getTopicStd(), @@ -172,7 +170,7 @@ SignalDisplay::~SignalDisplay() speed_sub_.reset(); turn_signals_sub_.reset(); hazard_lights_sub_.reset(); - // traffic_sub_.reset(); + traffic_sub_.reset(); steering_wheel_display_.reset(); gear_display_.reset(); @@ -192,7 +190,7 @@ SignalDisplay::~SignalDisplay() speed_topic_property_.reset(); steering_topic_property_.reset(); hazard_lights_topic_property_.reset(); - // traffic_topic_property_.reset(); + traffic_topic_property_.reset(); } void SignalDisplay::update(float /* wall_dt */, float /* ros_dt */) @@ -230,17 +228,15 @@ void SignalDisplay::onDisable() } } -// void SignalDisplay::updateTrafficLightData( -// const autoware_perception_msgs::msg::TrafficSignalArray::ConstSharedPtr msg) -// { -// std::lock_guard lock(property_mutex_); -// -// (void)msg; -// -// if (traffic_display_) { -// traffic_display_->updateTrafficLightData(msg); -// } -// } +void SignalDisplay::updateTrafficLightData( + const rviz_2d_overlay_msgs::msg::TrafficSignalArrayUI::ConstSharedPtr msg) +{ + std::lock_guard lock(property_mutex_); + + if (traffic_display_) { + traffic_display_->updateTrafficLightData(msg); + } +} void SignalDisplay::updateSpeedLimitData( const tier4_planning_msgs::msg::VelocityLimit::ConstSharedPtr msg) @@ -476,21 +472,19 @@ void SignalDisplay::topic_updated_hazard_lights() }); } -// void SignalDisplay::topic_updated_traffic() -// { -// // resubscribe to the topic -// traffic_sub_.reset(); -// auto rviz_ros_node = context_->getRosNodeAbstraction().lock(); -// traffic_sub_ = rviz_ros_node->get_raw_node() -// ->create_subscription( -// traffic_topic_property_->getTopicStd(), -// rclcpp::QoS(rclcpp::KeepLast(10)).durability_volatile().reliable(), -// [this](const autoware_perception_msgs::msg::TrafficSignalArray::SharedPtr -// msg) { -// std::cout << "Traffic Light Data Received" << std::endl; -// // updateTrafficLightData(msg); -// }); -// } +void SignalDisplay::topic_updated_traffic() +{ + // resubscribe to the topic + traffic_sub_.reset(); + auto rviz_ros_node = context_->getRosNodeAbstraction().lock(); + traffic_sub_ = rviz_ros_node->get_raw_node() + ->create_subscription( + traffic_topic_property_->getTopicStd(), + rclcpp::QoS(rclcpp::KeepLast(10)).durability_volatile().reliable(), + [this](const rviz_2d_overlay_msgs::msg::TrafficSignalArrayUI::SharedPtr msg) { + updateTrafficLightData(msg); + }); +} } // namespace awf_2d_overlay_vehicle