Skip to content

Commit

Permalink
fix:实车测试
Browse files Browse the repository at this point in the history
  • Loading branch information
@six9527
six9527 committed Jun 20, 2024
1 parent 1473543 commit f497d82
Show file tree
Hide file tree
Showing 5 changed files with 63 additions and 42 deletions.
2 changes: 1 addition & 1 deletion include/obu/data_process.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<uint8_t>() const;
Message& operator=(const std::vector<uint8_t>& byte_stream);
Expand Down
74 changes: 43 additions & 31 deletions src/data_process.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,7 +3,7 @@ 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,
heading_sub_ = node_->create_subscription<Float32>("sensing/gnss/chc/heading", 10,
std::bind(&data_process::callback_heading, this, std::placeholders::_1));
};
data_process:: ~data_process(){};
Expand Down Expand Up @@ -169,15 +169,11 @@ std::string data_process::get_string(){
}

void data_process::callback_heading(const Float32::SharedPtr msg){
if (msg->data < 180){
if (270<msg->data || 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"<<std::endl;
}
Expand All @@ -202,31 +198,47 @@ void data_process::parse_data(std::vector<uint8_t> 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:"<<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_.UP_ARROW;
traffic_light_msg_.status = traffic_light_msg_.SOLID_ON;
}
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;
if (receive_message.data_type == 0x01){
json parsed_json = json::parse(receive_message.message_body);
// 3：红，6：绿，7：黄
if (sizeof(parsed_json["scene"])>1){
// std::cout<<parsed_json<<std::endl;
// std::cout<<parsed_json["scene"]["glosa"]["glosa_info"][Lanlet_id]["state"]<<std::endl;
for (int i=0;i<=3;i++){
if ((parsed_json["scene"]["glosa"]["glosa_info"][i]["state_active"]) == 1 && (parsed_json["scene"]["glosa"]["glosa_info"][i]["state"]) != 0){
std::cout<<parsed_json["scene"]["glosa"]["glosa_info"][i]<<std::endl;
if ((parsed_json["scene"]["glosa"]["glosa_info"][i]["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;
}
else if ((parsed_json["scene"]["glosa"]["glosa_info"][i]["state"]) == 6){
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;
}
else if ((parsed_json["scene"]["glosa"]["glosa_info"][i]["state"]) == 7){
traffic_light_msg_.color = traffic_light_msg_.AMBER;
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_light_msg_.confidence = 1;
traffic_signal_msg_.lights.push_back(traffic_light_msg_);
traffic_light_msg_.confidence = 1;
traffic_signal_msg_.lights.push_back(traffic_light_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_);
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<<"没有找到灯数据"<<std::endl;
}
}
}
}
else{
std::cout<<"未收到数据"<<std::endl;
}
}
8 changes: 4 additions & 4 deletions src/obu_socket_node.cpp
100755 → 100644
View file
Original file line number Diff line number Diff line change
Expand Up @@ -20,10 +20,10 @@ int main(int argc, char** argv) {
// std::cout<<"send over"<<std::endl;
std::vector<uint8_t> received_data= udp_server.receive_data();
std::cout<<sizeof(received_data)<<std::endl;
if (sizeof(received_data)>28){
data_process_.parse_data(received_data);
std::cout<<"receive over"<<std::endl;
}
// if (sizeof(received_data)>28){
data_process_.parse_data(received_data);
std::cout<<"receive over"<<std::endl;
// }

rclcpp::spin_some(node);
sleep(1);
Expand Down
21 changes: 15 additions & 6 deletions src/udp_server.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -16,8 +16,8 @@ udp_server::udp_server(const std::string& serverAddress, int serverPort) {
// // 设置客户端地址
memset(&client_address, 0, sizeof(client_address));
client_address.sin_family = AF_INET;
client_address.sin_port = htons(9886); // 目标端口
client_address.sin_addr.s_addr = inet_addr("127.0.0.2"); // 目标 IP 地址
// client_address.sin_port = htons(9886); // 目标端口
// client_address.sin_addr.s_addr = inet_addr("127.0.0.2"); // 目标 IP 地址

// 将套接字与服务器地址绑定
if (bind(socket_fd_, (struct sockaddr*)&server_address_, sizeof(server_address_)) == -1) {
Expand All @@ -40,14 +40,23 @@ std::vector<uint8_t> udp_server::receive_data(){
(struct sockaddr*)&client_address, &client_address_len_);
if (bytes_received == -1) {
std::cerr << "Error: Send failed.bytesReceived:" <<bytes_received<< std::endl;
}else{
// char client_ip[INET_ADDRSTRLEN];
// inet_ntop(AF_INET, &(client_address.sin_addr), client_ip, INET_ADDRSTRLEN);
// std::cout << "Received data from " << client_ip << ":" << ntohs(client_address.sin_port) << std::endl;

// 输出接收到的数据
// std::cout << "Received data (" << bytes_received << " bytes): ";
// for (ssize_t i = 0; i < bytes_received; ++i) {
// printf("%02x ", buffer[i]); // 以十六进制格式输出每个字节
// }
std::vector<uint8_t> received_data(buffer, buffer + bytes_received);
return received_data;
}

std::vector<uint8_t> received_data(buffer, buffer + bytes_received);
return received_data;
// return 0;
}

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) {
Expand Down
Empty file modified test/server.py
100755 → 100644
View file
Empty file.

0 comments on commit f497d82

Please sign in to comment.