diff --git a/include/obu/data_process.hpp b/include/obu/data_process.hpp index 8632a1b..7fa5703 100644 --- a/include/obu/data_process.hpp +++ b/include/obu/data_process.hpp @@ -24,7 +24,7 @@ struct Message uint32_t reserved=0xffffffff; uint16_t message_length=0; std::string message_body; - uint16_t check_code=0x0005; + uint8_t check_code=0x05; Message() = default; operator std::vector() const; Message& operator=(const std::vector& byte_stream); diff --git a/src/data_process.cpp b/src/data_process.cpp index e9dd948..7c8f515 100644 --- a/src/data_process.cpp +++ b/src/data_process.cpp @@ -3,7 +3,7 @@ data_process::data_process(rclcpp::Node::SharedPtr node){ node_ = node; traffic_signal_array_pub_ = node_->create_publisher("/perception/traffic_light_recognition/traffic_signals", rclcpp::QoS(1)); - heading_sub_ = node_->create_subscription("/sensing/gnss/heading", 10, + heading_sub_ = node_->create_subscription("sensing/gnss/chc/heading", 10, std::bind(&data_process::callback_heading, this, std::placeholders::_1)); }; data_process:: ~data_process(){}; @@ -169,15 +169,11 @@ std::string data_process::get_string(){ } void data_process::callback_heading(const Float32::SharedPtr msg){ - if (msg->data < 180){ + if (270data || msg->data< 90){ traffic_signal_msg_.map_primitive_id = 11071; - //0 左转,1 直行,2 右转 - Lanlet_id = 0; - } else{ traffic_signal_msg_.map_primitive_id = 15666; - Lanlet_id = 1; } std::cout<<"test"< received_data){ receive_message = received_data; traffic_signal_msg_.lights.clear(); traffic_signal_array_msg_.signals.clear(); - json parsed_json = json::parse(receive_message.message_body); - // 3:红,6:绿,7:黄 - std::cout<<"state_active:"<1){ + // std::cout<now(); - traffic_signal_array_msg_.header.frame_id = "obu"; - traffic_signal_array_msg_.signals.push_back(traffic_signal_msg_); - traffic_signal_array_pub_->publish(traffic_signal_array_msg_); + traffic_signal_array_msg_.header.stamp = node_->now(); + traffic_signal_array_msg_.header.frame_id = "obu"; + traffic_signal_array_msg_.signals.push_back(traffic_signal_msg_); + traffic_signal_array_pub_->publish(traffic_signal_array_msg_); + } + else{ + std::cout<<"没有找到灯数据"< received_data= udp_server.receive_data(); std::cout<28){ - data_process_.parse_data(received_data); - std::cout<<"receive over"<28){ + data_process_.parse_data(received_data); + std::cout<<"receive over"< udp_server::receive_data(){ (struct sockaddr*)&client_address, &client_address_len_); if (bytes_received == -1) { std::cerr << "Error: Send failed.bytesReceived:" < received_data(buffer, buffer + bytes_received); + return received_data; } - - std::vector received_data(buffer, buffer + bytes_received); - return received_data; + // return 0; } void udp_server::send_to_client(std::vector message , size_t len_) { - std::cout<<"std::"<