dispatch_plc.cpp 55 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675676677678679680681682683684685686687688689690691692693694695696697698699700701702703704705706707708709710711712713714715716717718719720721722723724725726727728729730731732733734735736737738739740741742743744745746747748749750751752753754755756757758759760761762763764765766767768769770771772773774775776777778779780781782783784785786787788789790791792793794795796797798799800801802803804805806807808809810811812813814815816817818819820821822823824825826827828829830831832833834835836837838839840841842843844845846847848849850851852853854855856857858859860861862863864865866867868869870871872873874875876877878879880881882883884885886887888889890891892893894895896897898899900901902903904905906907908909910911912913914915916917918919920921922923924925926927928929930931932933934935936937938939940941942943944945946947948949950951952953954955956957958959960961962963964965966967968969970971972973974975976977978979980981982983984985986987988989990991992993994995996997998999100010011002100310041005100610071008100910101011101210131014101510161017101810191020102110221023102410251026102710281029103010311032103310341035103610371038103910401041104210431044104510461047104810491050105110521053105410551056105710581059106010611062106310641065106610671068106910701071107210731074107510761077107810791080108110821083108410851086108710881089109010911092109310941095109610971098109911001101110211031104110511061107110811091110111111121113111411151116111711181119112011211122112311241125112611271128112911301131113211331134113511361137113811391140114111421143114411451146114711481149115011511152115311541155115611571158115911601161116211631164116511661167116811691170117111721173117411751176117711781179118011811182118311841185118611871188118911901191119211931194119511961197119811991200120112021203120412051206120712081209121012111212121312141215121612171218121912201221122212231224122512261227
  1. //
  2. // Created by huli on 2021/8/3.
  3. //
  4. #include "dispatch_plc.h"
  5. #include "../system/system_communication.h"
  6. #include "dispatch_manager.h"
  7. Dispatch_plc::Dispatch_plc()
  8. {
  9. m_dispatch_plc_status = Dispatch_plc::Dispatch_plc_status::DISPATCH_PLC_UNKNOW;
  10. m_plc_id = 0;
  11. mp_execute_thread = NULL;
  12. mp_data_storage_thread = NULL;
  13. }
  14. Dispatch_plc::~Dispatch_plc()
  15. {
  16. dispatch_plc_uninit();
  17. }
  18. //调度plc 初始化
  19. Error_manager Dispatch_plc::dispatch_plc_init(int plc_id)
  20. {
  21. m_plc_id = plc_id;
  22. m_status_updata_time = std::chrono::system_clock::now();
  23. m_last_heartbeat = 0;
  24. // 线程默认开启
  25. m_execute_condition.reset(false, true, false);
  26. mp_execute_thread = new std::thread(&Dispatch_plc::execute_thread_fun, this);
  27. // m_data_storage_condition.reset(false, true, false);
  28. // mp_data_storage_thread = new std::thread(&Dispatch_plc::data_storage_thread_fun, this);
  29. m_dispatch_plc_status = Dispatch_plc::Dispatch_plc_status::DISPATCH_PLC_READY;
  30. return Error_code::SUCCESS;
  31. }
  32. //调度plc 反初始化
  33. Error_manager Dispatch_plc::dispatch_plc_uninit()
  34. {
  35. if (mp_execute_thread)
  36. {
  37. m_execute_condition.kill_all();
  38. }
  39. if (mp_data_storage_thread)
  40. {
  41. m_data_storage_condition.kill_all();
  42. }
  43. if (mp_execute_thread)
  44. {
  45. mp_execute_thread->join();
  46. delete mp_execute_thread;
  47. mp_execute_thread = NULL;
  48. }
  49. if (mp_data_storage_thread)
  50. {
  51. mp_data_storage_thread->join();
  52. delete mp_data_storage_thread;
  53. mp_data_storage_thread = NULL;
  54. }
  55. m_dispatch_plc_status = Dispatch_plc::Dispatch_plc_status::DISPATCH_PLC_UNKNOW;
  56. return Error_code::SUCCESS;
  57. }
  58. //调度plc 执行请求
  59. //Error_manager Dispatch_plc::execute_for_dispatch_request_msg(message::Dispatch_request_msg& dispatch_request_msg)
  60. //{
  61. //
  62. // if ( m_dispatch_plc_status == Dispatch_plc::Dispatch_plc_status::DISPATCH_PLC_READY )
  63. // {
  64. // std::unique_lock<std::mutex> t_lock(m_lock);
  65. //
  66. // //设定超时时间, 默认比任务指令里面的时间少10秒,
  67. // if ( dispatch_request_msg.base_info().has_timeout_ms() )
  68. // {
  69. // m_timeout_ms = dispatch_request_msg.base_info().timeout_ms() - DISPATCH_PROCESS_ATTENUATION_TIMEOUT_MS;
  70. // }
  71. // else
  72. // {
  73. // m_timeout_ms = DISPATCH_PROCESS_TIMEOUT_MS - DISPATCH_PROCESS_ATTENUATION_TIMEOUT_MS;
  74. // }
  75. // m_command_key = dispatch_request_msg.command_key();
  76. // m_start_time = std::chrono::system_clock::now();
  77. //
  78. //
  79. //// m_dispatch_request_msg = dispatch_request_msg;
  80. //// if ( dispatch_request_msg.dispatch_motion_direction() == message::Dispatch_motion_direction::E_STORE_CAR )
  81. //// {
  82. //// m_dispatch_process_type = Common_data::Dispatch_process_type::DISPATCH_PROCESS_STORE;
  83. //// }
  84. //// else if ( dispatch_request_msg.dispatch_motion_direction() == message::Dispatch_motion_direction::E_PICKUP_CAR )
  85. //// {
  86. //// m_dispatch_process_type = Common_data::Dispatch_process_type::DISPATCH_PROCESS_PICKUP;
  87. //// }
  88. //// else
  89. //// {
  90. //// return Error_manager(Error_code::DISPATCH_PLC_REQUEST_ERROR, Error_level::MINOR_ERROR,
  91. //// " dispatch_request_msg.dispatch_motion_direction() PARAMRTER ERROR ");
  92. //// }
  93. //
  94. // m_dispatch_process_type = Common_data::Dispatch_process_type::DISPATCH_PROCESS_TYPE_UNKNOW;
  95. // m_dispatch_plc_status = Dispatch_plc::Dispatch_plc_status::DISPATCH_PLC_REQUEST;
  96. // }
  97. // else
  98. // {
  99. // return Error_manager(Error_code::DISPATCH_PLC_STATUS_ERROR, Error_level::MINOR_ERROR,
  100. // " m_dispatch_plc_status error ");
  101. // }
  102. //
  103. // return Error_code::SUCCESS;
  104. //}
  105. //新版调度plc 执行请求 存车
  106. Error_manager Dispatch_plc::execute_for_dispatch_request_msg(park_table &park_table_msg, Common_data::Dispatch_process_type dispatch_process_type, Common_data::Car_type car_type)
  107. {
  108. if ( m_dispatch_plc_status == Dispatch_plc::Dispatch_plc_status::DISPATCH_PLC_READY )
  109. {
  110. std::unique_lock<std::mutex> t_lock(m_lock);
  111. //超时默认一小时.
  112. m_timeout_ms = DISPATCH_PROCESS_TIMEOUT_MS - DISPATCH_PROCESS_ATTENUATION_TIMEOUT_MS;
  113. //合成唯一码.
  114. // char t_buf[256] = {0};
  115. // sprintf(t_buf, "%d+%s+%s", park_table_msg.queue_id(), park_table_msg.car_number().c_str(), park_table_msg.primary_key().c_str());
  116. // m_command_key = park_table_msg.primary_key()+"_park";
  117. m_start_time = std::chrono::system_clock::now();
  118. m_park_table_msg = park_table_msg;
  119. m_dispatch_process_type = dispatch_process_type;
  120. m_car_type = car_type;
  121. if ( m_dispatch_process_type == Common_data::DISPATCH_PROCESS_STORE )
  122. {
  123. m_command_key = park_table_msg.primary_key()+"_store";
  124. }
  125. if ( m_dispatch_process_type == Common_data::DISPATCH_PROCESS_REALLOCATE )
  126. {
  127. m_command_key = park_table_msg.primary_key()+"_reallocate";
  128. }
  129. m_dispatch_plc_status = Dispatch_plc::Dispatch_plc_status::DISPATCH_PLC_REQUEST;
  130. }
  131. else
  132. {
  133. return Error_manager(Error_code::DISPATCH_PLC_STATUS_ERROR, Error_level::MINOR_ERROR,
  134. " m_dispatch_plc_status error ");
  135. }
  136. return Error_code::SUCCESS;
  137. }
  138. //新版调度plc 执行请求 取车
  139. Error_manager Dispatch_plc::execute_for_dispatch_request_msg(pick_table &pick_table_msg, Common_data::Dispatch_process_type dispatch_process_type, Common_data::Car_type car_type)
  140. {
  141. if ( m_dispatch_plc_status == Dispatch_plc::Dispatch_plc_status::DISPATCH_PLC_READY )
  142. {
  143. std::unique_lock<std::mutex> t_lock(m_lock);
  144. //超时默认一小时.
  145. m_timeout_ms = DISPATCH_PROCESS_TIMEOUT_MS - DISPATCH_PROCESS_ATTENUATION_TIMEOUT_MS;
  146. //合成唯一码.
  147. // char t_buf[256] = {0};
  148. // sprintf(t_buf, "%d+%s+%s", pick_table_msg.queue_id(), pick_table_msg.car_number().c_str(), pick_table_msg.primary_key().c_str());
  149. // m_command_key = pick_table_msg.primary_key()+"_pick";
  150. m_start_time = std::chrono::system_clock::now();
  151. m_pick_table_msg = pick_table_msg;
  152. m_dispatch_process_type = dispatch_process_type;
  153. m_car_type = car_type;
  154. if ( m_dispatch_process_type == Common_data::DISPATCH_PROCESS_PICKUP )
  155. {
  156. m_command_key = pick_table_msg.primary_key()+"_pickup";
  157. }
  158. if ( m_dispatch_process_type == Common_data::DISPATCH_PROCESS_REVOCATION )
  159. {
  160. m_command_key = pick_table_msg.primary_key()+"_revocation";
  161. }
  162. m_dispatch_plc_status = Dispatch_plc::Dispatch_plc_status::DISPATCH_PLC_REQUEST;
  163. }
  164. else
  165. {
  166. return Error_manager(Error_code::DISPATCH_PLC_STATUS_ERROR, Error_level::MINOR_ERROR,
  167. " m_dispatch_plc_status error ");
  168. }
  169. return Error_code::SUCCESS;
  170. }
  171. Dispatch_plc::Dispatch_plc_status Dispatch_plc::get_dispatch_plc_status()
  172. {
  173. return m_dispatch_plc_status;
  174. }
  175. //message::Dispatch_request_msg Dispatch_plc::get_dispatch_request_msg()
  176. //{
  177. // std::unique_lock<std::mutex> t_lock(m_lock);
  178. // return m_dispatch_request_msg;
  179. //}
  180. Error_manager Dispatch_plc::get_result()
  181. {
  182. return m_result;
  183. }
  184. float Dispatch_plc::get_response_working_total_time()
  185. {
  186. std::unique_lock<std::mutex> t_lock(m_lock);
  187. return m_response_working_total_time;
  188. }
  189. float Dispatch_plc::get_response_working_remaining_time()
  190. {
  191. std::unique_lock<std::mutex> t_lock(m_lock);
  192. return m_response_working_remaining_time;
  193. }
  194. void Dispatch_plc::clear_request_msg()
  195. {
  196. m_request_key.clear(); //请求唯一码, 用作识别
  197. m_request_status = 0; //请求确认标志
  198. // 调度指令的起点,终点,方向
  199. m_request_dispatch_motion_direction = Common_data::Dispatch_process_type::DISPATCH_PROCESS_TYPE_UNKNOW; //调度方向 0=未知,1=存车,2=取车
  200. m_request_passageway_id = 0; //出入口id 6个入口和6个出口
  201. m_request_passageway_direction = Common_data::Passageway_direction::PASSAGEWAY_DIRECTION_UNKNOWN; //出入口方向 0=未知,1=入口,2=出口
  202. m_request_parkingspace_index_id = 0; //楼上车位索引id 1~180(3*6*13)
  203. m_request_parkingspace_unit_id = 0; //楼上车位单元号 1~3
  204. m_request_parkingspace_label_id = 0; //楼上车位标签号 1~78
  205. m_request_parkingspace_floor_id = 0; //楼上车位楼层号 2~11
  206. m_request_parkingspace_room_id = 0; //楼上车位房间号 1~6
  207. m_request_parkingspace_direction = Common_data::Parkingspace_direction::PARKINGSPACE_DIRECTION_UNKNOWN; //楼上车位方向 0=未知,1=朝南,2=朝北
  208. //汽车的定位信息(地面雷达)
  209. m_request_car_center_x = 0; //整车的中心点x值, 四轮的中心, 单位:米 m
  210. m_request_car_center_y = 0; //整车的中心点y值, 四轮的中心, 单位:米 m
  211. m_request_car_angle = 0; //整车的车身旋转角, 单位:度 (-90~90)
  212. m_request_car_front_theta = 0; //整车的前轮的旋转角, 单位:度 (-90~90)
  213. m_request_car_length = 0; //整车的长度, 用于规避碰撞, 单位:米 m
  214. m_request_car_width = 0; //整车的宽度, 用于规避碰撞, 单位:米 m
  215. m_request_car_height = 0; //整车的高度, 用于规避碰撞, 单位:米 m
  216. m_request_car_wheel_base = 0; //整车的轮距, 前后轮的距离, 用于机器人或agv的抓车, 单位:米 m
  217. m_request_car_wheel_width = 0; //整车的轮距, 左右轮的距离, 用于机器人或agv的抓车, 单位:米 m
  218. m_request_car_license.clear(); //车牌号(汽车唯一标示) 例如: 鄂A12345
  219. m_request_car_type = Common_data::Car_type::UNKNOW_CAR_TYPE; //车的大小
  220. m_request_uniformed_car_x = 0; //转角复位后,车辆中心点x
  221. m_request_uniformed_car_y = 0; //转角复位后,车辆中心点y
  222. //防撞雷达
  223. m_request_anticollision_lidar_flag = Common_data::Anticollision_lidar_flag::ANTICOLLISION_LIDAR_UNKNOWN; //汽车是否停到正确的位置, 防撞雷达 预留, 楚天暂时不用,0=未知,1=位置正常,2=位置异常
  224. //轮距雷达
  225. m_request_car_wheel_base_exact_value = 0; //汽车前后轮距,轮距雷达的精确值 预留, 楚天暂时不用,单位:米 m
  226. }
  227. //jiancha qingqiu xiaoxi
  228. //Error_manager Dispatch_plc::check_dispatch_request_msg(message::Dispatch_request_msg & dispatch_request_msg)
  229. //{
  230. // return Error_code::SUCCESS;
  231. // /*
  232. // Error_manager t_error;
  233. //
  234. // if(dispatch_request_msg.has_locate_information() &&
  235. // dispatch_request_msg.has_dispatch_motion_direction() &&
  236. // dispatch_request_msg.has_car_type() &&
  237. // dispatch_request_msg.has_id_struct() &&
  238. // dispatch_request_msg.has_command_key())
  239. // {
  240. // if ( m_dispatch_request_msg.dispatch_motion_direction() == message::Dispatch_motion_direction::E_STORE_CAR )
  241. // {
  242. // if ( dispatch_request_msg.id_struct().has_unit_id() )
  243. // {
  244. //
  245. // }
  246. // }
  247. // else if( m_dispatch_request_msg.dispatch_motion_direction() == message::Dispatch_motion_direction::E_PICKUP_CAR )
  248. //
  249. // return Error_code::SUCCESS;
  250. // }
  251. // else
  252. // {
  253. // return Error_manager(Error_code::DISPATCH_PLC_REQUEST_ERROR, Error_level::MINOR_ERROR,
  254. // " m_dispatch_plc_status error ");
  255. // }
  256. // */
  257. // return Error_code::SUCCESS;
  258. //}
  259. //执行外界任务的执行函数
  260. void Dispatch_plc::execute_thread_fun()
  261. {
  262. LOG(INFO) << " Dispatch_plc::execute_thread_fun() start " << this;
  263. Error_manager t_error;
  264. while (m_execute_condition.is_alive())
  265. {
  266. m_execute_condition.wait();
  267. if (m_execute_condition.is_alive())
  268. {
  269. std::this_thread::sleep_for(std::chrono::microseconds(1));
  270. std::this_thread::sleep_for(std::chrono::milliseconds(1));
  271. // std::this_thread::sleep_for(std::chrono::seconds(1));
  272. std::this_thread::yield();
  273. // std::unique_lock<std::mutex> t_lock(m_lock);
  274. static int t_count = 0;
  275. if (t_count % 1000 == 0 )
  276. {
  277. std::cout << " huli test :::: " << " m_plc_id = " << m_plc_id << std::endl;
  278. std::cout << " huli test :::: " << " m_dispatch_plc_status = " << m_dispatch_plc_status << std::endl;
  279. }
  280. t_count++;
  281. switch ((Dispatch_plc_status) m_dispatch_plc_status)
  282. {
  283. case DISPATCH_PLC_READY:
  284. {
  285. if ( m_dispatch_process_type == Common_data::Dispatch_process_type::DISPATCH_PROCESS_TYPE_UNKNOW )
  286. {
  287. update_dispatch_plc_communication();
  288. }
  289. else
  290. {
  291. // m_result = check_dispatch_request_msg(m_dispatch_request_msg);
  292. //收到新指令, 进入工作阶段,强制清空错误码.
  293. m_result.error_manager_clear_all();
  294. if(m_result == Error_code::SUCCESS)
  295. {
  296. m_dispatch_plc_status = DISPATCH_PLC_REQUEST;
  297. }
  298. else
  299. {
  300. m_dispatch_plc_status = DISPATCH_PLC_RESPONSE;
  301. }
  302. }
  303. break;
  304. }
  305. case DISPATCH_PLC_REQUEST://给plc发送请求
  306. {
  307. //收到新指令, 进入工作阶段,强制清空错误码.
  308. m_result.error_manager_clear_all();
  309. #ifdef PLC_OVER_TIME_TO_ERROR
  310. if ( std::chrono::system_clock::now() - m_start_time > std::chrono::milliseconds(m_timeout_ms) )
  311. {
  312. //超时直接报错
  313. m_result = Error_manager(Error_code::DISPATCH_PLC_TIME_OUT, Error_level::MINOR_ERROR,
  314. " DISPATCH_PLC_TIME_OUT fun error ");
  315. m_dispatch_plc_status = DISPATCH_PLC_RESPONSE;
  316. }
  317. else
  318. #endif
  319. {
  320. //封装 给plc的调度请求
  321. // m_result = encapsulate_dispatch_request_to_plc();
  322. m_result = encapsulate_dispatch_request_to_plc_new();
  323. test_start_time = std::chrono::system_clock::now();
  324. m_result = Error_code::SUCCESS;
  325. if ( m_result == Error_code::SUCCESS )
  326. {
  327. m_dispatch_plc_status = DISPATCH_PLC_WORKING;
  328. }
  329. else
  330. {
  331. m_dispatch_plc_status = DISPATCH_PLC_RESPONSE;
  332. }
  333. }
  334. break;
  335. }
  336. case DISPATCH_PLC_WORKING:
  337. {
  338. #ifdef PLC_OVER_TIME_TO_ERROR
  339. if ( std::chrono::system_clock::now() - m_start_time > std::chrono::milliseconds(m_timeout_ms) )
  340. {
  341. //超时直接报错
  342. m_result = Error_manager(Error_code::DISPATCH_PLC_TIME_OUT, Error_level::MINOR_ERROR,
  343. " DISPATCH_PLC_TIME_OUT fun error ");
  344. m_dispatch_plc_status = DISPATCH_PLC_RESPONSE;
  345. }
  346. else
  347. #endif
  348. {
  349. update_dispatch_plc_communication();
  350. //检查plc答复反馈
  351. // m_result = check_response_from_plc();
  352. m_result = check_response_from_plc_new();
  353. if ( m_result == Error_code::SUCCESS )
  354. {
  355. m_dispatch_plc_status = DISPATCH_PLC_RESPONSE;
  356. }
  357. else if ( m_result != Error_code::NODATA )
  358. {
  359. // m_dispatch_plc_status = DISPATCH_PLC_FAULT;
  360. m_dispatch_plc_status = DISPATCH_PLC_RESPONSE;
  361. }
  362. //else NODATA 原地等待
  363. }
  364. break;
  365. }
  366. case DISPATCH_PLC_RESPONSE:
  367. {
  368. test_end_time = std::chrono::system_clock::now();
  369. auto time_dur = test_end_time - test_start_time;
  370. if ( time_dur < std::chrono::seconds(2) )
  371. {
  372. LOG(INFO) << " 调度完成, 时间太短, 按任意键继续 = "<< time_dur.count() << " --- " << this;
  373. getchar();
  374. }
  375. //20211213. huli m_request_status = 0 after over process
  376. m_request_status = 0;
  377. clear_request_msg();
  378. //发送调度答复信息给主控
  379. // send_dispatch_response_to_main_control();
  380. // send_dispatch_response_to_main_control_new();
  381. //无论前面的流程是否正确,都要给主控答复, 反馈里面填充错误码即可.
  382. //最新版本不用答复消息, Dispatch_plc 切回 DISPATCH_PLC_READY 后, 由 Dispatch_manager 答复数据库
  383. if ( m_result == Error_code::SUCCESS )
  384. {
  385. /*
  386. //response add to _map
  387. if ( m_dispatch_request_msg.dispatch_motion_direction() == message::Dispatch_motion_direction::E_STORE_CAR )
  388. {
  389. std::unique_lock<std::mutex> t_lock(Dispatch_manager::get_instance_references().m_lock);
  390. std::chrono::system_clock::time_point t_time = std::chrono::system_clock::now();
  391. Dispatch_manager::get_instance_references().m_dispatch_response_store_map[t_time] = m_dispatch_response_msg;
  392. Dispatch_manager::get_instance_references().m_store_updata_time = std::chrono::system_clock::now();
  393. }
  394. else if( m_dispatch_request_msg.dispatch_motion_direction() == message::Dispatch_motion_direction::E_PICKUP_CAR )
  395. {
  396. std::unique_lock<std::mutex> t_lock(Dispatch_manager::get_instance_references().m_lock);
  397. int t_terminal_id = m_dispatch_request_msg.id_struct().terminal_id();
  398. Dispatch_manager::get_instance_references().m_dispatch_response_pickup_map[t_terminal_id] = m_dispatch_response_msg;
  399. Dispatch_manager::get_instance_references().m_pickup_updata_time = std::chrono::system_clock::now();
  400. }
  401. */
  402. //流程正常结束, 恢复到待机状态.
  403. m_dispatch_plc_status = DISPATCH_PLC_READY;
  404. m_dispatch_process_type = Common_data::Dispatch_process_type::DISPATCH_PROCESS_TYPE_UNKNOW;
  405. // m_result.error_manager_clear_all();
  406. }
  407. else
  408. {
  409. //流程异常结束, 进入到故障状态.
  410. m_dispatch_plc_status = DISPATCH_PLC_FAULT;
  411. }
  412. break;
  413. }
  414. case DISPATCH_PLC_FAULT:
  415. case DISPATCH_PLC_DISCONNECT:
  416. {
  417. //20211213. huli m_request_status = 0 after over process
  418. m_request_status = 0;
  419. clear_request_msg();
  420. //不在接受新的任务,但是保持基本的通信正常
  421. update_dispatch_plc_communication();
  422. /*
  423. //response add to _map
  424. if ( m_dispatch_request_msg.dispatch_motion_direction() == message::Dispatch_motion_direction::E_STORE_CAR )
  425. {
  426. std::unique_lock<std::mutex> t_lock(Dispatch_manager::get_instance_references().m_lock);
  427. std::chrono::system_clock::time_point t_time = std::chrono::system_clock::now();
  428. Dispatch_manager::get_instance_references().m_dispatch_response_store_map[t_time] = m_dispatch_response_msg;
  429. Dispatch_manager::get_instance_references().m_store_updata_time = std::chrono::system_clock::now();
  430. }
  431. else if( m_dispatch_request_msg.dispatch_motion_direction() == message::Dispatch_motion_direction::E_PICKUP_CAR )
  432. {
  433. std::unique_lock<std::mutex> t_lock(Dispatch_manager::get_instance_references().m_lock);
  434. int t_terminal_id = m_dispatch_request_msg.id_struct().terminal_id();
  435. Dispatch_manager::get_instance_references().m_dispatch_response_pickup_map[t_terminal_id] = m_dispatch_response_msg;
  436. Dispatch_manager::get_instance_references().m_pickup_updata_time = std::chrono::system_clock::now();
  437. }
  438. */
  439. //20211209 huli //流程异常结束, 进入到 ready.
  440. m_dispatch_plc_status = DISPATCH_PLC_READY;
  441. m_dispatch_process_type = Common_data::Dispatch_process_type::DISPATCH_PROCESS_TYPE_UNKNOW;
  442. // m_result.error_manager_clear_all();
  443. LOG(ERROR) << " Dispatch_plc::execute_thread_fun() dispatch_plc is fault, now recover to normal " << this;
  444. break;
  445. }
  446. }
  447. }
  448. }
  449. }
  450. //数据储存线程函数
  451. void Dispatch_plc::data_storage_thread_fun()
  452. {
  453. LOG(INFO) << " Dispatch_plc::data_storage_thread_fun() start " << this;
  454. Error_manager t_error;
  455. while (m_data_storage_condition.is_alive())
  456. {
  457. m_data_storage_condition.wait();
  458. if (m_data_storage_condition.is_alive())
  459. {
  460. std::this_thread::sleep_for(std::chrono::microseconds(1));
  461. std::this_thread::sleep_for(std::chrono::milliseconds(1));
  462. std::this_thread::sleep_for(std::chrono::seconds(1));
  463. std::this_thread::yield();
  464. // std::unique_lock<std::mutex> t_lock(m_lock);
  465. char buf[16] = {0}; //数据转字符串然后存数据库
  466. std::string t_plc_data_string;
  467. float t_heartbeat = Dispatch_communication::get_instance_references().m_data_storage_from_plc_to_manager.m_heartbeat;
  468. {
  469. std::unique_lock<std::mutex> t_lock1(Dispatch_communication::get_instance_references().m_data_lock);
  470. char * tp = buf;
  471. for (int i = 0; i < DATA_STORAGE_PLC_DATA_LENGTH; ++i)
  472. {
  473. sprintf(buf, "%g\t", Dispatch_communication::get_instance_references().m_data_storage_from_plc_to_manager.m_plc_data[i]);
  474. t_plc_data_string += buf;
  475. }
  476. // std::cout << " huli test :::: " << " t_plc_data_string = " << t_plc_data_string << std::endl;
  477. }
  478. Dispatch_manager::get_instance_references().m_dispatch_command.insert_plc_data(t_plc_data_string, t_heartbeat);
  479. }
  480. }
  481. }
  482. //封装 给plc的调度请求
  483. Error_manager Dispatch_plc::encapsulate_dispatch_request_to_plc()
  484. {
  485. // //把m_dispatch_request_msg的信息传给本地的数据缓存.
  486. // std::unique_lock<std::mutex> t_lock(m_lock);
  487. // m_request_key = m_dispatch_request_msg.command_key();
  488. // m_request_status = 1;
  489. // if ( m_dispatch_request_msg.dispatch_motion_direction() == message::Dispatch_motion_direction::E_STORE_CAR )
  490. // {
  491. // m_request_dispatch_motion_direction = Common_data::Dispatch_motion_direction::DISPATCH_MOTION_DIRECTION_STORE;
  492. // m_request_passageway_direction = Common_data::Passageway_direction::PASSAGEWAY_DIRECTION_INLET;
  493. // m_request_passageway_id = m_dispatch_request_msg.mutable_id_struct()->terminal_id()+1;//0~5入口终端号改为1~6
  494. // }
  495. // else if( m_dispatch_request_msg.dispatch_motion_direction() == message::Dispatch_motion_direction::E_PICKUP_CAR )
  496. // {
  497. // m_request_dispatch_motion_direction = Common_data::Dispatch_motion_direction::DISPATCH_MOTION_DIRECTION_PICKUP;
  498. // m_request_passageway_direction = Common_data::Passageway_direction::PASSAGEWAY_DIRECTION_OUTLET;
  499. // m_request_passageway_id = m_dispatch_request_msg.mutable_id_struct()->terminal_id()+1;//0~5入口终端号改为1~6
  500. //
  501. // }
  502. // else
  503. // {
  504. // m_request_dispatch_motion_direction = Common_data::Dispatch_motion_direction::DISPATCH_MOTION_DIRECTION_UNKNOWN;
  505. // m_request_passageway_direction = Common_data::Passageway_direction::PASSAGEWAY_DIRECTION_UNKNOWN;
  506. // m_request_passageway_id = -1;
  507. // }
  508. //
  509. // if ( m_dispatch_request_msg.locate_information().locate_correct() )
  510. // {
  511. // m_request_car_center_x = m_dispatch_request_msg.locate_information().locate_x();
  512. // m_request_car_center_y = m_dispatch_request_msg.locate_information().locate_y();
  513. //#ifdef MEASURE_TO_PLC_CORRECTION
  514. // m_request_car_angle = m_dispatch_request_msg.locate_information().locate_angle()-90+0.3;//80~100改为-10~+10
  515. //#endif
  516. //#ifndef MEASURE_TO_PLC_CORRECTION
  517. // m_request_car_angle = m_dispatch_request_msg.locate_information().locate_angle();
  518. //#endif
  519. // m_request_car_front_theta = m_dispatch_request_msg.locate_information().locate_front_theta();
  520. // m_request_car_length = m_dispatch_request_msg.locate_information().locate_length();
  521. // m_request_car_width = m_dispatch_request_msg.locate_information().locate_width();
  522. // m_request_car_height = m_dispatch_request_msg.locate_information().locate_height();
  523. // m_request_car_wheel_base = m_dispatch_request_msg.locate_information().locate_wheel_base();
  524. // m_request_car_wheel_width = m_dispatch_request_msg.locate_information().locate_wheel_width();
  525. // m_request_uniformed_car_x = m_dispatch_request_msg.locate_information().uniformed_car_x();
  526. // m_request_uniformed_car_y = m_dispatch_request_msg.locate_information().uniformed_car_y();
  527. // }
  528. // else
  529. // {
  530. // m_request_car_center_x = m_dispatch_request_msg.locate_information().locate_x();
  531. // m_request_car_center_y = m_dispatch_request_msg.locate_information().locate_y();
  532. //#ifdef MEASURE_TO_PLC_CORRECTION
  533. // m_request_car_angle = m_dispatch_request_msg.locate_information().locate_angle()-90+0.3;//80~100改为-10~+10
  534. //#endif
  535. //#ifndef MEASURE_TO_PLC_CORRECTION
  536. // m_request_car_angle = m_dispatch_request_msg.locate_information().locate_angle();
  537. //#endif
  538. // m_request_car_front_theta = m_dispatch_request_msg.locate_information().locate_front_theta();
  539. // m_request_car_length = m_dispatch_request_msg.locate_information().locate_length();
  540. // m_request_car_width = m_dispatch_request_msg.locate_information().locate_width();
  541. // m_request_car_height = m_dispatch_request_msg.locate_information().locate_height();
  542. // m_request_car_wheel_base = m_dispatch_request_msg.locate_information().locate_wheel_base();
  543. // m_request_car_wheel_width = m_dispatch_request_msg.locate_information().locate_wheel_width();
  544. // m_request_uniformed_car_x = m_dispatch_request_msg.locate_information().uniformed_car_x();
  545. // m_request_uniformed_car_y = m_dispatch_request_msg.locate_information().uniformed_car_y();
  546. // }
  547. //
  548. // if ( m_dispatch_request_msg.parkspace_info_ex_size() ==1 )
  549. // {
  550. // m_request_parkingspace_index_id = m_dispatch_request_msg.parkspace_info_ex(0).parkingspace_index_id();
  551. // m_request_parkingspace_unit_id = m_dispatch_request_msg.parkspace_info_ex(0).parkingspace_unit_id()+1;//0~2单元号改为1~3
  552. // m_request_parkingspace_label_id = m_dispatch_request_msg.parkspace_info_ex(0).parkingspace_label_id();
  553. // m_request_parkingspace_floor_id = m_dispatch_request_msg.parkspace_info_ex(0).parkingspace_floor_id();
  554. // m_request_parkingspace_room_id = m_dispatch_request_msg.parkspace_info_ex(0).parkingspace_room_id();
  555. // //车位朝向, 小号朝前朝南, 大号朝后朝北
  556. // m_request_parkingspace_direction = (Common_data::Parkingspace_direction)m_dispatch_request_msg.parkspace_info_ex(0).parkingspace_direction();
  557. //
  558. //
  559. // //20211220, utf-8 -> GBK
  560. // m_request_car_license = String_convert::utf8_to_gbk( m_dispatch_request_msg.parkspace_info_ex(0).car_info().car_numberplate() );
  561. //
  562. // m_request_car_type = (Common_data::Car_type)m_dispatch_request_msg.parkspace_info_ex(0).car_type();
  563. //
  564. // //20211207, huli add wheel_base for pickup_car
  565. // if( m_dispatch_request_msg.dispatch_motion_direction() == message::Dispatch_motion_direction::E_PICKUP_CAR )
  566. // {
  567. // m_request_car_wheel_base = m_dispatch_request_msg.parkspace_info_ex(0).car_info().car_wheel_base();
  568. // m_request_car_wheel_width = m_dispatch_request_msg.parkspace_info_ex(0).car_info().car_wheel_width();
  569. // }
  570. // }
  571. // else
  572. // {
  573. // return Error_manager(Error_code::DISPATCH_PLC_REQUEST_ERROR, Error_level::MINOR_ERROR,
  574. // " m_dispatch_request_msg.parkspace_info_ex_size() !=1 error ");
  575. // }
  576. return Error_code::SUCCESS;
  577. }
  578. //封装 给plc的调度请求
  579. Error_manager Dispatch_plc::encapsulate_dispatch_request_to_plc_new()
  580. {
  581. //把m_dispatch_request_msg的信息传给本地的数据缓存.
  582. std::unique_lock<std::mutex> t_lock(m_lock);
  583. m_request_key = m_command_key;
  584. m_request_status = 1;
  585. if ( m_dispatch_process_type == Common_data::Dispatch_process_type::DISPATCH_PROCESS_STORE ||
  586. m_dispatch_process_type == Common_data::Dispatch_process_type::DISPATCH_PROCESS_REALLOCATE)
  587. {
  588. //基本信息
  589. m_request_dispatch_motion_direction = m_dispatch_process_type;
  590. m_request_passageway_direction = Common_data::Passageway_direction::PASSAGEWAY_DIRECTION_INLET;
  591. m_request_passageway_id = m_park_table_msg.terminal_id();
  592. // m_request_passageway_id = m_terminal_id;
  593. //感测雷达信息, 存车专有
  594. // if ( m_park_table_msg.entrance_measure_info().border_statu() )
  595. if ( true )
  596. {
  597. m_request_car_center_x = m_park_table_msg.entrance_measure_info().cx();
  598. m_request_car_center_y = m_park_table_msg.entrance_measure_info().cy();
  599. m_request_car_angle = m_park_table_msg.entrance_measure_info().theta();
  600. m_request_car_front_theta = m_park_table_msg.entrance_measure_info().front_theta();
  601. m_request_car_length = m_park_table_msg.entrance_measure_info().length();
  602. m_request_car_width = m_park_table_msg.entrance_measure_info().width();
  603. m_request_car_height = m_park_table_msg.entrance_measure_info().height();
  604. m_request_car_wheel_base = m_park_table_msg.entrance_measure_info().wheelbase();
  605. m_request_car_wheel_width = m_park_table_msg.entrance_measure_info().width();
  606. m_request_uniformed_car_x = m_park_table_msg.entrance_measure_info().cx();
  607. m_request_uniformed_car_y = m_park_table_msg.entrance_measure_info().cy();
  608. }
  609. //车位信息
  610. m_request_parkingspace_index_id = m_park_table_msg.allocated_space_info().id();
  611. m_request_parkingspace_unit_id = m_park_table_msg.allocated_space_info().unit_id();
  612. int t_parkingspace_label_id = (m_park_table_msg.allocated_space_info().id() - 1 )%78;
  613. m_request_parkingspace_label_id = t_parkingspace_label_id+1;
  614. m_request_parkingspace_floor_id = m_park_table_msg.allocated_space_info().floor();
  615. m_request_parkingspace_room_id = m_park_table_msg.allocated_space_info().room_id();
  616. //车位朝向, 奇数朝南, 偶数朝北
  617. if ( m_request_parkingspace_index_id %2 == 1 )
  618. {
  619. m_request_parkingspace_direction = Common_data::Parkingspace_direction::PARKINGSPACE_DIRECTION_SOUTH;
  620. }
  621. else
  622. {
  623. m_request_parkingspace_direction = Common_data::Parkingspace_direction::PARKINGSPACE_DIRECTION_NORTH;
  624. }
  625. //20211220, utf-8 -> GBK
  626. m_request_car_license = String_convert::utf8_to_gbk( m_park_table_msg.car_number() );
  627. m_request_car_type = Common_data::judge_car_type_with_car_height(m_request_car_height);
  628. }
  629. else if( m_dispatch_process_type == Common_data::Dispatch_process_type::DISPATCH_PROCESS_PICKUP ||
  630. m_dispatch_process_type == Common_data::Dispatch_process_type::DISPATCH_PROCESS_REVOCATION)
  631. {
  632. m_request_dispatch_motion_direction = m_dispatch_process_type;
  633. m_request_passageway_direction = Common_data::Passageway_direction::PASSAGEWAY_DIRECTION_OUTLET;
  634. m_request_passageway_id = m_pick_table_msg.terminal_id();
  635. // m_request_passageway_id = m_terminal_id;
  636. //20211207, 取车专有, 数据库保存的汽车信息, 轮距
  637. m_request_car_center_x = m_pick_table_msg.actually_measure_info().cx();
  638. m_request_car_center_y = m_pick_table_msg.actually_measure_info().cy();
  639. m_request_car_angle = m_pick_table_msg.actually_measure_info().theta();
  640. m_request_car_front_theta = m_pick_table_msg.actually_measure_info().front_theta();
  641. m_request_car_length = m_pick_table_msg.actually_measure_info().length();
  642. m_request_car_width = m_pick_table_msg.actually_measure_info().width();
  643. m_request_car_height = m_pick_table_msg.actually_measure_info().height();
  644. m_request_car_wheel_base = m_pick_table_msg.actually_measure_info().wheelbase();
  645. m_request_car_wheel_width = m_pick_table_msg.actually_measure_info().width();
  646. m_request_uniformed_car_x = m_pick_table_msg.actually_measure_info().cx();
  647. m_request_uniformed_car_y = m_pick_table_msg.actually_measure_info().cy();
  648. //车位信息
  649. m_request_parkingspace_index_id = m_pick_table_msg.actually_space_info().id();
  650. m_request_parkingspace_unit_id = m_pick_table_msg.actually_space_info().unit_id();
  651. int t_parkingspace_label_id = (m_pick_table_msg.actually_space_info().id() -1 )%78;
  652. m_request_parkingspace_label_id = t_parkingspace_label_id +1 ;
  653. m_request_parkingspace_floor_id = m_pick_table_msg.actually_space_info().floor();
  654. m_request_parkingspace_room_id = m_pick_table_msg.actually_space_info().room_id();
  655. //车位朝向, 奇数朝南, 偶数朝北
  656. if ( m_request_parkingspace_index_id %2 == 1 )
  657. {
  658. m_request_parkingspace_direction = Common_data::Parkingspace_direction::PARKINGSPACE_DIRECTION_SOUTH;
  659. }
  660. else
  661. {
  662. m_request_parkingspace_direction = Common_data::Parkingspace_direction::PARKINGSPACE_DIRECTION_NORTH;
  663. }
  664. //20211220, utf-8 -> GBK
  665. m_request_car_license = String_convert::utf8_to_gbk( m_pick_table_msg.car_number() );
  666. m_request_car_type = Common_data::judge_car_type_with_car_height(m_request_car_height);
  667. }
  668. else
  669. {
  670. m_request_dispatch_motion_direction = Common_data::Dispatch_process_type::DISPATCH_PROCESS_TYPE_UNKNOW;
  671. m_request_passageway_direction = Common_data::Passageway_direction::PASSAGEWAY_DIRECTION_UNKNOWN;
  672. m_request_passageway_id = -1;
  673. }
  674. return Error_code::SUCCESS;
  675. }
  676. //更新plc通信
  677. Error_manager Dispatch_plc::update_dispatch_plc_communication()
  678. {
  679. std::unique_lock<std::mutex> t_lock(m_lock);
  680. std::unique_lock<std::mutex> t_lock1(Dispatch_communication::get_instance_references().m_data_lock);
  681. /*
  682. //请求消息, 调度->plc
  683. Dispatch_communication::Dispatch_request_from_manager_to_plc_for_data * tp_dispatch_request_from_manager_to_plc_for_data =
  684. & Dispatch_communication::get_instance_references().m_dispatch_request_from_manager_to_plc_for_data;
  685. Dispatch_communication::Dispatch_request_from_manager_to_plc_for_key * m_dispatch_request_from_manager_to_plc_for_key =
  686. & Dispatch_communication::get_instance_references().m_dispatch_request_from_manager_to_plc_for_key;
  687. memset(m_dispatch_request_from_manager_to_plc_for_key->m_request_key, 0, 50);
  688. int t_size1 = m_request_key.size()<=50 ? m_request_key.size() : 50 ;
  689. m_request_key = "123456789";
  690. memcpy(m_dispatch_request_from_manager_to_plc_for_key->m_request_key, m_request_key.c_str(), t_size1);
  691. static unsigned char index = 0;
  692. index++;
  693. tp_dispatch_request_from_manager_to_plc_for_data->m_request_dispatch_motion_direction = index;
  694. tp_dispatch_request_from_manager_to_plc_for_data->m_request_passageway_id = 02;
  695. tp_dispatch_request_from_manager_to_plc_for_data->m_request_passageway_direction = 03;
  696. tp_dispatch_request_from_manager_to_plc_for_data->m_request_parkingspace_index_id = 04;
  697. tp_dispatch_request_from_manager_to_plc_for_data->m_request_parkingspace_unit_id = 05;
  698. tp_dispatch_request_from_manager_to_plc_for_data->m_request_parkingspace_floor_id = 6;
  699. tp_dispatch_request_from_manager_to_plc_for_data->m_request_parkingspace_room_id = 7;
  700. tp_dispatch_request_from_manager_to_plc_for_data->m_request_parkingspace_direction = 8;
  701. tp_dispatch_request_from_manager_to_plc_for_data->m_request_car_center_x = 11;
  702. tp_dispatch_request_from_manager_to_plc_for_data->m_request_car_center_y = 12;
  703. tp_dispatch_request_from_manager_to_plc_for_data->m_request_car_angle = 13;
  704. tp_dispatch_request_from_manager_to_plc_for_data->m_request_car_front_theta = 14;
  705. tp_dispatch_request_from_manager_to_plc_for_data->m_request_car_length = 15;
  706. tp_dispatch_request_from_manager_to_plc_for_data->m_request_car_width = 16;
  707. tp_dispatch_request_from_manager_to_plc_for_data->m_request_car_height = 17;
  708. tp_dispatch_request_from_manager_to_plc_for_data->m_request_car_wheel_base = 18;
  709. tp_dispatch_request_from_manager_to_plc_for_data->m_request_car_wheel_width = 19;
  710. memset(tp_dispatch_request_from_manager_to_plc_for_data->m_request_car_license, 0, 20);
  711. int t_size2 = m_request_key.size()<=20 ? m_request_key.size() : 20 ;
  712. m_request_car_license = "ABCDEF";
  713. memcpy(tp_dispatch_request_from_manager_to_plc_for_data->m_request_car_license, m_request_car_license.c_str(), t_size2);
  714. tp_dispatch_request_from_manager_to_plc_for_data->m_request_car_type = 33;
  715. tp_dispatch_request_from_manager_to_plc_for_data->m_request_anticollision_lidar_flag = 44;
  716. tp_dispatch_request_from_manager_to_plc_for_data->m_request_car_wheel_base_exact_value = 55;
  717. */
  718. //请求消息, 调度->plc
  719. Dispatch_communication::Dispatch_request_from_manager_to_plc_for_data * tp_dispatch_request_from_manager_to_plc_for_data =
  720. & Dispatch_communication::get_instance_references().m_dispatch_request_from_manager_to_plc_for_data;
  721. Dispatch_communication::Dispatch_request_from_manager_to_plc_for_key * m_dispatch_request_from_manager_to_plc_for_key =
  722. & Dispatch_communication::get_instance_references().m_dispatch_request_from_manager_to_plc_for_key;
  723. memset(m_dispatch_request_from_manager_to_plc_for_key->m_request_key, 0, 50);
  724. int t_size1 = m_request_key.size()<=50 ? m_request_key.size() : 50 ;
  725. memcpy(m_dispatch_request_from_manager_to_plc_for_key->m_request_key, m_request_key.c_str(), t_size1);
  726. tp_dispatch_request_from_manager_to_plc_for_data->m_request_status = m_request_status;
  727. tp_dispatch_request_from_manager_to_plc_for_data->m_request_dispatch_motion_direction = m_request_dispatch_motion_direction;
  728. tp_dispatch_request_from_manager_to_plc_for_data->m_request_passageway_id = m_request_passageway_id;
  729. tp_dispatch_request_from_manager_to_plc_for_data->m_request_passageway_direction = m_request_passageway_direction;
  730. tp_dispatch_request_from_manager_to_plc_for_data->m_request_parkingspace_index_id = m_request_parkingspace_index_id;
  731. tp_dispatch_request_from_manager_to_plc_for_data->m_request_parkingspace_unit_id = m_request_parkingspace_unit_id;
  732. tp_dispatch_request_from_manager_to_plc_for_data->m_request_parkingspace_label_id = m_request_parkingspace_label_id;
  733. tp_dispatch_request_from_manager_to_plc_for_data->m_request_parkingspace_floor_id = m_request_parkingspace_floor_id;
  734. tp_dispatch_request_from_manager_to_plc_for_data->m_request_parkingspace_room_id = m_request_parkingspace_room_id;
  735. tp_dispatch_request_from_manager_to_plc_for_data->m_request_parkingspace_direction = m_request_parkingspace_direction;
  736. tp_dispatch_request_from_manager_to_plc_for_data->m_request_car_center_x = m_request_car_center_x;
  737. tp_dispatch_request_from_manager_to_plc_for_data->m_request_car_center_y = m_request_car_center_y;
  738. tp_dispatch_request_from_manager_to_plc_for_data->m_request_car_angle = m_request_car_angle;
  739. tp_dispatch_request_from_manager_to_plc_for_data->m_request_car_front_theta = m_request_car_front_theta;
  740. tp_dispatch_request_from_manager_to_plc_for_data->m_request_car_length = m_request_car_length;
  741. tp_dispatch_request_from_manager_to_plc_for_data->m_request_car_width = m_request_car_width;
  742. tp_dispatch_request_from_manager_to_plc_for_data->m_request_car_height = m_request_car_height;
  743. tp_dispatch_request_from_manager_to_plc_for_data->m_request_car_wheel_base = m_request_car_wheel_base;
  744. tp_dispatch_request_from_manager_to_plc_for_data->m_request_car_wheel_width = m_request_car_wheel_width;
  745. memset(tp_dispatch_request_from_manager_to_plc_for_data->m_request_car_license, 0, 20);
  746. tp_dispatch_request_from_manager_to_plc_for_data->m_request_car_license[0] = 18;
  747. tp_dispatch_request_from_manager_to_plc_for_data->m_request_car_license[1] = m_request_car_license.length();
  748. unsigned char * p_ch = tp_dispatch_request_from_manager_to_plc_for_data->m_request_car_license;
  749. memcpy(p_ch+2, m_request_car_license.c_str(), m_request_car_license.length());
  750. tp_dispatch_request_from_manager_to_plc_for_data->m_request_car_type = m_request_car_type;
  751. tp_dispatch_request_from_manager_to_plc_for_data->m_request_uniformed_car_x = m_request_uniformed_car_x;
  752. tp_dispatch_request_from_manager_to_plc_for_data->m_request_uniformed_car_y = m_request_uniformed_car_y;
  753. tp_dispatch_request_from_manager_to_plc_for_data->m_request_anticollision_lidar_flag = m_request_anticollision_lidar_flag;
  754. tp_dispatch_request_from_manager_to_plc_for_data->m_request_car_wheel_base_exact_value = m_request_car_wheel_base_exact_value;
  755. tp_dispatch_request_from_manager_to_plc_for_data->m_request_clamp_lidar_deviation_1 = m_request_clamp_lidar_deviation_1;
  756. tp_dispatch_request_from_manager_to_plc_for_data->m_request_clamp_lidar_deviation_2 = m_request_clamp_lidar_deviation_2;
  757. tp_dispatch_request_from_manager_to_plc_for_data->m_request_clamp_lidar_deviation_3 = m_request_clamp_lidar_deviation_3;
  758. tp_dispatch_request_from_manager_to_plc_for_data->m_request_clamp_lidar_deviation_4 = m_request_clamp_lidar_deviation_4;
  759. //答复消息, plc->调度
  760. Dispatch_communication::Dispatch_response_from_plc_to_manager * tp_dispatch_response_from_plc_to_manager =
  761. & Dispatch_communication::get_instance_references().m_dispatch_response_from_plc_to_manager;
  762. m_response_key = (char*) tp_dispatch_response_from_plc_to_manager->m_response_key;
  763. m_response_status = (Dispatch_plc::Response_status)tp_dispatch_response_from_plc_to_manager->m_response_status;
  764. m_response_working_total_time = tp_dispatch_response_from_plc_to_manager->m_response_working_total_time;
  765. m_response_working_remaining_time = tp_dispatch_response_from_plc_to_manager->m_response_working_remaining_time;
  766. m_response_dispatch_motion_direction = (Common_data::Dispatch_process_type)tp_dispatch_response_from_plc_to_manager->m_response_dispatch_motion_direction;
  767. m_response_passageway_id = tp_dispatch_response_from_plc_to_manager->m_response_passageway_id;
  768. m_response_passageway_direction = (Common_data::Passageway_direction)tp_dispatch_response_from_plc_to_manager->m_response_passageway_direction;
  769. m_response_parkingspace_index_id = tp_dispatch_response_from_plc_to_manager->m_response_parkingspace_index_id;
  770. m_response_parkingspace_unit_id = tp_dispatch_response_from_plc_to_manager->m_response_parkingspace_unit_id;
  771. m_response_parkingspace_label_id = tp_dispatch_response_from_plc_to_manager->m_response_parkingspace_label_id;
  772. m_response_parkingspace_floor_id = tp_dispatch_response_from_plc_to_manager->m_response_parkingspace_floor_id;
  773. m_response_parkingspace_room_id = tp_dispatch_response_from_plc_to_manager->m_response_parkingspace_room_id;
  774. m_response_parkingspace_direction = (Common_data::Parkingspace_direction)tp_dispatch_response_from_plc_to_manager->m_response_parkingspace_direction;
  775. m_response_car_center_x = tp_dispatch_response_from_plc_to_manager->m_response_car_center_x;
  776. m_response_car_center_y = tp_dispatch_response_from_plc_to_manager->m_response_car_center_y;
  777. m_response_car_angle = tp_dispatch_response_from_plc_to_manager->m_response_car_angle;
  778. m_response_car_front_theta = tp_dispatch_response_from_plc_to_manager->m_response_car_front_theta;
  779. m_response_car_length = tp_dispatch_response_from_plc_to_manager->m_response_car_length;
  780. m_response_car_width = tp_dispatch_response_from_plc_to_manager->m_response_car_width;
  781. m_response_car_height = tp_dispatch_response_from_plc_to_manager->m_response_car_height;
  782. m_response_car_wheel_base = tp_dispatch_response_from_plc_to_manager->m_response_car_wheel_base;
  783. m_response_car_wheel_width = tp_dispatch_response_from_plc_to_manager->m_response_car_wheel_width;
  784. m_response_car_license = (char*) (tp_dispatch_response_from_plc_to_manager->m_response_car_license+2);
  785. m_response_car_type = (Common_data::Car_type) tp_dispatch_response_from_plc_to_manager->m_response_car_type;
  786. m_response_uniformed_car_x = tp_dispatch_response_from_plc_to_manager->m_response_uniformed_car_x;
  787. m_response_uniformed_car_y = tp_dispatch_response_from_plc_to_manager->m_response_uniformed_car_y;
  788. m_response_anticollision_lidar_flag = (Common_data::Anticollision_lidar_flag)tp_dispatch_response_from_plc_to_manager->m_response_anticollision_lidar_flag;
  789. m_response_car_wheel_base_exact_value = tp_dispatch_response_from_plc_to_manager->m_response_car_wheel_base_exact_value;
  790. m_response_clamp_lidar_deviation_1 = tp_dispatch_response_from_plc_to_manager->m_response_clamp_lidar_deviation_1;
  791. m_response_clamp_lidar_deviation_2 = tp_dispatch_response_from_plc_to_manager->m_response_clamp_lidar_deviation_2;
  792. m_response_clamp_lidar_deviation_3 = tp_dispatch_response_from_plc_to_manager->m_response_clamp_lidar_deviation_3;
  793. m_response_clamp_lidar_deviation_4 = tp_dispatch_response_from_plc_to_manager->m_response_clamp_lidar_deviation_4;
  794. /*
  795. std::cout << " huli test :::: " << " ------------------------------------------------------ = " << std::endl;
  796. std::cout << " huli test :::: " << " m_response_key = " << m_response_key << std::endl;
  797. std::cout << " huli test :::: " << " m_response_status = " << (int)m_response_status << std::endl;
  798. std::cout << " huli test :::: " << " m_response_dispatch_motion_direction = " << (int)m_response_dispatch_motion_direction << std::endl;
  799. std::cout << " huli test :::: " << " m_response_passageway_id = " << m_response_passageway_id << std::endl;
  800. std::cout << " huli test :::: " << " m_response_passageway_direction = " << (int)m_response_passageway_direction << std::endl;
  801. std::cout << " huli test :::: " << " m_response_parkingspace_index_id = " << m_response_parkingspace_index_id << std::endl;
  802. std::cout << " huli test :::: " << " m_response_parkingspace_unit_id = " << m_response_parkingspace_unit_id << std::endl;
  803. std::cout << " huli test :::: " << " m_response_parkingspace_floor_id = " << m_response_parkingspace_floor_id << std::endl;
  804. std::cout << " huli test :::: " << " m_response_parkingspace_room_id = " << m_response_parkingspace_room_id << std::endl;
  805. std::cout << " huli test :::: " << " m_response_parkingspace_direction = " << (int)m_response_parkingspace_direction << std::endl;
  806. std::cout << " huli test :::: " << " m_response_car_center_x = " << m_response_car_center_x << std::endl;
  807. std::cout << " huli test :::: " << " m_response_car_center_y = " << m_response_car_center_y << std::endl;
  808. std::cout << " huli test :::: " << " m_response_car_license = " << m_response_car_license << std::endl;
  809. */
  810. //状态消息, 调度->plc
  811. Dispatch_communication::Dispatch_plc_status_from_manager_to_plc * tp_dispatch_plc_status_from_manager_to_plc =
  812. & Dispatch_communication::get_instance_references().m_dispatch_plc_status_from_manager_to_plc;
  813. m_dispatch_heartbeat++;
  814. tp_dispatch_plc_status_from_manager_to_plc->m_dispatch_heartbeat = m_dispatch_heartbeat;
  815. //状态消息, plc->调度
  816. Dispatch_communication::Dispatch_plc_status_from_plc_to_manager * tp_dispatch_plc_status_from_plc_to_manager =
  817. & Dispatch_communication::get_instance_references().m_dispatch_plc_status_from_plc_to_manager;
  818. m_plc_heartbeat = tp_dispatch_plc_status_from_plc_to_manager->m_plc_heartbeat;
  819. m_plc_status_info = tp_dispatch_plc_status_from_plc_to_manager->m_plc_status_info;
  820. m_turnplate_angle_min1 = tp_dispatch_plc_status_from_plc_to_manager->m_turnplate_angle_min1;
  821. m_turnplate_angle_max1 = tp_dispatch_plc_status_from_plc_to_manager->m_turnplate_angle_max1;
  822. m_turnplate_angle_min2 = tp_dispatch_plc_status_from_plc_to_manager->m_turnplate_angle_min2;
  823. m_turnplate_angle_max2 = tp_dispatch_plc_status_from_plc_to_manager->m_turnplate_angle_max2;
  824. //need
  825. //通过心跳帧来判断通信是否正常
  826. // if ( m_last_heartbeat != tp_dispatch_plc_status_from_plc_to_manager->m_plc_heartbeat )
  827. // {
  828. // m_last_heartbeat = tp_dispatch_plc_status_from_plc_to_manager->m_plc_heartbeat;
  829. // m_status_updata_time = std::chrono::system_clock::now();
  830. //
  831. // if ( m_dispatch_plc_status == Dispatch_plc::Dispatch_plc_status::DISPATCH_PLC_DISCONNECT )
  832. // {
  833. // m_dispatch_plc_status = Dispatch_plc::Dispatch_plc_status::DISPATCH_PLC_READY;
  834. // }
  835. // }
  836. // else if(std::chrono::system_clock::now() - m_status_updata_time > std::chrono::milliseconds(COMMUNICATION_OVER_TIME_MS))
  837. // {
  838. // m_dispatch_plc_status = Dispatch_plc::Dispatch_plc_status::DISPATCH_PLC_DISCONNECT;
  839. // }
  840. //else 继续等待,直到消息刷新或者超时.
  841. return Error_code::SUCCESS;
  842. }
  843. //检查plc答复反馈
  844. Error_manager Dispatch_plc::check_response_from_plc()
  845. {
  846. // std::unique_lock<std::mutex> t_lock(m_lock);
  847. //
  848. //
  849. //#ifndef WAIT_PLC_RESPONSE
  850. // //need, 调试代码, 延时10s, 直接返回成功
  851. // if ( std::chrono::system_clock::now() - m_start_time > std::chrono::seconds(20) )
  852. // {
  853. // return Error_code::SUCCESS;
  854. // }
  855. // //返回没有收到数据
  856. // else
  857. // {
  858. // return Error_code::NODATA;
  859. // }
  860. //#endif
  861. //
  862. //
  863. //#ifdef WAIT_PLC_RESPONSE
  864. //
  865. // static bool s_update_flag = false;
  866. // //检查唯一码
  867. // if ( m_response_key == m_request_key )
  868. // {
  869. // //第一次同步时,刷新时间
  870. // if ( s_update_flag == false )
  871. // {
  872. // s_update_flag = true;
  873. // if ( m_dispatch_request_msg.dispatch_motion_direction() == message::Dispatch_motion_direction::E_STORE_CAR )
  874. // {
  875. // std::unique_lock<std::mutex> t_lock(Dispatch_manager::get_instance_references().m_lock);
  876. // Dispatch_manager::get_instance_references().m_store_updata_time = std::chrono::system_clock::now();
  877. // }
  878. // else if( m_dispatch_request_msg.dispatch_motion_direction() == message::Dispatch_motion_direction::E_PICKUP_CAR )
  879. // {
  880. // std::unique_lock<std::mutex> t_lock(Dispatch_manager::get_instance_references().m_lock);
  881. // Dispatch_manager::get_instance_references().m_pickup_updata_time = std::chrono::system_clock::now();
  882. // }
  883. // }
  884. //
  885. //
  886. // if(m_response_status == RESPONS_WORKING)
  887. // {
  888. // return Error_code::NODATA;//返回没有收到数据
  889. // }
  890. // else
  891. // {
  892. // std::cout<< "m_response_status = " << m_response_status << std::endl;
  893. // //如果故障,则添加错误码
  894. // if ( m_response_status != RESPONS_OVER )
  895. // {
  896. // //添加错误码
  897. // Error_manager t_error(DISPATCH_PLC_RESPONS_ERROR, MINOR_ERROR, " Dispatch_plc::check_response_from_plc() m_respons_status is error");
  898. // return t_error;
  899. // }
  900. //
  901. // return Error_code::SUCCESS;
  902. // }
  903. //
  904. // }
  905. // else
  906. // {
  907. // s_update_flag = false;
  908. // return Error_code::NODATA;//返回没有收到数据
  909. // }
  910. //#endif
  911. return Error_code::SUCCESS;
  912. }
  913. //检查plc答复反馈
  914. Error_manager Dispatch_plc::check_response_from_plc_new()
  915. {
  916. std::unique_lock<std::mutex> t_lock(m_lock);
  917. #ifndef WAIT_PLC_RESPONSE
  918. //need, 调试代码, 延时10s, 直接返回成功
  919. if ( std::chrono::system_clock::now() - m_start_time > std::chrono::seconds(10) )
  920. {
  921. return Error_code::SUCCESS;
  922. }
  923. //返回没有收到数据
  924. else
  925. {
  926. return Error_code::NODATA;
  927. }
  928. #endif
  929. #ifdef WAIT_PLC_RESPONSE
  930. static bool s_update_flag = false;
  931. //检查唯一码
  932. if ( m_response_key == m_request_key )
  933. {
  934. //20230213 huli add plc car_wheel_base
  935. m_plc_car_wheel_base = m_response_car_wheel_base_exact_value;
  936. m_plc_clamp_lidar_deviation_1 = m_response_clamp_lidar_deviation_1;
  937. m_plc_clamp_lidar_deviation_2 = m_response_clamp_lidar_deviation_2;
  938. m_plc_clamp_lidar_deviation_3 = m_response_clamp_lidar_deviation_3;
  939. m_plc_clamp_lidar_deviation_4 = m_response_clamp_lidar_deviation_4;
  940. //第一次同步时,刷新时间
  941. if ( s_update_flag == false )
  942. {
  943. s_update_flag = true;
  944. // if ( m_dispatch_request_msg.dispatch_motion_direction() == message::Dispatch_motion_direction::E_STORE_CAR )
  945. // {
  946. // std::unique_lock<std::mutex> t_lock(Dispatch_manager::get_instance_references().m_lock);
  947. // Dispatch_manager::get_instance_references().m_store_updata_time = std::chrono::system_clock::now();
  948. // }
  949. // else if( m_dispatch_request_msg.dispatch_motion_direction() == message::Dispatch_motion_direction::E_PICKUP_CAR )
  950. // {
  951. // std::unique_lock<std::mutex> t_lock(Dispatch_manager::get_instance_references().m_lock);
  952. // Dispatch_manager::get_instance_references().m_pickup_updata_time = std::chrono::system_clock::now();
  953. // }
  954. }
  955. if(m_response_status == RESPONS_WORKING)
  956. {
  957. return Error_code::NODATA;//返回没有收到数据
  958. }
  959. else
  960. {
  961. std::cout<< "m_response_status = " << m_response_status << std::endl;
  962. //如果故障,则添加错误码
  963. if ( m_response_status == RESPONS_OVER )
  964. {
  965. return Error_code::SUCCESS;
  966. }
  967. else if(m_response_status == RESPONS_REALLOCATE_MIN_CAR)
  968. {
  969. //重新分配小车
  970. return DISPATCH_PLC_REALLOCATE_MIN_CAR;
  971. }
  972. else if(m_response_status == RESPONS_REALLOCATE_MID_CAR)
  973. {
  974. //重新分配中车
  975. return DISPATCH_PLC_REALLOCATE_MID_CAR;
  976. }
  977. else if(m_response_status == RESPONS_REALLOCATE_BIG_CAR)
  978. {
  979. //重新分配大车
  980. return DISPATCH_PLC_REALLOCATE_BIG_CAR;
  981. }
  982. else if(m_response_status == RESPONS_REALLOCATE_HUGE_CAR)
  983. {
  984. //重新分配fault
  985. return DISPATCH_PLC_REALLOCATE_HUGE_CAR;
  986. }
  987. else if(m_response_status == RESPONS_REALLOCATE_FAULT_CAR)
  988. {
  989. //重新分配fault
  990. return DISPATCH_PLC_REALLOCATE_FAULT_CAR;
  991. }
  992. else //如果故障,则添加错误码
  993. {
  994. //添加错误码
  995. Error_manager t_error(DISPATCH_PLC_RESPONS_ERROR, MINOR_ERROR, " Dispatch_plc::check_response_from_plc() m_respons_status is error");
  996. return t_error;
  997. }
  998. }
  999. }
  1000. else
  1001. {
  1002. s_update_flag = false;
  1003. return Error_code::NODATA;//返回没有收到数据
  1004. }
  1005. #endif
  1006. return Error_code::SUCCESS;
  1007. }
  1008. //发送调度答复信息给主控
  1009. Error_manager Dispatch_plc::send_dispatch_response_to_main_control()
  1010. {
  1011. // std::unique_lock<std::mutex> t_lock(m_lock);
  1012. // m_dispatch_response_msg.mutable_base_info()->set_msg_type(message::Message_type::eDispatch_response_msg);
  1013. // m_dispatch_response_msg.mutable_base_info()->set_timeout_ms(m_dispatch_request_msg.base_info().timeout_ms());
  1014. // m_dispatch_response_msg.mutable_base_info()->set_sender(message::Communicator::eDispatch_manager);
  1015. // m_dispatch_response_msg.mutable_base_info()->set_receiver(message::Communicator::eMain);
  1016. //
  1017. // m_dispatch_response_msg.set_command_key(m_dispatch_request_msg.command_key());
  1018. // m_dispatch_response_msg.set_dispatch_motion_direction(m_dispatch_request_msg.dispatch_motion_direction());
  1019. // m_dispatch_response_msg.mutable_id_struct()->set_terminal_id(m_dispatch_request_msg.mutable_id_struct()->terminal_id());
  1020. // m_dispatch_response_msg.mutable_id_struct()->set_unit_id(m_dispatch_request_msg.mutable_id_struct()->unit_id());
  1021. // m_dispatch_response_msg.mutable_locate_information()->CopyFrom(m_dispatch_request_msg.locate_information());
  1022. // m_dispatch_response_msg.mutable_parkspace_info_ex()->CopyFrom(m_dispatch_request_msg.parkspace_info_ex());
  1023. // m_dispatch_response_msg.set_car_type(m_dispatch_request_msg.car_type());
  1024. //
  1025. // if ( m_dispatch_response_msg.dispatch_motion_direction() == message::E_STORE_CAR )
  1026. // {
  1027. // for (int i = 0; i < m_dispatch_response_msg.parkspace_info_ex_size(); ++i)
  1028. // {
  1029. // if ( m_result == Error_code::SUCCESS )
  1030. // {
  1031. // m_dispatch_response_msg.mutable_parkspace_info_ex(i)->set_parkspace_status_target(message::Parkspace_status::eParkspace_occupied);
  1032. // }
  1033. // else
  1034. // {
  1035. // m_dispatch_response_msg.mutable_parkspace_info_ex(i)->set_parkspace_status_target(message::Parkspace_status::eParkspace_empty);
  1036. // }
  1037. // }
  1038. // }
  1039. // else if( m_dispatch_response_msg.dispatch_motion_direction() == message::E_PICKUP_CAR )
  1040. // {
  1041. // for (int i = 0; i < m_dispatch_response_msg.parkspace_info_ex_size(); ++i)
  1042. // {
  1043. // if ( m_result == Error_code::SUCCESS )
  1044. // {
  1045. // m_dispatch_response_msg.mutable_parkspace_info_ex(i)->set_parkspace_status_target(message::Parkspace_status::eParkspace_empty);
  1046. // }
  1047. // else
  1048. // {
  1049. // m_dispatch_response_msg.mutable_parkspace_info_ex(i)->set_parkspace_status_target(message::Parkspace_status::eParkspace_occupied);
  1050. // }
  1051. // }
  1052. // }
  1053. //
  1054. //
  1055. // m_dispatch_response_msg.mutable_error_manager()->set_error_code(m_result.get_error_code());
  1056. // m_dispatch_response_msg.mutable_error_manager()->set_error_level((message::Error_level)m_result.get_error_level());
  1057. // m_dispatch_response_msg.mutable_error_manager()->set_error_description(m_result.get_error_description());
  1058. //
  1059. //
  1060. // std::cout << " huli test :::: " << " m_dispatch_response_msg = " << m_dispatch_response_msg.DebugString()<< std::endl;
  1061. //
  1062. // std::string t_msg = m_dispatch_response_msg.SerializeAsString();
  1063. // System_communication::get_instance_references().encapsulate_task_msg(t_msg);
  1064. return Error_code::SUCCESS;
  1065. }
  1066. //发送调度答复信息给主控
  1067. Error_manager Dispatch_plc::send_dispatch_response_to_main_control_new()
  1068. {
  1069. // std::unique_lock<std::mutex> t_lock(m_lock);
  1070. //
  1071. // std::string t_msg;
  1072. // if ( m_dispatch_process_type == Common_data::Dispatch_process_type::DISPATCH_PROCESS_STORE )
  1073. // {
  1074. // measure_info t_measure_info = m_park_table_msg.entrance_measure_info();
  1075. // m_park_table_msg.mutable_actually_measure_info()->CopyFrom(t_measure_info);
  1076. // parkspace_info t_parkspace_info = m_park_table_msg.allocated_space_info();
  1077. // m_park_table_msg.mutable_actually_space_info()->CopyFrom(t_parkspace_info);
  1078. // //存车真实轴距
  1079. // m_park_table_msg.mutable_actually_measure_info()->set_wheelbase(m_response_car_wheel_base_exact_value);
  1080. //
  1081. // if ( m_result != Error_code::SUCCESS )
  1082. // {
  1083. // std::string t_error_string = m_result.to_string();
  1084. // m_park_table_msg.mutable_statu()->set_execute_statu(eError);
  1085. // m_park_table_msg.mutable_statu()->set_statu_description(t_error_string);
  1086. // }
  1087. // t_msg = m_park_table_msg.DebugString();
  1088. // }
  1089. // else if ( m_dispatch_process_type == Common_data::Dispatch_process_type::DISPATCH_PROCESS_PICKUP )
  1090. // {
  1091. // m_pick_table_msg.set_export_id(1);
  1092. // if ( m_result != Error_code::SUCCESS )
  1093. // {
  1094. // std::string t_error_string = m_result.to_string();
  1095. // m_pick_table_msg.mutable_statu()->set_execute_statu(eError);
  1096. // m_pick_table_msg.mutable_statu()->set_statu_description(t_error_string);
  1097. // }
  1098. // t_msg = m_pick_table_msg.DebugString();
  1099. // }
  1100. //
  1101. // System_communication::get_instance_references().encapsulate_task_msg(t_msg, 1);
  1102. // std::cout << " huli test :::: " << " m_command_completed_msg = " << t_msg << std::endl;
  1103. // Dispatch_manager::get_instance_references().ack_rabbitmq_message_new();
  1104. return Error_code::SUCCESS;
  1105. }