|
@@ -312,6 +312,329 @@ bool System_executor::update_measure_info(message::Ground_status_msg &msg, Commo
|
|
|
return true;
|
|
|
}
|
|
|
|
|
|
+// ***************************** rabbitmq *********************************
|
|
|
+Error_manager System_executor::encapsulate_send_mq_status()
|
|
|
+{
|
|
|
+ static int cloud_count=-1;
|
|
|
+ cloud_count++;
|
|
|
+ // if(!g_debug_file.is_open())
|
|
|
+ // {
|
|
|
+ // g_debug_file.open("./filter_debug_result.txt", std::ios::app);
|
|
|
+ // }
|
|
|
+
|
|
|
+ Error_manager t_error;
|
|
|
+ //创建一条状态消息
|
|
|
+ message::Ground_status_msg t_ground_status_msg;
|
|
|
+ t_ground_status_msg.mutable_base_info()->set_msg_type(message::Message_type::eGround_status_msg);
|
|
|
+ t_ground_status_msg.mutable_base_info()->set_timeout_ms(5000);
|
|
|
+ t_ground_status_msg.mutable_base_info()->set_sender(message::Communicator::eGround_measurer);
|
|
|
+ t_ground_status_msg.mutable_base_info()->set_receiver(message::Communicator::eEmpty);
|
|
|
+
|
|
|
+ // 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);
|
|
|
+
|
|
|
+ // 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)
|
|
|
+ {
|
|
|
+ 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()));
|
|
|
+ }
|
|
|
+
|
|
|
+ // 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)
|
|
|
+ {
|
|
|
+ // 以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) << "vlp16 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("vlp16_region_") + std::to_string(iter->first) + "_cloud.txt";
|
|
|
+ save_cloud_txt(t_region_cloud, 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++)
|
|
|
+ // {
|
|
|
+ // 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);
|
|
|
+ // }
|
|
|
+
|
|
|
+ // 检查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())
|
|
|
+ {
|
|
|
+ wj::Region t_param = t_wj_region_iter->second->get_param();
|
|
|
+ //
|
|
|
+ for (size_t j = 0; j < t_param.lidar_exts_size(); j++)
|
|
|
+ {
|
|
|
+#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);
|
|
|
+ }
|
|
|
+ }
|
|
|
+ //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 == 8)
|
|
|
+ {
|
|
|
+ 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;
|
|
|
+ }
|
|
|
+ // 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);
|
|
|
+ // }
|
|
|
+
|
|
|
+ // LOG_IF(WARNING, iter->second->get_terminal_id() == DISP_TERM_ID)<<"\n"<<t_car_wheel_information.to_string()<<"\n"
|
|
|
+ // <<t_wj_car_wheel_information.to_string()<<"\n"
|
|
|
+ // <<t_car_wheel_information.range_status<<", "<<t_wj_car_wheel_information.range_status;
|
|
|
+ // 融合测量结果,验证中心点差异x<0.03, y<0.06, 角度差异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.06 && fabs(dang) < 0.6 && fabs(dwb) < 0.055)
|
|
|
+ {
|
|
|
+ t_car_wheel_information.car_wheel_base = t_car_wheel_information.car_wheel_base*0.3 + t_car_wheel_information.car_wheel_base*0.7;
|
|
|
+ }else
|
|
|
+ {
|
|
|
+ LOG_IF(WARNING, iter->second->get_terminal_id() == DISP_TERM_ID)<<
|
|
|
+ "detect mismatch, region "<<iter->first<<", dx dy dang dwb: "<<dx<<", "<<dy<<", "<<dang<<", "<< dwb;
|
|
|
+ }
|
|
|
+
|
|
|
+ }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_mq::get_instance_references().encapsulate_status_msg(t_msg, 0);
|
|
|
+ if (t_multi_status_msg.id_struct().terminal_id() == DISP_TERM_ID)
|
|
|
+ std::cout << t_multi_status_msg.DebugString() << std::endl
|
|
|
+ << std::endl;
|
|
|
+ }
|
|
|
+}
|
|
|
+#endif
|
|
|
+
|
|
|
+ return Error_code::SUCCESS;
|
|
|
+}
|
|
|
+
|
|
|
+// ***************************** nanomsg *********************************
|
|
|
//定时发送状态信息
|
|
|
Error_manager System_executor::encapsulate_send_status()
|
|
|
{
|