Skip to content

Commit

Permalink
re-integrate traffic light subscriptions with new message type and to…
Browse files Browse the repository at this point in the history 
…pic from the adapter message converter
  • Loading branch information
@KhalilSelyan
KhalilSelyan committed Jan 15, 2024
1 parent 144c20d commit 9ab9633
Show file tree
Hide file tree
Showing 2 changed files with 40 additions and 47 deletions.
11 changes: 5 additions & 6 deletions common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/include/signal_display.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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_;
Expand All @@ -101,7 +101,7 @@ private Q_SLOTS:
std::unique_ptr<rviz_common::properties::RosTopicProperty> speed_topic_property_;
std::unique_ptr<rviz_common::properties::RosTopicProperty> turn_signals_topic_property_;
std::unique_ptr<rviz_common::properties::RosTopicProperty> hazard_lights_topic_property_;
// std::unique_ptr<rviz_common::properties::RosTopicProperty> traffic_topic_property_;
std::unique_ptr<rviz_common::properties::RosTopicProperty> traffic_topic_property_;
std::unique_ptr<rviz_common::properties::RosTopicProperty> speed_limit_topic_property_;

void drawBackground(QPainter & painter, const QRectF & backgroundRect);
Expand All @@ -121,8 +121,7 @@ private Q_SLOTS:
turn_signals_sub_;
rclcpp::Subscription<autoware_auto_vehicle_msgs::msg::HazardLightsReport>::SharedPtr
hazard_lights_sub_;
// rclcpp::Subscription<autoware_perception_msgs::msg::TrafficSignalArray>::SharedPtr
// traffic_sub_;
rclcpp::Subscription<rviz_2d_overlay_msgs::msg::TrafficSignalArrayUI>::SharedPtr traffic_sub_;
rclcpp::Subscription<tier4_planning_msgs::msg::VelocityLimit>::SharedPtr speed_limit_sub_;

std::mutex property_mutex_;
Expand All @@ -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
Expand Down
76 changes: 35 additions & 41 deletions common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/src/signal_display.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<rviz_common::properties::RosTopicProperty>(
// "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<rviz_common::properties::RosTopicProperty>(
"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()
Expand Down Expand Up @@ -145,14 +145,12 @@ void SignalDisplay::setupRosSubscriptions()
updateHazardLightsData(msg);
});

// traffic_sub_ =
// rviz_node_->create_subscription<autoware_perception_msgs::msg::TrafficSignalArray>(
// 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<rviz_2d_overlay_msgs::msg::TrafficSignalArrayUI>(
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<tier4_planning_msgs::msg::VelocityLimit>(
speed_limit_topic_property_->getTopicStd(),
Expand All @@ -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();
Expand All @@ -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 */)
Expand Down Expand Up @@ -230,17 +228,15 @@ void SignalDisplay::onDisable()
}
}

// void SignalDisplay::updateTrafficLightData(
// const autoware_perception_msgs::msg::TrafficSignalArray::ConstSharedPtr msg)
// {
// std::lock_guard<std::mutex> 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<std::mutex> lock(property_mutex_);

if (traffic_display_) {
traffic_display_->updateTrafficLightData(msg);
}
}

void SignalDisplay::updateSpeedLimitData(
const tier4_planning_msgs::msg::VelocityLimit::ConstSharedPtr msg)
Expand Down Expand Up @@ -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<autoware_perception_msgs::msg::TrafficSignalArray>(
// 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<rviz_2d_overlay_msgs::msg::TrafficSignalArrayUI>(
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

Expand Down

0 comments on commit 9ab9633

Please sign in to comment.