wanji_manager.h 4.7 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128
  1. //
  2. // Created by huli on 2020/9/9.
  3. //
  4. #ifndef NNXX_TESTS_WANJI_MANAGER_H
  5. #define NNXX_TESTS_WANJI_MANAGER_H
  6. #include "../tool/thread_condition.h"
  7. #include "../tool/singleton.h"
  8. #include "../error_code/error_code.h"
  9. #include <thread>
  10. #include <map>
  11. #include "../wanji_lidar/wanji_lidar_device.h"
  12. #include "../wanji_lidar/region_worker.h"
  13. #include "../wanji_lidar/wanji_manager_task.h"
  14. #include "../wanji_lidar/wj_lidar_conf.pb.h"
  15. class Wanji_manager:public Singleton<Wanji_manager>
  16. {
  17. public:
  18. // 子类必须把父类设定为友元函数,这样父类才能使用子类的私有构造函数。
  19. friend class Singleton<Wanji_manager>;
  20. #define WANJI_MANAGER_PARAMETER_PATH "../setting/wanji_manager.prototxt"
  21. public:
  22. //雷达管理的状态
  23. enum Wanji_manager_status
  24. {
  25. E_UNKNOW = 0, //未知
  26. E_READY = 1, //准备,待机
  27. E_BUSY = 2, //工作正忙
  28. E_ISSUED_SCAN = 3, //下发任务, 获取点云
  29. E_WAIT_SCAN = 4, //等待任务, 扫描点云
  30. E_ISSUED_DETECT = 5, //下发任务, 算法预测
  31. E_WAIT_DETECT = 6, //等待任务, 算法预测
  32. E_FAULT = 10, //故障
  33. };
  34. private:
  35. Wanji_manager();
  36. public:
  37. Wanji_manager(const Wanji_manager& other)= delete;
  38. Wanji_manager& operator =(const Wanji_manager& other)= delete;
  39. ~Wanji_manager();
  40. public://API functions
  41. //初始化 雷达 管理模块。如下三选一
  42. Error_manager wanji_manager_init();
  43. //初始化 雷达 管理模块。从文件读取
  44. Error_manager wanji_manager_init_from_protobuf(std::string prototxt_path);
  45. //初始化 雷达 管理模块。从protobuf读取
  46. Error_manager wanji_manager_init_from_protobuf(wj::wjManagerParams& wanji_parameters);
  47. //反初始化 雷达 管理模块。
  48. Error_manager wanji_manager_uninit();
  49. //对外的接口函数,负责接受并处理任务单,
  50. Error_manager execute_task(Task_Base* p_wanji_manager_task);
  51. //结束任务单,里面会根据任务的故障等级修正雷达管理模块的状态和任务单的状态
  52. Error_manager end_task();
  53. //取消任务单,由发送方提前取消任务单
  54. Error_manager cancel_task(Task_Base* p_wanji_manager_task);
  55. //检查雷达状态,是否正常运行
  56. Error_manager check_status();
  57. //判断是否为待机,如果已经准备好,则可以执行任务。
  58. bool is_ready();
  59. public://get or set member variable
  60. Wanji_manager_status get_status();
  61. std::map<int, Wanji_lidar_device*> & get_wanji_lidar_device_map();
  62. std::map<int, Region_worker *> & get_region_worker_map();
  63. protected://member functions
  64. //自动收集点云的线程函数
  65. void collect_cloud_thread_fun();
  66. //唤醒自动预测的算法线程
  67. Error_manager wake_auto_detect();
  68. //执行外界任务的执行函数
  69. void execute_thread_fun();
  70. //下发给子雷达任务。这里为下发雷达任务分配内存, 后续要记得回收 m_laser_task_map 。
  71. Error_manager issued_scan_task(int lidar_index);
  72. //释放下发的任务单
  73. Error_manager wanji_lidar_scan_task_map_clear_and_delete();
  74. protected://member variable
  75. //状态
  76. Wanji_manager_status m_wanji_manager_status; //万集管理模块的状态
  77. std::map<int, Wanji_lidar_device *> m_wanji_lidar_device_map; // 万集雷达实例指针数组, 内存由本类管理
  78. std::map<int, Region_worker *> m_region_worker_map; // 区域功能实例指针数组, 内存由本类管理
  79. //万集雷达的自动功能, 定时收集所有点云, 然后通知detect去计算车轮信息.
  80. std::mutex m_cloud_collection_mutex; // 点云更新互斥锁
  81. pcl::PointCloud<pcl::PointXYZ>::Ptr mp_cloud_collection; //扫描点的点云集合, 内存由本类管理
  82. std::chrono::system_clock::time_point m_cloud_updata_time; //扫描点的更新时间.
  83. std::thread* mp_collect_cloud_thread; //收集点云的线程指针,内存由本类管理
  84. Thread_condition m_collect_cloud_condition; //收集点云的条件变量
  85. //任务执行线程
  86. std::thread* mp_execute_thread; //执行的线程指针,内存由本类管理
  87. Thread_condition m_execute_condition; //执行的条件变量
  88. Wanji_manager_task* mp_wanji_manager_task; //万集管理模块的任务单的指针,内存由发送方管理。
  89. std::mutex m_task_cloud_mutex; //任务点云更新互斥锁
  90. pcl::PointCloud<pcl::PointXYZ>::Ptr mp_task_cloud; //任务点云集合, 内存由本类管理
  91. std::map<int, Wanji_lidar_scan_task*> m_wanji_lidar_scan_task_map; //万集管理下发给雷达子模块的任务map,内存由本类管理
  92. // std::map<int, Wanji_lidar_scan_task*> m_wanji_lidar_scan_task_map; //万集管理下发给雷达子模块的任务map,内存由本类管理
  93. private:
  94. };
  95. #endif //NNXX_TESTS_WANJI_MANAGER_H