fix:修改逻辑使用heading判断和红绿灯的id

include/obu/data_process.hpp

@@ -7,6 +7,7 @@
 #include <autoware_auto_perception_msgs/msg/traffic_signal_array.hpp>
 #include <autoware_auto_perception_msgs/msg/traffic_light.hpp>
 #include <autoware_auto_perception_msgs/msg/traffic_signal.hpp>
+#include <std_msgs/msg/float32.hpp>
 #include "rclcpp/rclcpp.hpp"
 
 using json = nlohmann::json;
@@ -39,11 +40,21 @@ class data_process {
     // 将message打包为字节流
     std::vector<uint8_t> packed_data();
     void parse_data(std::vector<uint8_t> received_data);
+
     using TrafficSignalArray = autoware_auto_perception_msgs::msg::TrafficSignalArray;
     using TrafficSignal = autoware_auto_perception_msgs::msg::TrafficSignal;
     using TrafficLight = autoware_auto_perception_msgs::msg::TrafficLight;
+    using Float32 = std_msgs::msg::Float32;
+    void callback_heading(const Float32::SharedPtr msg);
+    int Lanlet_id = 0;
 
     rclcpp::Publisher<TrafficSignalArray>::SharedPtr traffic_signal_array_pub_;
+    rclcpp::Subscription<Float32>::SharedPtr heading_sub_;
+
+    Message receive_message;
+    TrafficSignalArray traffic_signal_array_msg_;
+    TrafficSignal traffic_signal_msg_;
+    TrafficLight traffic_light_msg_;
 
 private:
     json message_json;

package.xml

@@ -13,6 +13,7 @@
   <depend>geometry_msgs</depend>
   <depend>jsoncpp</depend>
   <depend>autoware_auto_perception_msgs</depend>
+  <depend>ublox_msgs</depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>

src/data_process.cpp

@@ -1,6 +1,10 @@
 #include "obu/data_process.hpp"
 data_process::data_process(rclcpp::Node::SharedPtr node){
     node_ = node;
+    traffic_signal_array_pub_ = node_->create_publisher<TrafficSignalArray>("/perception/traffic_light_recognition/traffic_signals", rclcpp::QoS(1));
+
+    heading_sub_ = node_->create_subscription<Float32>("/sensing/gnss/heading", 10,
+    std::bind(&data_process::callback_heading, this, std::placeholders::_1));
 };
 data_process:: ~data_process(){};
 std::string data_process::get_string(){
@@ -164,6 +168,21 @@ std::string data_process::get_string(){
     return jsonStr;
 }
 
+void data_process::callback_heading(const Float32::SharedPtr msg){
+    // Float32 heading_msg;
+    // heading_msg.data = msg;
+    if (msg->data < 180){
+        traffic_signal_msg_.map_primitive_id = 123456;
+        Lanlet_id = 0;
+
+    }
+    else{
+        traffic_signal_msg_.map_primitive_id = 123457;
+        Lanlet_id = 1;
+    }
+    std::cout<<"test"<<std::endl;
+}
+
 std::vector<uint8_t> data_process::packed_data(){
     Message msg;
     time_t timep;
@@ -181,23 +200,30 @@ std::vector<uint8_t> data_process::packed_data(){
 }
 
 void data_process::parse_data(std::vector<uint8_t> received_data){
-    Message receive_message;
-    TrafficSignalArray traffic_signal_array_msg_;
-    TrafficSignal traffic_signal_msg_;
-    TrafficLight traffic_light_msg_;
-    traffic_signal_array_pub_ = node_->create_publisher<TrafficSignalArray>("/perception/traffic_light_recognition/traffic_signals", rclcpp::QoS(1));
-
     receive_message = received_data;
+    traffic_signal_msg_.lights.clear();
+    traffic_signal_array_msg_.signals.clear();
     json parsed_json = json::parse(receive_message.message_body);
-    std::cout<<"state_active:"<<parsed_json["scene"]["glosa"]["glosa_info"][0]["state_active"]<<std::endl;
-    if ((parsed_json["scene"]["glosa"]["glosa_info"][0]["state"]) == 3){
+    // 3：红，6：绿，7：黄
+    std::cout<<"state_active:"<<parsed_json["scene"]["glosa"]["glosa_info"][Lanlet_id]["state"]<<std::endl;
+    if ((parsed_json["scene"]["glosa"]["glosa_info"][Lanlet_id]["state"]) == 3){
+        traffic_light_msg_.color = traffic_light_msg_.RED;
+        traffic_light_msg_.shape = traffic_light_msg_.UP_ARROW;
+        traffic_light_msg_.status = traffic_light_msg_.SOLID_ON;
+    }
+    if ((parsed_json["scene"]["glosa"]["glosa_info"][Lanlet_id]["state"]) == 6){
         traffic_light_msg_.color = traffic_light_msg_.GREEN;
-        traffic_light_msg_.shape = traffic_light_msg_.LEFT_ARROW;
+        traffic_light_msg_.shape = traffic_light_msg_.UP_ARROW;
         traffic_light_msg_.status = traffic_light_msg_.SOLID_ON;
     }
-    traffic_light_msg_.confidence = 1;
+    if ((parsed_json["scene"]["glosa"]["glosa_info"][Lanlet_id]["state"]) == 7){
+        traffic_light_msg_.color = traffic_light_msg_.GREEN;
+        traffic_light_msg_.shape = traffic_light_msg_.UP_ARROW;
+        traffic_light_msg_.status = traffic_light_msg_.SOLID_ON;
+    }
+    // std::cout<<"state_active:"<<std::endl;
 
-    traffic_signal_msg_.map_primitive_id = 123456;
+    traffic_light_msg_.confidence = 1;
     traffic_signal_msg_.lights.push_back(traffic_light_msg_);
 
     traffic_signal_array_msg_.header.stamp = node_->now();

src/obu_socket_node.cpp

@@ -15,11 +15,16 @@ int main(int argc, char** argv) {
     data_process data_process_(node);
 
     while(rclcpp::ok()){
+
         udp_server.send_to_client(data_process_.packed_data(),29+data_process_.get_string().length());
+        std::cout<<"send over"<<std::endl;
         data_process_.parse_data(udp_server.receive_data());
+        std::cout<<"receive over"<<std::endl;
+        rclcpp::spin_some(node);
+        sleep(1);
     }
 
-    rclcpp::spin(node);
+
     rclcpp::shutdown();
     return 0;
 

src/udp_server.cpp

@@ -47,6 +47,7 @@ std::vector<uint8_t> udp_server::receive_data(){
 }
 
 void udp_server::send_to_client(std::vector<uint8_t> message , size_t len_) {
+    std::cout<<"std::"<<message[1]<<std::endl;
     ssize_t bytes_sent = sendto(socket_fd_, message.data(),len_, 0,
                                (struct sockaddr*)&client_address, sizeof(client_address));
     if (bytes_sent == -1) {