dispatch_plc.cpp 35 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675676677678679680681682683684685686687688689690691692693694695696697698699700701702703704705706707708709710711712713714715716717718719720721722723724725726727728729730731732733734735736737738739740741742743
  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. }
  13. Dispatch_plc::~Dispatch_plc()
  14. {
  15. dispatch_plc_uninit();
  16. }
  17. //调度plc 初始化
  18. Error_manager Dispatch_plc::dispatch_plc_init(int plc_id)
  19. {
  20. m_plc_id = plc_id;
  21. m_status_updata_time = std::chrono::system_clock::now();
  22. m_last_heartbeat = 0;
  23. // 线程默认开启
  24. m_execute_condition.reset(false, true, false);
  25. mp_execute_thread = new std::thread(&Dispatch_plc::execute_thread_fun, this);
  26. m_dispatch_plc_status = Dispatch_plc::Dispatch_plc_status::DISPATCH_PLC_READY;
  27. return Error_code::SUCCESS;
  28. }
  29. //调度plc 反初始化
  30. Error_manager Dispatch_plc::dispatch_plc_uninit()
  31. {
  32. if (mp_execute_thread)
  33. {
  34. m_execute_condition.kill_all();
  35. }
  36. if (mp_execute_thread)
  37. {
  38. mp_execute_thread->join();
  39. delete mp_execute_thread;
  40. mp_execute_thread = NULL;
  41. }
  42. m_dispatch_plc_status = Dispatch_plc::Dispatch_plc_status::DISPATCH_PLC_UNKNOW;
  43. return Error_code::SUCCESS;
  44. }
  45. //调度plc 执行请求
  46. Error_manager Dispatch_plc::execute_for_dispatch_request_msg(message::Dispatch_request_msg& dispatch_request_msg)
  47. {
  48. if ( m_dispatch_plc_status == Dispatch_plc::Dispatch_plc_status::DISPATCH_PLC_READY )
  49. {
  50. std::unique_lock<std::mutex> t_lock(m_lock);
  51. //设定超时时间, 默认比任务指令里面的时间少10秒,
  52. if ( dispatch_request_msg.base_info().has_timeout_ms() )
  53. {
  54. m_timeout_ms = dispatch_request_msg.base_info().timeout_ms() - DISPATCH_PROCESS_ATTENUATION_TIMEOUT_MS;
  55. }
  56. else
  57. {
  58. m_timeout_ms = DISPATCH_PROCESS_TIMEOUT_MS - DISPATCH_PROCESS_ATTENUATION_TIMEOUT_MS;
  59. }
  60. m_command_key = dispatch_request_msg.command_key();
  61. m_start_time = std::chrono::system_clock::now();
  62. m_dispatch_request_msg = dispatch_request_msg;
  63. // if ( dispatch_request_msg.dispatch_motion_direction() == message::Dispatch_motion_direction::E_STORE_CAR )
  64. // {
  65. // m_dispatch_process_type = Common_data::Dispatch_process_type::DISPATCH_PROCESS_STORE;
  66. // }
  67. // else if ( dispatch_request_msg.dispatch_motion_direction() == message::Dispatch_motion_direction::E_PICKUP_CAR )
  68. // {
  69. // m_dispatch_process_type = Common_data::Dispatch_process_type::DISPATCH_PROCESS_PICKUP;
  70. // }
  71. // else
  72. // {
  73. // return Error_manager(Error_code::DISPATCH_PLC_REQUEST_ERROR, Error_level::MINOR_ERROR,
  74. // " dispatch_request_msg.dispatch_motion_direction() PARAMRTER ERROR ");
  75. // }
  76. m_dispatch_process_type = Common_data::Dispatch_process_type::DISPATCH_PROCESS_TYPE_UNKNOW;
  77. m_dispatch_plc_status = Dispatch_plc::Dispatch_plc_status::DISPATCH_PLC_REQUEST;
  78. }
  79. else
  80. {
  81. return Error_manager(Error_code::DISPATCH_PLC_STATUS_ERROR, Error_level::MINOR_ERROR,
  82. " m_dispatch_plc_status error ");
  83. }
  84. return Error_code::SUCCESS;
  85. }
  86. Dispatch_plc::Dispatch_plc_status Dispatch_plc::get_dispatch_plc_status()
  87. {
  88. return m_dispatch_plc_status;
  89. }
  90. message::Dispatch_request_msg Dispatch_plc::get_dispatch_request_msg()
  91. {
  92. std::unique_lock<std::mutex> t_lock(m_lock);
  93. return m_dispatch_request_msg;
  94. }
  95. void Dispatch_plc::clear_request_msg()
  96. {
  97. m_request_key.clear(); //请求唯一码, 用作识别
  98. m_request_status = 0; //请求确认标志
  99. // 调度指令的起点,终点,方向
  100. m_request_dispatch_motion_direction = Common_data::Dispatch_motion_direction::DISPATCH_MOTION_DIRECTION_UNKNOWN; //调度方向 0=未知,1=存车,2=取车
  101. m_request_passageway_id = 0; //出入口id 6个入口和6个出口
  102. m_request_passageway_direction = Common_data::Passageway_direction::PASSAGEWAY_DIRECTION_UNKNOWN; //出入口方向 0=未知,1=入口,2=出口
  103. m_request_parkingspace_index_id = 0; //楼上车位索引id 1~180(3*6*13)
  104. m_request_parkingspace_unit_id = 0; //楼上车位单元号 1~3
  105. m_request_parkingspace_floor_id = 0; //楼上车位楼层号 2~11
  106. m_request_parkingspace_room_id = 0; //楼上车位房间号 1~6
  107. m_request_parkingspace_direction = Common_data::Parkingspace_direction::PARKINGSPACE_DIRECTION_UNKNOWN; //楼上车位方向 0=未知,1=朝南,2=朝北
  108. //汽车的定位信息(地面雷达)
  109. m_request_car_center_x = 0; //整车的中心点x值, 四轮的中心, 单位:米 m
  110. m_request_car_center_y = 0; //整车的中心点y值, 四轮的中心, 单位:米 m
  111. m_request_car_angle = 0; //整车的车身旋转角, 单位:度 (-90~90)
  112. m_request_car_front_theta = 0; //整车的前轮的旋转角, 单位:度 (-90~90)
  113. m_request_car_length = 0; //整车的长度, 用于规避碰撞, 单位:米 m
  114. m_request_car_width = 0; //整车的宽度, 用于规避碰撞, 单位:米 m
  115. m_request_car_height = 0; //整车的高度, 用于规避碰撞, 单位:米 m
  116. m_request_car_wheel_base = 0; //整车的轮距, 前后轮的距离, 用于机器人或agv的抓车, 单位:米 m
  117. m_request_car_wheel_width = 0; //整车的轮距, 左右轮的距离, 用于机器人或agv的抓车, 单位:米 m
  118. m_request_car_license.clear(); //车牌号(汽车唯一标示) 例如: 鄂A12345
  119. m_request_car_type = Common_data::Car_type::UNKNOW_CAR_TYPE; //车的大小
  120. m_request_uniformed_car_x = 0; //转角复位后,车辆中心点x
  121. m_request_uniformed_car_y = 0; //转角复位后,车辆中心点y
  122. //防撞雷达
  123. m_request_anticollision_lidar_flag = Common_data::Anticollision_lidar_flag::ANTICOLLISION_LIDAR_UNKNOWN; //汽车是否停到正确的位置, 防撞雷达 预留, 楚天暂时不用,0=未知,1=位置正常,2=位置异常
  124. //轮距雷达
  125. m_request_car_wheel_base_exact_value = 0; //汽车前后轮距,轮距雷达的精确值 预留, 楚天暂时不用,单位:米 m
  126. }
  127. //jiancha qingqiu xiaoxi
  128. Error_manager Dispatch_plc::check_dispatch_request_msg(message::Dispatch_request_msg & dispatch_request_msg)
  129. {
  130. Error_manager t_error;
  131. if(dispatch_request_msg.has_locate_information() &&
  132. dispatch_request_msg.has_dispatch_motion_direction() &&
  133. dispatch_request_msg.has_car_type() &&
  134. dispatch_request_msg.has_id_struct() &&
  135. dispatch_request_msg.has_command_key())
  136. {
  137. if ( m_dispatch_request_msg.dispatch_motion_direction() == message::Dispatch_motion_direction::E_STORE_CAR )
  138. {
  139. if ( dispatch_request_msg.id_struct().has_unit_id() )
  140. {
  141. }
  142. }
  143. else if( m_dispatch_request_msg.dispatch_motion_direction() == message::Dispatch_motion_direction::E_PICKUP_CAR )
  144. return Error_code::SUCCESS;
  145. }
  146. else
  147. {
  148. return Error_manager(Error_code::DISPATCH_PLC_REQUEST_ERROR, Error_level::MINOR_ERROR,
  149. " m_dispatch_plc_status error ");
  150. }
  151. return Error_code::SUCCESS;
  152. }
  153. //执行外界任务的执行函数
  154. void Dispatch_plc::execute_thread_fun()
  155. {
  156. LOG(INFO) << " Dispatch_plc::execute_thread_fun() start " << this;
  157. Error_manager t_error;
  158. while (m_execute_condition.is_alive())
  159. {
  160. m_execute_condition.wait();
  161. if (m_execute_condition.is_alive())
  162. {
  163. std::this_thread::sleep_for(std::chrono::microseconds(1));
  164. std::this_thread::sleep_for(std::chrono::seconds(1));
  165. std::this_thread::yield();
  166. // std::unique_lock<std::mutex> t_lock(m_lock);
  167. static int t_count = 0;
  168. if (t_count % 10 == 0 )
  169. {
  170. std::cout << " huli test :::: " << " m_plc_id = " << m_plc_id << std::endl;
  171. std::cout << " huli test :::: " << " m_dispatch_plc_status = " << m_dispatch_plc_status << std::endl;
  172. }
  173. t_count++;
  174. switch ((Dispatch_plc_status) m_dispatch_plc_status)
  175. {
  176. case DISPATCH_PLC_READY:
  177. {
  178. if ( m_dispatch_process_type == Common_data::Dispatch_process_type::DISPATCH_PROCESS_TYPE_UNKNOW )
  179. {
  180. update_dispatch_plc_communication();
  181. }
  182. else
  183. {
  184. m_result = check_dispatch_request_msg(m_dispatch_request_msg);
  185. if(m_result == Error_code::SUCCESS)
  186. {
  187. m_dispatch_plc_status = DISPATCH_PLC_REQUEST;
  188. }
  189. else
  190. {
  191. m_dispatch_plc_status = DISPATCH_PLC_RESPONSE;
  192. }
  193. }
  194. break;
  195. }
  196. case DISPATCH_PLC_REQUEST://给plc发送请求
  197. {
  198. if ( std::chrono::system_clock::now() - m_start_time > std::chrono::milliseconds(m_timeout_ms) )
  199. {
  200. //超时直接报错
  201. m_result = Error_manager(Error_code::DISPATCH_PLC_TIME_OUT, Error_level::MINOR_ERROR,
  202. " DISPATCH_PLC_TIME_OUT fun error ");
  203. m_dispatch_plc_status = DISPATCH_PLC_RESPONSE;
  204. }
  205. else
  206. {
  207. //封装 给plc的调度请求
  208. m_result = encapsulate_dispatch_request_to_plc();
  209. if ( m_result == Error_code::SUCCESS )
  210. {
  211. m_dispatch_plc_status = DISPATCH_PLC_WORKING;
  212. }
  213. else
  214. {
  215. m_dispatch_plc_status = DISPATCH_PLC_RESPONSE;
  216. }
  217. }
  218. break;
  219. }
  220. case DISPATCH_PLC_WORKING:
  221. {
  222. if ( std::chrono::system_clock::now() - m_start_time > std::chrono::milliseconds(m_timeout_ms) )
  223. {
  224. //超时直接报错
  225. m_result = Error_manager(Error_code::DISPATCH_PLC_TIME_OUT, Error_level::MINOR_ERROR,
  226. " DISPATCH_PLC_TIME_OUT fun error ");
  227. m_dispatch_plc_status = DISPATCH_PLC_RESPONSE;
  228. }
  229. else
  230. {
  231. update_dispatch_plc_communication();
  232. //检查plc答复反馈
  233. m_result = check_response_from_plc();
  234. if ( m_result == Error_code::SUCCESS )
  235. {
  236. m_dispatch_plc_status = DISPATCH_PLC_RESPONSE;
  237. }
  238. else if ( m_result != Error_code::NODATA )
  239. {
  240. // m_dispatch_plc_status = DISPATCH_PLC_FAULT;
  241. m_dispatch_plc_status = DISPATCH_PLC_RESPONSE;
  242. }
  243. //else 原地等待
  244. }
  245. break;
  246. }
  247. case DISPATCH_PLC_RESPONSE:
  248. {
  249. //20211213. huli m_request_status = 0 after over process
  250. m_request_status = 0;
  251. clear_request_msg();
  252. //发送调度答复信息给主控
  253. send_dispatch_response_to_main_control();
  254. //无论前面的流程是否正确,都要给主控答复, 反馈里面填充错误码即可.
  255. if ( m_result == Error_code::SUCCESS )
  256. {
  257. //response add to _map
  258. if ( m_dispatch_request_msg.dispatch_motion_direction() == message::Dispatch_motion_direction::E_STORE_CAR )
  259. {
  260. std::unique_lock<std::mutex> t_lock(Dispatch_manager::get_instance_references().m_lock);
  261. std::chrono::system_clock::time_point t_time = std::chrono::system_clock::now();
  262. Dispatch_manager::get_instance_references().m_dispatch_response_store_map[t_time] = m_dispatch_response_msg;
  263. Dispatch_manager::get_instance_references().m_store_updata_time = std::chrono::system_clock::now();
  264. }
  265. else if( m_dispatch_request_msg.dispatch_motion_direction() == message::Dispatch_motion_direction::E_PICKUP_CAR )
  266. {
  267. std::unique_lock<std::mutex> t_lock(Dispatch_manager::get_instance_references().m_lock);
  268. Dispatch_manager::get_instance_references().m_dispatch_response_pickup_map[m_outlet_id] = m_dispatch_response_msg;
  269. Dispatch_manager::get_instance_references().m_pickup_updata_time = std::chrono::system_clock::now();
  270. }
  271. //流程正常结束, 恢复到待机状态.
  272. m_dispatch_plc_status = DISPATCH_PLC_READY;
  273. m_dispatch_process_type = Common_data::Dispatch_process_type::DISPATCH_PROCESS_TYPE_UNKNOW;
  274. m_result.error_manager_clear_all();
  275. }
  276. else
  277. {
  278. //流程异常结束, 进入到故障状态.
  279. m_dispatch_plc_status = DISPATCH_PLC_FAULT;
  280. }
  281. break;
  282. }
  283. case DISPATCH_PLC_FAULT:
  284. {
  285. //20211213. huli m_request_status = 0 after over process
  286. m_request_status = 0;
  287. clear_request_msg();
  288. //不在接受新的任务,但是保持基本的通信正常
  289. update_dispatch_plc_communication();
  290. //response add to _map
  291. if ( m_dispatch_request_msg.dispatch_motion_direction() == message::Dispatch_motion_direction::E_STORE_CAR )
  292. {
  293. std::unique_lock<std::mutex> t_lock(Dispatch_manager::get_instance_references().m_lock);
  294. std::chrono::system_clock::time_point t_time = std::chrono::system_clock::now();
  295. Dispatch_manager::get_instance_references().m_dispatch_response_store_map[t_time] = m_dispatch_response_msg;
  296. Dispatch_manager::get_instance_references().m_store_updata_time = std::chrono::system_clock::now();
  297. }
  298. else if( m_dispatch_request_msg.dispatch_motion_direction() == message::Dispatch_motion_direction::E_PICKUP_CAR )
  299. {
  300. std::unique_lock<std::mutex> t_lock(Dispatch_manager::get_instance_references().m_lock);
  301. Dispatch_manager::get_instance_references().m_dispatch_response_pickup_map[m_outlet_id] = m_dispatch_response_msg;
  302. Dispatch_manager::get_instance_references().m_pickup_updata_time = std::chrono::system_clock::now();
  303. }
  304. //20211209 huli //流程异常结束, 进入到 ready.
  305. m_dispatch_plc_status = DISPATCH_PLC_READY;
  306. m_dispatch_process_type = Common_data::Dispatch_process_type::DISPATCH_PROCESS_TYPE_UNKNOW;
  307. m_result.error_manager_clear_all();
  308. LOG(ERROR) << " Dispatch_plc::execute_thread_fun() dispatch_plc is fault, now recover to normal " << this;
  309. break;
  310. }
  311. }
  312. }
  313. }
  314. }
  315. //封装 给plc的调度请求
  316. Error_manager Dispatch_plc::encapsulate_dispatch_request_to_plc()
  317. {
  318. //把m_dispatch_request_msg的信息传给本地的数据缓存.
  319. std::unique_lock<std::mutex> t_lock(m_lock);
  320. m_request_key = m_dispatch_request_msg.command_key();
  321. m_request_status = 1;
  322. if ( m_dispatch_request_msg.dispatch_motion_direction() == message::Dispatch_motion_direction::E_STORE_CAR )
  323. {
  324. m_request_dispatch_motion_direction = Common_data::Dispatch_motion_direction::DISPATCH_MOTION_DIRECTION_STORE;
  325. m_request_passageway_direction = Common_data::Passageway_direction::PASSAGEWAY_DIRECTION_INLET;
  326. m_request_passageway_id = m_dispatch_request_msg.mutable_id_struct()->terminal_id()+1;//0~5入口终端号改为1~6
  327. }
  328. else if( m_dispatch_request_msg.dispatch_motion_direction() == message::Dispatch_motion_direction::E_PICKUP_CAR )
  329. {
  330. m_request_dispatch_motion_direction = Common_data::Dispatch_motion_direction::DISPATCH_MOTION_DIRECTION_PICKUP;
  331. m_request_passageway_direction = Common_data::Passageway_direction::PASSAGEWAY_DIRECTION_OUTLET;
  332. m_request_passageway_id = m_dispatch_request_msg.mutable_id_struct()->terminal_id()+1;//0~5入口终端号改为1~6
  333. }
  334. else
  335. {
  336. m_request_dispatch_motion_direction = Common_data::Dispatch_motion_direction::DISPATCH_MOTION_DIRECTION_UNKNOWN;
  337. m_request_passageway_direction = Common_data::Passageway_direction::PASSAGEWAY_DIRECTION_UNKNOWN;
  338. m_request_passageway_id = -1;
  339. }
  340. if ( m_dispatch_request_msg.locate_information().locate_correct() )
  341. {
  342. m_request_car_center_x = m_dispatch_request_msg.locate_information().locate_x();
  343. m_request_car_center_y = m_dispatch_request_msg.locate_information().locate_y();
  344. #ifdef MEASURE_TO_PLC_CORRECTION
  345. m_request_car_angle = m_dispatch_request_msg.locate_information().locate_angle()-90+0.3;//80~100改为-10~+10
  346. #endif
  347. #ifndef MEASURE_TO_PLC_CORRECTION
  348. m_request_car_angle = m_dispatch_request_msg.locate_information().locate_angle();
  349. #endif
  350. m_request_car_front_theta = m_dispatch_request_msg.locate_information().locate_front_theta();
  351. m_request_car_length = m_dispatch_request_msg.locate_information().locate_length();
  352. m_request_car_width = m_dispatch_request_msg.locate_information().locate_width();
  353. m_request_car_height = m_dispatch_request_msg.locate_information().locate_height();
  354. m_request_car_wheel_base = m_dispatch_request_msg.locate_information().locate_wheel_base();
  355. m_request_car_wheel_width = m_dispatch_request_msg.locate_information().locate_wheel_width();
  356. m_request_uniformed_car_x = m_dispatch_request_msg.locate_information().uniformed_car_x();
  357. m_request_uniformed_car_x = m_dispatch_request_msg.locate_information().uniformed_car_y();
  358. }
  359. else
  360. {
  361. m_request_car_center_x = m_dispatch_request_msg.locate_information().locate_x();
  362. m_request_car_center_y = m_dispatch_request_msg.locate_information().locate_y();
  363. #ifdef MEASURE_TO_PLC_CORRECTION
  364. m_request_car_angle = m_dispatch_request_msg.locate_information().locate_angle()-90+0.3;//80~100改为-10~+10
  365. #endif
  366. #ifndef MEASURE_TO_PLC_CORRECTION
  367. m_request_car_angle = m_dispatch_request_msg.locate_information().locate_angle();
  368. #endif
  369. m_request_car_front_theta = m_dispatch_request_msg.locate_information().locate_front_theta();
  370. m_request_car_length = m_dispatch_request_msg.locate_information().locate_length();
  371. m_request_car_width = m_dispatch_request_msg.locate_information().locate_width();
  372. m_request_car_height = m_dispatch_request_msg.locate_information().locate_height();
  373. m_request_car_wheel_base = m_dispatch_request_msg.locate_information().locate_wheel_base();
  374. m_request_car_wheel_width = m_dispatch_request_msg.locate_information().locate_wheel_width();
  375. m_request_uniformed_car_x = m_dispatch_request_msg.locate_information().uniformed_car_x();
  376. m_request_uniformed_car_x = m_dispatch_request_msg.locate_information().uniformed_car_y();
  377. }
  378. if ( m_dispatch_request_msg.parkspace_info_ex_size() ==1 )
  379. {
  380. m_request_parkingspace_index_id = m_dispatch_request_msg.parkspace_info_ex(0).parkingspace_index_id();
  381. m_request_parkingspace_unit_id = m_dispatch_request_msg.parkspace_info_ex(0).parkingspace_unit_id();
  382. m_request_parkingspace_floor_id = m_dispatch_request_msg.parkspace_info_ex(0).parkingspace_floor_id();
  383. m_request_parkingspace_room_id = m_dispatch_request_msg.parkspace_info_ex(0).parkingspace_room_id();
  384. //车位朝向, 小号朝前朝南, 大号朝后朝北
  385. m_request_parkingspace_direction = (Common_data::Parkingspace_direction)m_dispatch_request_msg.parkspace_info_ex(0).parkingspace_direction();
  386. //20211220, utf-8 -> GBK
  387. m_request_car_license = String_convert::utf8_to_gbk( m_dispatch_request_msg.parkspace_info_ex(0).car_info().car_numberplate() );
  388. m_request_car_type = (Common_data::Car_type)m_dispatch_request_msg.parkspace_info_ex(0).car_type();
  389. //20211207, huli add wheel_base for pickup_car
  390. if( m_dispatch_request_msg.dispatch_motion_direction() == message::Dispatch_motion_direction::E_PICKUP_CAR )
  391. {
  392. m_request_car_wheel_base = m_dispatch_request_msg.parkspace_info_ex(0).car_info().car_wheel_base();
  393. m_request_car_wheel_width = m_dispatch_request_msg.parkspace_info_ex(0).car_info().car_wheel_width();
  394. }
  395. }
  396. else
  397. {
  398. return Error_manager(Error_code::DISPATCH_PLC_REQUEST_ERROR, Error_level::MINOR_ERROR,
  399. " m_dispatch_request_msg.parkspace_info_ex_size() !=1 error ");
  400. }
  401. return Error_code::SUCCESS;
  402. }
  403. //更新plc通信
  404. Error_manager Dispatch_plc::update_dispatch_plc_communication()
  405. {
  406. std::unique_lock<std::mutex> t_lock(m_lock);
  407. std::unique_lock<std::mutex> t_lock1(Dispatch_communication::get_instance_references().m_data_lock);
  408. /*
  409. //请求消息, 调度->plc
  410. Dispatch_communication::Dispatch_request_from_manager_to_plc_for_data * tp_dispatch_request_from_manager_to_plc_for_data =
  411. & Dispatch_communication::get_instance_references().m_dispatch_request_from_manager_to_plc_for_data;
  412. Dispatch_communication::Dispatch_request_from_manager_to_plc_for_key * m_dispatch_request_from_manager_to_plc_for_key =
  413. & Dispatch_communication::get_instance_references().m_dispatch_request_from_manager_to_plc_for_key;
  414. memset(m_dispatch_request_from_manager_to_plc_for_key->m_request_key, 0, 50);
  415. int t_size1 = m_request_key.size()<=50 ? m_request_key.size() : 50 ;
  416. m_request_key = "123456789";
  417. memcpy(m_dispatch_request_from_manager_to_plc_for_key->m_request_key, m_request_key.c_str(), t_size1);
  418. static unsigned char index = 0;
  419. index++;
  420. tp_dispatch_request_from_manager_to_plc_for_data->m_request_dispatch_motion_direction = index;
  421. tp_dispatch_request_from_manager_to_plc_for_data->m_request_passageway_id = 02;
  422. tp_dispatch_request_from_manager_to_plc_for_data->m_request_passageway_direction = 03;
  423. tp_dispatch_request_from_manager_to_plc_for_data->m_request_parkingspace_index_id = 04;
  424. tp_dispatch_request_from_manager_to_plc_for_data->m_request_parkingspace_unit_id = 05;
  425. tp_dispatch_request_from_manager_to_plc_for_data->m_request_parkingspace_floor_id = 6;
  426. tp_dispatch_request_from_manager_to_plc_for_data->m_request_parkingspace_room_id = 7;
  427. tp_dispatch_request_from_manager_to_plc_for_data->m_request_parkingspace_direction = 8;
  428. tp_dispatch_request_from_manager_to_plc_for_data->m_request_car_center_x = 11;
  429. tp_dispatch_request_from_manager_to_plc_for_data->m_request_car_center_y = 12;
  430. tp_dispatch_request_from_manager_to_plc_for_data->m_request_car_angle = 13;
  431. tp_dispatch_request_from_manager_to_plc_for_data->m_request_car_front_theta = 14;
  432. tp_dispatch_request_from_manager_to_plc_for_data->m_request_car_length = 15;
  433. tp_dispatch_request_from_manager_to_plc_for_data->m_request_car_width = 16;
  434. tp_dispatch_request_from_manager_to_plc_for_data->m_request_car_height = 17;
  435. tp_dispatch_request_from_manager_to_plc_for_data->m_request_car_wheel_base = 18;
  436. tp_dispatch_request_from_manager_to_plc_for_data->m_request_car_wheel_width = 19;
  437. memset(tp_dispatch_request_from_manager_to_plc_for_data->m_request_car_license, 0, 20);
  438. int t_size2 = m_request_key.size()<=20 ? m_request_key.size() : 20 ;
  439. m_request_car_license = "ABCDEF";
  440. memcpy(tp_dispatch_request_from_manager_to_plc_for_data->m_request_car_license, m_request_car_license.c_str(), t_size2);
  441. tp_dispatch_request_from_manager_to_plc_for_data->m_request_car_type = 33;
  442. tp_dispatch_request_from_manager_to_plc_for_data->m_request_anticollision_lidar_flag = 44;
  443. tp_dispatch_request_from_manager_to_plc_for_data->m_request_car_wheel_base_exact_value = 55;
  444. */
  445. //请求消息, 调度->plc
  446. Dispatch_communication::Dispatch_request_from_manager_to_plc_for_data * tp_dispatch_request_from_manager_to_plc_for_data =
  447. & Dispatch_communication::get_instance_references().m_dispatch_request_from_manager_to_plc_for_data;
  448. Dispatch_communication::Dispatch_request_from_manager_to_plc_for_key * m_dispatch_request_from_manager_to_plc_for_key =
  449. & Dispatch_communication::get_instance_references().m_dispatch_request_from_manager_to_plc_for_key;
  450. memset(m_dispatch_request_from_manager_to_plc_for_key->m_request_key, 0, 50);
  451. int t_size1 = m_request_key.size()<=50 ? m_request_key.size() : 50 ;
  452. memcpy(m_dispatch_request_from_manager_to_plc_for_key->m_request_key, m_request_key.c_str(), t_size1);
  453. tp_dispatch_request_from_manager_to_plc_for_data->m_request_status = m_request_status;
  454. tp_dispatch_request_from_manager_to_plc_for_data->m_request_dispatch_motion_direction = m_request_dispatch_motion_direction;
  455. tp_dispatch_request_from_manager_to_plc_for_data->m_request_passageway_id = m_request_passageway_id;
  456. tp_dispatch_request_from_manager_to_plc_for_data->m_request_passageway_direction = m_request_passageway_direction;
  457. tp_dispatch_request_from_manager_to_plc_for_data->m_request_parkingspace_index_id = m_request_parkingspace_index_id;
  458. tp_dispatch_request_from_manager_to_plc_for_data->m_request_parkingspace_unit_id = m_request_parkingspace_unit_id;
  459. tp_dispatch_request_from_manager_to_plc_for_data->m_request_parkingspace_floor_id = m_request_parkingspace_floor_id;
  460. tp_dispatch_request_from_manager_to_plc_for_data->m_request_parkingspace_room_id = m_request_parkingspace_room_id;
  461. tp_dispatch_request_from_manager_to_plc_for_data->m_request_parkingspace_direction = m_request_parkingspace_direction;
  462. tp_dispatch_request_from_manager_to_plc_for_data->m_request_car_center_x = m_request_car_center_x;
  463. tp_dispatch_request_from_manager_to_plc_for_data->m_request_car_center_y = m_request_car_center_y;
  464. tp_dispatch_request_from_manager_to_plc_for_data->m_request_car_angle = m_request_car_angle;
  465. tp_dispatch_request_from_manager_to_plc_for_data->m_request_car_front_theta = m_request_car_front_theta;
  466. tp_dispatch_request_from_manager_to_plc_for_data->m_request_car_length = m_request_car_length;
  467. tp_dispatch_request_from_manager_to_plc_for_data->m_request_car_width = m_request_car_width;
  468. tp_dispatch_request_from_manager_to_plc_for_data->m_request_car_height = m_request_car_height;
  469. tp_dispatch_request_from_manager_to_plc_for_data->m_request_car_wheel_base = m_request_car_wheel_base;
  470. tp_dispatch_request_from_manager_to_plc_for_data->m_request_car_wheel_width = m_request_car_wheel_width;
  471. memset(tp_dispatch_request_from_manager_to_plc_for_data->m_request_car_license, 0, 20);
  472. tp_dispatch_request_from_manager_to_plc_for_data->m_request_car_license[0] = 18;
  473. tp_dispatch_request_from_manager_to_plc_for_data->m_request_car_license[1] = m_request_car_license.length();
  474. unsigned char * p_ch = tp_dispatch_request_from_manager_to_plc_for_data->m_request_car_license;
  475. memcpy(p_ch+2, m_request_car_license.c_str(), m_request_car_license.length());
  476. tp_dispatch_request_from_manager_to_plc_for_data->m_request_car_type = m_request_car_type;
  477. tp_dispatch_request_from_manager_to_plc_for_data->m_request_uniformed_car_x = m_request_uniformed_car_x;
  478. tp_dispatch_request_from_manager_to_plc_for_data->m_request_uniformed_car_y = m_request_uniformed_car_y;
  479. tp_dispatch_request_from_manager_to_plc_for_data->m_request_anticollision_lidar_flag = m_request_anticollision_lidar_flag;
  480. tp_dispatch_request_from_manager_to_plc_for_data->m_request_car_wheel_base_exact_value = m_request_car_wheel_base_exact_value;
  481. //答复消息, plc->调度
  482. Dispatch_communication::Dispatch_response_from_plc_to_manager * tp_dispatch_response_from_plc_to_manager =
  483. & Dispatch_communication::get_instance_references().m_dispatch_response_from_plc_to_manager;
  484. m_response_key = (char*) tp_dispatch_response_from_plc_to_manager->m_response_key;
  485. m_response_status = (Dispatch_plc::Response_status)tp_dispatch_response_from_plc_to_manager->m_response_status;
  486. m_response_dispatch_motion_direction = (Common_data::Dispatch_motion_direction)tp_dispatch_response_from_plc_to_manager->m_response_dispatch_motion_direction;
  487. m_response_passageway_id = tp_dispatch_response_from_plc_to_manager->m_response_passageway_id;
  488. m_response_passageway_direction = (Common_data::Passageway_direction)tp_dispatch_response_from_plc_to_manager->m_response_passageway_direction;
  489. m_response_parkingspace_index_id = tp_dispatch_response_from_plc_to_manager->m_response_parkingspace_index_id;
  490. m_response_parkingspace_unit_id = tp_dispatch_response_from_plc_to_manager->m_response_parkingspace_unit_id;
  491. m_response_parkingspace_floor_id = tp_dispatch_response_from_plc_to_manager->m_response_parkingspace_floor_id;
  492. m_response_parkingspace_room_id = tp_dispatch_response_from_plc_to_manager->m_response_parkingspace_room_id;
  493. m_response_parkingspace_direction = (Common_data::Parkingspace_direction)tp_dispatch_response_from_plc_to_manager->m_response_parkingspace_direction;
  494. m_response_car_center_x = tp_dispatch_response_from_plc_to_manager->m_response_car_center_x;
  495. m_response_car_center_y = tp_dispatch_response_from_plc_to_manager->m_response_car_center_y;
  496. m_response_car_angle = tp_dispatch_response_from_plc_to_manager->m_response_car_angle;
  497. m_response_car_front_theta = tp_dispatch_response_from_plc_to_manager->m_response_car_front_theta;
  498. m_response_car_length = tp_dispatch_response_from_plc_to_manager->m_response_car_length;
  499. m_response_car_width = tp_dispatch_response_from_plc_to_manager->m_response_car_width;
  500. m_response_car_height = tp_dispatch_response_from_plc_to_manager->m_response_car_height;
  501. m_response_car_wheel_base = tp_dispatch_response_from_plc_to_manager->m_response_car_wheel_base;
  502. m_response_car_wheel_width = tp_dispatch_response_from_plc_to_manager->m_response_car_wheel_width;
  503. m_response_car_license = (char*) (tp_dispatch_response_from_plc_to_manager->m_response_car_license+2);
  504. m_response_car_type = (Common_data::Car_type) tp_dispatch_response_from_plc_to_manager->m_response_car_type;
  505. m_response_uniformed_car_x = tp_dispatch_response_from_plc_to_manager->m_response_uniformed_car_x;
  506. m_response_uniformed_car_y = tp_dispatch_response_from_plc_to_manager->m_response_uniformed_car_y;
  507. m_response_anticollision_lidar_flag = (Common_data::Anticollision_lidar_flag)tp_dispatch_response_from_plc_to_manager->m_response_anticollision_lidar_flag;
  508. m_response_car_wheel_base_exact_value = tp_dispatch_response_from_plc_to_manager->m_response_car_wheel_base_exact_value;
  509. /*
  510. std::cout << " huli test :::: " << " ------------------------------------------------------ = " << std::endl;
  511. std::cout << " huli test :::: " << " m_response_key = " << m_response_key << std::endl;
  512. std::cout << " huli test :::: " << " m_response_status = " << (int)m_response_status << std::endl;
  513. std::cout << " huli test :::: " << " m_response_dispatch_motion_direction = " << (int)m_response_dispatch_motion_direction << std::endl;
  514. std::cout << " huli test :::: " << " m_response_passageway_id = " << m_response_passageway_id << std::endl;
  515. std::cout << " huli test :::: " << " m_response_passageway_direction = " << (int)m_response_passageway_direction << std::endl;
  516. std::cout << " huli test :::: " << " m_response_parkingspace_index_id = " << m_response_parkingspace_index_id << std::endl;
  517. std::cout << " huli test :::: " << " m_response_parkingspace_unit_id = " << m_response_parkingspace_unit_id << std::endl;
  518. std::cout << " huli test :::: " << " m_response_parkingspace_floor_id = " << m_response_parkingspace_floor_id << std::endl;
  519. std::cout << " huli test :::: " << " m_response_parkingspace_room_id = " << m_response_parkingspace_room_id << std::endl;
  520. std::cout << " huli test :::: " << " m_response_parkingspace_direction = " << (int)m_response_parkingspace_direction << std::endl;
  521. std::cout << " huli test :::: " << " m_response_car_center_x = " << m_response_car_center_x << std::endl;
  522. std::cout << " huli test :::: " << " m_response_car_center_y = " << m_response_car_center_y << std::endl;
  523. std::cout << " huli test :::: " << " m_response_car_license = " << m_response_car_license << std::endl;
  524. */
  525. //状态消息, plc->调度
  526. Dispatch_communication::Dispatch_plc_status_from_plc_to_manager * tp_dispatch_plc_status_from_plc_to_manager =
  527. & Dispatch_communication::get_instance_references().m_dispatch_plc_status_from_plc_to_manager;
  528. //need
  529. // //通过心跳帧来判断通信是否正常
  530. // if ( m_last_heartbeat != tp_dispatch_plc_status_from_plc_to_manager->m_heartbeat )
  531. // {
  532. // m_last_heartbeat = tp_dispatch_plc_status_from_plc_to_manager->m_heartbeat;
  533. // m_status_updata_time = std::chrono::system_clock::now();
  534. //
  535. // if ( m_dispatch_plc_status == Dispatch_plc::Dispatch_plc_status::DISPATCH_PLC_DISCONNECT )
  536. // {
  537. // m_dispatch_plc_status = Dispatch_plc::Dispatch_plc_status::DISPATCH_PLC_READY;
  538. // }
  539. // }
  540. // else if(std::chrono::system_clock::now() - m_status_updata_time > std::chrono::milliseconds(COMMUNICATION_OVER_TIME_MS))
  541. // {
  542. // m_dispatch_plc_status = Dispatch_plc::Dispatch_plc_status::DISPATCH_PLC_DISCONNECT;
  543. // }
  544. // //else 继续等待,直到消息刷新或者超时.
  545. return Error_code::SUCCESS;
  546. }
  547. //检查plc答复反馈
  548. Error_manager Dispatch_plc::check_response_from_plc()
  549. {
  550. std::unique_lock<std::mutex> t_lock(m_lock);
  551. #ifndef WAIT_PLC_RESPONSE
  552. //need, 调试代码, 延时10s, 直接返回成功
  553. if ( std::chrono::system_clock::now() - m_start_time > std::chrono::seconds(20) )
  554. {
  555. return Error_code::SUCCESS;
  556. }
  557. //返回没有收到数据
  558. else
  559. {
  560. return Error_code::NODATA;
  561. }
  562. #endif
  563. #ifdef WAIT_PLC_RESPONSE
  564. //检查唯一码
  565. if ( m_response_key == m_request_key )
  566. {
  567. std::cout<< "m_response_status = " << m_response_status << std::endl;
  568. //如果故障,则添加错误码
  569. if ( m_response_status != RESPONS_OVER )
  570. {
  571. //添加错误码
  572. Error_manager t_error(DISPATCH_PLC_RESPONS_ERROR, MINOR_ERROR, " Dispatch_plc::check_response_from_plc() m_respons_status is error");
  573. return t_error;
  574. }
  575. return Error_code::SUCCESS;
  576. }
  577. //返回没有收到数据
  578. else
  579. {
  580. return Error_code::NODATA;
  581. }
  582. #endif
  583. return Error_code::SUCCESS;
  584. }
  585. //发送调度答复信息给主控
  586. Error_manager Dispatch_plc::send_dispatch_response_to_main_control()
  587. {
  588. std::unique_lock<std::mutex> t_lock(m_lock);
  589. m_dispatch_response_msg.mutable_base_info()->set_msg_type(message::Message_type::eDispatch_response_msg);
  590. m_dispatch_response_msg.mutable_base_info()->set_timeout_ms(m_dispatch_request_msg.base_info().timeout_ms());
  591. m_dispatch_response_msg.mutable_base_info()->set_sender(message::Communicator::eDispatch_manager);
  592. m_dispatch_response_msg.mutable_base_info()->set_receiver(message::Communicator::eMain);
  593. m_dispatch_response_msg.set_command_key(m_dispatch_request_msg.command_key());
  594. m_dispatch_response_msg.set_dispatch_motion_direction(m_dispatch_request_msg.dispatch_motion_direction());
  595. //need,need
  596. if ( m_dispatch_request_msg.dispatch_motion_direction() == message::Dispatch_motion_direction::E_STORE_CAR )
  597. {
  598. m_dispatch_response_msg.mutable_id_struct()->set_terminal_id(m_dispatch_request_msg.mutable_id_struct()->terminal_id());
  599. m_dispatch_response_msg.mutable_id_struct()->set_unit_id(m_dispatch_request_msg.mutable_id_struct()->terminal_id()/2);
  600. }
  601. else if( m_dispatch_request_msg.dispatch_motion_direction() == message::Dispatch_motion_direction::E_PICKUP_CAR )
  602. {
  603. m_dispatch_response_msg.mutable_id_struct()->set_terminal_id(m_dispatch_request_msg.mutable_id_struct()->unit_id()*2+m_outlet_id);
  604. m_dispatch_response_msg.mutable_id_struct()->set_unit_id(m_dispatch_request_msg.mutable_id_struct()->unit_id());
  605. }
  606. // m_dispatch_response_msg.set_terminal_id(m_dispatch_request_msg.terminal_id());
  607. m_dispatch_response_msg.mutable_locate_information()->CopyFrom(m_dispatch_request_msg.locate_information());
  608. m_dispatch_response_msg.mutable_parkspace_info_ex()->CopyFrom(m_dispatch_request_msg.parkspace_info_ex());
  609. m_dispatch_response_msg.set_car_type(m_dispatch_request_msg.car_type());
  610. if ( m_dispatch_response_msg.dispatch_motion_direction() == message::E_STORE_CAR )
  611. {
  612. for (int i = 0; i < m_dispatch_response_msg.parkspace_info_ex_size(); ++i)
  613. {
  614. if ( m_result == Error_code::SUCCESS )
  615. {
  616. m_dispatch_response_msg.mutable_parkspace_info_ex(i)->set_parkspace_status_target(message::Parkspace_status::eParkspace_occupied);
  617. }
  618. else
  619. {
  620. m_dispatch_response_msg.mutable_parkspace_info_ex(i)->set_parkspace_status_target(message::Parkspace_status::eParkspace_empty);
  621. }
  622. }
  623. }
  624. else if( m_dispatch_response_msg.dispatch_motion_direction() == message::E_PICKUP_CAR )
  625. {
  626. for (int i = 0; i < m_dispatch_response_msg.parkspace_info_ex_size(); ++i)
  627. {
  628. if ( m_result == Error_code::SUCCESS )
  629. {
  630. m_dispatch_response_msg.mutable_parkspace_info_ex(i)->set_parkspace_status_target(message::Parkspace_status::eParkspace_empty);
  631. }
  632. else
  633. {
  634. m_dispatch_response_msg.mutable_parkspace_info_ex(i)->set_parkspace_status_target(message::Parkspace_status::eParkspace_occupied);
  635. }
  636. }
  637. }
  638. m_dispatch_response_msg.mutable_error_manager()->set_error_code(m_result.get_error_code());
  639. m_dispatch_response_msg.mutable_error_manager()->set_error_level((message::Error_level)m_result.get_error_level());
  640. m_dispatch_response_msg.mutable_error_manager()->set_error_description(m_result.get_error_description());
  641. std::cout << " huli test :::: " << " m_dispatch_response_msg = " << m_dispatch_response_msg.DebugString()<< std::endl;
  642. std::string t_msg = m_dispatch_response_msg.SerializeAsString();
  643. System_communication::get_instance_references().encapsulate_msg(t_msg);
  644. return Error_code::SUCCESS;
  645. }