|
@@ -105,7 +105,11 @@ void Dispatch_ground_lidar::execute_thread_fun()
|
|
|
std::unique_lock<std::mutex> t_lock(Dispatch_communication::get_instance_references().m_data_lock);
|
|
|
std::unique_lock<std::mutex> t_lock2(m_lock);
|
|
|
|
|
|
- //将nnxx的protobuf 转化为 snap7的DB块, 把店面雷达数据转发给plc
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ //将nnxx的protobuf 转化为 snap7的DB块, 把店面雷达数据转发给plc
|
|
|
// int t_inlet_id = m_ground_status_msg.mutable_id_struct()->terminal_id() % 2;
|
|
|
int t_inlet_id = m_ground_lidar_id;
|
|
|
Dispatch_communication::Ground_lidar_response_from_manager_to_plc_for_data *p_response_data = &Dispatch_communication::get_instance_references().m_ground_lidar_response_from_manager_to_plc_for_data[t_inlet_id];
|
|
@@ -132,22 +136,50 @@ void Dispatch_ground_lidar::execute_thread_fun()
|
|
|
// p_response_data->m_response_uniformed_car_y = m_ground_status_msg.locate_information_realtime().uniformed_car_y();
|
|
|
// p_response_data->m_response_locate_correct = m_ground_status_msg.locate_information_realtime().locate_correct();
|
|
|
|
|
|
- p_response_data->m_response_car_center_x = m_measure_info.cx();
|
|
|
- p_response_data->m_response_car_center_y = m_measure_info.cy();
|
|
|
- p_response_data->m_response_car_angle = m_measure_info.theta();
|
|
|
- p_response_data->m_response_car_front_theta = m_measure_info.front_theta();
|
|
|
- p_response_data->m_response_car_length = m_measure_info.length();
|
|
|
- p_response_data->m_response_car_width = m_measure_info.width();
|
|
|
- p_response_data->m_response_car_height = m_measure_info.height();
|
|
|
- p_response_data->m_response_car_wheel_base = m_measure_info.wheelbase();
|
|
|
- p_response_data->m_response_car_wheel_width = m_measure_info.width();
|
|
|
- p_response_data->m_response_uniformed_car_x = m_measure_info.cx();
|
|
|
- p_response_data->m_response_uniformed_car_y = m_measure_info.cy();
|
|
|
+
|
|
|
if ( m_measure_info.ground_status() == 0 ) //ground_status :ok 1,nothing 2,noise 3,border
|
|
|
{
|
|
|
+ p_response_data->m_response_car_center_x = m_measure_info.cx();
|
|
|
+ p_response_data->m_response_car_center_y = m_measure_info.cy();
|
|
|
+ p_response_data->m_response_car_angle = m_measure_info.theta();
|
|
|
+ p_response_data->m_response_car_front_theta = m_measure_info.front_theta();
|
|
|
+ p_response_data->m_response_car_length = m_measure_info.length();
|
|
|
+ p_response_data->m_response_car_width = m_measure_info.width();
|
|
|
+ p_response_data->m_response_car_height = m_measure_info.height();
|
|
|
+ p_response_data->m_response_car_wheel_base = m_measure_info.wheelbase();
|
|
|
+ p_response_data->m_response_car_wheel_width = m_measure_info.width();
|
|
|
+ p_response_data->m_response_uniformed_car_x = m_measure_info.cx();
|
|
|
+ p_response_data->m_response_uniformed_car_y = m_measure_info.cy();
|
|
|
+
|
|
|
p_response_data->m_response_locate_correct = 1; //locate_correct 0=无效, 1=有效,
|
|
|
}
|
|
|
- }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ p_response_data->m_response_car_center_x = 0;
|
|
|
+ p_response_data->m_response_car_center_y = 0;
|
|
|
+ p_response_data->m_response_car_angle = 0;
|
|
|
+ p_response_data->m_response_car_front_theta = 0;
|
|
|
+ p_response_data->m_response_car_length = 0;
|
|
|
+ p_response_data->m_response_car_width = 0;
|
|
|
+ p_response_data->m_response_car_height = 0;
|
|
|
+ p_response_data->m_response_car_wheel_base = 0;
|
|
|
+ p_response_data->m_response_car_wheel_width = 0;
|
|
|
+ p_response_data->m_response_uniformed_car_x = 0;
|
|
|
+ p_response_data->m_response_uniformed_car_y = 0;
|
|
|
+
|
|
|
+ p_response_data->m_response_locate_correct = 0;
|
|
|
+ }
|
|
|
+
|
|
|
+// LOG(WARNING) << "------------------------------------------------------------ "<<std::endl;
|
|
|
+// LOG(WARNING) << "m_measure_info = " << m_measure_info.ground_status() <<std::endl;
|
|
|
+// LOG(WARNING) << "m_measure_info = " << m_measure_info.DebugString() <<std::endl;
|
|
|
+// LOG(WARNING) << "m_measure_info.wheelbase() = " << m_measure_info.wheelbase() <<std::endl;
|
|
|
+// LOG(WARNING) << "m_measure_info.ground_status() = " << m_measure_info.ground_status() <<std::endl;
|
|
|
+// LOG(WARNING) << "p_response_data->m_response_locate_correct = " << p_response_data->m_response_locate_correct <<std::endl;
|
|
|
+// printf("p_response_data->m_response_locate_correct = %d, \n", p_response_data->m_response_locate_correct);
|
|
|
+// LOG(WARNING) << "-++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ "<<std::endl;
|
|
|
+
|
|
|
+ }
|
|
|
else if (p_response_data->m_response_refresh_command == p_request->m_request_refresh_command)
|
|
|
{
|
|
|
p_response_data->m_response_heartbeat = 1+p_response_data->m_response_heartbeat;
|
|
@@ -173,21 +205,38 @@ void Dispatch_ground_lidar::execute_thread_fun()
|
|
|
// p_response_data->m_response_uniformed_car_y = m_ground_status_msg.locate_information_realtime().uniformed_car_y();
|
|
|
// p_response_data->m_response_locate_correct = m_ground_status_msg.locate_information_realtime().locate_correct();
|
|
|
|
|
|
- p_response_data->m_response_car_center_x = m_measure_info.cx();
|
|
|
- p_response_data->m_response_car_center_y = m_measure_info.cy();
|
|
|
- p_response_data->m_response_car_angle = m_measure_info.theta();
|
|
|
- p_response_data->m_response_car_front_theta = m_measure_info.front_theta();
|
|
|
- p_response_data->m_response_car_length = m_measure_info.length();
|
|
|
- p_response_data->m_response_car_width = m_measure_info.width();
|
|
|
- p_response_data->m_response_car_height = m_measure_info.height();
|
|
|
- p_response_data->m_response_car_wheel_base = m_measure_info.wheelbase();
|
|
|
- p_response_data->m_response_car_wheel_width = m_measure_info.width();
|
|
|
- p_response_data->m_response_uniformed_car_x = m_measure_info.cx();
|
|
|
- p_response_data->m_response_uniformed_car_y = m_measure_info.cy();
|
|
|
- if ( m_measure_info.ground_status() == 0 ) //ground_status :ok 1,nothing 2,noise 3,border
|
|
|
- {
|
|
|
- p_response_data->m_response_locate_correct = 1; //locate_correct 0=无效, 1=有效,
|
|
|
- }
|
|
|
+ if ( m_measure_info.ground_status() == 0 ) //ground_status :ok 1,nothing 2,noise 3,border
|
|
|
+ {
|
|
|
+ p_response_data->m_response_car_center_x = m_measure_info.cx();
|
|
|
+ p_response_data->m_response_car_center_y = m_measure_info.cy();
|
|
|
+ p_response_data->m_response_car_angle = m_measure_info.theta();
|
|
|
+ p_response_data->m_response_car_front_theta = m_measure_info.front_theta();
|
|
|
+ p_response_data->m_response_car_length = m_measure_info.length();
|
|
|
+ p_response_data->m_response_car_width = m_measure_info.width();
|
|
|
+ p_response_data->m_response_car_height = m_measure_info.height();
|
|
|
+ p_response_data->m_response_car_wheel_base = m_measure_info.wheelbase();
|
|
|
+ p_response_data->m_response_car_wheel_width = m_measure_info.width();
|
|
|
+ p_response_data->m_response_uniformed_car_x = m_measure_info.cx();
|
|
|
+ p_response_data->m_response_uniformed_car_y = m_measure_info.cy();
|
|
|
+
|
|
|
+ p_response_data->m_response_locate_correct = 1; //locate_correct 0=无效, 1=有效,
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ p_response_data->m_response_car_center_x = 0;
|
|
|
+ p_response_data->m_response_car_center_y = 0;
|
|
|
+ p_response_data->m_response_car_angle = 0;
|
|
|
+ p_response_data->m_response_car_front_theta = 0;
|
|
|
+ p_response_data->m_response_car_length = 0;
|
|
|
+ p_response_data->m_response_car_width = 0;
|
|
|
+ p_response_data->m_response_car_height = 0;
|
|
|
+ p_response_data->m_response_car_wheel_base = 0;
|
|
|
+ p_response_data->m_response_car_wheel_width = 0;
|
|
|
+ p_response_data->m_response_uniformed_car_x = 0;
|
|
|
+ p_response_data->m_response_uniformed_car_y = 0;
|
|
|
+
|
|
|
+ p_response_data->m_response_locate_correct = 0;
|
|
|
+ }
|
|
|
}
|
|
|
|
|
|
m_ground_status_msg_updata_flag = false;
|