|
|
@@ -3,13 +3,6 @@
|
|
|
//
|
|
|
|
|
|
#include "system_executor.h"
|
|
|
-#include "../tool/common_data.h"
|
|
|
-#include "../tool/measure_filter.h"
|
|
|
-#include "../message/measure_message.pb.h"
|
|
|
-#include "../system/system_communication.h"
|
|
|
-
|
|
|
-#include "../wanji_lidar/wanji_manager.h"
|
|
|
-#include "../velodyne_lidar/velodyne_manager.h"
|
|
|
|
|
|
// std::ofstream g_debug_file; // 用于测试滤波功能
|
|
|
|
|
|
@@ -226,6 +219,99 @@ Error_manager System_executor::check_status()
|
|
|
}
|
|
|
}
|
|
|
|
|
|
+// 更新消息中关于定位结果与超界信息的内容
|
|
|
+bool System_executor::update_measure_info(message::Ground_status_msg &msg, Common_data::Car_wheel_information &measure_info, int cloud_size)
|
|
|
+{
|
|
|
+ Error_manager t_error;
|
|
|
+ //无论定位结果如何, 都要填充数据, 即使全部写0, 必须保证通信格式正确.
|
|
|
+ message::Locate_information t_locate_information;
|
|
|
+ //= t_multi_status_msg.add_locate_information_realtime();
|
|
|
+ t_locate_information.set_locate_x(measure_info.car_center_x);
|
|
|
+ t_locate_information.set_locate_y(measure_info.car_center_y);
|
|
|
+ t_locate_information.set_locate_angle(measure_info.car_angle);
|
|
|
+ t_locate_information.set_locate_length(0);
|
|
|
+ t_locate_information.set_locate_width(0);
|
|
|
+ t_locate_information.set_locate_height(0);
|
|
|
+ t_locate_information.set_locate_wheel_base(measure_info.car_wheel_base);
|
|
|
+ t_locate_information.set_locate_wheel_width(measure_info.car_wheel_width);
|
|
|
+ t_locate_information.set_locate_front_theta(measure_info.car_front_theta);
|
|
|
+ t_locate_information.set_locate_correct(measure_info.correctness);
|
|
|
+ t_locate_information.set_uniformed_car_x(measure_info.uniform_car_x);
|
|
|
+ t_locate_information.set_uniformed_car_y(measure_info.uniform_car_y);
|
|
|
+ msg.mutable_locate_information_realtime()->CopyFrom(t_locate_information);
|
|
|
+ // 当前超界提示仅保留一项
|
|
|
+ // 20211026已修改，分离为电子围栏状态与超界状态，只有测量正确时超界状态才有意义
|
|
|
+ msg.set_border_status(measure_info.range_status);
|
|
|
+ if (!measure_info.correctness)
|
|
|
+ {
|
|
|
+ if (cloud_size > 0)
|
|
|
+ msg.set_ground_status(message::Ground_statu::Noise);
|
|
|
+ else
|
|
|
+ msg.set_ground_status(message::Ground_statu::Nothing);
|
|
|
+ }
|
|
|
+ else if (measure_info.range_status == int(Ground_region::Range_status::Range_correct))
|
|
|
+ {
|
|
|
+ msg.set_ground_status(message::Ground_statu::Car_correct);
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ msg.set_ground_status(message::Ground_statu::Car_border_reached);
|
|
|
+ // 更新待提示错误信息
|
|
|
+ std::string t_error_str;
|
|
|
+ if (measure_info.range_status & Ground_region::Range_status::Range_front != 0)
|
|
|
+ {
|
|
|
+ t_error_str.append("前超界 ");
|
|
|
+ }
|
|
|
+ if (measure_info.range_status & Ground_region::Range_status::Range_back != 0)
|
|
|
+ {
|
|
|
+ t_error_str.append("后超界 ");
|
|
|
+ }
|
|
|
+ if (measure_info.range_status & Ground_region::Range_status::Range_left != 0)
|
|
|
+ {
|
|
|
+ t_error_str.append("左超界 ");
|
|
|
+ }
|
|
|
+ if (measure_info.range_status & Ground_region::Range_status::Range_right != 0)
|
|
|
+ {
|
|
|
+ t_error_str.append("右超界 ");
|
|
|
+ }
|
|
|
+ if (measure_info.range_status & Ground_region::Range_status::Range_bottom != 0)
|
|
|
+ {
|
|
|
+ t_error_str.append("底盘超界 ");
|
|
|
+ }
|
|
|
+ if (measure_info.range_status & Ground_region::Range_status::Range_top != 0)
|
|
|
+ {
|
|
|
+ t_error_str.append("顶超界 ");
|
|
|
+ }
|
|
|
+ if (measure_info.range_status & Ground_region::Range_status::Range_car_width != 0)
|
|
|
+ {
|
|
|
+ t_error_str.append("车宽超界 ");
|
|
|
+ }
|
|
|
+ if (measure_info.range_status & Ground_region::Range_status::Range_car_wheelbase != 0)
|
|
|
+ {
|
|
|
+ t_error_str.append("轴距超界 ");
|
|
|
+ }
|
|
|
+ if (measure_info.range_status & Ground_region::Range_status::Range_angle_anti_clock != 0)
|
|
|
+ {
|
|
|
+ t_error_str.append("车辆角度左超界 ");
|
|
|
+ }
|
|
|
+ if (measure_info.range_status & Ground_region::Range_status::Range_angle_clock != 0)
|
|
|
+ {
|
|
|
+ t_error_str.append("车辆角度右超界 ");
|
|
|
+ }
|
|
|
+ if (measure_info.range_status & Ground_region::Range_status::Range_steering_wheel_nozero != 0)
|
|
|
+ {
|
|
|
+ t_error_str.append("车辆前轮角超界 ");
|
|
|
+ }
|
|
|
+
|
|
|
+ t_error.set_error_description(t_error_str);
|
|
|
+ }
|
|
|
+
|
|
|
+ msg.mutable_error_manager()->set_error_code(t_error.get_error_code());
|
|
|
+ msg.mutable_error_manager()->set_error_level((message::Error_level)t_error.get_error_level());
|
|
|
+ msg.mutable_error_manager()->set_error_description(t_error.get_error_description());
|
|
|
+ return true;
|
|
|
+}
|
|
|
+
|
|
|
//定时发送状态信息
|
|
|
Error_manager System_executor::encapsulate_send_status()
|
|
|
{
|
|
|
@@ -247,13 +333,153 @@ Error_manager System_executor::encapsulate_send_status()
|
|
|
// t_ground_status_msg.set_terminal_id(m_terminal_id);
|
|
|
t_ground_status_msg.mutable_id_struct()->set_terminal_id(m_terminal_id);
|
|
|
|
|
|
- // 创建各区域状态消息,
|
|
|
+ // 创建各区域状态消息,
|
|
|
// 注意！！！目前公共消息名字依旧使用wj，不做修改
|
|
|
// manager
|
|
|
+#if WJ_VELO == 1
|
|
|
+ {
|
|
|
+ Velodyne_manager::Velodyne_manager_status t_velodyne_manager_status = Velodyne_manager::get_instance_references().get_status();
|
|
|
+ t_ground_status_msg.set_wanji_manager_status((message::Wanji_manager_status)t_velodyne_manager_status);
|
|
|
+
|
|
|
+ // lidar
|
|
|
+ std::map<int, Velodyne_lidar_device *> t_velodyne_lidar_device_map = Velodyne_manager::get_instance_references().get_velodyne_lidar_device_map();
|
|
|
+ std::map<int, Velodyne_lidar_device::Velodyne_lidar_device_status> t_velodyne_lidar_status_map;
|
|
|
+ for (auto iter = t_velodyne_lidar_device_map.begin(); iter != t_velodyne_lidar_device_map.end(); ++iter)
|
|
|
+ {
|
|
|
+ t_velodyne_lidar_status_map.emplace(std::pair<int, Velodyne_lidar_device::Velodyne_lidar_device_status>(iter->second->get_lidar_id(), iter->second->get_status()));
|
|
|
+ }
|
|
|
+
|
|
|
+ // region
|
|
|
+ std::map<int, Ground_region *> t_ground_region_map = Velodyne_manager::get_instance_references().get_ground_region_map();
|
|
|
+ int region_index = 0;
|
|
|
+ for (auto iter = t_ground_region_map.begin(); iter != t_ground_region_map.end(); ++iter)
|
|
|
+ {
|
|
|
+ // 以t_ground_status_msg为模板创建各区域心跳消息
|
|
|
+ message::Ground_status_msg t_multi_status_msg;
|
|
|
+ t_multi_status_msg.CopyFrom(t_ground_status_msg);
|
|
|
+ t_multi_status_msg.mutable_id_struct()->set_terminal_id(iter->second->get_terminal_id());
|
|
|
+ t_multi_status_msg.set_region_worker_status((message::Region_worker_status)(iter->second->get_status()));
|
|
|
+ velodyne::Region t_param = iter->second->get_param();
|
|
|
+ for (size_t j = 0; j < t_param.lidar_exts_size(); j++)
|
|
|
+ {
|
|
|
+ std::map<int, Velodyne_lidar_device::Velodyne_lidar_device_status>::iterator t_status_iter = t_velodyne_lidar_status_map.find(t_param.lidar_exts(j).lidar_id());
|
|
|
+ if (t_status_iter == t_velodyne_lidar_status_map.end())
|
|
|
+ {
|
|
|
+ LOG(WARNING) << "lidar status " << t_param.lidar_exts(j).lidar_id() << " cannot be found, param error";
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ t_multi_status_msg.add_wanji_lidar_device_status((message::Wanji_lidar_device_status)t_status_iter->second);
|
|
|
+ }
|
|
|
+ }
|
|
|
+ //velodyne雷达的自动定位信息
|
|
|
+ Common_data::Car_wheel_information t_car_wheel_information;
|
|
|
+ t_error = (*iter).second->get_last_wheel_information(&t_car_wheel_information, std::chrono::system_clock::now());
|
|
|
+
|
|
|
+ // 获取区域点云填入信息
|
|
|
+ pcl::PointCloud<pcl::PointXYZ>::Ptr t_region_cloud = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
|
|
|
+ iter->second->get_region_cloud(t_region_cloud, Ground_region::Region_cloud_type::filtered);
|
|
|
+ if (cloud_count == 5)
|
|
|
+ {
|
|
|
+ std::string t_filename = std::string("region_") + std::to_string(iter->first) + "_cloud.txt";
|
|
|
+ save_cloud_txt(t_region_cloud, t_filename);
|
|
|
+ LOG(INFO) << "region " << iter->first << " cloud has been saved in " + t_filename;
|
|
|
+ }
|
|
|
+ // for (size_t j = 0; j < t_region_cloud->size(); j++)
|
|
|
+ // {
|
|
|
+ // message::Cloud_coordinate *tp_cloud = t_multi_status_msg.add_cloud();
|
|
|
+ // tp_cloud->set_x(t_region_cloud->points[j].x);
|
|
|
+ // tp_cloud->set_y(t_region_cloud->points[j].y);
|
|
|
+ // tp_cloud->set_z(t_region_cloud->points[j].z);
|
|
|
+ // }
|
|
|
+
|
|
|
+ update_measure_info(t_multi_status_msg, t_car_wheel_information, t_region_cloud->size());
|
|
|
+
|
|
|
+ std::string t_msg = t_multi_status_msg.SerializeAsString();
|
|
|
+ System_communication::get_instance_references().encapsulate_msg(t_msg);
|
|
|
+ if (t_multi_status_msg.id_struct().terminal_id() == DISP_TERM_ID)
|
|
|
+ std::cout << t_multi_status_msg.DebugString() << std::endl
|
|
|
+ << std::endl;
|
|
|
+ }
|
|
|
+ }
|
|
|
+#elif WJ_VELO == 0
|
|
|
+ {
|
|
|
+ // wj_support 20220718
|
|
|
+ Wanji_manager::Wanji_manager_status t_wj_manager_status = Wanji_manager::get_instance_references().get_status();
|
|
|
+ t_ground_status_msg.set_wanji_manager_status((message::Wanji_manager_status)t_wj_manager_status);
|
|
|
+
|
|
|
+ // lidar
|
|
|
+ std::map<int, Wanji_lidar_device *> t_wj_lidar_device_map = Wanji_manager::get_instance_references().get_wanji_lidar_device_map();
|
|
|
+ std::map<int, Wanji_lidar_device::Wanji_lidar_device_status> t_wj_lidar_status_map;
|
|
|
+ for (auto iter = t_wj_lidar_device_map.begin(); iter != t_wj_lidar_device_map.end(); ++iter)
|
|
|
+ {
|
|
|
+ t_wj_lidar_status_map.emplace(std::pair<int, Wanji_lidar_device::Wanji_lidar_device_status>(iter->second->get_lidar_id(), iter->second->get_status()));
|
|
|
+ }
|
|
|
+
|
|
|
+ // region
|
|
|
+ std::map<int, Region_worker *> t_region_worker_map = Wanji_manager::get_instance_references().get_region_worker_map();
|
|
|
+ int region_index = 0;
|
|
|
+ for (auto iter = t_region_worker_map.begin(); iter != t_region_worker_map.end(); ++iter)
|
|
|
+ {
|
|
|
+ // 以t_ground_status_msg为模板创建各区域心跳消息
|
|
|
+ message::Ground_status_msg t_multi_status_msg;
|
|
|
+ t_multi_status_msg.CopyFrom(t_ground_status_msg);
|
|
|
+ t_multi_status_msg.mutable_id_struct()->set_terminal_id(iter->second->get_terminal_id());
|
|
|
+ t_multi_status_msg.set_region_worker_status((message::Region_worker_status)(iter->second->get_status()));
|
|
|
+ wj::Region t_param = iter->second->get_param();
|
|
|
+ for (size_t j = 0; j < t_param.lidar_exts_size(); j++)
|
|
|
+ {
|
|
|
+ std::map<int, Wanji_lidar_device::Wanji_lidar_device_status>::iterator t_status_iter = t_wj_lidar_status_map.find(t_param.lidar_exts(j).lidar_id());
|
|
|
+ if (t_status_iter == t_wj_lidar_status_map.end())
|
|
|
+ {
|
|
|
+ LOG(WARNING) << "lidar status " << t_param.lidar_exts(j).lidar_id() << " cannot be found, param error";
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ t_multi_status_msg.add_wanji_lidar_device_status((message::Wanji_lidar_device_status)t_status_iter->second);
|
|
|
+ }
|
|
|
+ }
|
|
|
+ //velodyne雷达的自动定位信息
|
|
|
+ Common_data::Car_wheel_information t_car_wheel_information;
|
|
|
+ t_error = (*iter).second->get_last_wheel_information(&t_car_wheel_information, std::chrono::system_clock::now());
|
|
|
+
|
|
|
+ // 获取区域点云填入信息
|
|
|
+ pcl::PointCloud<pcl::PointXYZ>::Ptr t_region_cloud = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
|
|
|
+ iter->second->get_region_cloud(t_region_cloud);
|
|
|
+ if (cloud_count == 5)
|
|
|
+ {
|
|
|
+ std::string t_filename = std::string("region_") + std::to_string(iter->first) + "_cloud.txt";
|
|
|
+ save_cloud_txt(t_region_cloud, t_filename);
|
|
|
+ LOG(INFO) << "region " << iter->first << " cloud has been saved in " + t_filename;
|
|
|
+ }
|
|
|
+ // for (size_t j = 0; j < t_region_cloud->size(); j++)
|
|
|
+ // {
|
|
|
+ // message::Cloud_coordinate *tp_cloud = t_multi_status_msg.add_cloud();
|
|
|
+ // tp_cloud->set_x(t_region_cloud->points[j].x);
|
|
|
+ // tp_cloud->set_y(t_region_cloud->points[j].y);
|
|
|
+ // tp_cloud->set_z(t_region_cloud->points[j].z);
|
|
|
+ // }
|
|
|
+
|
|
|
+ update_measure_info(t_multi_status_msg, t_car_wheel_information, t_region_cloud->size());
|
|
|
+
|
|
|
+ std::string t_msg = t_multi_status_msg.SerializeAsString();
|
|
|
+ System_communication::get_instance_references().encapsulate_msg(t_msg);
|
|
|
+ if (t_multi_status_msg.id_struct().terminal_id() == DISP_TERM_ID)
|
|
|
+ LOG(INFO) << t_multi_status_msg.DebugString();
|
|
|
+ }
|
|
|
+ }
|
|
|
+ // std::cout << " huli test :::: " << " t_ground_status_msg.DebugString() = " << std::endl;
|
|
|
+ // std::cout << t_ground_status_msg.DebugString() << std::endl;
|
|
|
+
|
|
|
+ // std::cout << " huli test :::: " << " t_ground_status_msg.DebugString() = " << t_ground_status_msg.DebugString() << std::endl;
|
|
|
+#else
|
|
|
+{
|
|
|
+ // mix vlp16 and wj716
|
|
|
+ // 以多线雷达为主
|
|
|
Velodyne_manager::Velodyne_manager_status t_velodyne_manager_status = Velodyne_manager::get_instance_references().get_status();
|
|
|
t_ground_status_msg.set_wanji_manager_status((message::Wanji_manager_status)t_velodyne_manager_status);
|
|
|
|
|
|
- // lidar
|
|
|
+ // vlp16 lidar
|
|
|
std::map<int, Velodyne_lidar_device *> t_velodyne_lidar_device_map = Velodyne_manager::get_instance_references().get_velodyne_lidar_device_map();
|
|
|
std::map<int, Velodyne_lidar_device::Velodyne_lidar_device_status> t_velodyne_lidar_status_map;
|
|
|
for (auto iter = t_velodyne_lidar_device_map.begin(); iter != t_velodyne_lidar_device_map.end(); ++iter)
|
|
|
@@ -261,44 +487,65 @@ Error_manager System_executor::encapsulate_send_status()
|
|
|
t_velodyne_lidar_status_map.emplace(std::pair<int, Velodyne_lidar_device::Velodyne_lidar_device_status>(iter->second->get_lidar_id(), iter->second->get_status()));
|
|
|
}
|
|
|
|
|
|
- // region
|
|
|
+ // wj lidar
|
|
|
+#if LIDAR_TYPE == 1
|
|
|
+ std::map<int, Wanji_lidar_device *> t_wj_lidar_device_map = Wanji_manager::get_instance_references().get_wanji_lidar_device_map();
|
|
|
+ std::map<int, Wanji_lidar_device::Wanji_lidar_device_status> t_wj_lidar_status_map;
|
|
|
+ for (auto iter = t_wj_lidar_device_map.begin(); iter != t_wj_lidar_device_map.end(); ++iter)
|
|
|
+ {
|
|
|
+ t_wj_lidar_status_map.emplace(std::pair<int, Wanji_lidar_device::Wanji_lidar_device_status>(iter->second->get_lidar_id(), iter->second->get_status()));
|
|
|
+ }
|
|
|
+#else
|
|
|
+ std::map<int, Wanji_716N_lidar_device *> t_wj_lidar_device_map = Wanji_manager::get_instance_references().get_wanji_lidar_device_map();
|
|
|
+ std::map<int, Wanji_716N_lidar_device::Wanji_716N_device_status> t_wj_lidar_status_map;
|
|
|
+ for (auto iter = t_wj_lidar_device_map.begin(); iter != t_wj_lidar_device_map.end(); ++iter)
|
|
|
+ {
|
|
|
+ t_wj_lidar_status_map.emplace(std::pair<int, Wanji_716N_lidar_device::Wanji_716N_device_status>(iter->second->get_lidar_id(), iter->second->get_status()));
|
|
|
+ }
|
|
|
+#endif
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ // wj region
|
|
|
+ std::map<int, Region_worker *> t_region_worker_map = Wanji_manager::get_instance_references().get_region_worker_map();
|
|
|
+
|
|
|
+ // vlp16 region
|
|
|
std::map<int, Ground_region *> t_ground_region_map = Velodyne_manager::get_instance_references().get_ground_region_map();
|
|
|
int region_index = 0;
|
|
|
for (auto iter = t_ground_region_map.begin(); iter != t_ground_region_map.end(); ++iter)
|
|
|
{
|
|
|
- bool has_mulfunctional_device = false;
|
|
|
// 以t_ground_status_msg为模板创建各区域心跳消息
|
|
|
message::Ground_status_msg t_multi_status_msg;
|
|
|
t_multi_status_msg.CopyFrom(t_ground_status_msg);
|
|
|
- t_multi_status_msg.mutable_id_struct()->set_terminal_id(iter->second->get_terminal_id());
|
|
|
+ t_multi_status_msg.mutable_id_struct()->set_terminal_id(iter->second->get_terminal_id());
|
|
|
t_multi_status_msg.set_region_worker_status((message::Region_worker_status)(iter->second->get_status()));
|
|
|
+ // 查找多线区域内对应多线激光雷达
|
|
|
velodyne::Region t_param = iter->second->get_param();
|
|
|
- for (size_t j = 0; j <t_param.lidar_exts_size(); j++)
|
|
|
+ for (size_t j = 0; j < t_param.lidar_exts_size(); j++)
|
|
|
{
|
|
|
std::map<int, Velodyne_lidar_device::Velodyne_lidar_device_status>::iterator t_status_iter = t_velodyne_lidar_status_map.find(t_param.lidar_exts(j).lidar_id());
|
|
|
- if(t_status_iter== t_velodyne_lidar_status_map.end())
|
|
|
+ if (t_status_iter == t_velodyne_lidar_status_map.end())
|
|
|
+ {
|
|
|
+ LOG(WARNING) << "vlp16 lidar status " << t_param.lidar_exts(j).lidar_id() << " cannot be found, param error";
|
|
|
+ }
|
|
|
+ else
|
|
|
{
|
|
|
- LOG(WARNING) << "lidar status "<<t_param.lidar_exts(j).lidar_id()<<" cannot be found, param error";
|
|
|
- }else{
|
|
|
t_multi_status_msg.add_wanji_lidar_device_status((message::Wanji_lidar_device_status)t_status_iter->second);
|
|
|
- if(t_status_iter->second != Velodyne_lidar_device::E_READY && t_status_iter->second != Velodyne_lidar_device::E_BUSY)
|
|
|
- {
|
|
|
- has_mulfunctional_device = true;
|
|
|
- }
|
|
|
}
|
|
|
}
|
|
|
+
|
|
|
//velodyne雷达的自动定位信息
|
|
|
- Common_data::Car_wheel_information t_car_wheel_information;
|
|
|
+ Common_data::Car_wheel_information t_car_wheel_information;
|
|
|
t_error = (*iter).second->get_last_wheel_information(&t_car_wheel_information, std::chrono::system_clock::now());
|
|
|
|
|
|
// 获取区域点云填入信息
|
|
|
pcl::PointCloud<pcl::PointXYZ>::Ptr t_region_cloud = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
|
|
|
iter->second->get_region_cloud(t_region_cloud, Ground_region::Region_cloud_type::filtered);
|
|
|
- if(cloud_count == 5)
|
|
|
+ if (cloud_count == 5)
|
|
|
{
|
|
|
- std::string t_filename = std::string("region_")+std::to_string(iter->first)+"_cloud.txt";
|
|
|
+ std::string t_filename = std::string("vlp16_region_") + std::to_string(iter->first) + "_cloud.txt";
|
|
|
save_cloud_txt(t_region_cloud, t_filename);
|
|
|
- LOG(INFO) << "region "<< iter->first <<" cloud has been saved in " + t_filename;
|
|
|
+ LOG(INFO) << "vlp16 region " << iter->first << " cloud has been saved in " + t_filename;
|
|
|
}
|
|
|
// for (size_t j = 0; j < t_region_cloud->size(); j++)
|
|
|
// {
|
|
|
@@ -308,195 +555,77 @@ Error_manager System_executor::encapsulate_send_status()
|
|
|
// tp_cloud->set_z(t_region_cloud->points[j].z);
|
|
|
// }
|
|
|
|
|
|
- //无论定位结果如何, 都要填充数据, 即使全部写0, 必须保证通信格式正确.
|
|
|
- message::Locate_information t_locate_information;
|
|
|
- //= t_multi_status_msg.add_locate_information_realtime();
|
|
|
- t_locate_information.set_locate_x(t_car_wheel_information.car_center_x);
|
|
|
- t_locate_information.set_locate_y(t_car_wheel_information.car_center_y);
|
|
|
- t_locate_information.set_locate_angle(t_car_wheel_information.car_angle);
|
|
|
- t_locate_information.set_locate_length(0);
|
|
|
- t_locate_information.set_locate_width(0);
|
|
|
- t_locate_information.set_locate_height(0);
|
|
|
- t_locate_information.set_locate_wheel_base(t_car_wheel_information.car_wheel_base);
|
|
|
- t_locate_information.set_locate_wheel_width(t_car_wheel_information.car_wheel_width);
|
|
|
- t_locate_information.set_locate_front_theta(t_car_wheel_information.car_front_theta);
|
|
|
- t_locate_information.set_locate_correct(t_car_wheel_information.correctness);
|
|
|
- t_locate_information.set_uniformed_car_x(t_car_wheel_information.uniform_car_x);
|
|
|
- t_locate_information.set_uniformed_car_y(t_car_wheel_information.uniform_car_y);
|
|
|
- t_multi_status_msg.mutable_locate_information_realtime()->CopyFrom(t_locate_information);
|
|
|
- // 当前超界提示仅保留一项
|
|
|
- // 20211026已修改，分离为电子围栏状态与超界状态，只有测量正确时超界状态才有意义
|
|
|
- t_multi_status_msg.set_border_status(t_car_wheel_information.range_status);
|
|
|
- if(!t_car_wheel_information.correctness)
|
|
|
+ // 检查vlp16区域与wj区域对应关系，wj存在对应区域则填充设备状态，并融合测量结果
|
|
|
+ auto t_wj_region_iter = t_region_worker_map.find(iter->first);
|
|
|
+ if(t_wj_region_iter != t_region_worker_map.end())
|
|
|
{
|
|
|
- if(t_region_cloud->size() > 0)
|
|
|
- t_multi_status_msg.set_ground_status(message::Ground_statu::Noise);
|
|
|
- else
|
|
|
- t_multi_status_msg.set_ground_status(message::Ground_statu::Nothing);
|
|
|
- }else if(t_car_wheel_information.range_status == int(Ground_region::Range_status::Range_correct))
|
|
|
- {
|
|
|
- t_multi_status_msg.set_ground_status(message::Ground_statu::Car_correct);
|
|
|
- }else {
|
|
|
- t_multi_status_msg.set_ground_status(message::Ground_statu::Car_border_reached);
|
|
|
- // 更新待提示错误信息
|
|
|
- std::string t_error_str;
|
|
|
- if (t_car_wheel_information.range_status & Ground_region::Range_status::Range_front != 0)
|
|
|
+ wj::Region t_param = t_wj_region_iter->second->get_param();
|
|
|
+ //
|
|
|
+ for (size_t j = 0; j < t_param.lidar_exts_size(); j++)
|
|
|
{
|
|
|
- t_error_str.append("前超界 ");
|
|
|
- }
|
|
|
- if (t_car_wheel_information.range_status & Ground_region::Range_status::Range_back != 0)
|
|
|
- {
|
|
|
- t_error_str.append("后超界 ");
|
|
|
- }
|
|
|
- if (t_car_wheel_information.range_status & Ground_region::Range_status::Range_left != 0)
|
|
|
- {
|
|
|
- t_error_str.append("左超界 ");
|
|
|
- }
|
|
|
- if (t_car_wheel_information.range_status & Ground_region::Range_status::Range_right != 0)
|
|
|
- {
|
|
|
- t_error_str.append("右超界 ");
|
|
|
- }
|
|
|
- if (t_car_wheel_information.range_status & Ground_region::Range_status::Range_bottom != 0)
|
|
|
- {
|
|
|
- t_error_str.append("底盘超界 ");
|
|
|
- }
|
|
|
- if (t_car_wheel_information.range_status & Ground_region::Range_status::Range_top != 0)
|
|
|
- {
|
|
|
- t_error_str.append("顶超界 ");
|
|
|
- }
|
|
|
- if (t_car_wheel_information.range_status & Ground_region::Range_status::Range_car_width != 0)
|
|
|
- {
|
|
|
- t_error_str.append("车宽超界 ");
|
|
|
- }
|
|
|
- if (t_car_wheel_information.range_status & Ground_region::Range_status::Range_car_wheelbase != 0)
|
|
|
- {
|
|
|
- t_error_str.append("轴距超界 ");
|
|
|
+#if LIDAR_TYPE == 1
|
|
|
+ std::map<int, Wanji_lidar_device::Wanji_lidar_device_status>::iterator t_status_iter = t_wj_lidar_status_map.find(t_param.lidar_exts(j).lidar_id());
|
|
|
+#else
|
|
|
+ std::map<int, Wanji_716N_lidar_device::Wanji_716N_device_status>::iterator t_status_iter = t_wj_lidar_status_map.find(t_param.lidar_exts(j).lidar_id());
|
|
|
+#endif
|
|
|
+ if (t_status_iter == t_wj_lidar_status_map.end())
|
|
|
+ {
|
|
|
+ LOG(WARNING) << "wj lidar status " << t_param.lidar_exts(j).lidar_id() << " cannot be found, param error";
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ t_multi_status_msg.add_wanji_lidar_device_status((message::Wanji_lidar_device_status)t_status_iter->second);
|
|
|
+ }
|
|
|
}
|
|
|
- if (t_car_wheel_information.range_status & Ground_region::Range_status::Range_angle_anti_clock != 0)
|
|
|
+ //wj雷达的自动定位信息
|
|
|
+ Common_data::Car_wheel_information t_wj_car_wheel_information;
|
|
|
+ t_error = t_wj_region_iter->second->get_last_wheel_information(&t_wj_car_wheel_information, std::chrono::system_clock::now());
|
|
|
+
|
|
|
+ // 获取区域点云填入信息
|
|
|
+ pcl::PointCloud<pcl::PointXYZ>::Ptr t_wj_region_cloud = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
|
|
|
+ t_wj_region_iter->second->get_region_cloud(t_wj_region_cloud);
|
|
|
+ if (cloud_count == 5)
|
|
|
{
|
|
|
- t_error_str.append("车辆角度左超界 ");
|
|
|
+ std::string t_filename = std::string("wj_region_") + std::to_string(t_wj_region_iter->first) + "_cloud.txt";
|
|
|
+ save_cloud_txt(t_wj_region_cloud, t_filename);
|
|
|
+ LOG(INFO) << "wj region " << t_wj_region_iter->first << " cloud has been saved in " + t_filename;
|
|
|
}
|
|
|
- if (t_car_wheel_information.range_status & Ground_region::Range_status::Range_angle_clock != 0)
|
|
|
+ // for (size_t j = 0; j < t_wj_region_cloud->size(); j++)
|
|
|
+ // {
|
|
|
+ // message::Cloud_coordinate *tp_cloud = t_multi_status_msg.add_cloud();
|
|
|
+ // tp_cloud->set_x(t_wj_region_cloud->points[j].x);
|
|
|
+ // tp_cloud->set_y(t_wj_region_cloud->points[j].y);
|
|
|
+ // tp_cloud->set_z(t_wj_region_cloud->points[j].z);
|
|
|
+ // }
|
|
|
+
|
|
|
+ // 融合测量结果，验证中心点差异x<0.03, y<0.03, 角度差异ang<0.6, 轴距差异wb<0.055
|
|
|
+ double dx = t_car_wheel_information.car_center_x - t_wj_car_wheel_information.car_center_x;
|
|
|
+ double dy = t_car_wheel_information.car_center_y - t_wj_car_wheel_information.car_center_y;
|
|
|
+ double dang = t_car_wheel_information.car_angle - t_wj_car_wheel_information.car_angle;
|
|
|
+ double dwb = t_car_wheel_information.car_wheel_base - t_wj_car_wheel_information.car_wheel_base;
|
|
|
+ if(fabs(dx) < 0.03 && fabs(dy) < 0.03 && fabs(dang) < 0.6 && fabs(dwb) < 0.055)
|
|
|
{
|
|
|
- t_error_str.append("车辆角度右超界 ");
|
|
|
- }
|
|
|
- if (t_car_wheel_information.range_status & Ground_region::Range_status::Range_steering_wheel_nozero != 0)
|
|
|
+ t_car_wheel_information.car_wheel_base = t_car_wheel_information.car_wheel_base*0.2 + t_car_wheel_information.car_wheel_base*0.8;
|
|
|
+ }else
|
|
|
{
|
|
|
- t_error_str.append("车辆前轮角超界 ");
|
|
|
+ LOG_IF(WARNING, iter->second->get_terminal_id() == DISP_TERM_ID)<<
|
|
|
+ "detect mismatch, region "<<iter->first<<", dx dy dang dwb: "<<dx<<", "<<dy<<", "<<dang<<", "<< dwb;
|
|
|
}
|
|
|
-
|
|
|
- t_error.set_error_description(t_error_str);
|
|
|
- }
|
|
|
|
|
|
- if(has_mulfunctional_device)
|
|
|
- {
|
|
|
- t_multi_status_msg.mutable_error_manager()->set_error_code(Error_code::VELODYNE_LIDAR_COMMUNICATION_DISCONNECT);
|
|
|
- t_multi_status_msg.mutable_error_manager()->set_error_level(message::Error_level::MAJOR_ERROR);
|
|
|
- t_multi_status_msg.mutable_error_manager()->set_error_description("has mulfunctional lidar device, please check.");
|
|
|
- }
|
|
|
- else
|
|
|
- {
|
|
|
- t_multi_status_msg.mutable_error_manager()->set_error_code(t_error.get_error_code());
|
|
|
- t_multi_status_msg.mutable_error_manager()->set_error_level((message::Error_level)t_error.get_error_level());
|
|
|
- t_multi_status_msg.mutable_error_manager()->set_error_description(t_error.get_error_description());
|
|
|
+ }else{
|
|
|
+ LOG_IF(WARNING, iter->second->get_terminal_id() == DISP_TERM_ID)<<"region mismatch, vlp16 "<<iter->first<<", wj doesn't have corresponding region id";
|
|
|
}
|
|
|
|
|
|
+ update_measure_info(t_multi_status_msg, t_car_wheel_information, t_region_cloud->size());
|
|
|
+
|
|
|
std::string t_msg = t_multi_status_msg.SerializeAsString();
|
|
|
System_communication::get_instance_references().encapsulate_msg(t_msg);
|
|
|
- if(t_multi_status_msg.id_struct().terminal_id()==DISP_TERM_ID)
|
|
|
- std::cout<<t_multi_status_msg.DebugString()<<std::endl<<std::endl;
|
|
|
-
|
|
|
- usleep(1000*10);
|
|
|
+ if (t_multi_status_msg.id_struct().terminal_id() == DISP_TERM_ID)
|
|
|
+ std::cout << t_multi_status_msg.DebugString() << std::endl
|
|
|
+ << std::endl;
|
|
|
}
|
|
|
-
|
|
|
- // 普爱统一一个万集节点, 各终端消息分别发送
|
|
|
- // 此处将t_ground_status_msg当做模板创建各区域的心跳消息.
|
|
|
- //万集716
|
|
|
-/*Wanji_manager::Wanji_manager_status t_wanji_manager_status = Wanji_manager::get_instance_references().get_status();
|
|
|
- t_ground_status_msg.set_wanji_manager_status((message::Wanji_manager_status)t_wanji_manager_status);
|
|
|
-
|
|
|
- std::map<int, Wanji_lidar_device*> & t_wanji_lidar_device_map = Wanji_manager::get_instance_references().get_wanji_lidar_device_map();
|
|
|
- for (auto iter = t_wanji_lidar_device_map.begin(); iter != t_wanji_lidar_device_map.end(); ++iter)
|
|
|
- {
|
|
|
- Wanji_lidar_device::Wanji_lidar_device_status t_wanji_lidar_device_status = (*iter).second->get_status();
|
|
|
- t_ground_status_msg.add_wanji_lidar_device_status((message::Wanji_lidar_device_status)t_wanji_lidar_device_status);
|
|
|
- }
|
|
|
-
|
|
|
- std::map<int, Region_worker*> & t_region_worker_map = Wanji_manager::get_instance_references().get_region_worker_map();
|
|
|
- int region_index = 0;
|
|
|
- for (auto iter = t_region_worker_map.begin(); iter != t_region_worker_map.end(); ++iter)
|
|
|
- {
|
|
|
- message::Ground_status_msg t_multi_status_msg;
|
|
|
- t_multi_status_msg.CopyFrom(t_ground_status_msg);
|
|
|
- t_multi_status_msg.set_terminal_id(region_index++);
|
|
|
- Region_worker::Region_worker_status t_region_worker_status = (*iter).second->get_status();
|
|
|
- // t_multi_status_msg.add_region_worker_status((message::Region_worker_status)t_region_worker_status);
|
|
|
- t_multi_status_msg.set_region_worker_status((message::Region_worker_status)t_region_worker_status);
|
|
|
-
|
|
|
- //万集雷达的自动定位信息
|
|
|
- Common_data::Car_wheel_information t_car_wheel_information;
|
|
|
- t_error = (*iter).second->get_last_wheel_information(&t_car_wheel_information, std::chrono::system_clock::now());
|
|
|
-
|
|
|
- //无论定位结果如何, 都要填充数据, 即使全部写0, 必须保证通信格式正确.
|
|
|
- message::Locate_information t_locate_information;
|
|
|
- //= t_multi_status_msg.add_locate_information_realtime();
|
|
|
- t_locate_information.set_locate_x(t_car_wheel_information.center_x);
|
|
|
- t_locate_information.set_locate_y(t_car_wheel_information.center_y);
|
|
|
- t_locate_information.set_locate_angle(t_car_wheel_information.car_angle);
|
|
|
- t_locate_information.set_locate_length(0);
|
|
|
- t_locate_information.set_locate_width(0);
|
|
|
- t_locate_information.set_locate_height(0);
|
|
|
- t_locate_information.set_locate_wheel_base(t_car_wheel_information.wheel_base);
|
|
|
- t_locate_information.set_locate_wheel_width(t_car_wheel_information.wheel_width);
|
|
|
- t_locate_information.set_locate_front_theta(t_car_wheel_information.front_theta);
|
|
|
- t_locate_information.set_locate_correct(t_car_wheel_information.correctness);
|
|
|
- t_multi_status_msg.mutable_locate_information_realtime()->CopyFrom(t_locate_information);
|
|
|
- t_multi_status_msg.set_ground_status(t_car_wheel_information.correctness?message::Ground_statu::Car_correct:message::Ground_statu::Noise);
|
|
|
-
|
|
|
- t_multi_status_msg.mutable_error_manager()->set_error_code(t_error.get_error_code());
|
|
|
- t_multi_status_msg.mutable_error_manager()->set_error_level((message::Error_level)t_error.get_error_level());
|
|
|
- t_multi_status_msg.mutable_error_manager()->set_error_description(t_error.get_error_description(), t_error.get_description_length());
|
|
|
- std::string t_msg = t_multi_status_msg.SerializeAsString();
|
|
|
- System_communication::get_instance_references().encapsulate_msg(t_msg);
|
|
|
-
|
|
|
-// // 记录滤波前后测量数据，用于判断测量精度变化
|
|
|
-// static int t_line_count = 0;
|
|
|
-// if(region_index == 6 && ++t_line_count < 120) {
|
|
|
-// if (g_debug_file.is_open() && t_car_wheel_information.correctness) {
|
|
|
-// g_debug_file << "basic:"<<t_line_count<<"\n" <<std::setprecision(6)<<std::setiosflags(std::ios::fixed)
|
|
|
-// << t_car_wheel_information.center_x << ", "
|
|
|
-// << t_car_wheel_information.center_y << ", "
|
|
|
-// << t_car_wheel_information.car_angle << ", "
|
|
|
-// << t_car_wheel_information.wheel_base << ", "
|
|
|
-// << t_car_wheel_information.wheel_width << ", "
|
|
|
-// << t_car_wheel_information.front_theta << std::endl;
|
|
|
-// }
|
|
|
-// Common_data::Car_wheel_information t_filtered_result;
|
|
|
-// Error_manager ec = Measure_filter::get_instance_references().get_filtered_wheel_information(region_index - 1, t_filtered_result);
|
|
|
-// if(ec==SUCCESS){
|
|
|
-// if (g_debug_file.is_open()) {
|
|
|
-// g_debug_file << "filtered:"<<t_line_count<<"\n" <<std::setprecision(6)<<std::setiosflags(std::ios::fixed)
|
|
|
-// << t_filtered_result.center_x << ", "
|
|
|
-// << t_filtered_result.center_y << ", "
|
|
|
-// << t_filtered_result.car_angle << ", "
|
|
|
-// << t_filtered_result.wheel_base << ", "
|
|
|
-// << t_filtered_result.wheel_width << ", "
|
|
|
-// << t_filtered_result.front_theta << std::endl;
|
|
|
-// }
|
|
|
-// }else{
|
|
|
-// LOG(WARNING)<<ec.to_string();
|
|
|
-// }
|
|
|
-//// LOG(WARNING) << t_multi_status_msg.DebugString();
|
|
|
-// }
|
|
|
-}*/
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-// std::cout << " huli test :::: " << " t_ground_status_msg.DebugString() = " << std::endl;
|
|
|
-// std::cout << t_ground_status_msg.DebugString() << std::endl;
|
|
|
-
|
|
|
-// std::cout << " huli test :::: " << " t_ground_status_msg.DebugString() = " << t_ground_status_msg.DebugString() << std::endl;
|
|
|
+}
|
|
|
+#endif
|
|
|
|
|
|
return Error_code::SUCCESS;
|
|
|
}
|
|
|
@@ -504,7 +633,7 @@ Error_manager System_executor::encapsulate_send_status()
|
|
|
//判断是否为待机，如果已经准备好，则可以执行任务。
|
|
|
bool System_executor::is_ready()
|
|
|
{
|
|
|
- if ( m_system_executor_status == SYSTEM_EXECUTOR_READY && m_thread_pool.thread_is_full_load() == false )
|
|
|
+ if (m_system_executor_status == SYSTEM_EXECUTOR_READY && m_thread_pool.thread_is_full_load() == false)
|
|
|
{
|
|
|
return true;
|
|
|
}
|
|
|
@@ -514,180 +643,178 @@ bool System_executor::is_ready()
|
|
|
}
|
|
|
}
|
|
|
|
|
|
-System_executor::System_executor_status System_executor::get_system_executor_status()
|
|
|
-{
|
|
|
- return m_system_executor_status;
|
|
|
-}
|
|
|
-
|
|
|
-int System_executor::get_terminal_id()
|
|
|
-{
|
|
|
- return m_terminal_id;
|
|
|
-}
|
|
|
-
|
|
|
-//雷达感测定位 的处理函数
|
|
|
-//input::command_id, 消息指令id, 由主控制系统生成的唯一码
|
|
|
-//input::command_id, 终端id, 对应具体的某个车位
|
|
|
-//return::void, 没有返回, 执行结果直接生成一条答复消息, 然后通过通信返回
|
|
|
-void System_executor::execute_for_measure(std::string command_info, int terminal_id,std::chrono::system_clock::time_point receive_time)
|
|
|
-{
|
|
|
- Error_manager t_error;
|
|
|
- Error_manager t_result;
|
|
|
- LOG(INFO) << " System_executor::execute_for_measure run "<< this;
|
|
|
-
|
|
|
- //第1步, 按照时间生成中间文件的保存路径
|
|
|
- time_t nowTime;
|
|
|
- nowTime = time(NULL);
|
|
|
- struct tm* sysTime = localtime(&nowTime);
|
|
|
- char t_save_path[256] = { 0 };
|
|
|
- sprintf(t_save_path, "../data/%04d%02d%02d_%02d%02d%02d",
|
|
|
- sysTime->tm_year + 1900, sysTime->tm_mon + 1, sysTime->tm_mday, sysTime->tm_hour, sysTime->tm_min, sysTime->tm_sec);
|
|
|
-
|
|
|
-
|
|
|
- //第2步, 创建万集管理模块的任务单, 并发送到 wanji_manager
|
|
|
- // Wanji_manager_task t_wanji_manager_task ;
|
|
|
- // Common_data::Car_wheel_information t_car_information_by_wanji;
|
|
|
- // t_wanji_manager_task.task_init(TASK_CREATED,"",NULL,std::chrono::milliseconds(3000),
|
|
|
- // terminal_id, receive_time);
|
|
|
- // t_error = Task_command_manager::get_instance_references().execute_task(&t_wanji_manager_task);
|
|
|
- // 或者创建velodyne管理模块任务单，并发送到velodyne_manager
|
|
|
- Velodyne_manager_task t_velodyne_manager_task;
|
|
|
- Common_data::Car_wheel_information t_car_information_by_velodyne;
|
|
|
- t_velodyne_manager_task.task_init(TASK_CREATED,"",NULL,std::chrono::milliseconds(3000),
|
|
|
- terminal_id, receive_time);
|
|
|
- t_error = Task_command_manager::get_instance_references().execute_task(&t_velodyne_manager_task);
|
|
|
-
|
|
|
- //第3步, 等待任务单完成
|
|
|
- if ( t_error != Error_code::SUCCESS )
|
|
|
+ System_executor::System_executor_status System_executor::get_system_executor_status()
|
|
|
{
|
|
|
- LOG(INFO) << " Velodyne_manager/Velodyne_manager execute_task error "<< this;
|
|
|
+ return m_system_executor_status;
|
|
|
}
|
|
|
- else
|
|
|
+
|
|
|
+ int System_executor::get_terminal_id()
|
|
|
{
|
|
|
- // //判断任务是否结束, 阻塞 等待, 直到任务完成或者超时
|
|
|
- // while ( t_wanji_manager_task.is_task_end() == false)
|
|
|
- // {
|
|
|
- // if ( t_wanji_manager_task.is_over_time() )
|
|
|
- // {
|
|
|
- // //超时处理。取消任务。
|
|
|
- // Wanji_manager::get_instance_pointer()->cancel_task(&t_wanji_manager_task);
|
|
|
- // t_error.error_manager_reset(Error_code::WJ_MANAGER_TASK_OVER_TIME, Error_level::MINOR_ERROR,
|
|
|
- // " t_wanjestaui_manager_task is_over_time ");
|
|
|
- // t_wanji_manager_task.set_task_error_manager(t_error);
|
|
|
- // }
|
|
|
- // else
|
|
|
- // {
|
|
|
- // //继续等待
|
|
|
- // std::this_thread::sleep_for(std::chrono::microseconds(1));
|
|
|
- // std::this_thread::yield();
|
|
|
- // }
|
|
|
- // }
|
|
|
+ return m_terminal_id;
|
|
|
+ }
|
|
|
|
|
|
- // //提取任务单 的错误码
|
|
|
- // t_error = t_wanji_manager_task.get_task_error_manager();
|
|
|
- // if ( t_error == Error_code::SUCCESS )
|
|
|
- // {
|
|
|
- // t_car_information_by_wanji = t_wanji_manager_task.get_car_wheel_information();
|
|
|
- // }
|
|
|
- // else
|
|
|
- // {
|
|
|
- // LOG(INFO) << " wanji_manager_task error :::::::" << t_wanji_manager_task.get_task_error_manager().to_string() << this;
|
|
|
- // }
|
|
|
- // ------------------- 切换为velodyne ------------------
|
|
|
- //判断任务是否结束, 阻塞 等待, 直到任务完成或者超时
|
|
|
- while ( t_velodyne_manager_task.is_task_end() == false)
|
|
|
+ //雷达感测定位 的处理函数
|
|
|
+ //input::command_id, 消息指令id, 由主控制系统生成的唯一码
|
|
|
+ //input::command_id, 终端id, 对应具体的某个车位
|
|
|
+ //return::void, 没有返回, 执行结果直接生成一条答复消息, 然后通过通信返回
|
|
|
+ void System_executor::execute_for_measure(std::string command_info, int terminal_id, std::chrono::system_clock::time_point receive_time)
|
|
|
+ {
|
|
|
+ Error_manager t_error;
|
|
|
+ Error_manager t_result;
|
|
|
+ LOG(INFO) << " System_executor::execute_for_measure run " << this;
|
|
|
+
|
|
|
+ //第1步, 按照时间生成中间文件的保存路径
|
|
|
+ time_t nowTime;
|
|
|
+ nowTime = time(NULL);
|
|
|
+ struct tm *sysTime = localtime(&nowTime);
|
|
|
+ char t_save_path[256] = {0};
|
|
|
+ sprintf(t_save_path, "../data/%04d%02d%02d_%02d%02d%02d",
|
|
|
+ sysTime->tm_year + 1900, sysTime->tm_mon + 1, sysTime->tm_mday, sysTime->tm_hour, sysTime->tm_min, sysTime->tm_sec);
|
|
|
+
|
|
|
+ //第2步, 创建万集管理模块的任务单, 并发送到 wanji_manager
|
|
|
+ // Wanji_manager_task t_wanji_manager_task ;
|
|
|
+ // Common_data::Car_wheel_information t_car_information_by_wanji;
|
|
|
+ // t_wanji_manager_task.task_init(TASK_CREATED,"",NULL,std::chrono::milliseconds(3000),
|
|
|
+ // terminal_id, receive_time);
|
|
|
+ // t_error = Task_command_manager::get_instance_references().execute_task(&t_wanji_manager_task);
|
|
|
+ // 或者创建velodyne管理模块任务单，并发送到velodyne_manager
|
|
|
+ Velodyne_manager_task t_velodyne_manager_task;
|
|
|
+ Common_data::Car_wheel_information t_car_information_by_velodyne;
|
|
|
+ t_velodyne_manager_task.task_init(TASK_CREATED, "", NULL, std::chrono::milliseconds(3000),
|
|
|
+ terminal_id, receive_time);
|
|
|
+ t_error = Task_command_manager::get_instance_references().execute_task(&t_velodyne_manager_task);
|
|
|
+
|
|
|
+ //第3步, 等待任务单完成
|
|
|
+ if (t_error != Error_code::SUCCESS)
|
|
|
{
|
|
|
- if ( t_velodyne_manager_task.is_over_time() )
|
|
|
+ LOG(INFO) << " Velodyne_manager/Velodyne_manager execute_task error " << this;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ // //判断任务是否结束, 阻塞 等待, 直到任务完成或者超时
|
|
|
+ // while ( t_wanji_manager_task.is_task_end() == false)
|
|
|
+ // {
|
|
|
+ // if ( t_wanji_manager_task.is_over_time() )
|
|
|
+ // {
|
|
|
+ // //超时处理。取消任务。
|
|
|
+ // Wanji_manager::get_instance_pointer()->cancel_task(&t_wanji_manager_task);
|
|
|
+ // t_error.error_manager_reset(Error_code::WJ_MANAGER_TASK_OVER_TIME, Error_level::MINOR_ERROR,
|
|
|
+ // " t_wanjestaui_manager_task is_over_time ");
|
|
|
+ // t_wanji_manager_task.set_task_error_manager(t_error);
|
|
|
+ // }
|
|
|
+ // else
|
|
|
+ // {
|
|
|
+ // //继续等待
|
|
|
+ // std::this_thread::sleep_for(std::chrono::microseconds(1));
|
|
|
+ // std::this_thread::yield();
|
|
|
+ // }
|
|
|
+ // }
|
|
|
+
|
|
|
+ // //提取任务单 的错误码
|
|
|
+ // t_error = t_wanji_manager_task.get_task_error_manager();
|
|
|
+ // if ( t_error == Error_code::SUCCESS )
|
|
|
+ // {
|
|
|
+ // t_car_information_by_wanji = t_wanji_manager_task.get_car_wheel_information();
|
|
|
+ // }
|
|
|
+ // else
|
|
|
+ // {
|
|
|
+ // LOG(INFO) << " wanji_manager_task error :::::::" << t_wanji_manager_task.get_task_error_manager().to_string() << this;
|
|
|
+ // }
|
|
|
+ // ------------------- 切换为velodyne ------------------
|
|
|
+ //判断任务是否结束, 阻塞 等待, 直到任务完成或者超时
|
|
|
+ while (t_velodyne_manager_task.is_task_end() == false)
|
|
|
+ {
|
|
|
+ if (t_velodyne_manager_task.is_over_time())
|
|
|
+ {
|
|
|
+ //超时处理。取消任务。
|
|
|
+ Velodyne_manager::get_instance_pointer()->cancel_task(&t_velodyne_manager_task);
|
|
|
+ t_error.error_manager_reset(Error_code::VELODYNE_MANAGER_TASK_OVER_TIME, Error_level::MINOR_ERROR,
|
|
|
+ " t_velodyne_manager_task is_over_time ");
|
|
|
+ t_velodyne_manager_task.set_task_error_manager(t_error);
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ //继续等待
|
|
|
+ std::this_thread::sleep_for(std::chrono::microseconds(1));
|
|
|
+ std::this_thread::yield();
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ //提取任务单 的错误码
|
|
|
+ t_error = t_velodyne_manager_task.get_task_error_manager();
|
|
|
+ if (t_error == Error_code::SUCCESS)
|
|
|
{
|
|
|
- //超时处理。取消任务。
|
|
|
- Velodyne_manager::get_instance_pointer()->cancel_task(&t_velodyne_manager_task);
|
|
|
- t_error.error_manager_reset(Error_code::VELODYNE_MANAGER_TASK_OVER_TIME, Error_level::MINOR_ERROR,
|
|
|
- " t_velodyne_manager_task is_over_time ");
|
|
|
- t_velodyne_manager_task.set_task_error_manager(t_error);
|
|
|
+ t_car_information_by_velodyne = t_velodyne_manager_task.get_car_wheel_information();
|
|
|
}
|
|
|
else
|
|
|
{
|
|
|
- //继续等待
|
|
|
- std::this_thread::sleep_for(std::chrono::microseconds(1));
|
|
|
- std::this_thread::yield();
|
|
|
+ LOG(INFO) << " velodyne_manager_task error :::::::" << t_velodyne_manager_task.get_task_error_manager().to_string() << this;
|
|
|
}
|
|
|
}
|
|
|
+ t_result.compare_and_cover_error(t_error);
|
|
|
|
|
|
- //提取任务单 的错误码
|
|
|
- t_error = t_velodyne_manager_task.get_task_error_manager();
|
|
|
- if ( t_error == Error_code::SUCCESS )
|
|
|
- {
|
|
|
- t_car_information_by_velodyne = t_velodyne_manager_task.get_car_wheel_information();
|
|
|
- }
|
|
|
- else
|
|
|
- {
|
|
|
- LOG(INFO) << " velodyne_manager_task error :::::::" << t_velodyne_manager_task.get_task_error_manager().to_string() << this;
|
|
|
- }
|
|
|
- }
|
|
|
- t_result.compare_and_cover_error(t_error);
|
|
|
-
|
|
|
- //measure 测量模块的最终结果
|
|
|
- Common_data::Car_measure_information t_car_information_result;
|
|
|
-
|
|
|
- //第4步, 生成反馈消息
|
|
|
- // if(t_car_information_by_wanji.correctness == true )
|
|
|
- // {
|
|
|
- // t_car_information_result.center_x = t_car_information_by_wanji.center_x;
|
|
|
- // t_car_information_result.center_y = t_car_information_by_wanji.center_y;
|
|
|
-
|
|
|
- // t_car_information_result.car_angle = t_car_information_by_wanji.car_angle;
|
|
|
- // t_car_information_result.wheel_base = t_car_information_by_wanji.wheel_base;
|
|
|
-
|
|
|
- // t_car_information_result.wheel_width = t_car_information_by_wanji.wheel_width;
|
|
|
- // t_car_information_result.correctness = true;
|
|
|
- // }else{
|
|
|
- // t_result.set_error_level_down(NEGLIGIBLE_ERROR);
|
|
|
- // }
|
|
|
- // ------------------- 切换为velodyne ------------------
|
|
|
- if(t_car_information_by_velodyne.correctness == true )
|
|
|
- {
|
|
|
- t_car_information_result.car_center_x = t_car_information_by_velodyne.car_center_x;
|
|
|
- t_car_information_result.car_center_y = t_car_information_by_velodyne.car_center_y;
|
|
|
-
|
|
|
- t_car_information_result.car_angle = t_car_information_by_velodyne.car_angle;
|
|
|
- t_car_information_result.car_wheel_base = t_car_information_by_velodyne.car_wheel_base;
|
|
|
-
|
|
|
- t_car_information_result.car_wheel_width = t_car_information_by_velodyne.car_wheel_width;
|
|
|
- t_car_information_result.correctness = true;
|
|
|
- }else{
|
|
|
- t_result.set_error_level_down(NEGLIGIBLE_ERROR);
|
|
|
- }
|
|
|
+ //measure 测量模块的最终结果
|
|
|
+ Common_data::Car_measure_information t_car_information_result;
|
|
|
|
|
|
+ //第4步, 生成反馈消息
|
|
|
+ // if(t_car_information_by_wanji.correctness == true )
|
|
|
+ // {
|
|
|
+ // t_car_information_result.center_x = t_car_information_by_wanji.center_x;
|
|
|
+ // t_car_information_result.center_y = t_car_information_by_wanji.center_y;
|
|
|
|
|
|
- //第七步, 创建一条答复消息
|
|
|
- message::Ground_detect_response_msg t_ground_detect_response_msg;
|
|
|
- t_ground_detect_response_msg.mutable_base_info()->set_msg_type(message::Message_type::eGround_detect_response_msg);
|
|
|
- t_ground_detect_response_msg.mutable_base_info()->set_timeout_ms(5000);
|
|
|
- t_ground_detect_response_msg.mutable_base_info()->set_sender(message::Communicator::eGround_measurer);
|
|
|
- t_ground_detect_response_msg.mutable_base_info()->set_receiver(message::Communicator::eMain);
|
|
|
-
|
|
|
- t_ground_detect_response_msg.set_command_key(command_info);
|
|
|
- t_ground_detect_response_msg.mutable_id_struct()->set_terminal_id(terminal_id);
|
|
|
- t_ground_detect_response_msg.mutable_error_manager()->set_error_code(t_result.get_error_code());
|
|
|
- t_ground_detect_response_msg.mutable_error_manager()->set_error_level((message::Error_level)t_result.get_error_level());
|
|
|
- t_ground_detect_response_msg.mutable_error_manager()->set_error_description(t_result.get_error_description());
|
|
|
+ // t_car_information_result.car_angle = t_car_information_by_wanji.car_angle;
|
|
|
+ // t_car_information_result.wheel_base = t_car_information_by_wanji.wheel_base;
|
|
|
|
|
|
- t_ground_detect_response_msg.mutable_locate_information()->set_locate_x(t_car_information_result.car_center_x);
|
|
|
- t_ground_detect_response_msg.mutable_locate_information()->set_locate_y(t_car_information_result.car_center_y);
|
|
|
- t_ground_detect_response_msg.mutable_locate_information()->set_locate_angle(t_car_information_result.car_angle);
|
|
|
- t_ground_detect_response_msg.mutable_locate_information()->set_locate_length(t_car_information_result.car_length);
|
|
|
- t_ground_detect_response_msg.mutable_locate_information()->set_locate_width(t_car_information_result.car_width);
|
|
|
- t_ground_detect_response_msg.mutable_locate_information()->set_locate_height(t_car_information_result.car_height);
|
|
|
- t_ground_detect_response_msg.mutable_locate_information()->set_locate_wheel_base(t_car_information_result.car_wheel_base);
|
|
|
- t_ground_detect_response_msg.mutable_locate_information()->set_locate_wheel_width(t_car_information_result.car_wheel_width);
|
|
|
- t_ground_detect_response_msg.mutable_locate_information()->set_locate_front_theta(t_car_information_result.car_front_theta);
|
|
|
- t_ground_detect_response_msg.mutable_locate_information()->set_locate_correct(t_car_information_result.correctness);
|
|
|
+ // t_car_information_result.wheel_width = t_car_information_by_wanji.wheel_width;
|
|
|
+ // t_car_information_result.correctness = true;
|
|
|
+ // }else{
|
|
|
+ // t_result.set_error_level_down(NEGLIGIBLE_ERROR);
|
|
|
+ // }
|
|
|
+ // ------------------- 切换为velodyne ------------------
|
|
|
+ if (t_car_information_by_velodyne.correctness == true)
|
|
|
+ {
|
|
|
+ t_car_information_result.car_center_x = t_car_information_by_velodyne.car_center_x;
|
|
|
+ t_car_information_result.car_center_y = t_car_information_by_velodyne.car_center_y;
|
|
|
|
|
|
+ t_car_information_result.car_angle = t_car_information_by_velodyne.car_angle;
|
|
|
+ t_car_information_result.car_wheel_base = t_car_information_by_velodyne.car_wheel_base;
|
|
|
|
|
|
- std::string t_msg = t_ground_detect_response_msg.SerializeAsString();
|
|
|
- System_communication::get_instance_references().encapsulate_msg(t_msg);
|
|
|
+ t_car_information_result.car_wheel_width = t_car_information_by_velodyne.car_wheel_width;
|
|
|
+ t_car_information_result.correctness = true;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ t_result.set_error_level_down(NEGLIGIBLE_ERROR);
|
|
|
+ }
|
|
|
|
|
|
- std::cout << "huli t_measure_response_msg = " << t_ground_detect_response_msg.DebugString() << std::endl;
|
|
|
+ //第七步, 创建一条答复消息
|
|
|
+ message::Ground_detect_response_msg t_ground_detect_response_msg;
|
|
|
+ t_ground_detect_response_msg.mutable_base_info()->set_msg_type(message::Message_type::eGround_detect_response_msg);
|
|
|
+ t_ground_detect_response_msg.mutable_base_info()->set_timeout_ms(5000);
|
|
|
+ t_ground_detect_response_msg.mutable_base_info()->set_sender(message::Communicator::eGround_measurer);
|
|
|
+ t_ground_detect_response_msg.mutable_base_info()->set_receiver(message::Communicator::eMain);
|
|
|
+
|
|
|
+ t_ground_detect_response_msg.set_command_key(command_info);
|
|
|
+ t_ground_detect_response_msg.mutable_id_struct()->set_terminal_id(terminal_id);
|
|
|
+ t_ground_detect_response_msg.mutable_error_manager()->set_error_code(t_result.get_error_code());
|
|
|
+ t_ground_detect_response_msg.mutable_error_manager()->set_error_level((message::Error_level)t_result.get_error_level());
|
|
|
+ t_ground_detect_response_msg.mutable_error_manager()->set_error_description(t_result.get_error_description());
|
|
|
+
|
|
|
+ t_ground_detect_response_msg.mutable_locate_information()->set_locate_x(t_car_information_result.car_center_x);
|
|
|
+ t_ground_detect_response_msg.mutable_locate_information()->set_locate_y(t_car_information_result.car_center_y);
|
|
|
+ t_ground_detect_response_msg.mutable_locate_information()->set_locate_angle(t_car_information_result.car_angle);
|
|
|
+ t_ground_detect_response_msg.mutable_locate_information()->set_locate_length(t_car_information_result.car_length);
|
|
|
+ t_ground_detect_response_msg.mutable_locate_information()->set_locate_width(t_car_information_result.car_width);
|
|
|
+ t_ground_detect_response_msg.mutable_locate_information()->set_locate_height(t_car_information_result.car_height);
|
|
|
+ t_ground_detect_response_msg.mutable_locate_information()->set_locate_wheel_base(t_car_information_result.car_wheel_base);
|
|
|
+ t_ground_detect_response_msg.mutable_locate_information()->set_locate_wheel_width(t_car_information_result.car_wheel_width);
|
|
|
+ t_ground_detect_response_msg.mutable_locate_information()->set_locate_front_theta(t_car_information_result.car_front_theta);
|
|
|
+ t_ground_detect_response_msg.mutable_locate_information()->set_locate_correct(t_car_information_result.correctness);
|
|
|
+
|
|
|
+ std::string t_msg = t_ground_detect_response_msg.SerializeAsString();
|
|
|
+ System_communication::get_instance_references().encapsulate_msg(t_msg);
|
|
|
|
|
|
+ std::cout << "huli t_measure_response_msg = " << t_ground_detect_response_msg.DebugString() << std::endl;
|
|
|
|
|
|
- return ;
|
|
|
-}
|
|
|
+ return;
|
|
|
+ }
|
yct