dispatch_process.cpp 133 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675676677678679680681682683684685686687688689690691692693694695696697698699700701702703704705706707708709710711712713714715716717718719720721722723724725726727728729730731732733734735736737738739740741742743744745746747748749750751752753754755756757758759760761762763764765766767768769770771772773774775776777778779780781782783784785786787788789790791792793794795796797798799800801802803804805806807808809810811812813814815816817818819820821822823824825826827828829830831832833834835836837838839840841842843844845846847848849850851852853854855856857858859860861862863864865866867868869870871872873874875876877878879880881882883884885886887888889890891892893894895896897898899900901902903904905906907908909910911912913914915916917918919920921922923924925926927928929930931932933934935936937938939940941942943944945946947948949950951952953954955956957958959960961962963964965966967968969970971972973974975976977978979980981982983984985986987988989990991992993994995996997998999100010011002100310041005100610071008100910101011101210131014101510161017101810191020102110221023102410251026102710281029103010311032103310341035103610371038103910401041104210431044104510461047104810491050105110521053105410551056105710581059106010611062106310641065106610671068106910701071107210731074107510761077107810791080108110821083108410851086108710881089109010911092109310941095109610971098109911001101110211031104110511061107110811091110111111121113111411151116111711181119112011211122112311241125112611271128112911301131113211331134113511361137113811391140114111421143114411451146114711481149115011511152115311541155115611571158115911601161116211631164116511661167116811691170117111721173117411751176117711781179118011811182118311841185118611871188118911901191119211931194119511961197119811991200120112021203120412051206120712081209121012111212121312141215121612171218121912201221122212231224122512261227122812291230123112321233123412351236123712381239124012411242124312441245124612471248124912501251125212531254125512561257125812591260126112621263126412651266126712681269127012711272127312741275127612771278127912801281128212831284128512861287128812891290129112921293129412951296129712981299130013011302130313041305130613071308130913101311131213131314131513161317131813191320132113221323132413251326132713281329133013311332133313341335133613371338133913401341134213431344134513461347134813491350135113521353135413551356135713581359136013611362136313641365136613671368136913701371137213731374137513761377137813791380138113821383138413851386138713881389139013911392139313941395139613971398139914001401140214031404140514061407140814091410141114121413141414151416141714181419142014211422142314241425142614271428142914301431143214331434143514361437143814391440144114421443144414451446144714481449145014511452145314541455145614571458145914601461146214631464146514661467146814691470147114721473147414751476147714781479148014811482148314841485148614871488148914901491149214931494149514961497149814991500150115021503150415051506150715081509151015111512151315141515151615171518151915201521152215231524152515261527152815291530153115321533153415351536153715381539154015411542154315441545154615471548154915501551155215531554155515561557155815591560156115621563156415651566156715681569157015711572157315741575157615771578157915801581158215831584158515861587158815891590159115921593159415951596159715981599160016011602160316041605160616071608160916101611161216131614161516161617161816191620162116221623162416251626162716281629163016311632163316341635163616371638163916401641164216431644164516461647164816491650165116521653165416551656165716581659166016611662166316641665166616671668166916701671167216731674167516761677167816791680168116821683168416851686168716881689169016911692169316941695169616971698169917001701170217031704170517061707170817091710171117121713171417151716171717181719172017211722172317241725172617271728172917301731173217331734173517361737173817391740174117421743174417451746174717481749175017511752175317541755175617571758175917601761176217631764176517661767176817691770177117721773177417751776177717781779178017811782178317841785178617871788178917901791179217931794179517961797179817991800180118021803180418051806180718081809181018111812181318141815181618171818181918201821182218231824182518261827182818291830183118321833183418351836183718381839184018411842184318441845184618471848184918501851185218531854185518561857185818591860186118621863186418651866186718681869187018711872187318741875187618771878187918801881188218831884188518861887188818891890189118921893189418951896189718981899190019011902190319041905190619071908190919101911191219131914191519161917191819191920192119221923192419251926192719281929193019311932193319341935193619371938193919401941194219431944194519461947194819491950195119521953195419551956195719581959196019611962196319641965196619671968196919701971197219731974197519761977197819791980198119821983198419851986198719881989199019911992199319941995199619971998199920002001200220032004200520062007200820092010201120122013201420152016201720182019202020212022202320242025202620272028202920302031203220332034203520362037203820392040204120422043204420452046204720482049205020512052205320542055205620572058205920602061206220632064206520662067206820692070207120722073207420752076207720782079208020812082208320842085208620872088208920902091209220932094209520962097209820992100210121022103210421052106210721082109211021112112211321142115211621172118211921202121212221232124212521262127212821292130213121322133213421352136213721382139214021412142214321442145214621472148214921502151215221532154215521562157215821592160216121622163216421652166216721682169217021712172217321742175217621772178217921802181218221832184218521862187218821892190219121922193219421952196219721982199220022012202220322042205220622072208220922102211221222132214221522162217221822192220222122222223222422252226222722282229223022312232223322342235223622372238223922402241224222432244224522462247224822492250225122522253225422552256225722582259226022612262226322642265226622672268226922702271227222732274227522762277227822792280228122822283228422852286228722882289229022912292229322942295229622972298229923002301230223032304230523062307230823092310231123122313231423152316231723182319232023212322232323242325232623272328232923302331233223332334233523362337233823392340234123422343234423452346234723482349235023512352235323542355235623572358235923602361236223632364236523662367236823692370237123722373237423752376237723782379238023812382238323842385238623872388238923902391239223932394239523962397239823992400240124022403240424052406240724082409241024112412241324142415241624172418241924202421242224232424242524262427242824292430243124322433243424352436243724382439244024412442244324442445244624472448244924502451245224532454245524562457245824592460246124622463246424652466246724682469247024712472247324742475247624772478247924802481248224832484248524862487248824892490249124922493249424952496249724982499250025012502250325042505250625072508250925102511251225132514251525162517251825192520252125222523252425252526252725282529253025312532253325342535253625372538253925402541254225432544254525462547254825492550255125522553255425552556255725582559256025612562256325642565256625672568256925702571257225732574257525762577257825792580258125822583258425852586258725882589259025912592259325942595259625972598259926002601260226032604260526062607260826092610261126122613261426152616261726182619262026212622262326242625262626272628262926302631263226332634263526362637263826392640264126422643264426452646264726482649265026512652265326542655265626572658265926602661266226632664266526662667266826692670267126722673267426752676267726782679268026812682268326842685268626872688268926902691269226932694269526962697269826992700270127022703270427052706270727082709271027112712271327142715271627172718271927202721272227232724272527262727272827292730273127322733273427352736273727382739274027412742274327442745274627472748274927502751275227532754275527562757275827592760276127622763276427652766276727682769277027712772277327742775277627772778277927802781278227832784278527862787278827892790279127922793279427952796279727982799280028012802280328042805280628072808280928102811281228132814281528162817281828192820282128222823282428252826282728282829283028312832283328342835283628372838283928402841284228432844284528462847284828492850285128522853285428552856285728582859286028612862286328642865286628672868286928702871287228732874287528762877287828792880288128822883288428852886288728882889289028912892289328942895289628972898289929002901290229032904290529062907290829092910291129122913291429152916291729182919292029212922292329242925292629272928292929302931293229332934293529362937293829392940294129422943294429452946294729482949295029512952295329542955295629572958295929602961296229632964296529662967296829692970297129722973297429752976297729782979298029812982298329842985298629872988298929902991299229932994299529962997299829993000300130023003300430053006300730083009301030113012301330143015301630173018301930203021302230233024302530263027302830293030303130323033303430353036303730383039304030413042304330443045304630473048304930503051305230533054305530563057305830593060306130623063306430653066306730683069307030713072307330743075307630773078307930803081308230833084308530863087308830893090309130923093309430953096309730983099310031013102310331043105310631073108310931103111311231133114311531163117311831193120312131223123312431253126312731283129313031313132313331343135313631373138313931403141314231433144314531463147314831493150315131523153315431553156
  1. //
  2. // Created by huli on 2021/3/22.
  3. //
  4. #include "dispatch_process.h"
  5. #include "../system/system_communication.h"
  6. #include "../dispatch/dispatch_manager.h"
  7. Dispatch_process::Dispatch_process()
  8. {
  9. m_dispatch_process_status = DISPATCH_PROCESS_STATUS_UNKNOW;
  10. m_dispatch_process_type = Common_data::Dispatch_process_type::DISPATCH_PROCESS_TYPE_UNKNOW;
  11. m_dispatch_source = 0;
  12. m_dispatch_destination = 0;
  13. m_wheel_base = 0;
  14. }
  15. Dispatch_process::~Dispatch_process()
  16. {
  17. Dispatch_process_uninit();
  18. }
  19. //初始化, 就把主控发送的请求传入即可.
  20. Error_manager Dispatch_process::Dispatch_process_init(message::Dispatch_request_msg &dispatch_request_msg)
  21. {
  22. //设定超时时间, 默认比任务指令里面的时间少10秒,
  23. if ( dispatch_request_msg.base_info().has_timeout_ms() )
  24. {
  25. m_timeout_ms = dispatch_request_msg.base_info().timeout_ms() - DISPATCH_PROCESS_ATTENUATION_TIMEOUT_MS;
  26. }
  27. else
  28. {
  29. m_timeout_ms = DISPATCH_PROCESS_TIMEOUT_MS - DISPATCH_PROCESS_ATTENUATION_TIMEOUT_MS;
  30. }
  31. m_command_key = dispatch_request_msg.command_key();
  32. m_start_time = std::chrono::system_clock::now();
  33. //检查调度请求消息
  34. if ( dispatch_request_msg.terminal_id() < Dispatch_coordinates::get_instance_references().m_passageway_terminal_id_min ||
  35. dispatch_request_msg.terminal_id() > Dispatch_coordinates::get_instance_references().m_passageway_terminal_id_max)
  36. {
  37. return Error_manager(Error_code::DISPATCH_PROCESS_INIT_ERROR, Error_level::MINOR_ERROR,
  38. " dispatch_request_msg.terminal_id() is error ");
  39. }
  40. //解析调度请求消息
  41. if ( dispatch_request_msg.dispatch_motion_direction() == message::E_STORE_CAR )
  42. {
  43. //检查调度请求消息
  44. if ( Dispatch_coordinates::get_instance_references().m_passageway_functioning_pattern_map[dispatch_request_msg.terminal_id()] != Dispatch_coordinates::PASSAGEWAY_FUNCTIONING_PATTERN_INLET &&
  45. Dispatch_coordinates::get_instance_references().m_passageway_functioning_pattern_map[dispatch_request_msg.terminal_id()] != Dispatch_coordinates::PASSAGEWAY_FUNCTIONING_PATTERN_BIDIRECTION )
  46. {
  47. return Error_manager(Error_code::DISPATCH_PROCESS_INIT_ERROR, Error_level::MINOR_ERROR,
  48. " m_passageway_functioning_pattern_map[dispatch_request_msg.terminal_id()] is error ");
  49. }
  50. //存车的车位可以是1~3个
  51. if ( dispatch_request_msg.parkspace_info_ex_size() <=0 || dispatch_request_msg.parkspace_info_ex_size() >3 )
  52. {
  53. return Error_manager(Error_code::DISPATCH_PROCESS_INIT_ERROR, Error_level::MINOR_ERROR,
  54. " dispatch_request_msg.parkspace_info_ex_size() is error ");
  55. }
  56. //检查定位信息
  57. if ( dispatch_request_msg.has_locate_information() == false )
  58. {
  59. return Error_manager(Error_code::DISPATCH_PROCESS_INIT_ERROR, Error_level::MINOR_ERROR,
  60. " dispatch_request_msg.has_locate_information() is error ");
  61. }
  62. m_dispatch_process_type = Common_data::Dispatch_process_type::DISPATCH_PROCESS_STORE;
  63. m_dispatch_source = dispatch_request_msg.terminal_id() + PASSAGEWAY_ID_BASE ;
  64. //终点在运动过程中动态分配
  65. // m_dispatch_destination = dispatch_request_msg.parkspace_info().parkingspace_index_id() + PARKSPACE_ID_BASE;
  66. // Common_data::copy_data(m_parkspace_information, dispatch_request_msg.parkspace_info());
  67. // Common_data::scaling(m_parkspace_information, 1000);
  68. Common_data::copy_data(m_car_measure_information, dispatch_request_msg.locate_information());
  69. Common_data::scaling(m_car_measure_information, 1000);
  70. m_wheel_base = m_car_measure_information.car_wheel_base;
  71. }
  72. else if( dispatch_request_msg.dispatch_motion_direction() == message::E_PICKUP_CAR )
  73. {
  74. //检查调度请求消息
  75. if ( Dispatch_coordinates::get_instance_references().m_passageway_functioning_pattern_map[dispatch_request_msg.terminal_id()] != Dispatch_coordinates::PASSAGEWAY_FUNCTIONING_PATTERN_OUTLET &&
  76. Dispatch_coordinates::get_instance_references().m_passageway_functioning_pattern_map[dispatch_request_msg.terminal_id()] != Dispatch_coordinates::PASSAGEWAY_FUNCTIONING_PATTERN_BIDIRECTION )
  77. {
  78. return Error_manager(Error_code::DISPATCH_PROCESS_INIT_ERROR, Error_level::MINOR_ERROR,
  79. " m_passageway_functioning_pattern_map[dispatch_request_msg.terminal_id()] is error ");
  80. }
  81. //取车的车位必须是1个
  82. if ( dispatch_request_msg.parkspace_info_ex_size() != 1 )
  83. {
  84. return Error_manager(Error_code::DISPATCH_PROCESS_INIT_ERROR, Error_level::MINOR_ERROR,
  85. " dispatch_request_msg.parkspace_info_ex_size() is error ");
  86. }
  87. m_dispatch_process_type = Common_data::Dispatch_process_type::DISPATCH_PROCESS_PICKUP;
  88. m_dispatch_source = dispatch_request_msg.parkspace_info().parkingspace_index_id() + PARKSPACE_ID_BASE;
  89. //终点在运动过程中动态分配
  90. //目前不控制门, 所以出口有主控决定. 如果后续控制门, 那么出口可以由调度决定.
  91. m_dispatch_destination = dispatch_request_msg.terminal_id() + PASSAGEWAY_ID_BASE ;
  92. Common_data::copy_data(m_parkspace_information, dispatch_request_msg.parkspace_info_ex(0));
  93. Common_data::scaling(m_parkspace_information, 1000);
  94. m_wheel_base = m_parkspace_information.car_information.car_wheel_base;
  95. }
  96. else
  97. {
  98. m_dispatch_process_type = Common_data::Dispatch_process_type::DISPATCH_PROCESS_TYPE_UNKNOW;
  99. return Error_manager(Error_code::DISPATCH_PROCESS_INIT_ERROR, Error_level::MINOR_ERROR,
  100. " Dispatch_process::Dispatch_process_init ERROR ");
  101. }
  102. //轮距
  103. if ( m_wheel_base < Dispatch_coordinates::get_instance_references().m_car_wheel_base_min ||
  104. m_wheel_base > Dispatch_coordinates::get_instance_references().m_car_wheel_base_max)
  105. {
  106. return Error_manager(Error_code::DISPATCH_PROCESS_INIT_ERROR, Error_level::MINOR_ERROR,
  107. " m_wheel_base < 1000 ERROR ");
  108. }
  109. m_dispatch_request_msg = dispatch_request_msg;
  110. m_dispatch_process_status = DISPATCH_PROCESS_CREATED;
  111. return Error_code::SUCCESS;
  112. }
  113. //反初始化
  114. Error_manager Dispatch_process::Dispatch_process_uninit()
  115. {
  116. std::unique_lock<std::mutex> t_lock(m_lock);
  117. for (auto iter = m_dispatch_control_node_map.begin(); iter != m_dispatch_control_node_map.end(); ++iter)
  118. {
  119. if ( iter->second.mp_dispatch_task.get() != NULL )
  120. {
  121. iter->second.mp_dispatch_task->set_task_statu(Task_Base::Task_statu::TASK_WITHDRAW);
  122. iter->second.mp_dispatch_task.reset();
  123. iter->second.mp_dispatch_device.reset();
  124. }
  125. }
  126. return Error_code::SUCCESS;
  127. }
  128. //检查流程是否空闲待机
  129. Error_manager Dispatch_process::check_process_ready()
  130. {
  131. return Error_code::SUCCESS;
  132. if ( m_dispatch_process_status == DISPATCH_PROCESS_READY )
  133. {
  134. return Error_code::SUCCESS;
  135. }
  136. else
  137. {
  138. return Error_manager(Error_code::DISPATCH_PROCESS_IS_NOT_READY, Error_level::MINOR_ERROR,
  139. " Dispatch_process::check_process_ready() fun error ");
  140. }
  141. }
  142. void Dispatch_process::Main()
  143. {
  144. Error_manager t_error;
  145. //主流程, 循环执行
  146. while ( std::chrono::system_clock::now() - m_start_time < std::chrono::milliseconds(m_timeout_ms) )
  147. {
  148. std::this_thread::sleep_for(std::chrono::microseconds(1));
  149. std::this_thread::sleep_for(std::chrono::milliseconds(1));
  150. // std::this_thread::sleep_for(std::chrono::seconds(1));
  151. // std::cout << " huli test :::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::: " << " m_dispatch_process_status = " << m_dispatch_process_status << std::endl;
  152. switch ( m_dispatch_process_status )
  153. {
  154. case DISPATCH_PROCESS_CREATED://流程创建,
  155. {
  156. //检查调度请求
  157. m_result = check_dispatch_request_msg();
  158. if ( m_result !=Error_code::SUCCESS)
  159. {
  160. m_dispatch_process_status = DISPATCH_PROCESS_FAULT;
  161. break;
  162. }
  163. //发送调度总计划
  164. m_result = send_dispatch_plan_request_msg();
  165. if ( m_result !=Error_code::SUCCESS)
  166. {
  167. m_dispatch_process_status = DISPATCH_PROCESS_FAULT;
  168. break;
  169. }
  170. //流程正常, 就进入等待状态, 等待调度控制发送动作指令
  171. m_dispatch_process_status = DISPATCH_PROCESS_READY;
  172. break;
  173. }
  174. case DISPATCH_PROCESS_READY://流程准备,待机
  175. {
  176. //调度控制, 并根据完成情况给答复
  177. dispatch_control_motion();
  178. //等待调度总计划答复
  179. m_result = wait_dispatch_plan_response_msg();
  180. if ( m_result ==Error_code::SUCCESS)
  181. {
  182. //流程正常, 就进入完成状态,
  183. m_dispatch_process_status = DISPATCH_PROCESS_OVER;
  184. break;
  185. }
  186. else if ( m_result !=Error_code::NODATA )
  187. {
  188. m_dispatch_process_status = DISPATCH_PROCESS_FAULT;
  189. break;
  190. }
  191. //else nodata, 就表示没有新的指令, 那么什么都不做, 原地待命
  192. break;
  193. }
  194. case DISPATCH_PROCESS_OVER://流程完成
  195. {
  196. //发送调度答复, 发给主控的
  197. m_result = send_dispatch_response_msg();
  198. if ( m_result !=Error_code::SUCCESS)
  199. {
  200. m_dispatch_process_status = DISPATCH_PROCESS_FAULT;
  201. break;
  202. }
  203. //流程正常, 就进入等待状态, 等待调度控制发送动作指令
  204. m_dispatch_process_status = DISPATCH_PROCESS_RELEASE;
  205. break;
  206. }
  207. case DISPATCH_PROCESS_RELEASE://流程释放
  208. {
  209. //通知调度管理, 释放资源,
  210. m_result = release_resource();
  211. if ( m_result !=Error_code::SUCCESS)
  212. {
  213. m_dispatch_process_status = DISPATCH_PROCESS_FAULT;
  214. break;
  215. }
  216. //在这里, 整个流程彻底结束, 之后线程池会自动回收 这个流程对象的资源
  217. return;
  218. break;
  219. }
  220. case DISPATCH_PROCESS_FAULT://故障
  221. {
  222. release_resource();
  223. Exception_handling();
  224. //在这里, 整个流程彻底结束, 之后线程池会自动回收 这个流程对象的资源
  225. return;
  226. break;
  227. }
  228. default:
  229. {
  230. break;
  231. }
  232. }
  233. }
  234. //任务超时
  235. LOG(INFO) << " Dispatch_process::Main() time out "<< this;
  236. return;
  237. }
  238. //检查调度请求
  239. Error_manager Dispatch_process::check_dispatch_request_msg()
  240. {
  241. // std::unique_lock<std::mutex> t_lock(m_lock);
  242. return Error_code::SUCCESS;
  243. }
  244. //发送调度总计划
  245. Error_manager Dispatch_process::send_dispatch_plan_request_msg()
  246. {
  247. std::unique_lock<std::mutex> t_lock(m_lock);
  248. m_dispatch_plan_request_msg.mutable_base_info()->set_msg_type(message::Message_type::eDispatch_plan_request_msg);
  249. m_dispatch_plan_request_msg.mutable_base_info()->set_timeout_ms(m_timeout_ms);
  250. m_dispatch_plan_request_msg.mutable_base_info()->set_sender(message::Communicator::eDispatch_manager);
  251. m_dispatch_plan_request_msg.mutable_base_info()->set_receiver(message::Communicator::eDispatch_control);
  252. m_dispatch_plan_request_msg.set_command_key(m_dispatch_request_msg.command_key());
  253. m_dispatch_plan_request_msg.set_dispatch_task_type((message::Dispatch_task_type)m_dispatch_process_type);
  254. m_dispatch_plan_request_msg.set_dispatch_source(m_dispatch_source);
  255. m_dispatch_plan_request_msg.set_dispatch_destination(m_dispatch_destination);
  256. //这里不写错误码
  257. std::string t_msg = m_dispatch_plan_request_msg.SerializeAsString();
  258. System_communication::get_instance_references().encapsulate_msg(t_msg);
  259. return Error_code::SUCCESS;
  260. }
  261. //执行调度控制指令, 并根据完成情况给答复
  262. Error_manager Dispatch_process::dispatch_control_motion()
  263. {
  264. Error_manager t_error;
  265. std::unique_lock<std::mutex> t_lock(m_lock);
  266. m_result.error_manager_clear_all();
  267. for (auto iter = m_dispatch_control_node_map.begin(); iter != m_dispatch_control_node_map.end(); ++iter)
  268. {
  269. switch ( iter->second.m_dispatch_control_status )
  270. {
  271. case DISPATCH_CONTROL_STATUS_UNKNOW:
  272. case DISPATCH_CONTROL_CREATED:
  273. case DISPATCH_CONTROL_READY:
  274. {
  275. t_error = wait_dispatch_control_request_msg(iter->first, iter->second);
  276. if ( t_error == Error_code::SUCCESS)
  277. {
  278. //流程正常, 就进入完成状态,
  279. iter->second.m_dispatch_control_status = DISPATCH_CONTROL_CONNECT_DEVICE;
  280. }
  281. //else 原地待命
  282. break;
  283. }
  284. case DISPATCH_CONTROL_CONNECT_DEVICE:
  285. {
  286. //连接调度设备
  287. t_error = connect_dispatch_device(iter->first, iter->second);
  288. if ( t_error !=Error_code::SUCCESS)
  289. {
  290. iter->second.m_error = t_error;
  291. iter->second.m_dispatch_control_status = DISPATCH_CONTROL_RESPONSE;
  292. }
  293. else
  294. {
  295. //流程正常, 就进入工作状态,
  296. iter->second.m_dispatch_control_status = DISPATCH_CONTROL_WORKING;
  297. iter->second.m_time_to_send_control_response = std::chrono::system_clock::now();
  298. }
  299. break;
  300. }
  301. case DISPATCH_CONTROL_WORKING:
  302. {
  303. //执行调度控制指令, 并根据完成情况给答复
  304. t_error = excute_dispatch_control(iter->first, iter->second);
  305. if ( t_error == Error_code::NODATA )
  306. {
  307. //继续 长流程, 什么也不做
  308. if ( std::chrono::system_clock::now() - iter->second.m_time_to_send_control_response >= std::chrono::seconds(1) )
  309. {
  310. //发送调度控制答复, 发给调度控制的
  311. t_error = send_dispatch_control_response_msg(iter->first, iter->second, message::E_TASK_WORKING);
  312. iter->second.m_time_to_send_control_response = std::chrono::system_clock::now();
  313. }
  314. }
  315. else
  316. {
  317. //长流程结束, 就答复 control_response_msg
  318. // 注:这里执行调度控制, 即使报错了 也要答复给调度控制, 交给调度控制来进行决策.
  319. iter->second.m_error = t_error;
  320. iter->second.m_dispatch_control_status = DISPATCH_CONTROL_TASK_WITHDRAW;
  321. }
  322. break;
  323. }
  324. case DISPATCH_CONTROL_TASK_WITHDRAW://流程 收回任务单
  325. {
  326. //发送调度控制答复, 发给调度控制的
  327. t_error = dispatch_control_withdraw_task(iter->first, iter->second);
  328. //流程正常, 就进入等待状态, 等待调度控制发送动作指令
  329. iter->second.m_dispatch_control_status = DISPATCH_CONTROL_RESPONSE;
  330. break;
  331. }
  332. case DISPATCH_CONTROL_RESPONSE://流程 给调度控制答复
  333. {
  334. //发送调度控制答复, 发给调度控制的
  335. t_error = send_dispatch_control_response_msg(iter->first, iter->second, message::E_TASK_OVER);
  336. //流程正常, 就进入等待状态, 等待调度控制发送动作指令
  337. iter->second.m_dispatch_control_status = DISPATCH_CONTROL_DISCONNECT_DEVICE;
  338. break;
  339. }
  340. case DISPATCH_CONTROL_DISCONNECT_DEVICE://流程 解除设备
  341. {
  342. //断开调度设备, 释放任务单 与设备解除连接
  343. t_error = disconnect_dispatch_device(iter->first, iter->second);
  344. if ( t_error == Error_code::SUCCESS )
  345. {
  346. //流程正常, 就回到 等待状态, 等待调度控制发送动作指令
  347. iter->second.m_error.error_manager_clear_all();
  348. iter->second.m_dispatch_control_status = DISPATCH_CONTROL_READY;
  349. }
  350. //else 保持不变继续等待
  351. break;
  352. }
  353. default:
  354. {
  355. break;
  356. }
  357. }
  358. }
  359. return Error_code::SUCCESS;
  360. }
  361. //等待控制指令
  362. Error_manager Dispatch_process::wait_dispatch_control_request_msg(int dispatch_device_type, Dispatch_control_node & dispatch_control_node)
  363. {
  364. //key不相等 就表示 收到了新的控制指令
  365. if ( dispatch_control_node.m_dispatch_control_request_msg.command_key() != dispatch_control_node.m_dispatch_control_response_msg.command_key() )
  366. {
  367. return Error_code::SUCCESS;
  368. }
  369. else
  370. {
  371. return Error_code::NODATA;
  372. }
  373. return Error_code::SUCCESS;
  374. }
  375. //连接调度设备, 创建新的任务单 与设备建立连接
  376. Error_manager Dispatch_process::connect_dispatch_device(int dispatch_device_type, Dispatch_control_node & dispatch_control_node)
  377. {
  378. Error_manager t_error;
  379. if ( dispatch_device_type == message::Dispatch_device_type::ROBOT_1 ||
  380. dispatch_device_type == message::Dispatch_device_type::ROBOT_2 )
  381. {
  382. //找到对应的设备
  383. if ( dispatch_device_type == message::Dispatch_device_type::ROBOT_1 )
  384. {
  385. dispatch_control_node.mp_dispatch_device = Dispatch_manager::get_instance_references().m_catcher_map[0];
  386. }
  387. if ( dispatch_device_type == message::Dispatch_device_type::ROBOT_2 )
  388. {
  389. dispatch_control_node.mp_dispatch_device = Dispatch_manager::get_instance_references().m_catcher_map[1];
  390. }
  391. Catcher* tp_catcher = (Catcher*)dispatch_control_node.mp_dispatch_device.get();
  392. //检查设备状态
  393. if ( tp_catcher->check_status() == Error_code::SUCCESS &&
  394. tp_catcher->m_actual_device_status == Dispatch_device_base::HARDWARE_DEVICE_READY )
  395. {
  396. //创建任务单
  397. dispatch_control_node.mp_dispatch_task = std::shared_ptr<Task_Base>(new Catcher_task);
  398. dispatch_control_node.mp_dispatch_task->task_init(NULL,std::chrono::milliseconds(15000));
  399. Catcher_task * tp_catcher_task = (Catcher_task *)dispatch_control_node.mp_dispatch_task.get();
  400. //第一次发送 空的唯一码, 可以和设备建立联系
  401. tp_catcher_task->m_request_key = "";
  402. tp_catcher_task->m_request_x = tp_catcher->m_actual_x;
  403. tp_catcher_task->m_request_y = tp_catcher->m_actual_y;
  404. tp_catcher_task->m_request_b = tp_catcher->m_actual_b;
  405. tp_catcher_task->m_request_z = tp_catcher->m_actual_z;
  406. tp_catcher_task->m_request_d1 = tp_catcher->m_actual_d1;
  407. tp_catcher_task->m_request_d2 = tp_catcher->m_actual_d2;
  408. tp_catcher_task->m_request_wheelbase = Dispatch_coordinates::get_instance_references().m_default_wheelbase;
  409. tp_catcher_task->m_request_clamp_motion = (Catcher_task::Clamp_motion)tp_catcher->m_actual_clamp_motion1;
  410. t_error = tp_catcher->execute_task(dispatch_control_node.mp_dispatch_task, Dispatch_device_base::DISPATCH_TASK_ONE_LEVEL);
  411. if ( t_error != Error_code::SUCCESS )
  412. {
  413. dispatch_control_node.m_error = t_error;
  414. return t_error;
  415. }
  416. }
  417. else
  418. {
  419. std::cout << " huli test :::: " << " 123123123123123123123 = " << 1233 << std::endl;
  420. std::cout << " huli test :::: " << " tp_catcher->check_status() = " << tp_catcher->check_status().to_string() << std::endl;
  421. std::cout << " huli test :::: " << " tp_catcher->m_actual_device_status = " << tp_catcher->m_actual_device_status << std::endl;
  422. t_error = Error_manager(Error_code::DISPATCH_PROCESS_DEVICE_STATUS_ERROR, Error_level::MINOR_ERROR,
  423. " tp_catcher->m_actual_device_status device_status error ");
  424. dispatch_control_node.m_error = t_error;
  425. return t_error;
  426. }
  427. //设置起点
  428. dispatch_control_node.m_source_coordinates = Dispatch_coordinates::get_instance_references().m_catcher_coordinates[dispatch_control_node.m_dispatch_control_request_msg.dispatch_source()];
  429. //设置终点
  430. dispatch_control_node.m_destination_coordinates = Dispatch_coordinates::get_instance_references().m_catcher_coordinates[dispatch_control_node.m_dispatch_control_request_msg.dispatch_destination()];
  431. std::cout << " huli test :::: " << " ---------------------------------------- = " << 123 << std::endl;
  432. std::cout << " huli test :::: " << " dispatch_control_node.m_dispatch_control_request_msg.dispatch_destination() = " << dispatch_control_node.m_dispatch_control_request_msg.dispatch_destination() << std::endl;
  433. std::cout << " huli test :::: " << " dispatch_control_node.m_destination_coordinates.x = " << dispatch_control_node.m_destination_coordinates.x << std::endl;
  434. std::cout << " huli test :::: " << " dispatch_control_node.m_destination_coordinates.y = " << dispatch_control_node.m_destination_coordinates.y << std::endl;
  435. std::cout << " huli test :::: " << " dispatch_control_node.m_destination_coordinates.z = " << dispatch_control_node.m_destination_coordinates.z << std::endl;
  436. std::cout << " huli test :::: " << "*******************************************" << 123 << std::endl;
  437. }
  438. //搬运器的配置 准备工作
  439. else if ( dispatch_device_type == message::Dispatch_device_type::CARRIER_1 ||
  440. dispatch_device_type == message::Dispatch_device_type::CARRIER_2 ||
  441. dispatch_device_type == message::Dispatch_device_type::CARRIER_3 )
  442. {
  443. //找到对应的设备
  444. if ( dispatch_device_type == message::Dispatch_device_type::CARRIER_1 )
  445. {
  446. dispatch_control_node.mp_dispatch_device = Dispatch_manager::get_instance_references().m_carrier_map[0];
  447. }
  448. if ( dispatch_device_type == message::Dispatch_device_type::CARRIER_2 )
  449. {
  450. dispatch_control_node.mp_dispatch_device = Dispatch_manager::get_instance_references().m_carrier_map[1];
  451. }
  452. if ( dispatch_device_type == message::Dispatch_device_type::CARRIER_3 )
  453. {
  454. dispatch_control_node.mp_dispatch_device = Dispatch_manager::get_instance_references().m_carrier_map[2];
  455. }
  456. Carrier* tp_carrier = (Carrier*)dispatch_control_node.mp_dispatch_device.get();
  457. //检查设备状态
  458. if ( tp_carrier->check_status() == Error_code::SUCCESS &&
  459. tp_carrier->m_actual_device_status == Dispatch_device_base::HARDWARE_DEVICE_READY )
  460. {
  461. //创建任务单
  462. dispatch_control_node.mp_dispatch_task = std::shared_ptr<Task_Base>(new Carrier_task);
  463. dispatch_control_node.mp_dispatch_task->task_init(NULL,std::chrono::milliseconds(15000));
  464. Carrier_task * tp_carrier_task = (Carrier_task *)dispatch_control_node.mp_dispatch_task.get();
  465. //第一次发送 空的唯一码, 可以和设备建立联系
  466. tp_carrier_task->m_request_key = "";
  467. tp_carrier_task->m_request_x = tp_carrier->m_actual_x;
  468. tp_carrier_task->m_request_y = tp_carrier->m_actual_y;
  469. tp_carrier_task->m_request_z = tp_carrier->m_actual_z;
  470. tp_carrier_task->m_request_y1 = tp_carrier->m_actual_y1;
  471. tp_carrier_task->m_request_y2 = tp_carrier->m_actual_y2;
  472. tp_carrier_task->m_request_clamp_motion = (Carrier_task::Clamp_motion)tp_carrier->m_actual_clamp_motion1;
  473. tp_carrier_task->m_request_joint_motion_x = (Carrier_task::Joint_motion)tp_carrier->m_actual_joint_motion_x1;
  474. tp_carrier_task->m_request_joint_motion_y = Carrier_task::Joint_motion::E_JOINT_NO_ACTION;
  475. tp_carrier_task->m_request_space_id = 0;
  476. tp_carrier_task->m_request_floor_id = 0;
  477. tp_carrier_task->m_request_wheelbase = Dispatch_coordinates::get_instance_references().m_default_wheelbase;
  478. t_error = tp_carrier->execute_task(dispatch_control_node.mp_dispatch_task, Dispatch_device_base::DISPATCH_TASK_ONE_LEVEL);
  479. if ( t_error != Error_code::SUCCESS )
  480. {
  481. dispatch_control_node.m_error = t_error;
  482. return t_error;
  483. }
  484. }
  485. else
  486. {
  487. std::cout << " huli test :::: " << " 123123123123123123123 = " << 4566 << std::endl;
  488. std::cout << " huli test :::: " << " tp_carrier->check_status() = " << tp_carrier->check_status().to_string() << std::endl;
  489. std::cout << " huli test :::: " << " tp_carrier->m_actual_device_status = " << tp_carrier->m_actual_device_status << std::endl;
  490. t_error = Error_manager(Error_code::DISPATCH_PROCESS_DEVICE_STATUS_ERROR, Error_level::MINOR_ERROR,
  491. " tp_carrier->m_actual_device_status device_status error ");
  492. dispatch_control_node.m_error = t_error;
  493. return t_error;
  494. }
  495. //设置起点
  496. dispatch_control_node.m_source_coordinates = Dispatch_coordinates::get_instance_references().m_carrier_coordinates[dispatch_control_node.m_dispatch_control_request_msg.dispatch_source()];
  497. //设置终点
  498. dispatch_control_node.m_destination_coordinates = Dispatch_coordinates::get_instance_references().m_carrier_coordinates[dispatch_control_node.m_dispatch_control_request_msg.dispatch_destination()];
  499. std::cout << " huli test :::: " << " ---------------------------------------- = " << 123 << std::endl;
  500. std::cout << " huli test :::: " << " dispatch_control_node.m_dispatch_control_request_msg.dispatch_destination() = " << dispatch_control_node.m_dispatch_control_request_msg.dispatch_destination() << std::endl;
  501. std::cout << " huli test :::: " << " dispatch_control_node.m_destination_coordinates.x = " << dispatch_control_node.m_destination_coordinates.x << std::endl;
  502. std::cout << " huli test :::: " << " dispatch_control_node.m_destination_coordinates.y = " << dispatch_control_node.m_destination_coordinates.y << std::endl;
  503. std::cout << " huli test :::: " << " dispatch_control_node.m_destination_coordinates.z = " << dispatch_control_node.m_destination_coordinates.z << std::endl;
  504. std::cout << " huli test :::: " << "*******************************************" << 123 << std::endl;
  505. }
  506. else
  507. {
  508. t_error = Error_manager(Error_code::DISPATCH_PROCESS_DEVICE_TYPE_ERROR, Error_level::MINOR_ERROR,
  509. " DISPATCH_PROCESS_DEVICE_TYPE_ERROR fun error ");
  510. dispatch_control_node.m_error = t_error;
  511. return t_error;
  512. }
  513. return Error_code::SUCCESS;
  514. }
  515. //执行调度控制指令, 并根据完成情况给答复
  516. Error_manager Dispatch_process::excute_dispatch_control(int dispatch_device_type, Dispatch_control_node & dispatch_control_node)
  517. {
  518. Error_manager t_error;
  519. if ( dispatch_device_type == message::Dispatch_device_type::ROBOT_1 ||
  520. dispatch_device_type == message::Dispatch_device_type::ROBOT_2 )
  521. {
  522. Catcher_task * tp_catcher_task = (Catcher_task *)dispatch_control_node.mp_dispatch_task.get();
  523. // while (1)
  524. // {
  525. // char in;
  526. // std::cin >> in ;
  527. // if ( in == 'p' )
  528. // {
  529. // break;
  530. // }
  531. // }
  532. // std::cout << " huli test :::: " << "ppppppppppppppppppppppppppppppppppppppppppppppppppppppppppppppppppppppppppppppppppppppppppppppppppppppppppppppppppppppppppppppppppppppppppppppppp = " << tp_catcher_task->m_step << std::endl;
  533. // std::cout << " huli test :::: " << "1111111 tp_catcher_task->m_step = " << tp_catcher_task->m_step << std::endl;
  534. //设备的动作也使用外部的Main()的线程来循环
  535. switch ( dispatch_control_node.m_dispatch_control_request_msg.dispatch_task_type() )
  536. {
  537. case message::Dispatch_task_type::ROBOT_CATCH_CAR_FROM_INLET:
  538. {
  539. t_error = excute_robot_catch_car_from_inlet(dispatch_control_node);
  540. break;
  541. }
  542. case message::Dispatch_task_type::ROBOT_PUT_CAR_TO_CARRIER:
  543. {
  544. t_error = excute_robot_put_car_to_carrier(dispatch_control_node);
  545. break;
  546. }
  547. case message::Dispatch_task_type::ROBOT_CATCH_CAR_FROM_CARRIER:
  548. {
  549. t_error = excute_robot_catch_car_from_carrier(dispatch_control_node);
  550. break;
  551. }
  552. case message::Dispatch_task_type::ROBOT_PUT_CAR_TO_OUTLET:
  553. {
  554. t_error = excute_robot_put_car_to_outlet(dispatch_control_node);
  555. break;
  556. }
  557. case message::Dispatch_task_type::ROBOT_MOVE:
  558. {
  559. t_error = excute_robot_move(dispatch_control_node);
  560. break;
  561. }
  562. default:
  563. {
  564. return Error_manager(Error_code::DISPATCH_PROCESS_TASK_STATUS_ERROR, Error_level::MINOR_ERROR,
  565. "Dispatch_process::excute_dispatch_control() fun error ");
  566. break;
  567. }
  568. }
  569. // std::cout << " huli test :::: " << "222222 tp_catcher_task->m_step = " << tp_catcher_task->m_step << std::endl;
  570. // std::cout << " huli test :::: " << " t_error_catcher = " << t_error.to_string() << std::endl;
  571. }
  572. else if ( dispatch_device_type == message::Dispatch_device_type::CARRIER_1 ||
  573. dispatch_device_type == message::Dispatch_device_type::CARRIER_2 ||
  574. dispatch_device_type == message::Dispatch_device_type::CARRIER_3 )
  575. {
  576. Catcher_task *tp_carrier_task = (Catcher_task *) dispatch_control_node.mp_dispatch_task.get();
  577. // while (1)
  578. // {
  579. // char in;
  580. // std::cin >> in;
  581. // if (in == 'l')
  582. // {
  583. // break;
  584. // }
  585. // }
  586. // std::cout << " huli test :::: "
  587. // << "lllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllll = "
  588. // << tp_carrier_task->m_step << std::endl;
  589. // std::cout << " huli test :::: " << "3333333 tp_carrier_task->m_step = " << tp_carrier_task->m_step
  590. // << std::endl;
  591. //设备的动作也使用外部的Main()的线程来循环
  592. switch (dispatch_control_node.m_dispatch_control_request_msg.dispatch_task_type())
  593. {
  594. case message::Dispatch_task_type::CARRIER_RECEIVE_CAR_FROM_ROBOT:
  595. {
  596. t_error = excute_carrier_receive_car_from_robot(dispatch_control_node);
  597. break;
  598. }
  599. case message::Dispatch_task_type::CARRIER_STORE_CAR_TO_PARKINGSPACE:
  600. {
  601. t_error = excute_carrier_store_car_to_parkingspace(dispatch_control_node);
  602. break;
  603. }
  604. case message::Dispatch_task_type::CARRIER_STORE_CAR_TO_PARKINGSPACE_EX:
  605. {
  606. t_error = excute_carrier_store_car_to_parkingspace_ex(dispatch_control_node);
  607. break;
  608. }
  609. case message::Dispatch_task_type::CARRIER_PICKUP_CAR_FROM_PARKINGSPACE:
  610. {
  611. t_error = excute_carrier_pickup_car_from_parkingspace(dispatch_control_node);
  612. break;
  613. }
  614. case message::Dispatch_task_type::CARRIER_PICKUP_CAR_FROM_PARKINGSPACE_EX:
  615. {
  616. t_error = excute_carrier_pickup_car_from_parkingspace_ex(dispatch_control_node);
  617. break;
  618. }
  619. case message::Dispatch_task_type::CARRIER_DELIVER_CAR_TO_ROBOT:
  620. {
  621. t_error = excute_carrier_deliver_car_to_robot(dispatch_control_node);
  622. break;
  623. }
  624. case message::Dispatch_task_type::CARRIER_MOVE:
  625. {
  626. t_error = excute_carrier_move(dispatch_control_node);
  627. break;
  628. }
  629. default:
  630. {
  631. return Error_manager(Error_code::DISPATCH_PROCESS_TASK_STATUS_ERROR, Error_level::MINOR_ERROR,
  632. "Dispatch_process::excute_dispatch_control() fun error ");
  633. break;
  634. }
  635. }
  636. // std::cout << " huli test :::: " << "44444444 tp_carrier_task->m_step = " << tp_carrier_task->m_step << std::endl;
  637. // std::cout << " huli test :::: " << " t_error_carrier = " << t_error.to_string() << std::endl;
  638. }
  639. dispatch_control_node.m_error = t_error;
  640. return t_error;
  641. }
  642. //流程通知设备 收回任务单
  643. Error_manager Dispatch_process::dispatch_control_withdraw_task(int dispatch_device_type, Dispatch_control_node & dispatch_control_node)
  644. {
  645. if ( dispatch_control_node.mp_dispatch_device.get() != NULL && dispatch_control_node.mp_dispatch_task.get() != NULL )
  646. {
  647. dispatch_control_node.mp_dispatch_task->set_task_statu(Task_Base::Task_statu::TASK_WITHDRAW);
  648. }
  649. return Error_code::SUCCESS;
  650. }
  651. //发送调度控制答复, 发给调度控制的
  652. Error_manager Dispatch_process::send_dispatch_control_response_msg(int dispatch_device_type, Dispatch_control_node & dispatch_control_node, message::Dispatch_device_task_status dispatch_device_task_status)
  653. {
  654. dispatch_control_node.m_dispatch_control_response_msg.mutable_base_info()->set_msg_type(message::Message_type::eDispatch_control_response_msg);
  655. dispatch_control_node.m_dispatch_control_response_msg.mutable_base_info()->set_timeout_ms(dispatch_control_node.m_dispatch_control_request_msg.base_info().timeout_ms());
  656. dispatch_control_node.m_dispatch_control_response_msg.mutable_base_info()->set_sender(message::Communicator::eDispatch_manager);
  657. dispatch_control_node.m_dispatch_control_response_msg.mutable_base_info()->set_receiver(message::Communicator::eDispatch_control);
  658. dispatch_control_node.m_dispatch_control_response_msg.set_command_key(dispatch_control_node.m_dispatch_control_request_msg.command_key());
  659. dispatch_control_node.m_dispatch_control_response_msg.set_dispatch_task_type(dispatch_control_node.m_dispatch_control_request_msg.dispatch_task_type());
  660. dispatch_control_node.m_dispatch_control_response_msg.set_dispatch_device_type(dispatch_control_node.m_dispatch_control_request_msg.dispatch_device_type());
  661. dispatch_control_node.m_dispatch_control_response_msg.set_dispatch_source(dispatch_control_node.m_dispatch_control_request_msg.dispatch_source());
  662. dispatch_control_node.m_dispatch_control_response_msg.set_dispatch_destination(dispatch_control_node.m_dispatch_control_request_msg.dispatch_destination());
  663. dispatch_control_node.m_dispatch_control_response_msg.mutable_error_manager()->set_error_code(dispatch_control_node.m_error.get_error_code());
  664. dispatch_control_node.m_dispatch_control_response_msg.mutable_error_manager()->set_error_level((message::Error_level)dispatch_control_node.m_error.get_error_level());
  665. dispatch_control_node.m_dispatch_control_response_msg.mutable_error_manager()->set_error_description(dispatch_control_node.m_error.get_error_description(), dispatch_control_node.m_error.get_description_length());
  666. dispatch_control_node.m_dispatch_control_response_msg.set_dispatch_device_target_status(dispatch_control_node.m_dispatch_control_request_msg.dispatch_device_target_status());
  667. dispatch_control_node.m_dispatch_control_response_msg.set_dispatch_device_task_status(dispatch_device_task_status);
  668. std::string t_msg = dispatch_control_node.m_dispatch_control_response_msg.SerializeAsString();
  669. System_communication::get_instance_references().encapsulate_msg(t_msg);
  670. if ( dispatch_device_task_status == message::Dispatch_device_task_status::E_TASK_OVER )
  671. {
  672. LOG(INFO) << " dispatch_control_node.m_dispatch_control_response_msg = "<< dispatch_control_node.m_dispatch_control_response_msg.DebugString() << " " << this;
  673. }
  674. return Error_code::SUCCESS;
  675. }
  676. //断开调度设备, 释放任务单 与设备解除连接
  677. Error_manager Dispatch_process::disconnect_dispatch_device(int dispatch_device_type, Dispatch_control_node & dispatch_control_node)
  678. {
  679. if ( dispatch_control_node.mp_dispatch_device.get() != NULL && dispatch_control_node.mp_dispatch_task.get() != NULL )
  680. {
  681. if ( dispatch_control_node.mp_dispatch_task->get_task_statu() == Task_Base::Task_statu::TASK_FREE )
  682. {
  683. dispatch_control_node.mp_dispatch_task.reset();
  684. dispatch_control_node.mp_dispatch_device.reset();
  685. return Error_code::SUCCESS;
  686. }
  687. else
  688. {
  689. return Error_code::NODATA;
  690. }
  691. }
  692. return Error_code::SUCCESS;
  693. }
  694. //等待调度总计划答复
  695. Error_manager Dispatch_process::wait_dispatch_plan_response_msg()
  696. {
  697. std::unique_lock<std::mutex> t_lock(m_lock);
  698. //key 相等 就表示 收到了总计划答复
  699. if ( m_dispatch_plan_request_msg.command_key() == m_dispatch_plan_response_msg.command_key() )
  700. {
  701. return Error_code::SUCCESS;
  702. }
  703. else
  704. {
  705. return Error_code::NODATA;
  706. }
  707. }
  708. //发送调度答复, 发给主控的
  709. Error_manager Dispatch_process::send_dispatch_response_msg()
  710. {
  711. std::unique_lock<std::mutex> t_lock(m_lock);
  712. m_dispatch_response_msg.mutable_base_info()->set_msg_type(message::Message_type::eDispatch_response_msg);
  713. m_dispatch_response_msg.mutable_base_info()->set_timeout_ms(m_dispatch_request_msg.base_info().timeout_ms());
  714. m_dispatch_response_msg.mutable_base_info()->set_sender(message::Communicator::eDispatch_manager);
  715. m_dispatch_response_msg.mutable_base_info()->set_receiver(message::Communicator::eMain);
  716. m_dispatch_response_msg.set_command_key(m_dispatch_request_msg.command_key());
  717. m_dispatch_response_msg.mutable_error_manager()->CopyFrom(m_dispatch_plan_response_msg.error_manager());
  718. std::string t_msg = m_dispatch_response_msg.SerializeAsString();
  719. System_communication::get_instance_references().encapsulate_msg(t_msg);
  720. return Error_code::SUCCESS;
  721. }
  722. //通知调度管理, 释放资源,
  723. Error_manager Dispatch_process::release_resource()
  724. {
  725. return Dispatch_manager::get_instance_references().release_dispatch_process(m_command_key);
  726. }
  727. //异常处理
  728. Error_manager Dispatch_process::Exception_handling()
  729. {
  730. LOG(INFO) << " -------------Dispatch_process::Exception_handling------------------- "<< this;
  731. LOG(INFO) << " m_result = "<< m_result.to_string() << " " << this;
  732. return Error_code::SUCCESS;
  733. }
  734. //机器手去入口抓车(例如:机器手移动到2号入口上方,然后下降到一楼抓车,最后上升到最上方)
  735. Error_manager Dispatch_process::excute_robot_catch_car_from_inlet(Dispatch_control_node & dispatch_control_node)
  736. {
  737. Error_manager t_error;
  738. Catcher * tp_catcher = NULL;
  739. Catcher_task * tp_catcher_task = NULL;
  740. Dispatch_coordinates * tp_dispatch_coordinates = Dispatch_coordinates::get_instance_pointer();
  741. if ( dispatch_control_node.mp_dispatch_device.get() == NULL || dispatch_control_node.mp_dispatch_task.get() == NULL )
  742. {
  743. return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
  744. "Dispatch_process::excute_robot_catch_car_from_inlet() POINTER IS NULL ");
  745. }
  746. else
  747. {
  748. tp_catcher = (Catcher *)dispatch_control_node.mp_dispatch_device.get();
  749. tp_catcher_task = (Catcher_task *)dispatch_control_node.mp_dispatch_task.get();
  750. }
  751. // std::cout << " huli test :::: " << " tp_catcher_task->m_step = " << tp_catcher_task->m_step << std::endl;
  752. // getchar();
  753. if ( tp_catcher_task->m_step == 0 )
  754. {
  755. return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_catcher_task->m_step);
  756. }
  757. if ( tp_catcher_task->m_step == 1 )//检查姿态
  758. {
  759. if ( tp_catcher->m_actual_load_status == Dispatch_device_base::Load_status::NO_CAR )
  760. {
  761. if ( tp_catcher->m_actual_clamp_motion1 != Dispatch_device_base::Clamp_motion::E_CLAMP_LOOSE ||
  762. tp_catcher->m_actual_d1 + tp_catcher->m_actual_d2 + tp_dispatch_coordinates->m_catcher_d1_d2_distance >
  763. tp_dispatch_coordinates->m_catcher_wheel_base_limit ||
  764. tp_catcher->m_catcher_direction == Dispatch_device_base::Catcher_direction::CATCHER_DIRECTION_UNKNOW )
  765. {
  766. catcher_adjust_to_ready(dispatch_control_node, tp_catcher, tp_catcher_task, tp_dispatch_coordinates);
  767. tp_catcher_task->m_step++;
  768. }
  769. else
  770. {
  771. tp_catcher_task->m_step +=2;
  772. }
  773. }
  774. else
  775. {
  776. return Error_manager(Error_code::CATCHER_POSE_ERROR, Error_level::MINOR_ERROR,
  777. "Dispatch_process::excute_robot_catch_car_from_inlet() fun error ");
  778. }
  779. }
  780. if ( tp_catcher_task->m_step == 2 )
  781. {
  782. return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_catcher_task->m_step);
  783. }
  784. if ( tp_catcher_task->m_step == 3 )//机器手z轴移到最高点.
  785. {
  786. catcher_move_z(dispatch_control_node, tp_catcher, tp_catcher_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_catcher_4th_floor_z);
  787. tp_catcher_task->m_step++;
  788. }
  789. if ( tp_catcher_task->m_step == 4 )
  790. {
  791. return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_catcher_task->m_step);
  792. }
  793. if ( tp_catcher_task->m_step == 5 )//机器手 调整x y b d1 d2, 根据感测模块的定位信息, 调整机器手的姿态, 准备抓车.
  794. {
  795. catcher_adjust_from_ground(dispatch_control_node, tp_catcher, tp_catcher_task, tp_dispatch_coordinates);
  796. tp_catcher_task->m_step++;
  797. }
  798. if ( tp_catcher_task->m_step == 6 )
  799. {
  800. return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_catcher_task->m_step);
  801. }
  802. if ( tp_catcher_task->m_step == 7 )//机器手z轴移到最低点.
  803. {
  804. catcher_move_z(dispatch_control_node, tp_catcher, tp_catcher_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_catcher_1th_floor_z);
  805. tp_catcher_task->m_step++;
  806. }
  807. if ( tp_catcher_task->m_step == 8 )
  808. {
  809. return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_catcher_task->m_step);
  810. }
  811. if ( tp_catcher_task->m_step == 9 )//修正轴距, 如果轴距大于3000mm, 那么就要修正轴距.
  812. {
  813. float temp_wheel_base = tp_catcher->m_actual_d1 + tp_catcher->m_actual_d2 + tp_dispatch_coordinates->m_catcher_d1_d2_distance;
  814. //修正轴距, 如果轴距大于3000mm, 那么就要修正轴距.
  815. if ( Common_data::approximate_difference(m_wheel_base, temp_wheel_base, DISPATCH_DEFAULT_DIFFERENCE) )
  816. {
  817. tp_catcher_task->m_step +=2;
  818. }
  819. else
  820. {
  821. catcher_adjust_wheel_base(dispatch_control_node, tp_catcher, tp_catcher_task, tp_dispatch_coordinates);
  822. tp_catcher_task->m_step++;
  823. }
  824. }
  825. if ( tp_catcher_task->m_step == 10 )
  826. {
  827. return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_catcher_task->m_step);
  828. }
  829. if ( tp_catcher_task->m_step == 11 )//机器手 夹车
  830. {
  831. catcher_move_c(dispatch_control_node, tp_catcher, tp_catcher_task, tp_dispatch_coordinates, Catcher_task::Clamp_motion::E_CLAMP_TIGHT);
  832. tp_catcher_task->m_step++;
  833. }
  834. if ( tp_catcher_task->m_step == 12 )
  835. {
  836. return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_catcher_task->m_step);
  837. }
  838. if ( tp_catcher_task->m_step == 13 )//机器手 z轴上升到最高处
  839. {
  840. catcher_move_z(dispatch_control_node, tp_catcher, tp_catcher_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_catcher_4th_floor_z);
  841. tp_catcher_task->m_step++;
  842. }
  843. if ( tp_catcher_task->m_step == 14 )
  844. {
  845. return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_catcher_task->m_step);
  846. }
  847. if ( tp_catcher_task->m_step == 15 )//机器手调整到 准备把车放到搬运器的姿态
  848. {
  849. //这里机器手要反向,
  850. catcher_adjust_to_carrier(dispatch_control_node, tp_catcher, tp_catcher_task, tp_dispatch_coordinates, true);
  851. tp_catcher_task->m_step++;
  852. }
  853. if ( tp_catcher_task->m_step == 16 )
  854. {
  855. return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_catcher_task->m_step);
  856. }
  857. if ( tp_catcher_task->m_step == 17 )
  858. {
  859. return Error_code::SUCCESS;
  860. }
  861. return Error_code::SUCCESS;
  862. }
  863. //机器手把车放到中跑车上面(例如:机器手下降到中跑车上放车,最后上升到最上方)(通过目标点 指定放到哪一个中跑车上)
  864. Error_manager Dispatch_process::excute_robot_put_car_to_carrier(Dispatch_control_node & dispatch_control_node)
  865. {
  866. Error_manager t_error;
  867. Catcher * tp_catcher = NULL;
  868. Catcher_task * tp_catcher_task = NULL;
  869. Dispatch_coordinates * tp_dispatch_coordinates = Dispatch_coordinates::get_instance_pointer();
  870. if ( dispatch_control_node.mp_dispatch_device.get() == NULL || dispatch_control_node.mp_dispatch_task.get() == NULL )
  871. {
  872. return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
  873. "Dispatch_process::excute_robot_catch_car_from_inlet() POINTER IS NULL ");
  874. }
  875. else
  876. {
  877. tp_catcher = (Catcher *)dispatch_control_node.mp_dispatch_device.get();
  878. tp_catcher_task = (Catcher_task *)dispatch_control_node.mp_dispatch_task.get();
  879. }
  880. // std::cout << " huli test :::: " << " tp_catcher_task->m_step = " << tp_catcher_task->m_step << std::endl;
  881. // getchar();
  882. if ( tp_catcher_task->m_step == 0 )
  883. {
  884. return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_catcher_task->m_step);
  885. }
  886. if ( tp_catcher_task->m_step == 1 )//检查姿态
  887. {
  888. if ( tp_catcher->m_actual_load_status == Dispatch_device_base::Load_status::HAVE_CAR &&
  889. tp_catcher->m_actual_clamp_motion1 == Dispatch_device_base::Clamp_motion::E_CLAMP_TIGHT &&
  890. (Common_data::approximate_difference(tp_catcher->m_actual_b, 90, DISPATCH_DEFAULT_DIFFERENCE) || Common_data::approximate_difference(tp_catcher->m_actual_b, 270, DISPATCH_DEFAULT_DIFFERENCE) ) )
  891. {
  892. //检测正常, 直接跳过即可
  893. tp_catcher_task->m_step +=2;
  894. }
  895. else
  896. {
  897. return Error_manager(Error_code::CATCHER_POSE_ERROR, Error_level::MINOR_ERROR,
  898. "Dispatch_process::excute_robot_put_car_to_carrier() fun error ");
  899. }
  900. }
  901. if ( tp_catcher_task->m_step == 2 )
  902. {
  903. return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_catcher_task->m_step);
  904. }
  905. if ( tp_catcher_task->m_step == 3 )//机器手 调整z轴
  906. {
  907. if ( tp_catcher->m_actual_z*1.02 >= dispatch_control_node.m_destination_coordinates.z )
  908. {
  909. //检测正常, 直接跳过即可
  910. tp_catcher_task->m_step +=2;
  911. }
  912. else
  913. {
  914. catcher_move_z(dispatch_control_node, tp_catcher, tp_catcher_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_catcher_4th_floor_z);
  915. tp_catcher_task->m_step++;
  916. }
  917. }
  918. if ( tp_catcher_task->m_step == 4 )
  919. {
  920. return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_catcher_task->m_step);
  921. }
  922. if ( tp_catcher_task->m_step == 5 )//机器手调整到 准备把车放到搬运器的姿态
  923. {
  924. float temp_y = tp_dispatch_coordinates->m_carrier_default_y1_back - (m_wheel_base /2);
  925. if ( Common_data::approximate_difference(tp_catcher->m_actual_x, dispatch_control_node.m_destination_coordinates.x, DISPATCH_DEFAULT_DIFFERENCE) &&
  926. Common_data::approximate_difference(tp_catcher->m_actual_y, temp_y, DISPATCH_DEFAULT_DIFFERENCE))
  927. {
  928. //检测正常, 直接跳过即可
  929. tp_catcher_task->m_step +=2;
  930. }
  931. else
  932. {
  933. catcher_adjust_to_carrier(dispatch_control_node, tp_catcher, tp_catcher_task, tp_dispatch_coordinates, false);
  934. tp_catcher_task->m_step++;
  935. }
  936. }
  937. if ( tp_catcher_task->m_step == 6 )
  938. {
  939. return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_catcher_task->m_step);
  940. }
  941. if ( tp_catcher_task->m_step == 7 )//机器手 z轴下降
  942. {
  943. catcher_move_z(dispatch_control_node, tp_catcher, tp_catcher_task, tp_dispatch_coordinates, dispatch_control_node.m_destination_coordinates.z);
  944. tp_catcher_task->m_step++;
  945. }
  946. if ( tp_catcher_task->m_step == 8 )
  947. {
  948. return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_catcher_task->m_step);
  949. }
  950. if ( tp_catcher_task->m_step == 9 )//机器手 松开夹杆
  951. {
  952. catcher_move_c(dispatch_control_node, tp_catcher, tp_catcher_task, tp_dispatch_coordinates, Catcher_task::Clamp_motion::E_CLAMP_LOOSE);
  953. tp_catcher_task->m_step++;
  954. }
  955. if ( tp_catcher_task->m_step == 10 )
  956. {
  957. return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_catcher_task->m_step);
  958. }
  959. if ( tp_catcher_task->m_step == 11 )//机器手 z轴上升
  960. {
  961. catcher_move_z(dispatch_control_node, tp_catcher, tp_catcher_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_catcher_4th_floor_z);
  962. tp_catcher_task->m_step++;
  963. }
  964. if ( tp_catcher_task->m_step == 12 )
  965. {
  966. return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_catcher_task->m_step);
  967. }
  968. if ( tp_catcher_task->m_step == 13 )
  969. {
  970. return Error_code::SUCCESS;
  971. }
  972. return Error_code::SUCCESS;
  973. }
  974. //机器手去中跑车上抓车(例如:机器手移动到1号中跑车上方,然后下降到中跑车抓车,最后上升到最上方)
  975. Error_manager Dispatch_process::excute_robot_catch_car_from_carrier(Dispatch_control_node & dispatch_control_node)
  976. {
  977. Error_manager t_error;
  978. Catcher * tp_catcher = NULL;
  979. Catcher_task * tp_catcher_task = NULL;
  980. Dispatch_coordinates * tp_dispatch_coordinates = Dispatch_coordinates::get_instance_pointer();
  981. if ( dispatch_control_node.mp_dispatch_device.get() == NULL || dispatch_control_node.mp_dispatch_task.get() == NULL )
  982. {
  983. return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
  984. "Dispatch_process::excute_robot_catch_car_from_inlet() POINTER IS NULL ");
  985. }
  986. else
  987. {
  988. tp_catcher = (Catcher *)dispatch_control_node.mp_dispatch_device.get();
  989. tp_catcher_task = (Catcher_task *)dispatch_control_node.mp_dispatch_task.get();
  990. }
  991. // std::cout << " huli test :::: " << " tp_catcher_task->m_step = " << tp_catcher_task->m_step << std::endl;
  992. // getchar();
  993. if ( tp_catcher_task->m_step == 0 )
  994. {
  995. return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_catcher_task->m_step);
  996. }
  997. if ( tp_catcher_task->m_step == 1 )
  998. {
  999. if ( tp_catcher->m_actual_load_status == Dispatch_device_base::Load_status::NO_CAR )
  1000. {
  1001. if ( tp_catcher->m_actual_clamp_motion1 != Dispatch_device_base::Clamp_motion::E_CLAMP_LOOSE ||
  1002. tp_catcher->m_actual_d1 + tp_catcher->m_actual_d2 + tp_dispatch_coordinates->m_catcher_d1_d2_distance >
  1003. tp_dispatch_coordinates->m_catcher_wheel_base_limit ||
  1004. tp_catcher->m_catcher_direction == Dispatch_device_base::Catcher_direction::CATCHER_DIRECTION_UNKNOW )
  1005. {
  1006. catcher_adjust_to_ready(dispatch_control_node, tp_catcher, tp_catcher_task, tp_dispatch_coordinates);
  1007. tp_catcher_task->m_step++;
  1008. }
  1009. else
  1010. {
  1011. tp_catcher_task->m_step +=2;
  1012. }
  1013. }
  1014. else
  1015. {
  1016. return Error_manager(Error_code::CATCHER_POSE_ERROR, Error_level::MINOR_ERROR,
  1017. "Dispatch_process::excute_robot_catch_car_from_carrier() fun error ");
  1018. }
  1019. }
  1020. if ( tp_catcher_task->m_step == 2 )
  1021. {
  1022. return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_catcher_task->m_step);
  1023. }
  1024. if ( tp_catcher_task->m_step == 3 )//机器手 调整z轴
  1025. {
  1026. if ( tp_catcher->m_actual_z*1.02 >= dispatch_control_node.m_destination_coordinates.z )
  1027. {
  1028. //检测正常, 直接跳过即可
  1029. tp_catcher_task->m_step +=2;
  1030. }
  1031. else
  1032. {
  1033. catcher_move_z(dispatch_control_node, tp_catcher, tp_catcher_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_catcher_4th_floor_z);
  1034. tp_catcher_task->m_step++;
  1035. }
  1036. }
  1037. if ( tp_catcher_task->m_step == 4 )
  1038. {
  1039. return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_catcher_task->m_step);
  1040. }
  1041. if ( tp_catcher_task->m_step == 5 )//机器手调整到 准备把车放到搬运器的姿态
  1042. {
  1043. float temp_y = tp_dispatch_coordinates->m_carrier_default_y1_back - (m_wheel_base /2);
  1044. if ( Common_data::approximate_difference(tp_catcher->m_actual_x, dispatch_control_node.m_destination_coordinates.x, DISPATCH_DEFAULT_DIFFERENCE) &&
  1045. Common_data::approximate_difference(tp_catcher->m_actual_y, temp_y, DISPATCH_DEFAULT_DIFFERENCE))
  1046. {
  1047. //检测正常, 直接跳过即可
  1048. tp_catcher_task->m_step +=2;
  1049. }
  1050. else
  1051. {
  1052. catcher_adjust_from_carrier(dispatch_control_node, tp_catcher, tp_catcher_task, tp_dispatch_coordinates);
  1053. tp_catcher_task->m_step++;
  1054. }
  1055. }
  1056. if ( tp_catcher_task->m_step == 6 )
  1057. {
  1058. return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_catcher_task->m_step);
  1059. }
  1060. if ( tp_catcher_task->m_step == 7 )//机器手 z轴下降
  1061. {
  1062. catcher_move_z(dispatch_control_node, tp_catcher, tp_catcher_task, tp_dispatch_coordinates, dispatch_control_node.m_destination_coordinates.z);
  1063. tp_catcher_task->m_step++;
  1064. }
  1065. if ( tp_catcher_task->m_step == 8 )
  1066. {
  1067. return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_catcher_task->m_step);
  1068. }
  1069. if ( tp_catcher_task->m_step == 9 )//修正轴距, 如果轴距大于3000mm, 那么就要修正轴距.
  1070. {
  1071. float temp_wheel_base = tp_catcher->m_actual_d1 + tp_catcher->m_actual_d2 + tp_dispatch_coordinates->m_catcher_d1_d2_distance;
  1072. //修正轴距, 如果轴距大于3000mm, 那么就要修正轴距.
  1073. if ( Common_data::approximate_difference(m_wheel_base, temp_wheel_base, DISPATCH_DEFAULT_DIFFERENCE) )
  1074. {
  1075. tp_catcher_task->m_step +=2;
  1076. }
  1077. else
  1078. {
  1079. catcher_adjust_wheel_base(dispatch_control_node, tp_catcher, tp_catcher_task, tp_dispatch_coordinates);
  1080. tp_catcher_task->m_step++;
  1081. }
  1082. }
  1083. if ( tp_catcher_task->m_step == 10 )
  1084. {
  1085. return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_catcher_task->m_step);
  1086. }
  1087. if ( tp_catcher_task->m_step == 11 )//机器手 夹车
  1088. {
  1089. catcher_move_c(dispatch_control_node, tp_catcher, tp_catcher_task, tp_dispatch_coordinates, Catcher_task::Clamp_motion::E_CLAMP_TIGHT);
  1090. tp_catcher_task->m_step++;
  1091. }
  1092. if ( tp_catcher_task->m_step == 12 )
  1093. {
  1094. return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_catcher_task->m_step);
  1095. }
  1096. if ( tp_catcher_task->m_step == 13 )//机器手 z轴上升
  1097. {
  1098. catcher_move_z(dispatch_control_node, tp_catcher, tp_catcher_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_catcher_4th_floor_z);
  1099. tp_catcher_task->m_step++;
  1100. }
  1101. if ( tp_catcher_task->m_step == 14 )
  1102. {
  1103. return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_catcher_task->m_step);
  1104. }
  1105. if ( tp_catcher_task->m_step == 15 )
  1106. {
  1107. return Error_code::SUCCESS;
  1108. }
  1109. return Error_code::SUCCESS;
  1110. }
  1111. //机器手去出口放车(例如:机器手移动到3号出口上方,然后下降到一楼放车,最后上升到最上方)
  1112. Error_manager Dispatch_process::excute_robot_put_car_to_outlet(Dispatch_control_node & dispatch_control_node)
  1113. {
  1114. Error_manager t_error;
  1115. Catcher * tp_catcher = NULL;
  1116. Catcher_task * tp_catcher_task = NULL;
  1117. Dispatch_coordinates * tp_dispatch_coordinates = Dispatch_coordinates::get_instance_pointer();
  1118. if ( dispatch_control_node.mp_dispatch_device.get() == NULL || dispatch_control_node.mp_dispatch_task.get() == NULL )
  1119. {
  1120. return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
  1121. "Dispatch_process::excute_robot_catch_car_from_inlet() POINTER IS NULL ");
  1122. }
  1123. else
  1124. {
  1125. tp_catcher = (Catcher *)dispatch_control_node.mp_dispatch_device.get();
  1126. tp_catcher_task = (Catcher_task *)dispatch_control_node.mp_dispatch_task.get();
  1127. }
  1128. // std::cout << " huli test :::: " << " tp_catcher_task->m_step = " << tp_catcher_task->m_step << std::endl;
  1129. // getchar();
  1130. if ( tp_catcher_task->m_step == 0 )
  1131. {
  1132. return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_catcher_task->m_step);
  1133. }
  1134. if ( tp_catcher_task->m_step == 1 )//检查姿态
  1135. {
  1136. if ( tp_catcher->m_actual_load_status == Dispatch_device_base::Load_status::HAVE_CAR &&
  1137. tp_catcher->m_actual_clamp_motion1 == Dispatch_device_base::Clamp_motion::E_CLAMP_TIGHT &&
  1138. (Common_data::approximate_difference(tp_catcher->m_actual_b, 90, DISPATCH_DEFAULT_DIFFERENCE) || Common_data::approximate_difference(tp_catcher->m_actual_b, 270, DISPATCH_DEFAULT_DIFFERENCE) ) )
  1139. {
  1140. //检测正常, 直接跳过即可
  1141. tp_catcher_task->m_step +=2;
  1142. }
  1143. else
  1144. {
  1145. return Error_manager(Error_code::CATCHER_POSE_ERROR, Error_level::MINOR_ERROR,
  1146. "Dispatch_process::excute_robot_put_car_to_carrier() fun error ");
  1147. }
  1148. }
  1149. if ( tp_catcher_task->m_step == 2 )
  1150. {
  1151. return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_catcher_task->m_step);
  1152. }
  1153. if ( tp_catcher_task->m_step == 3 )//机器手 调整z轴
  1154. {
  1155. if ( tp_catcher->m_actual_z*1.02 >= dispatch_control_node.m_destination_coordinates.z )
  1156. {
  1157. //检测正常, 直接跳过即可
  1158. tp_catcher_task->m_step +=2;
  1159. }
  1160. else
  1161. {
  1162. catcher_move_z(dispatch_control_node, tp_catcher, tp_catcher_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_catcher_4th_floor_z);
  1163. tp_catcher_task->m_step++;
  1164. }
  1165. }
  1166. if ( tp_catcher_task->m_step == 4 )
  1167. {
  1168. return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_catcher_task->m_step);
  1169. }
  1170. if ( tp_catcher_task->m_step == 5 )//机器手调整到 准备把车放到搬运器的姿态
  1171. {
  1172. float temp_y = tp_dispatch_coordinates->m_carrier_default_y1_back - (m_wheel_base /2);
  1173. if ( Common_data::approximate_difference(tp_catcher->m_actual_x, dispatch_control_node.m_destination_coordinates.x, DISPATCH_DEFAULT_DIFFERENCE) &&
  1174. Common_data::approximate_difference(tp_catcher->m_actual_y, temp_y, DISPATCH_DEFAULT_DIFFERENCE))
  1175. {
  1176. //检测正常, 直接跳过即可
  1177. tp_catcher_task->m_step +=2;
  1178. }
  1179. else
  1180. {
  1181. catcher_adjust_to_ground(dispatch_control_node, tp_catcher, tp_catcher_task, tp_dispatch_coordinates);
  1182. tp_catcher_task->m_step++;
  1183. }
  1184. }
  1185. if ( tp_catcher_task->m_step == 6 )
  1186. {
  1187. return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_catcher_task->m_step);
  1188. }
  1189. if ( tp_catcher_task->m_step == 7 )//机器手 z轴下降
  1190. {
  1191. catcher_move_z(dispatch_control_node, tp_catcher, tp_catcher_task, tp_dispatch_coordinates, dispatch_control_node.m_destination_coordinates.z);
  1192. tp_catcher_task->m_step++;
  1193. }
  1194. if ( tp_catcher_task->m_step == 8 )
  1195. {
  1196. return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_catcher_task->m_step);
  1197. }
  1198. if ( tp_catcher_task->m_step == 9 )//机器手 松开夹杆
  1199. {
  1200. catcher_move_c(dispatch_control_node, tp_catcher, tp_catcher_task, tp_dispatch_coordinates, Catcher_task::Clamp_motion::E_CLAMP_LOOSE);
  1201. tp_catcher_task->m_step++;
  1202. }
  1203. if ( tp_catcher_task->m_step == 10 )
  1204. {
  1205. return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_catcher_task->m_step);
  1206. }
  1207. if ( tp_catcher_task->m_step == 11 )//机器手 z轴上升
  1208. {
  1209. catcher_move_z(dispatch_control_node, tp_catcher, tp_catcher_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_catcher_4th_floor_z);
  1210. tp_catcher_task->m_step++;
  1211. }
  1212. if ( tp_catcher_task->m_step == 12 )
  1213. {
  1214. return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_catcher_task->m_step);
  1215. }
  1216. if ( tp_catcher_task->m_step == 13 )
  1217. {
  1218. return Error_code::SUCCESS;
  1219. }
  1220. return Error_code::SUCCESS;
  1221. }
  1222. //机器手的自由移动(例如:机器手移动到6号出入口的上方4楼处,给其他设备避让)(不进行抓车和放车的操作)
  1223. Error_manager Dispatch_process::excute_robot_move(Dispatch_control_node & dispatch_control_node)
  1224. {
  1225. Error_manager t_error;
  1226. Catcher * tp_catcher = NULL;
  1227. Catcher_task * tp_catcher_task = NULL;
  1228. Dispatch_coordinates * tp_dispatch_coordinates = Dispatch_coordinates::get_instance_pointer();
  1229. if ( dispatch_control_node.mp_dispatch_device.get() == NULL || dispatch_control_node.mp_dispatch_task.get() == NULL )
  1230. {
  1231. return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
  1232. "Dispatch_process::excute_robot_catch_car_from_inlet() POINTER IS NULL ");
  1233. }
  1234. else
  1235. {
  1236. tp_catcher = (Catcher *)dispatch_control_node.mp_dispatch_device.get();
  1237. tp_catcher_task = (Catcher_task *)dispatch_control_node.mp_dispatch_task.get();
  1238. }
  1239. if ( tp_catcher_task->m_step == 0 )
  1240. {
  1241. return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_catcher_task->m_step);
  1242. }
  1243. if ( tp_catcher_task->m_step == 1 )//检查姿态
  1244. {
  1245. if ( tp_catcher->m_actual_d1 + tp_catcher->m_actual_d2 + tp_dispatch_coordinates->m_catcher_d1_d2_distance >
  1246. tp_dispatch_coordinates->m_catcher_wheel_base_limit ||
  1247. tp_catcher->m_catcher_direction == Dispatch_device_base::Catcher_direction::CATCHER_DIRECTION_UNKNOW )
  1248. {
  1249. catcher_adjust_to_ready(dispatch_control_node, tp_catcher, tp_catcher_task, tp_dispatch_coordinates);
  1250. tp_catcher_task->m_step++;
  1251. }
  1252. else
  1253. {
  1254. tp_catcher_task->m_step +=2;
  1255. }
  1256. }
  1257. if ( tp_catcher_task->m_step == 2 )
  1258. {
  1259. return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_catcher_task->m_step);
  1260. }
  1261. if ( tp_catcher_task->m_step == 3 )//机器手 调整z轴
  1262. {
  1263. catcher_move_z(dispatch_control_node, tp_catcher, tp_catcher_task, tp_dispatch_coordinates, dispatch_control_node.m_destination_coordinates.z);
  1264. tp_catcher_task->m_step++;
  1265. }
  1266. if ( tp_catcher_task->m_step == 4 )
  1267. {
  1268. return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_catcher_task->m_step);
  1269. }
  1270. if ( tp_catcher_task->m_step == 5 )//机器手调整到 准备把车放到搬运器的姿态
  1271. {
  1272. catcher_adjust_to_ground(dispatch_control_node, tp_catcher, tp_catcher_task, tp_dispatch_coordinates);
  1273. tp_catcher_task->m_step++;
  1274. }
  1275. if ( tp_catcher_task->m_step == 6 )
  1276. {
  1277. return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_catcher_task->m_step);
  1278. }
  1279. if ( tp_catcher_task->m_step == 7 )
  1280. {
  1281. return Error_code::SUCCESS;
  1282. }
  1283. return Error_code::SUCCESS;
  1284. }
  1285. //搬运器从机器手上接车(例如:搬运器移动到2号入口的上方,然后等待机器手把车放到中跑车上,小跑车保持松夹杆状态)
  1286. Error_manager Dispatch_process::excute_carrier_receive_car_from_robot(Dispatch_control_node & dispatch_control_node)
  1287. {
  1288. Error_manager t_error;
  1289. Carrier * tp_carrier = NULL;
  1290. Carrier_task * tp_carrier_task = NULL;
  1291. Dispatch_coordinates * tp_dispatch_coordinates = Dispatch_coordinates::get_instance_pointer();
  1292. if ( dispatch_control_node.mp_dispatch_device.get() == NULL || dispatch_control_node.mp_dispatch_task.get() == NULL )
  1293. {
  1294. return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
  1295. "Dispatch_process::excute_robot_catch_car_from_inlet() POINTER IS NULL ");
  1296. }
  1297. else
  1298. {
  1299. tp_carrier = (Carrier *)dispatch_control_node.mp_dispatch_device.get();
  1300. tp_carrier_task = (Carrier_task *)dispatch_control_node.mp_dispatch_task.get();
  1301. }
  1302. // std::cout << " huli test :::: " << " tp_catcher_task->m_step = " << tp_carrier_task->m_step << std::endl;
  1303. // getchar();
  1304. if ( tp_carrier_task->m_step == 0 )
  1305. {
  1306. return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step);
  1307. }
  1308. if ( tp_carrier_task->m_step == 1 )//检查姿态
  1309. {
  1310. //检查姿态
  1311. if ( tp_carrier->m_actual_load_status == Dispatch_device_base::Load_status::NO_CAR )
  1312. {
  1313. if ( tp_carrier_task->m_request_clamp_motion == Carrier_task::Clamp_motion::E_CLAMP_LOOSE &&
  1314. Common_data::approximate_difference(m_wheel_base, tp_carrier->m_actual_y1-tp_carrier->m_actual_y2, DISPATCH_DEFAULT_DIFFERENCE))
  1315. {
  1316. tp_carrier_task->m_step +=2;
  1317. }
  1318. else
  1319. {
  1320. carrier_adjust_to_ready(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates);
  1321. tp_carrier_task->m_step++;
  1322. }
  1323. }
  1324. else
  1325. {
  1326. return Error_manager(Error_code::CARRIER_POSE_ERROR, Error_level::MINOR_ERROR,
  1327. "tp_carrier->m_actual_load_status != Dispatch_device_base::Load_status::NO_CAR fun error ");
  1328. }
  1329. }
  1330. if ( tp_carrier_task->m_step == 2 )
  1331. {
  1332. return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step);
  1333. }
  1334. if ( tp_carrier_task->m_step == 3 )//让小跑车回到中跑车上
  1335. {
  1336. if ( Common_data::approximate_difference(tp_carrier->m_actual_y, tp_dispatch_coordinates->m_carrier_default_y_back, DISPATCH_DEFAULT_DIFFERENCE) )
  1337. {
  1338. tp_carrier_task->m_step +=2;
  1339. }
  1340. else
  1341. {
  1342. carrier_move_y(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_carrier_default_y_back);
  1343. tp_carrier_task->m_step++;
  1344. }
  1345. }
  1346. if ( tp_carrier_task->m_step == 4 )
  1347. {
  1348. return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step);
  1349. }
  1350. if ( tp_carrier_task->m_step == 5 )//让中跑车回到电梯井
  1351. {
  1352. if ( Common_data::approximate_difference(tp_carrier->m_actual_z, dispatch_control_node.m_destination_coordinates.z, DISPATCH_DEFAULT_DIFFERENCE) )
  1353. {
  1354. tp_carrier_task->m_step +=2;
  1355. }
  1356. else
  1357. {
  1358. if ( dispatch_control_node.mp_dispatch_device->get_device_id() == 0 )
  1359. {
  1360. carrier_move_x(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_carrier_default_x_left);
  1361. tp_carrier_task->m_step++;
  1362. }
  1363. else if ( dispatch_control_node.mp_dispatch_device->get_device_id() == 1 )
  1364. {
  1365. carrier_move_x(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_carrier_default_x_right);
  1366. tp_carrier_task->m_step++;
  1367. }
  1368. else
  1369. {
  1370. return Error_manager(Error_code::CARRIER_CONRTOL_PARAMETER_ERROR, Error_level::MINOR_ERROR,
  1371. " m_destination_coordinates.z PARAMRTER ERROR ");
  1372. }
  1373. }
  1374. }
  1375. if ( tp_carrier_task->m_step == 6 )
  1376. {
  1377. return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step);
  1378. }
  1379. if ( tp_carrier_task->m_step == 7 )//收回对接,之后中跑车固定在电梯上不能X轴移动,电梯可以Z轴移动
  1380. {
  1381. if ( Common_data::approximate_difference(tp_carrier->m_actual_z, dispatch_control_node.m_destination_coordinates.z, DISPATCH_DEFAULT_DIFFERENCE) )
  1382. {
  1383. tp_carrier_task->m_step +=2;
  1384. }
  1385. else
  1386. {
  1387. carrier_joint_x(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, Carrier_task::Joint_motion::E_JOINT_TAKE_BACK);
  1388. tp_carrier_task->m_step++;
  1389. }
  1390. }
  1391. if ( tp_carrier_task->m_step == 8 )
  1392. {
  1393. return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step);
  1394. }
  1395. if ( tp_carrier_task->m_step == 9 )//电梯移动到对应的楼层
  1396. {
  1397. if ( Common_data::approximate_difference(tp_carrier->m_actual_z, dispatch_control_node.m_destination_coordinates.z, DISPATCH_DEFAULT_DIFFERENCE) )
  1398. {
  1399. tp_carrier_task->m_step +=2;
  1400. }
  1401. else
  1402. {
  1403. carrier_move_z(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, dispatch_control_node.m_destination_coordinates.z);
  1404. tp_carrier_task->m_step++;
  1405. }
  1406. }
  1407. if ( tp_carrier_task->m_step == 10 )
  1408. {
  1409. return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step);
  1410. }
  1411. if ( tp_carrier_task->m_step == 11 )//伸出对接,之后中跑车可以x轴移动,电梯不能Z轴移动
  1412. {
  1413. if ( tp_carrier->m_actual_joint_motion_x1 == Dispatch_device_base::Joint_motion::E_JOINT_TAKE_BACK )
  1414. {
  1415. carrier_joint_x(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, Carrier_task::Joint_motion::E_JOINT_HOLD_OUT);
  1416. tp_carrier_task->m_step++;
  1417. }
  1418. else
  1419. {
  1420. tp_carrier_task->m_step +=2;
  1421. }
  1422. }
  1423. if ( tp_carrier_task->m_step == 12 )
  1424. {
  1425. return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step);
  1426. }
  1427. if ( tp_carrier_task->m_step == 13 )//中跑车 x轴移动
  1428. {
  1429. if ( Common_data::approximate_difference(tp_carrier->m_actual_x, dispatch_control_node.m_destination_coordinates.x, DISPATCH_DEFAULT_DIFFERENCE) )
  1430. {
  1431. tp_carrier_task->m_step +=2;
  1432. }
  1433. else
  1434. {
  1435. carrier_move_x(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, dispatch_control_node.m_destination_coordinates.x);
  1436. tp_carrier_task->m_step++;
  1437. }
  1438. }
  1439. if ( tp_carrier_task->m_step == 14 )
  1440. {
  1441. return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step);
  1442. }
  1443. if ( tp_carrier_task->m_step == 15 )
  1444. {
  1445. return Error_code::SUCCESS;
  1446. }
  1447. return Error_code::SUCCESS;
  1448. }
  1449. //搬运器把车存到停车位上(例如:小跑车夹车后,搬运器移动到56号停车位,然后小跑车将车存入车位,之后搬运器退回至电梯井)
  1450. Error_manager Dispatch_process::excute_carrier_store_car_to_parkingspace(Dispatch_control_node & dispatch_control_node)
  1451. {
  1452. Error_manager t_error;
  1453. Carrier * tp_carrier = NULL;
  1454. Carrier_task * tp_carrier_task = NULL;
  1455. Dispatch_coordinates * tp_dispatch_coordinates = Dispatch_coordinates::get_instance_pointer();
  1456. if ( dispatch_control_node.mp_dispatch_device.get() == NULL || dispatch_control_node.mp_dispatch_task.get() == NULL )
  1457. {
  1458. return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
  1459. "Dispatch_process::excute_robot_catch_car_from_inlet() POINTER IS NULL ");
  1460. }
  1461. else
  1462. {
  1463. tp_carrier = (Carrier *)dispatch_control_node.mp_dispatch_device.get();
  1464. tp_carrier_task = (Carrier_task *)dispatch_control_node.mp_dispatch_task.get();
  1465. }
  1466. // std::cout << " huli test :::: " << " tp_catcher_task->m_step = " << tp_carrier_task->m_step << std::endl;
  1467. // getchar();
  1468. if ( tp_carrier_task->m_step == 0 )
  1469. {
  1470. return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step);
  1471. }
  1472. if ( tp_carrier_task->m_step == 1 )//检查姿态
  1473. {
  1474. //检查姿态(注注注注注意了, 调试阶段是无车, 真实环境是有车的)
  1475. if ( tp_carrier->m_actual_load_status == Dispatch_device_base::Load_status::NO_CAR &&
  1476. tp_carrier->m_actual_clamp_motion1 == Dispatch_device_base::Clamp_motion::E_CLAMP_LOOSE)
  1477. {
  1478. //检测正常, 直接跳过即可
  1479. tp_carrier_task->m_step +=2;
  1480. }
  1481. //注意了, 如果是去7号出口, 那么就直接跳过轮距修正和夹车.
  1482. else if(dispatch_control_node.m_dispatch_control_request_msg.dispatch_destination() == 1107)
  1483. {
  1484. tp_carrier_task->m_step = 7;
  1485. }
  1486. else
  1487. {
  1488. return Error_manager(Error_code::CARRIER_POSE_ERROR, Error_level::MINOR_ERROR,
  1489. "Dispatch_process::excute_carrier_store_car_to_parkingspace() fun error ");
  1490. }
  1491. }
  1492. if ( tp_carrier_task->m_step == 2 )
  1493. {
  1494. return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step);
  1495. }
  1496. if ( tp_carrier_task->m_step == 3 )//修正轴距
  1497. {
  1498. float temp_wheel_base = tp_carrier->m_actual_y1 - tp_carrier->m_actual_y2;
  1499. if ( Common_data::approximate_difference(m_wheel_base, temp_wheel_base, DISPATCH_DEFAULT_DIFFERENCE) )
  1500. {
  1501. tp_carrier_task->m_step +=2;
  1502. }
  1503. else
  1504. {
  1505. carrier_adjust_wheel_base(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates);
  1506. tp_carrier_task->m_step++;
  1507. }
  1508. }
  1509. if ( tp_carrier_task->m_step == 4 )
  1510. {
  1511. return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step);
  1512. }
  1513. if ( tp_carrier_task->m_step == 5 )//让小跑车 夹车
  1514. {
  1515. carrier_move_c(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, Carrier_task::Clamp_motion::E_CLAMP_TIGHT);
  1516. tp_carrier_task->m_step++;
  1517. }
  1518. if ( tp_carrier_task->m_step == 6 )
  1519. {
  1520. return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step);
  1521. }
  1522. if ( tp_carrier_task->m_step == 7 )//让中跑车回到电梯井
  1523. {
  1524. if ( Common_data::approximate_difference(tp_carrier->m_actual_z, dispatch_control_node.m_destination_coordinates.z, DISPATCH_DEFAULT_DIFFERENCE) )
  1525. {
  1526. tp_carrier_task->m_step +=2;
  1527. }
  1528. else
  1529. {
  1530. if ( dispatch_control_node.mp_dispatch_device->get_device_id() == 0 )
  1531. {
  1532. carrier_move_x(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_carrier_default_x_left);
  1533. tp_carrier_task->m_step++;
  1534. }
  1535. else if ( dispatch_control_node.mp_dispatch_device->get_device_id() == 1 )
  1536. {
  1537. carrier_move_x(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_carrier_default_x_right);
  1538. tp_carrier_task->m_step++;
  1539. }
  1540. else
  1541. {
  1542. return Error_manager(Error_code::CARRIER_CONRTOL_PARAMETER_ERROR, Error_level::MINOR_ERROR,
  1543. " m_destination_coordinates.z PARAMRTER ERROR ");
  1544. }
  1545. }
  1546. }
  1547. if ( tp_carrier_task->m_step == 8 )
  1548. {
  1549. return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step);
  1550. }
  1551. if ( tp_carrier_task->m_step == 9 )//收回对接,之后中跑车固定在电梯上不能X轴移动,电梯可以Z轴移动
  1552. {
  1553. if ( Common_data::approximate_difference(tp_carrier->m_actual_z, dispatch_control_node.m_destination_coordinates.z, DISPATCH_DEFAULT_DIFFERENCE) )
  1554. {
  1555. tp_carrier_task->m_step +=2;
  1556. }
  1557. else
  1558. {
  1559. carrier_joint_x(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, Carrier_task::Joint_motion::E_JOINT_TAKE_BACK);
  1560. tp_carrier_task->m_step++;
  1561. }
  1562. }
  1563. if ( tp_carrier_task->m_step == 10 )
  1564. {
  1565. return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step);
  1566. }
  1567. if ( tp_carrier_task->m_step == 11 )//电梯移动到对应的楼层
  1568. {
  1569. if ( Common_data::approximate_difference(tp_carrier->m_actual_z, dispatch_control_node.m_destination_coordinates.z, DISPATCH_DEFAULT_DIFFERENCE) )
  1570. {
  1571. tp_carrier_task->m_step +=2;
  1572. }
  1573. else
  1574. {
  1575. carrier_move_z(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, dispatch_control_node.m_destination_coordinates.z);
  1576. tp_carrier_task->m_step++;
  1577. }
  1578. }
  1579. if ( tp_carrier_task->m_step == 12 )
  1580. {
  1581. return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step);
  1582. }
  1583. if ( tp_carrier_task->m_step == 13 )//伸出对接,之后中跑车可以x轴移动,电梯不能Z轴移动
  1584. {
  1585. if ( tp_carrier->m_actual_joint_motion_x1 == Dispatch_device_base::Joint_motion::E_JOINT_TAKE_BACK )
  1586. {
  1587. carrier_joint_x(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, Carrier_task::Joint_motion::E_JOINT_HOLD_OUT);
  1588. tp_carrier_task->m_step++;
  1589. }
  1590. else
  1591. {
  1592. tp_carrier_task->m_step +=2;
  1593. }
  1594. }
  1595. if ( tp_carrier_task->m_step == 14 )
  1596. {
  1597. return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step);
  1598. }
  1599. if ( tp_carrier_task->m_step == 15 )//中跑车 x轴移动
  1600. {
  1601. if ( Common_data::approximate_difference(tp_carrier->m_actual_x, dispatch_control_node.m_destination_coordinates.x, DISPATCH_DEFAULT_DIFFERENCE) )
  1602. {
  1603. tp_carrier_task->m_step +=2;
  1604. }
  1605. else
  1606. {
  1607. carrier_move_x(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, dispatch_control_node.m_destination_coordinates.x);
  1608. tp_carrier_task->m_step++;
  1609. }
  1610. }
  1611. if ( tp_carrier_task->m_step == 16 )
  1612. {
  1613. return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step);
  1614. }
  1615. if ( tp_carrier_task->m_step == 17 )//小跑车 进入车位
  1616. {
  1617. carrier_move_y(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_carrier_default_y_leave);
  1618. tp_carrier_task->m_step++;
  1619. }
  1620. if ( tp_carrier_task->m_step == 18 )
  1621. {
  1622. return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step);
  1623. }
  1624. if ( tp_carrier_task->m_step == 19 )//小跑车 松开夹杆
  1625. {
  1626. carrier_move_c(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, Carrier_task::Clamp_motion::E_CLAMP_LOOSE);
  1627. tp_carrier_task->m_step++;
  1628. }
  1629. if ( tp_carrier_task->m_step == 20 )
  1630. {
  1631. return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step);
  1632. }
  1633. if ( tp_carrier_task->m_step == 21 )//小跑车 回到中跑车
  1634. {
  1635. carrier_move_y(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_carrier_default_y_back);
  1636. tp_carrier_task->m_step++;
  1637. }
  1638. if ( tp_carrier_task->m_step == 22 )
  1639. {
  1640. return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step);
  1641. }
  1642. if ( tp_carrier_task->m_step == 23 )//让中跑车回到电梯井
  1643. {
  1644. if ( dispatch_control_node.mp_dispatch_device->get_device_id() == 0 )
  1645. {
  1646. carrier_move_x(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_carrier_default_x_left);
  1647. tp_carrier_task->m_step++;
  1648. }
  1649. else if ( dispatch_control_node.mp_dispatch_device->get_device_id() == 1 )
  1650. {
  1651. carrier_move_x(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_carrier_default_x_right);
  1652. tp_carrier_task->m_step++;
  1653. }
  1654. else
  1655. {
  1656. return Error_manager(Error_code::CARRIER_CONRTOL_PARAMETER_ERROR, Error_level::MINOR_ERROR,
  1657. " m_destination_coordinates.z PARAMRTER ERROR ");
  1658. }
  1659. }
  1660. if ( tp_carrier_task->m_step == 24 )
  1661. {
  1662. return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step);
  1663. }
  1664. if ( tp_carrier_task->m_step == 25 )//收回对接,之后中跑车固定在电梯上不能X轴移动,电梯可以Z轴移动
  1665. {
  1666. carrier_joint_x(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, Carrier_task::Joint_motion::E_JOINT_TAKE_BACK);
  1667. tp_carrier_task->m_step++;
  1668. }
  1669. if ( tp_carrier_task->m_step == 26 )
  1670. {
  1671. return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step);
  1672. }
  1673. if ( tp_carrier_task->m_step == 27 )
  1674. {
  1675. return Error_code::SUCCESS;
  1676. }
  1677. return Error_code::SUCCESS;
  1678. }
  1679. //搬运器把车存到停车位上(例如:小跑车夹车后,搬运器移动到56号停车位,然后小跑车将车存入车位,之后搬运器退回至56车位外面即可)
  1680. Error_manager Dispatch_process::excute_carrier_store_car_to_parkingspace_ex(Dispatch_control_node & dispatch_control_node)
  1681. {
  1682. Error_manager t_error;
  1683. Carrier * tp_carrier = NULL;
  1684. Carrier_task * tp_carrier_task = NULL;
  1685. Dispatch_coordinates * tp_dispatch_coordinates = Dispatch_coordinates::get_instance_pointer();
  1686. if ( dispatch_control_node.mp_dispatch_device.get() == NULL || dispatch_control_node.mp_dispatch_task.get() == NULL )
  1687. {
  1688. return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
  1689. "Dispatch_process::excute_robot_catch_car_from_inlet() POINTER IS NULL ");
  1690. }
  1691. else
  1692. {
  1693. tp_carrier = (Carrier *)dispatch_control_node.mp_dispatch_device.get();
  1694. tp_carrier_task = (Carrier_task *)dispatch_control_node.mp_dispatch_task.get();
  1695. }
  1696. // std::cout << " huli test :::: " << " tp_carrier_task->m_step = " << tp_carrier_task->m_step << std::endl;
  1697. // getchar();
  1698. if ( tp_carrier_task->m_step == 0 )
  1699. {
  1700. return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step);
  1701. }
  1702. if ( tp_carrier_task->m_step == 1 )//检查姿态
  1703. {
  1704. //检查姿态(注注注注注意了, 调试阶段是无车, 真实环境是有车的)
  1705. if ( tp_carrier->m_actual_load_status == Dispatch_device_base::Load_status::NO_CAR &&
  1706. tp_carrier->m_actual_clamp_motion1 == Dispatch_device_base::Clamp_motion::E_CLAMP_LOOSE)
  1707. {
  1708. //检测正常, 直接跳过即可
  1709. tp_carrier_task->m_step +=2;
  1710. }
  1711. //注意了, 如果是去7号出口, 那么就直接跳过轮距修正和夹车.
  1712. else if(dispatch_control_node.m_dispatch_control_request_msg.dispatch_destination() == 1107)
  1713. {
  1714. tp_carrier_task->m_step = 7;
  1715. }
  1716. else
  1717. {
  1718. return Error_manager(Error_code::CARRIER_POSE_ERROR, Error_level::MINOR_ERROR,
  1719. "Dispatch_process::excute_carrier_store_car_to_parkingspace() fun error ");
  1720. }
  1721. }
  1722. if ( tp_carrier_task->m_step == 2 )
  1723. {
  1724. return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step);
  1725. }
  1726. if ( tp_carrier_task->m_step == 3 )//修正轴距
  1727. {
  1728. float temp_wheel_base = tp_carrier->m_actual_y1 - tp_carrier->m_actual_y2;
  1729. if ( Common_data::approximate_difference(m_wheel_base, temp_wheel_base, DISPATCH_DEFAULT_DIFFERENCE) )
  1730. {
  1731. tp_carrier_task->m_step +=2;
  1732. }
  1733. else
  1734. {
  1735. carrier_adjust_wheel_base(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates);
  1736. tp_carrier_task->m_step++;
  1737. }
  1738. }
  1739. if ( tp_carrier_task->m_step == 4 )
  1740. {
  1741. return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step);
  1742. }
  1743. if ( tp_carrier_task->m_step == 5 )//让小跑车 夹车
  1744. {
  1745. carrier_move_c(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, Carrier_task::Clamp_motion::E_CLAMP_TIGHT);
  1746. tp_carrier_task->m_step++;
  1747. }
  1748. if ( tp_carrier_task->m_step == 6 )
  1749. {
  1750. return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step);
  1751. }
  1752. if ( tp_carrier_task->m_step == 7 )//让中跑车回到电梯井
  1753. {
  1754. if ( Common_data::approximate_difference(tp_carrier->m_actual_z, dispatch_control_node.m_destination_coordinates.z, DISPATCH_DEFAULT_DIFFERENCE) )
  1755. {
  1756. tp_carrier_task->m_step +=2;
  1757. }
  1758. else
  1759. {
  1760. if ( dispatch_control_node.mp_dispatch_device->get_device_id() == 0 )
  1761. {
  1762. carrier_move_x(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_carrier_default_x_left);
  1763. tp_carrier_task->m_step++;
  1764. }
  1765. else if ( dispatch_control_node.mp_dispatch_device->get_device_id() == 1 )
  1766. {
  1767. carrier_move_x(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_carrier_default_x_right);
  1768. tp_carrier_task->m_step++;
  1769. }
  1770. else
  1771. {
  1772. return Error_manager(Error_code::CARRIER_CONRTOL_PARAMETER_ERROR, Error_level::MINOR_ERROR,
  1773. " dispatch_control_node.m_destination_coordinates.z PARAMRTER ERROR ");
  1774. }
  1775. }
  1776. }
  1777. if ( tp_carrier_task->m_step == 8 )
  1778. {
  1779. return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step);
  1780. }
  1781. if ( tp_carrier_task->m_step == 9 )//收回对接,之后中跑车固定在电梯上不能X轴移动,电梯可以Z轴移动
  1782. {
  1783. if ( Common_data::approximate_difference(tp_carrier->m_actual_z, dispatch_control_node.m_destination_coordinates.z, DISPATCH_DEFAULT_DIFFERENCE) )
  1784. {
  1785. tp_carrier_task->m_step +=2;
  1786. }
  1787. else
  1788. {
  1789. carrier_joint_x(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, Carrier_task::Joint_motion::E_JOINT_TAKE_BACK);
  1790. tp_carrier_task->m_step++;
  1791. }
  1792. }
  1793. if ( tp_carrier_task->m_step == 10 )
  1794. {
  1795. return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step);
  1796. }
  1797. if ( tp_carrier_task->m_step == 11 )//电梯移动到对应的楼层
  1798. {
  1799. if ( Common_data::approximate_difference(tp_carrier->m_actual_z, dispatch_control_node.m_destination_coordinates.z, DISPATCH_DEFAULT_DIFFERENCE) )
  1800. {
  1801. tp_carrier_task->m_step +=2;
  1802. }
  1803. else
  1804. {
  1805. carrier_move_z(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, dispatch_control_node.m_destination_coordinates.z);
  1806. tp_carrier_task->m_step++;
  1807. }
  1808. }
  1809. if ( tp_carrier_task->m_step == 12 )
  1810. {
  1811. return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step);
  1812. }
  1813. if ( tp_carrier_task->m_step == 13 )//伸出对接,之后中跑车可以x轴移动,电梯不能Z轴移动
  1814. {
  1815. if ( tp_carrier->m_actual_joint_motion_x1 == Dispatch_device_base::Joint_motion::E_JOINT_TAKE_BACK )
  1816. {
  1817. carrier_joint_x(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, Carrier_task::Joint_motion::E_JOINT_HOLD_OUT);
  1818. tp_carrier_task->m_step++;
  1819. }
  1820. else
  1821. {
  1822. tp_carrier_task->m_step +=2;
  1823. }
  1824. }
  1825. if ( tp_carrier_task->m_step == 14 )
  1826. {
  1827. return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step);
  1828. }
  1829. if ( tp_carrier_task->m_step == 15 )//中跑车 x轴移动
  1830. {
  1831. if ( Common_data::approximate_difference(tp_carrier->m_actual_x, dispatch_control_node.m_destination_coordinates.x, DISPATCH_DEFAULT_DIFFERENCE) )
  1832. {
  1833. tp_carrier_task->m_step +=2;
  1834. }
  1835. else
  1836. {
  1837. carrier_move_x(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, dispatch_control_node.m_destination_coordinates.x);
  1838. tp_carrier_task->m_step++;
  1839. }
  1840. }
  1841. if ( tp_carrier_task->m_step == 16 )
  1842. {
  1843. return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step);
  1844. }
  1845. if ( tp_carrier_task->m_step == 17 )//小跑车 进入车位
  1846. {
  1847. carrier_move_y(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_carrier_default_y_leave);
  1848. tp_carrier_task->m_step++;
  1849. }
  1850. if ( tp_carrier_task->m_step == 18 )
  1851. {
  1852. return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step);
  1853. }
  1854. if ( tp_carrier_task->m_step == 19 )//小跑车 松开夹杆
  1855. {
  1856. carrier_move_c(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, Carrier_task::Clamp_motion::E_CLAMP_LOOSE);
  1857. tp_carrier_task->m_step++;
  1858. }
  1859. if ( tp_carrier_task->m_step == 20 )
  1860. {
  1861. return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step);
  1862. }
  1863. if ( tp_carrier_task->m_step == 21 )//小跑车 回到中跑车
  1864. {
  1865. carrier_move_y(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_carrier_default_y_back);
  1866. tp_carrier_task->m_step++;
  1867. }
  1868. if ( tp_carrier_task->m_step == 22 )
  1869. {
  1870. return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step);
  1871. }
  1872. if ( tp_carrier_task->m_step == 23 )
  1873. {
  1874. return Error_code::SUCCESS;
  1875. }
  1876. return Error_code::SUCCESS;
  1877. }
  1878. //搬运器从停车位上取车(例如:搬运器移动到56号停车位,然后小跑车将车从车位取出,之后搬运器退回至电梯井)
  1879. Error_manager Dispatch_process::excute_carrier_pickup_car_from_parkingspace(Dispatch_control_node & dispatch_control_node)
  1880. {
  1881. Error_manager t_error;
  1882. Carrier * tp_carrier = NULL;
  1883. Carrier_task * tp_carrier_task = NULL;
  1884. Dispatch_coordinates * tp_dispatch_coordinates = Dispatch_coordinates::get_instance_pointer();
  1885. if ( dispatch_control_node.mp_dispatch_device.get() == NULL || dispatch_control_node.mp_dispatch_task.get() == NULL )
  1886. {
  1887. return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
  1888. "Dispatch_process::excute_robot_catch_car_from_inlet() POINTER IS NULL ");
  1889. }
  1890. else
  1891. {
  1892. tp_carrier = (Carrier *)dispatch_control_node.mp_dispatch_device.get();
  1893. tp_carrier_task = (Carrier_task *)dispatch_control_node.mp_dispatch_task.get();
  1894. }
  1895. // std::cout << " huli test :::: " << " tp_carrier_task->m_step = " << tp_carrier_task->m_step << std::endl;
  1896. // getchar();
  1897. if ( tp_carrier_task->m_step == 0 )
  1898. {
  1899. return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step);
  1900. }
  1901. if ( tp_carrier_task->m_step == 1 )//检查姿态
  1902. {
  1903. //检查姿态
  1904. if ( tp_carrier->m_actual_load_status == Dispatch_device_base::Load_status::NO_CAR )
  1905. {
  1906. if ( tp_carrier_task->m_request_clamp_motion == Carrier_task::Clamp_motion::E_CLAMP_LOOSE &&
  1907. Common_data::approximate_difference(m_wheel_base, tp_carrier->m_actual_y1-tp_carrier->m_actual_y2, DISPATCH_DEFAULT_DIFFERENCE))
  1908. {
  1909. tp_carrier_task->m_step +=2;
  1910. }
  1911. else
  1912. {
  1913. carrier_adjust_to_ready(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates);
  1914. tp_carrier_task->m_step++;
  1915. }
  1916. }
  1917. else
  1918. {
  1919. return Error_manager(Error_code::CARRIER_POSE_ERROR, Error_level::MINOR_ERROR,
  1920. "tp_carrier->m_actual_load_status != Dispatch_device_base::Load_status::NO_CAR fun error ");
  1921. }
  1922. }
  1923. if ( tp_carrier_task->m_step == 2 )
  1924. {
  1925. return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step);
  1926. }
  1927. if ( tp_carrier_task->m_step == 3 )//让小跑车回到中跑车上
  1928. {
  1929. if ( Common_data::approximate_difference(tp_carrier->m_actual_y, tp_dispatch_coordinates->m_carrier_default_y_back, DISPATCH_DEFAULT_DIFFERENCE) )
  1930. {
  1931. tp_carrier_task->m_step +=2;
  1932. }
  1933. else
  1934. {
  1935. carrier_move_y(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_carrier_default_y_back);
  1936. tp_carrier_task->m_step++;
  1937. }
  1938. }
  1939. if ( tp_carrier_task->m_step == 4 )
  1940. {
  1941. return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step);
  1942. }
  1943. if ( tp_carrier_task->m_step == 5 )//让中跑车回到电梯井
  1944. {
  1945. if ( Common_data::approximate_difference(tp_carrier->m_actual_z, dispatch_control_node.m_destination_coordinates.z, DISPATCH_DEFAULT_DIFFERENCE) )
  1946. {
  1947. tp_carrier_task->m_step +=2;
  1948. }
  1949. else
  1950. {
  1951. if ( dispatch_control_node.mp_dispatch_device->get_device_id() == 0 )
  1952. {
  1953. carrier_move_x(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_carrier_default_x_left);
  1954. tp_carrier_task->m_step++;
  1955. }
  1956. else if ( dispatch_control_node.mp_dispatch_device->get_device_id() == 1 )
  1957. {
  1958. carrier_move_x(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_carrier_default_x_right);
  1959. tp_carrier_task->m_step++;
  1960. }
  1961. else
  1962. {
  1963. return Error_manager(Error_code::CARRIER_CONRTOL_PARAMETER_ERROR, Error_level::MINOR_ERROR,
  1964. " dispatch_control_node.m_destination_coordinates.z PARAMRTER ERROR ");
  1965. }
  1966. }
  1967. }
  1968. if ( tp_carrier_task->m_step == 6 )
  1969. {
  1970. return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step);
  1971. }
  1972. if ( tp_carrier_task->m_step == 7 )//收回对接,之后中跑车固定在电梯上不能X轴移动,电梯可以Z轴移动
  1973. {
  1974. if ( Common_data::approximate_difference(tp_carrier->m_actual_z, dispatch_control_node.m_destination_coordinates.z, DISPATCH_DEFAULT_DIFFERENCE) )
  1975. {
  1976. tp_carrier_task->m_step +=2;
  1977. }
  1978. else
  1979. {
  1980. carrier_joint_x(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, Carrier_task::Joint_motion::E_JOINT_TAKE_BACK);
  1981. tp_carrier_task->m_step++;
  1982. }
  1983. }
  1984. if ( tp_carrier_task->m_step == 8 )
  1985. {
  1986. return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step);
  1987. }
  1988. if ( tp_carrier_task->m_step == 9 )//电梯移动到对应的楼层
  1989. {
  1990. if ( Common_data::approximate_difference(tp_carrier->m_actual_z, dispatch_control_node.m_destination_coordinates.z, DISPATCH_DEFAULT_DIFFERENCE) )
  1991. {
  1992. tp_carrier_task->m_step +=2;
  1993. }
  1994. else
  1995. {
  1996. carrier_move_z(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, dispatch_control_node.m_destination_coordinates.z);
  1997. tp_carrier_task->m_step++;
  1998. }
  1999. }
  2000. if ( tp_carrier_task->m_step == 10 )
  2001. {
  2002. return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step);
  2003. }
  2004. if ( tp_carrier_task->m_step == 11 )//伸出对接,之后中跑车可以x轴移动,电梯不能Z轴移动
  2005. {
  2006. if ( tp_carrier->m_actual_joint_motion_x1 == Dispatch_device_base::Joint_motion::E_JOINT_TAKE_BACK )
  2007. {
  2008. carrier_joint_x(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, Carrier_task::Joint_motion::E_JOINT_HOLD_OUT);
  2009. tp_carrier_task->m_step++;
  2010. }
  2011. else
  2012. {
  2013. tp_carrier_task->m_step +=2;
  2014. }
  2015. }
  2016. if ( tp_carrier_task->m_step == 12 )
  2017. {
  2018. return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step);
  2019. }
  2020. if ( tp_carrier_task->m_step == 13 )//中跑车 x轴移动
  2021. {
  2022. if ( Common_data::approximate_difference(tp_carrier->m_actual_x, dispatch_control_node.m_destination_coordinates.x, DISPATCH_DEFAULT_DIFFERENCE) )
  2023. {
  2024. tp_carrier_task->m_step +=2;
  2025. }
  2026. else
  2027. {
  2028. carrier_move_x(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, dispatch_control_node.m_destination_coordinates.x);
  2029. tp_carrier_task->m_step++;
  2030. }
  2031. }
  2032. if ( tp_carrier_task->m_step == 14 )
  2033. {
  2034. return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step);
  2035. }
  2036. if ( tp_carrier_task->m_step == 15 )//小跑车 进入车位
  2037. {
  2038. carrier_move_y(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_carrier_default_y_leave);
  2039. tp_carrier_task->m_step++;
  2040. }
  2041. if ( tp_carrier_task->m_step == 16 )
  2042. {
  2043. return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step);
  2044. }
  2045. if ( tp_carrier_task->m_step == 17 )//小跑车 夹车
  2046. {
  2047. carrier_move_c(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, Carrier_task::Clamp_motion::E_CLAMP_TIGHT);
  2048. tp_carrier_task->m_step++;
  2049. }
  2050. if ( tp_carrier_task->m_step == 18 )
  2051. {
  2052. return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step);
  2053. }
  2054. if ( tp_carrier_task->m_step == 19 )//小跑车 回到中跑车
  2055. {
  2056. carrier_move_y(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_carrier_default_y_back);
  2057. tp_carrier_task->m_step++;
  2058. }
  2059. if ( tp_carrier_task->m_step == 20 )
  2060. {
  2061. return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step);
  2062. }
  2063. if ( tp_carrier_task->m_step == 21 )//让中跑车回到电梯井
  2064. {
  2065. if ( dispatch_control_node.mp_dispatch_device->get_device_id() == 0 )
  2066. {
  2067. carrier_move_x(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_carrier_default_x_left);
  2068. tp_carrier_task->m_step++;
  2069. }
  2070. else if ( dispatch_control_node.mp_dispatch_device->get_device_id() == 1 )
  2071. {
  2072. carrier_move_x(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_carrier_default_x_right);
  2073. tp_carrier_task->m_step++;
  2074. }
  2075. else
  2076. {
  2077. return Error_manager(Error_code::CARRIER_CONRTOL_PARAMETER_ERROR, Error_level::MINOR_ERROR,
  2078. " dispatch_control_node.m_destination_coordinates.z PARAMRTER ERROR ");
  2079. }
  2080. }
  2081. if ( tp_carrier_task->m_step == 22 )
  2082. {
  2083. return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step);
  2084. }
  2085. if ( tp_carrier_task->m_step == 23 )//收回对接,之后中跑车固定在电梯上不能X轴移动,电梯可以Z轴移动
  2086. {
  2087. carrier_joint_x(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, Carrier_task::Joint_motion::E_JOINT_TAKE_BACK);
  2088. tp_carrier_task->m_step++;
  2089. }
  2090. if ( tp_carrier_task->m_step == 24 )
  2091. {
  2092. return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step);
  2093. }
  2094. if ( tp_carrier_task->m_step == 25 )
  2095. {
  2096. return Error_code::SUCCESS;
  2097. }
  2098. return Error_code::SUCCESS;
  2099. }
  2100. //搬运器从停车位上取车(例如:搬运器移动到56号停车位,然后小跑车将车从车位取出,之后搬运器退回至56车位外面即可)
  2101. Error_manager Dispatch_process::excute_carrier_pickup_car_from_parkingspace_ex(Dispatch_control_node & dispatch_control_node)
  2102. {
  2103. Error_manager t_error;
  2104. Carrier * tp_carrier = NULL;
  2105. Carrier_task * tp_carrier_task = NULL;
  2106. Dispatch_coordinates * tp_dispatch_coordinates = Dispatch_coordinates::get_instance_pointer();
  2107. if ( dispatch_control_node.mp_dispatch_device.get() == NULL || dispatch_control_node.mp_dispatch_task.get() == NULL )
  2108. {
  2109. return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
  2110. "Dispatch_process::excute_robot_catch_car_from_inlet() POINTER IS NULL ");
  2111. }
  2112. else
  2113. {
  2114. tp_carrier = (Carrier *)dispatch_control_node.mp_dispatch_device.get();
  2115. tp_carrier_task = (Carrier_task *)dispatch_control_node.mp_dispatch_task.get();
  2116. }
  2117. // std::cout << " huli test :::: " << " tp_carrier_task->m_step = " << tp_carrier_task->m_step << std::endl;
  2118. // getchar();
  2119. if ( tp_carrier_task->m_step == 0 )
  2120. {
  2121. return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step);
  2122. }
  2123. if ( tp_carrier_task->m_step == 1 )//检查姿态
  2124. {
  2125. //检查姿态
  2126. if ( tp_carrier->m_actual_load_status == Dispatch_device_base::Load_status::NO_CAR )
  2127. {
  2128. if ( tp_carrier_task->m_request_clamp_motion == Carrier_task::Clamp_motion::E_CLAMP_LOOSE &&
  2129. Common_data::approximate_difference(m_wheel_base, tp_carrier->m_actual_y1-tp_carrier->m_actual_y2, DISPATCH_DEFAULT_DIFFERENCE))
  2130. {
  2131. tp_carrier_task->m_step +=2;
  2132. }
  2133. else
  2134. {
  2135. carrier_adjust_to_ready(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates);
  2136. tp_carrier_task->m_step++;
  2137. }
  2138. }
  2139. else
  2140. {
  2141. return Error_manager(Error_code::CARRIER_POSE_ERROR, Error_level::MINOR_ERROR,
  2142. "tp_carrier->m_actual_load_status != Dispatch_device_base::Load_status::NO_CAR fun error ");
  2143. }
  2144. }
  2145. if ( tp_carrier_task->m_step == 2 )
  2146. {
  2147. return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step);
  2148. }
  2149. if ( tp_carrier_task->m_step == 3 )//让小跑车回到中跑车上
  2150. {
  2151. if ( Common_data::approximate_difference(tp_carrier->m_actual_y, tp_dispatch_coordinates->m_carrier_default_y_back, DISPATCH_DEFAULT_DIFFERENCE) )
  2152. {
  2153. tp_carrier_task->m_step +=2;
  2154. }
  2155. else
  2156. {
  2157. carrier_move_y(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_carrier_default_y_back);
  2158. tp_carrier_task->m_step++;
  2159. }
  2160. }
  2161. if ( tp_carrier_task->m_step == 4 )
  2162. {
  2163. return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step);
  2164. }
  2165. if ( tp_carrier_task->m_step == 5 )//让中跑车回到电梯井
  2166. {
  2167. if ( Common_data::approximate_difference(tp_carrier->m_actual_z, dispatch_control_node.m_destination_coordinates.z, DISPATCH_DEFAULT_DIFFERENCE) )
  2168. {
  2169. tp_carrier_task->m_step +=2;
  2170. }
  2171. else
  2172. {
  2173. if ( dispatch_control_node.mp_dispatch_device->get_device_id() == 0 )
  2174. {
  2175. carrier_move_x(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_carrier_default_x_left);
  2176. tp_carrier_task->m_step++;
  2177. }
  2178. else if ( dispatch_control_node.mp_dispatch_device->get_device_id() == 1 )
  2179. {
  2180. carrier_move_x(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_carrier_default_x_right);
  2181. tp_carrier_task->m_step++;
  2182. }
  2183. else
  2184. {
  2185. return Error_manager(Error_code::CARRIER_CONRTOL_PARAMETER_ERROR, Error_level::MINOR_ERROR,
  2186. " dispatch_control_node.m_destination_coordinates.z PARAMRTER ERROR ");
  2187. }
  2188. }
  2189. }
  2190. if ( tp_carrier_task->m_step == 6 )
  2191. {
  2192. return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step);
  2193. }
  2194. if ( tp_carrier_task->m_step == 7 )//收回对接,之后中跑车固定在电梯上不能X轴移动,电梯可以Z轴移动
  2195. {
  2196. if ( Common_data::approximate_difference(tp_carrier->m_actual_z, dispatch_control_node.m_destination_coordinates.z, DISPATCH_DEFAULT_DIFFERENCE) )
  2197. {
  2198. tp_carrier_task->m_step +=2;
  2199. }
  2200. else
  2201. {
  2202. carrier_joint_x(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, Carrier_task::Joint_motion::E_JOINT_TAKE_BACK);
  2203. tp_carrier_task->m_step++;
  2204. }
  2205. }
  2206. if ( tp_carrier_task->m_step == 8 )
  2207. {
  2208. return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step);
  2209. }
  2210. if ( tp_carrier_task->m_step == 9 )//电梯移动到对应的楼层
  2211. {
  2212. if ( Common_data::approximate_difference(tp_carrier->m_actual_z, dispatch_control_node.m_destination_coordinates.z, DISPATCH_DEFAULT_DIFFERENCE) )
  2213. {
  2214. tp_carrier_task->m_step +=2;
  2215. }
  2216. else
  2217. {
  2218. carrier_move_z(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, dispatch_control_node.m_destination_coordinates.z);
  2219. tp_carrier_task->m_step++;
  2220. }
  2221. }
  2222. if ( tp_carrier_task->m_step == 10 )
  2223. {
  2224. return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step);
  2225. }
  2226. if ( tp_carrier_task->m_step == 11 )//伸出对接,之后中跑车可以x轴移动,电梯不能Z轴移动
  2227. {
  2228. if ( tp_carrier->m_actual_joint_motion_x1 == Dispatch_device_base::Joint_motion::E_JOINT_TAKE_BACK )
  2229. {
  2230. carrier_joint_x(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, Carrier_task::Joint_motion::E_JOINT_HOLD_OUT);
  2231. tp_carrier_task->m_step++;
  2232. }
  2233. else
  2234. {
  2235. tp_carrier_task->m_step +=2;
  2236. }
  2237. }
  2238. if ( tp_carrier_task->m_step == 12 )
  2239. {
  2240. return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step);
  2241. }
  2242. if ( tp_carrier_task->m_step == 13 )//中跑车 x轴移动
  2243. {
  2244. if ( Common_data::approximate_difference(tp_carrier->m_actual_x, dispatch_control_node.m_destination_coordinates.x, DISPATCH_DEFAULT_DIFFERENCE) )
  2245. {
  2246. tp_carrier_task->m_step +=2;
  2247. }
  2248. else
  2249. {
  2250. carrier_move_x(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, dispatch_control_node.m_destination_coordinates.x);
  2251. tp_carrier_task->m_step++;
  2252. }
  2253. }
  2254. if ( tp_carrier_task->m_step == 14 )
  2255. {
  2256. return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step);
  2257. }
  2258. if ( tp_carrier_task->m_step == 15 )//小跑车 进入车位
  2259. {
  2260. carrier_move_y(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_carrier_default_y_leave);
  2261. tp_carrier_task->m_step++;
  2262. }
  2263. if ( tp_carrier_task->m_step == 16 )
  2264. {
  2265. return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step);
  2266. }
  2267. if ( tp_carrier_task->m_step == 17 )//小跑车 夹车
  2268. {
  2269. carrier_move_c(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, Carrier_task::Clamp_motion::E_CLAMP_TIGHT);
  2270. tp_carrier_task->m_step++;
  2271. }
  2272. if ( tp_carrier_task->m_step == 18 )
  2273. {
  2274. return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step);
  2275. }
  2276. if ( tp_carrier_task->m_step == 19 )//小跑车 回到中跑车
  2277. {
  2278. carrier_move_y(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_carrier_default_y_back);
  2279. tp_carrier_task->m_step++;
  2280. }
  2281. if ( tp_carrier_task->m_step == 20 )
  2282. {
  2283. return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step);
  2284. }
  2285. if ( tp_carrier_task->m_step == 21 )
  2286. {
  2287. return Error_code::SUCCESS;
  2288. }
  2289. return Error_code::SUCCESS;
  2290. }
  2291. //搬运器把车交付给机器手(例如:搬运器移动到3号入口的上方,小跑车松夹杆,然后等待机器手把车从中跑车上取走)
  2292. Error_manager Dispatch_process::excute_carrier_deliver_car_to_robot(Dispatch_control_node & dispatch_control_node)
  2293. {
  2294. Error_manager t_error;
  2295. Carrier * tp_carrier = NULL;
  2296. Carrier_task * tp_carrier_task = NULL;
  2297. Dispatch_coordinates * tp_dispatch_coordinates = Dispatch_coordinates::get_instance_pointer();
  2298. if ( dispatch_control_node.mp_dispatch_device.get() == NULL || dispatch_control_node.mp_dispatch_task.get() == NULL )
  2299. {
  2300. return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
  2301. "Dispatch_process::excute_robot_catch_car_from_inlet() POINTER IS NULL ");
  2302. }
  2303. else
  2304. {
  2305. tp_carrier = (Carrier *)dispatch_control_node.mp_dispatch_device.get();
  2306. tp_carrier_task = (Carrier_task *)dispatch_control_node.mp_dispatch_task.get();
  2307. }
  2308. // std::cout << " huli test :::: " << " tp_carrier_task->m_step = " << tp_carrier_task->m_step << std::endl;
  2309. // getchar();
  2310. if ( tp_carrier_task->m_step == 0 )
  2311. {
  2312. return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step);
  2313. }
  2314. if ( tp_carrier_task->m_step == 1 )//检查姿态
  2315. {
  2316. //检查姿态
  2317. if ( tp_carrier->m_actual_load_status == Dispatch_device_base::Load_status::HAVE_CAR &&
  2318. tp_carrier->m_actual_clamp_motion1 == Dispatch_device_base::Clamp_motion::E_CLAMP_TIGHT)
  2319. {
  2320. //检测正常, 直接跳过
  2321. tp_carrier_task->m_step +=2;
  2322. }
  2323. else
  2324. {
  2325. return Error_manager(Error_code::CARRIER_POSE_ERROR, Error_level::MINOR_ERROR,
  2326. "tp_carrier->m_actual_load_status != Dispatch_device_base::Load_status::NO_CAR fun error ");
  2327. }
  2328. }
  2329. if ( tp_carrier_task->m_step == 2 )
  2330. {
  2331. return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step);
  2332. }
  2333. if ( tp_carrier_task->m_step == 3 )//让小跑车回到中跑车上
  2334. {
  2335. if ( Common_data::approximate_difference(tp_carrier->m_actual_y, tp_dispatch_coordinates->m_carrier_default_y_back, DISPATCH_DEFAULT_DIFFERENCE) )
  2336. {
  2337. tp_carrier_task->m_step +=2;
  2338. }
  2339. else
  2340. {
  2341. carrier_move_y(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_carrier_default_y_back);
  2342. tp_carrier_task->m_step++;
  2343. }
  2344. }
  2345. if ( tp_carrier_task->m_step == 4 )
  2346. {
  2347. return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step);
  2348. }
  2349. if ( tp_carrier_task->m_step == 5 )//让中跑车回到电梯井
  2350. {
  2351. if ( Common_data::approximate_difference(tp_carrier->m_actual_z, dispatch_control_node.m_destination_coordinates.z, DISPATCH_DEFAULT_DIFFERENCE) )
  2352. {
  2353. tp_carrier_task->m_step +=2;
  2354. }
  2355. else
  2356. {
  2357. if ( dispatch_control_node.mp_dispatch_device->get_device_id() == 0 )
  2358. {
  2359. carrier_move_x(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_carrier_default_x_left);
  2360. tp_carrier_task->m_step++;
  2361. }
  2362. else if ( dispatch_control_node.mp_dispatch_device->get_device_id() == 1 )
  2363. {
  2364. carrier_move_x(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_carrier_default_x_right);
  2365. tp_carrier_task->m_step++;
  2366. }
  2367. else
  2368. {
  2369. return Error_manager(Error_code::CARRIER_CONRTOL_PARAMETER_ERROR, Error_level::MINOR_ERROR,
  2370. " dispatch_control_node.m_destination_coordinates.z PARAMRTER ERROR ");
  2371. }
  2372. }
  2373. }
  2374. if ( tp_carrier_task->m_step == 6 )
  2375. {
  2376. return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step);
  2377. }
  2378. if ( tp_carrier_task->m_step == 7 )//收回对接,之后中跑车固定在电梯上不能X轴移动,电梯可以Z轴移动
  2379. {
  2380. if ( Common_data::approximate_difference(tp_carrier->m_actual_z, dispatch_control_node.m_destination_coordinates.z, DISPATCH_DEFAULT_DIFFERENCE) )
  2381. {
  2382. tp_carrier_task->m_step +=2;
  2383. }
  2384. else
  2385. {
  2386. carrier_joint_x(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, Carrier_task::Joint_motion::E_JOINT_TAKE_BACK);
  2387. tp_carrier_task->m_step++;
  2388. }
  2389. }
  2390. if ( tp_carrier_task->m_step == 8 )
  2391. {
  2392. return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step);
  2393. }
  2394. if ( tp_carrier_task->m_step == 9 )//电梯移动到对应的楼层
  2395. {
  2396. if ( Common_data::approximate_difference(tp_carrier->m_actual_z, dispatch_control_node.m_destination_coordinates.z, DISPATCH_DEFAULT_DIFFERENCE) )
  2397. {
  2398. tp_carrier_task->m_step +=2;
  2399. }
  2400. else
  2401. {
  2402. carrier_move_z(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, dispatch_control_node.m_destination_coordinates.z);
  2403. tp_carrier_task->m_step++;
  2404. }
  2405. }
  2406. if ( tp_carrier_task->m_step == 10 )
  2407. {
  2408. return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step);
  2409. }
  2410. if ( tp_carrier_task->m_step == 11 )//伸出对接,之后中跑车可以x轴移动,电梯不能Z轴移动
  2411. {
  2412. if ( tp_carrier->m_actual_joint_motion_x1 == Dispatch_device_base::Joint_motion::E_JOINT_TAKE_BACK )
  2413. {
  2414. carrier_joint_x(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, Carrier_task::Joint_motion::E_JOINT_HOLD_OUT);
  2415. tp_carrier_task->m_step++;
  2416. }
  2417. else
  2418. {
  2419. tp_carrier_task->m_step +=2;
  2420. }
  2421. }
  2422. if ( tp_carrier_task->m_step == 12 )
  2423. {
  2424. return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step);
  2425. }
  2426. if ( tp_carrier_task->m_step == 13 )//中跑车 x轴移动
  2427. {
  2428. if ( Common_data::approximate_difference(tp_carrier->m_actual_x, dispatch_control_node.m_destination_coordinates.x, DISPATCH_DEFAULT_DIFFERENCE) )
  2429. {
  2430. tp_carrier_task->m_step +=2;
  2431. }
  2432. else
  2433. {
  2434. carrier_move_x(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, dispatch_control_node.m_destination_coordinates.x);
  2435. tp_carrier_task->m_step++;
  2436. }
  2437. }
  2438. if ( tp_carrier_task->m_step == 14 )
  2439. {
  2440. return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step);
  2441. }
  2442. if ( tp_carrier_task->m_step == 15 )//小跑车 松开夹杆
  2443. {
  2444. carrier_move_c(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, Carrier_task::Clamp_motion::E_CLAMP_LOOSE);
  2445. tp_carrier_task->m_step++;
  2446. }
  2447. if ( tp_carrier_task->m_step == 16 )
  2448. {
  2449. return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step);
  2450. }
  2451. if ( tp_carrier_task->m_step == 17 )
  2452. {
  2453. return Error_code::SUCCESS;
  2454. }
  2455. return Error_code::SUCCESS;
  2456. }
  2457. //搬运器的自由移动(可以提前到2楼来准备接车,或者为了避让就退回至电梯井)(小跑车不进行取车和存车)
  2458. Error_manager Dispatch_process::excute_carrier_move(Dispatch_control_node & dispatch_control_node)
  2459. {
  2460. Error_manager t_error;
  2461. Carrier * tp_carrier = NULL;
  2462. Carrier_task * tp_carrier_task = NULL;
  2463. Dispatch_coordinates * tp_dispatch_coordinates = Dispatch_coordinates::get_instance_pointer();
  2464. if ( dispatch_control_node.mp_dispatch_device.get() == NULL || dispatch_control_node.mp_dispatch_task.get() == NULL )
  2465. {
  2466. return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
  2467. "Dispatch_process::excute_robot_catch_car_from_inlet() POINTER IS NULL ");
  2468. }
  2469. else
  2470. {
  2471. tp_carrier = (Carrier *)dispatch_control_node.mp_dispatch_device.get();
  2472. tp_carrier_task = (Carrier_task *)dispatch_control_node.mp_dispatch_task.get();
  2473. }
  2474. // std::cout << " huli test :::: " << " tp_carrier_task->m_step = " << tp_carrier_task->m_step << std::endl;
  2475. // getchar();
  2476. if ( tp_carrier_task->m_step == 0 )
  2477. {
  2478. return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step);
  2479. }
  2480. if ( tp_carrier_task->m_step == 1 )//让小跑车回到中跑车上
  2481. {
  2482. if ( Common_data::approximate_difference(tp_carrier->m_actual_y, tp_dispatch_coordinates->m_carrier_default_y_back, DISPATCH_DEFAULT_DIFFERENCE) )
  2483. {
  2484. tp_carrier_task->m_step +=2;
  2485. }
  2486. else
  2487. {
  2488. carrier_move_y(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_carrier_default_y_back);
  2489. tp_carrier_task->m_step++;
  2490. }
  2491. }
  2492. if ( tp_carrier_task->m_step == 2 )
  2493. {
  2494. return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step);
  2495. }
  2496. if ( tp_carrier_task->m_step == 3 )//让中跑车回到电梯井
  2497. {
  2498. if ( Common_data::approximate_difference(tp_carrier->m_actual_z, dispatch_control_node.m_destination_coordinates.z, DISPATCH_DEFAULT_DIFFERENCE) )
  2499. {
  2500. tp_carrier_task->m_step +=2;
  2501. }
  2502. else
  2503. {
  2504. if ( dispatch_control_node.mp_dispatch_device->get_device_id() == 0 )
  2505. {
  2506. carrier_move_x(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_carrier_default_x_left);
  2507. tp_carrier_task->m_step++;
  2508. }
  2509. else if ( dispatch_control_node.mp_dispatch_device->get_device_id() == 1 )
  2510. {
  2511. carrier_move_x(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, tp_dispatch_coordinates->m_carrier_default_x_right);
  2512. tp_carrier_task->m_step++;
  2513. }
  2514. else
  2515. {
  2516. return Error_manager(Error_code::CARRIER_CONRTOL_PARAMETER_ERROR, Error_level::MINOR_ERROR,
  2517. " dispatch_control_node.m_destination_coordinates.z PARAMRTER ERROR ");
  2518. }
  2519. }
  2520. }
  2521. if ( tp_carrier_task->m_step == 4 )
  2522. {
  2523. return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step);
  2524. }
  2525. if ( tp_carrier_task->m_step == 5 )//收回对接,之后中跑车固定在电梯上不能X轴移动,电梯可以Z轴移动
  2526. {
  2527. if ( Common_data::approximate_difference(tp_carrier->m_actual_z, dispatch_control_node.m_destination_coordinates.z, DISPATCH_DEFAULT_DIFFERENCE) )
  2528. {
  2529. tp_carrier_task->m_step +=2;
  2530. }
  2531. else
  2532. {
  2533. carrier_joint_x(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, Carrier_task::Joint_motion::E_JOINT_TAKE_BACK);
  2534. tp_carrier_task->m_step++;
  2535. }
  2536. }
  2537. if ( tp_carrier_task->m_step == 6 )
  2538. {
  2539. return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step);
  2540. }
  2541. if ( tp_carrier_task->m_step == 7 )//电梯移动到对应的楼层
  2542. {
  2543. if ( Common_data::approximate_difference(tp_carrier->m_actual_z, dispatch_control_node.m_destination_coordinates.z, DISPATCH_DEFAULT_DIFFERENCE) )
  2544. {
  2545. tp_carrier_task->m_step +=2;
  2546. }
  2547. else
  2548. {
  2549. carrier_move_z(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, dispatch_control_node.m_destination_coordinates.z);
  2550. tp_carrier_task->m_step++;
  2551. }
  2552. }
  2553. if ( tp_carrier_task->m_step == 8 )
  2554. {
  2555. return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step);
  2556. }
  2557. if ( tp_carrier_task->m_step == 9 )//伸出对接,之后中跑车可以x轴移动,电梯不能Z轴移动
  2558. {
  2559. if ( tp_carrier->m_actual_joint_motion_x1 == Dispatch_device_base::Joint_motion::E_JOINT_TAKE_BACK )
  2560. {
  2561. carrier_joint_x(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, Carrier_task::Joint_motion::E_JOINT_HOLD_OUT);
  2562. tp_carrier_task->m_step++;
  2563. }
  2564. else
  2565. {
  2566. tp_carrier_task->m_step +=2;
  2567. }
  2568. }
  2569. if ( tp_carrier_task->m_step == 10 )
  2570. {
  2571. return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step);
  2572. }
  2573. if ( tp_carrier_task->m_step == 11 )//中跑车 x轴移动
  2574. {
  2575. if ( Common_data::approximate_difference(tp_carrier->m_actual_x, dispatch_control_node.m_destination_coordinates.x, DISPATCH_DEFAULT_DIFFERENCE) )
  2576. {
  2577. tp_carrier_task->m_step +=2;
  2578. }
  2579. else
  2580. {
  2581. carrier_move_x(dispatch_control_node, tp_carrier, tp_carrier_task, tp_dispatch_coordinates, dispatch_control_node.m_destination_coordinates.x);
  2582. tp_carrier_task->m_step++;
  2583. }
  2584. }
  2585. if ( tp_carrier_task->m_step == 12 )
  2586. {
  2587. return check_task_ex(dispatch_control_node.mp_dispatch_task, tp_carrier_task->m_step);
  2588. }
  2589. if ( tp_carrier_task->m_step == 13 )
  2590. {
  2591. return Error_code::SUCCESS;
  2592. }
  2593. return Error_code::SUCCESS;
  2594. }
  2595. //执行通道口动作
  2596. Error_manager Dispatch_process::excute_passageway_motion(Dispatch_control_node & dispatch_control_node)
  2597. {
  2598. return Error_code::SUCCESS;
  2599. }
  2600. //检查 任务单 是否完成任务, 里面会调整短步骤
  2601. Error_manager Dispatch_process::check_task_ex(std::shared_ptr<Task_Base> p_task, int& step)
  2602. {
  2603. if ( p_task.get() == NULL )
  2604. {
  2605. return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
  2606. "Dispatch_process::check_passageway_task POINTER IS NULL ");
  2607. }
  2608. else
  2609. {
  2610. // std::cout << " huli test :::: " << " ffffffffffffffffffffffffffffffffffffffffffffffffffffff = " << 333 << std::endl;
  2611. // std::cout << " huli test :::: " << " p_task->get_task_statu() = " << p_task->get_task_statu() << std::endl;
  2612. // std::cout << " huli test :::: " << " gggggggggggggggggggggggggggggggggggggggggggggggggggggg = " << 333 << std::endl;
  2613. if ( p_task->get_task_statu() == Task_Base::Task_statu::TASK_OVER )
  2614. {
  2615. step++;
  2616. return Error_code::NODATA; //这里返回nodata 表示任务继续下一步
  2617. }
  2618. else if ( p_task->get_task_statu() == Task_Base::Task_statu::TASK_ERROR )
  2619. {
  2620. step = 0;
  2621. return p_task->get_task_error_manager();
  2622. }
  2623. else if ( p_task->get_task_statu() == Task_Base::Task_statu::TASK_WORKING ||
  2624. p_task->get_task_statu() == Task_Base::Task_statu::TASK_SIGNED ||
  2625. p_task->get_task_statu() == Task_Base::Task_statu::TASK_CREATED)
  2626. {
  2627. //继续等待任务, 直到状态改变
  2628. return Error_code::NODATA;
  2629. }
  2630. else
  2631. {
  2632. LOG(INFO) << "Dispatch_process::check_task_ex failed p_catcher_task->get_task_statu() = "<< p_task->get_task_statu() << " " << this;
  2633. return Error_code::NODATA;
  2634. }
  2635. }
  2636. return Error_code::SUCCESS;
  2637. }
  2638. //机器手调整到正常待机的姿态(调节夹杆和轴距)
  2639. Error_manager Dispatch_process::catcher_adjust_to_ready(Dispatch_control_node & dispatch_control_node, Catcher * tp_catcher, Catcher_task * tp_catcher_task, Dispatch_coordinates * tp_dispatch_coordinates)
  2640. {
  2641. std::unique_lock<std::mutex> t_lock2(tp_catcher_task->m_lock);
  2642. char t_key[50] = {0};
  2643. sprintf(t_key, "%s+%d+%d", dispatch_control_node.m_dispatch_control_request_msg.command_key().c_str(), dispatch_control_node.m_dispatch_control_request_msg.dispatch_task_type(), tp_catcher_task->m_step);
  2644. tp_catcher_task->m_request_key = t_key;
  2645. tp_catcher_task->set_task_statu(Task_Base::TASK_CREATED);
  2646. //调整姿态
  2647. tp_catcher_task->m_request_clamp_motion = Catcher_task::Clamp_motion::E_CLAMP_LOOSE;
  2648. tp_catcher_task->m_request_d1 = tp_dispatch_coordinates->m_catcher_d1_min;
  2649. tp_catcher_task->m_request_d2 = tp_dispatch_coordinates->m_catcher_d2_min;
  2650. if ( tp_catcher->m_catcher_direction == Dispatch_device_base::Catcher_direction::CATCHER_DIRECTION_NEGATIVE )
  2651. {
  2652. tp_catcher_task->m_request_b = 270;
  2653. }
  2654. else
  2655. {
  2656. tp_catcher_task->m_request_b = 90;
  2657. }
  2658. std::cout << " huli test :::: " << " catcher_adjust_to_ready = " << tp_catcher_task->m_request_key << std::endl;
  2659. std::cout << " huli test :::: " << " catcher_adjust_to_ready = " << tp_catcher_task->m_respons_key << std::endl;
  2660. std::cout << " huli test :::: " << " tp_catcher_task->m_request_clamp_motion = " << tp_catcher_task->m_request_clamp_motion << std::endl;
  2661. std::cout << " huli test :::: " << " tp_catcher_task->m_request_d1 = " << tp_catcher_task->m_request_d1 << std::endl;
  2662. std::cout << " huli test :::: " << " tp_catcher_task->m_request_d2 = " << tp_catcher_task->m_request_d2 << std::endl;
  2663. std::cout << " huli test :::: " << " tp_catcher->m_catcher_direction = " << tp_catcher->m_catcher_direction << std::endl;
  2664. std::cout << " huli test :::: " << " tp_catcher_task->m_request_b = " << tp_catcher_task->m_request_b << std::endl;
  2665. return Error_code::SUCCESS;
  2666. }
  2667. //机器手 移动z
  2668. Error_manager Dispatch_process::catcher_move_z(Dispatch_control_node & dispatch_control_node, Catcher * tp_catcher, Catcher_task * tp_catcher_task, Dispatch_coordinates * tp_dispatch_coordinates, float target)
  2669. {
  2670. std::unique_lock<std::mutex> t_lock2(tp_catcher_task->m_lock);
  2671. char t_key[50] = {0};
  2672. sprintf(t_key, "%s+%d+%d", dispatch_control_node.m_dispatch_control_request_msg.command_key().c_str(), dispatch_control_node.m_dispatch_control_request_msg.dispatch_task_type(), tp_catcher_task->m_step);
  2673. tp_catcher_task->m_request_key = t_key;
  2674. tp_catcher_task->set_task_statu(Task_Base::TASK_CREATED);
  2675. //机器手 移动z
  2676. tp_catcher_task->m_request_z = target;
  2677. std::cout << " huli test :::: " << " catcher_move_z = " << tp_catcher_task->m_request_key << std::endl;
  2678. std::cout << " huli test :::: " << " catcher_move_z = " << tp_catcher_task->m_respons_key << std::endl;
  2679. std::cout << " huli test :::: " << " tp_catcher_task->m_request_z = " << tp_catcher_task->m_request_z << std::endl;
  2680. return Error_code::SUCCESS;
  2681. }
  2682. //机器手调整到 准备从地面抓车前的姿态
  2683. Error_manager Dispatch_process::catcher_adjust_from_ground(Dispatch_control_node & dispatch_control_node, Catcher * tp_catcher, Catcher_task * tp_catcher_task, Dispatch_coordinates * tp_dispatch_coordinates)
  2684. {
  2685. std::unique_lock<std::mutex> t_lock2(tp_catcher_task->m_lock);
  2686. char t_key[50] = {0};
  2687. sprintf(t_key, "%s+%d+%d", dispatch_control_node.m_dispatch_control_request_msg.command_key().c_str(), dispatch_control_node.m_dispatch_control_request_msg.dispatch_task_type(), tp_catcher_task->m_step);
  2688. tp_catcher_task->m_request_key = t_key;
  2689. tp_catcher_task->set_task_statu(Task_Base::TASK_CREATED);
  2690. //机器手 调整x y b d1 d2, 根据感测模块的定位信息, 调整机器手的姿态, 准备抓车.
  2691. tp_catcher_task->m_request_x = m_car_measure_information.center_x;
  2692. tp_catcher_task->m_request_y = m_car_measure_information.center_y;
  2693. if ( tp_catcher->m_catcher_direction == Dispatch_device_base::Catcher_direction::CATCHER_DIRECTION_NEGATIVE )
  2694. {
  2695. //机器手如果旋转反向, 那就+180即可, 没有必要旋转归位.
  2696. tp_catcher_task->m_request_b = 180 + m_car_measure_information.car_angle;
  2697. }
  2698. else
  2699. {
  2700. tp_catcher_task->m_request_b = m_car_measure_information.car_angle;
  2701. }
  2702. //限制轮距, 在夹车杆松开时, 机器人下降时, 轴距不能超过3000mm, 必须下降到最下面才能调整轴距, 夹车后可以上升.
  2703. if ( m_wheel_base > tp_dispatch_coordinates->m_catcher_wheel_base_limit )
  2704. {
  2705. tp_catcher_task->m_request_d1 = (tp_dispatch_coordinates->m_catcher_wheel_base_limit - tp_dispatch_coordinates->m_catcher_d1_d2_distance)/2;
  2706. tp_catcher_task->m_request_d2 = (tp_dispatch_coordinates->m_catcher_wheel_base_limit - tp_dispatch_coordinates->m_catcher_d1_d2_distance)/2;
  2707. tp_catcher_task->m_request_wheelbase = tp_dispatch_coordinates->m_catcher_wheel_base_limit;
  2708. }
  2709. else
  2710. {
  2711. tp_catcher_task->m_request_d1 = (m_wheel_base - tp_dispatch_coordinates->m_catcher_d1_d2_distance)/2;
  2712. tp_catcher_task->m_request_d2 = (m_wheel_base - tp_dispatch_coordinates->m_catcher_d1_d2_distance)/2;
  2713. tp_catcher_task->m_request_wheelbase = m_wheel_base;
  2714. }
  2715. tp_catcher_task->m_request_clamp_motion = Catcher_task::Clamp_motion::E_CLAMP_LOOSE;
  2716. std::cout << " huli test :::: " << " catcher_adjust_from_ground = " << tp_catcher_task->m_request_key << std::endl;
  2717. std::cout << " huli test :::: " << " catcher_adjust_from_ground = " << tp_catcher_task->m_respons_key << std::endl;
  2718. std::cout << " huli test :::: " << " tp_catcher_task->m_request_x = " << tp_catcher_task->m_request_x << std::endl;
  2719. std::cout << " huli test :::: " << " tp_catcher_task->m_request_y = " << tp_catcher_task->m_request_y << std::endl;
  2720. std::cout << " huli test :::: " << " tp_catcher->m_catcher_direction = " << tp_catcher->m_catcher_direction << std::endl;
  2721. std::cout << " huli test :::: " << " tp_catcher_task->m_request_b = " << tp_catcher_task->m_request_b << std::endl;
  2722. std::cout << " huli test :::: " << " m_wheel_base = " << m_wheel_base << std::endl;
  2723. std::cout << " huli test :::: " << " tp_catcher_task->m_request_d1 = " << tp_catcher_task->m_request_d1 << std::endl;
  2724. std::cout << " huli test :::: " << " tp_catcher_task->m_request_d2 = " << tp_catcher_task->m_request_d2 << std::endl;
  2725. std::cout << " huli test :::: " << " tp_catcher_task->m_request_wheelbase = " << tp_catcher_task->m_request_wheelbase << std::endl;
  2726. std::cout << " huli test :::: " << " tp_catcher_task->m_request_clamp_motion = " << tp_catcher_task->m_request_clamp_motion << std::endl;
  2727. return Error_code::SUCCESS;
  2728. }
  2729. //机器手 修正轴距, 如果轴距大于3000mm, 那么就要修正轴距.
  2730. Error_manager Dispatch_process::catcher_adjust_wheel_base(Dispatch_control_node & dispatch_control_node, Catcher * tp_catcher, Catcher_task * tp_catcher_task, Dispatch_coordinates * tp_dispatch_coordinates)
  2731. {
  2732. std::unique_lock<std::mutex> t_lock2(tp_catcher_task->m_lock);
  2733. char t_key[50] = {0};
  2734. sprintf(t_key, "%s+%d+%d", dispatch_control_node.m_dispatch_control_request_msg.command_key().c_str(), dispatch_control_node.m_dispatch_control_request_msg.dispatch_task_type(), tp_catcher_task->m_step);
  2735. tp_catcher_task->m_request_key = t_key;
  2736. tp_catcher_task->set_task_statu(Task_Base::TASK_CREATED);
  2737. //修正轴距
  2738. tp_catcher_task->m_request_d1 = (m_wheel_base - tp_dispatch_coordinates->m_catcher_d1_d2_distance)/2;
  2739. tp_catcher_task->m_request_d2 = (m_wheel_base - tp_dispatch_coordinates->m_catcher_d1_d2_distance)/2;
  2740. tp_catcher_task->m_request_wheelbase = m_wheel_base;
  2741. std::cout << " huli test :::: " << " catcher_adjust_wheel_base = " << tp_catcher_task->m_request_key << std::endl;
  2742. std::cout << " huli test :::: " << " catcher_adjust_wheel_base = " << tp_catcher_task->m_respons_key << std::endl;
  2743. std::cout << " huli test :::: " << " tp_catcher_task->m_request_d1 = " << tp_catcher_task->m_request_d1 << std::endl;
  2744. std::cout << " huli test :::: " << " tp_catcher_task->m_request_d2 = " << tp_catcher_task->m_request_d2 << std::endl;
  2745. std::cout << " huli test :::: " << " tp_catcher_task->m_request_wheelbase = " << tp_catcher_task->m_request_wheelbase << std::endl;
  2746. return Error_code::SUCCESS;
  2747. }
  2748. //机器手 移动c轴 夹杆
  2749. Error_manager Dispatch_process::catcher_move_c(Dispatch_control_node & dispatch_control_node, Catcher * tp_catcher, Catcher_task * tp_catcher_task, Dispatch_coordinates * tp_dispatch_coordinates, Catcher_task::Clamp_motion target)
  2750. {
  2751. std::unique_lock<std::mutex> t_lock2(tp_catcher_task->m_lock);
  2752. char t_key[50] = {0};
  2753. sprintf(t_key, "%s+%d+%d", dispatch_control_node.m_dispatch_control_request_msg.command_key().c_str(), dispatch_control_node.m_dispatch_control_request_msg.dispatch_task_type(), tp_catcher_task->m_step);
  2754. tp_catcher_task->m_request_key = t_key;
  2755. tp_catcher_task->set_task_statu(Task_Base::TASK_CREATED);
  2756. tp_catcher_task->m_request_clamp_motion = target;
  2757. std::cout << " huli test :::: " << " catcher_move_c = " << tp_catcher_task->m_request_key << std::endl;
  2758. std::cout << " huli test :::: " << " catcher_move_c = " << tp_catcher_task->m_respons_key << std::endl;
  2759. std::cout << " huli test :::: " << " tp_catcher_task->m_request_clamp_motion = " << tp_catcher_task->m_request_clamp_motion << std::endl;
  2760. return Error_code::SUCCESS;
  2761. }
  2762. //机器手调整到 准备把车放到搬运器的姿态
  2763. Error_manager Dispatch_process::catcher_adjust_to_carrier(Dispatch_control_node & dispatch_control_node, Catcher * tp_catcher, Catcher_task * tp_catcher_task, Dispatch_coordinates * tp_dispatch_coordinates, bool reverse_flag)
  2764. {
  2765. std::unique_lock<std::mutex> t_lock2(tp_catcher_task->m_lock);
  2766. char t_key[50] = {0};
  2767. sprintf(t_key, "%s+%d+%d", dispatch_control_node.m_dispatch_control_request_msg.command_key().c_str(), dispatch_control_node.m_dispatch_control_request_msg.dispatch_task_type(), tp_catcher_task->m_step);
  2768. tp_catcher_task->m_request_key = t_key;
  2769. tp_catcher_task->set_task_statu(Task_Base::TASK_CREATED);
  2770. //机器手 调整x y b d1 d2, 调整机器手的姿态, 准备把车放置到中跑车上.
  2771. tp_catcher_task->m_request_x = dispatch_control_node.m_destination_coordinates.x;
  2772. tp_catcher_task->m_request_y = tp_dispatch_coordinates->m_carrier_default_y1_back - (m_wheel_base /2);
  2773. //存车需要反向, 取车不需要
  2774. if ( reverse_flag )
  2775. {
  2776. if ( tp_catcher->m_catcher_direction == Dispatch_device_base::Catcher_direction::CATCHER_DIRECTION_NEGATIVE )
  2777. {
  2778. tp_catcher_task->m_request_b = 90;
  2779. }
  2780. else
  2781. {
  2782. tp_catcher_task->m_request_b = 270;
  2783. }
  2784. }
  2785. else
  2786. {
  2787. if ( tp_catcher->m_catcher_direction == Dispatch_device_base::Catcher_direction::CATCHER_DIRECTION_NEGATIVE )
  2788. {
  2789. tp_catcher_task->m_request_b = 270;
  2790. }
  2791. else
  2792. {
  2793. tp_catcher_task->m_request_b = 90;
  2794. }
  2795. }
  2796. std::cout << " huli test :::: " << " catcher_adjust_to_carrier = " << tp_catcher_task->m_request_key << std::endl;
  2797. std::cout << " huli test :::: " << " catcher_adjust_to_carrier = " << tp_catcher_task->m_respons_key << std::endl;
  2798. std::cout << " huli test :::: " << " tp_catcher_task->m_request_x = " << tp_catcher_task->m_request_x << std::endl;
  2799. std::cout << " huli test :::: " << " tp_catcher_task->m_request_y = " << tp_catcher_task->m_request_y << std::endl;
  2800. std::cout << " huli test :::: " << " tp_catcher->m_catcher_direction = " << tp_catcher->m_catcher_direction << std::endl;
  2801. std::cout << " huli test :::: " << " tp_catcher->m_request_b = " << tp_catcher->m_request_b << std::endl;
  2802. return Error_code::SUCCESS;
  2803. }
  2804. //机器手调整到 准备把车放到地面的姿态
  2805. Error_manager Dispatch_process::catcher_adjust_to_ground(Dispatch_control_node & dispatch_control_node, Catcher * tp_catcher, Catcher_task * tp_catcher_task, Dispatch_coordinates * tp_dispatch_coordinates)
  2806. {
  2807. std::unique_lock<std::mutex> t_lock2(tp_catcher_task->m_lock);
  2808. char t_key[50] = {0};
  2809. sprintf(t_key, "%s+%d+%d", dispatch_control_node.m_dispatch_control_request_msg.command_key().c_str(), dispatch_control_node.m_dispatch_control_request_msg.dispatch_task_type(), tp_catcher_task->m_step);
  2810. tp_catcher_task->m_request_key = t_key;
  2811. tp_catcher_task->set_task_statu(Task_Base::TASK_CREATED);
  2812. //机器手 调整x y b d1 d2, 调整机器手的姿态, 准备把车放置到中跑车上.
  2813. tp_catcher_task->m_request_x = dispatch_control_node.m_destination_coordinates.x;
  2814. tp_catcher_task->m_request_y = dispatch_control_node.m_destination_coordinates.y;
  2815. if ( tp_catcher->m_catcher_direction == Dispatch_device_base::Catcher_direction::CATCHER_DIRECTION_NEGATIVE )
  2816. {
  2817. tp_catcher_task->m_request_b = 270;
  2818. }
  2819. else
  2820. {
  2821. tp_catcher_task->m_request_b = 90;
  2822. }
  2823. std::cout << " huli test :::: " << " catcher_adjust_to_ground = " << tp_catcher_task->m_request_key << std::endl;
  2824. std::cout << " huli test :::: " << " catcher_adjust_to_ground = " << tp_catcher_task->m_respons_key << std::endl;
  2825. std::cout << " huli test :::: " << " tp_catcher_task->m_request_x = " << tp_catcher_task->m_request_x << std::endl;
  2826. std::cout << " huli test :::: " << " tp_catcher_task->m_request_y = " << tp_catcher_task->m_request_y << std::endl;
  2827. std::cout << " huli test :::: " << " tp_catcher->m_catcher_direction = " << tp_catcher->m_catcher_direction << std::endl;
  2828. std::cout << " huli test :::: " << " tp_catcher->m_request_b = " << tp_catcher->m_request_b << std::endl;
  2829. return Error_code::SUCCESS;
  2830. }
  2831. //机器手调整到 对接搬运器的姿态
  2832. Error_manager Dispatch_process::catcher_adjust_from_carrier(Dispatch_control_node & dispatch_control_node, Catcher * tp_catcher, Catcher_task * tp_catcher_task, Dispatch_coordinates * tp_dispatch_coordinates)
  2833. {
  2834. std::unique_lock<std::mutex> t_lock2(tp_catcher_task->m_lock);
  2835. char t_key[50] = {0};
  2836. sprintf(t_key, "%s+%d+%d", dispatch_control_node.m_dispatch_control_request_msg.command_key().c_str(), dispatch_control_node.m_dispatch_control_request_msg.dispatch_task_type(), tp_catcher_task->m_step);
  2837. tp_catcher_task->m_request_key = t_key;
  2838. tp_catcher_task->set_task_statu(Task_Base::TASK_CREATED);
  2839. //机器手 调整x y b d1 d2, 调整机器手的姿态, 准备把车放置到中跑车上.
  2840. tp_catcher_task->m_request_x = dispatch_control_node.m_destination_coordinates.x;
  2841. tp_catcher_task->m_request_y = tp_dispatch_coordinates->m_carrier_default_y1_back - (m_wheel_base /2);
  2842. if ( tp_catcher->m_catcher_direction == Dispatch_device_base::Catcher_direction::CATCHER_DIRECTION_NEGATIVE )
  2843. {
  2844. tp_catcher_task->m_request_b = 270;
  2845. }
  2846. else
  2847. {
  2848. tp_catcher_task->m_request_b = 90;
  2849. }
  2850. //限制轮距, 在夹车杆松开时, 机器人下降时, 轴距不能超过3000mm, 必须下降到最下面才能调整轴距, 夹车后可以上升.
  2851. if ( m_wheel_base > tp_dispatch_coordinates->m_catcher_wheel_base_limit )
  2852. {
  2853. tp_catcher_task->m_request_d1 = (tp_dispatch_coordinates->m_catcher_wheel_base_limit - tp_dispatch_coordinates->m_catcher_d1_d2_distance)/2;
  2854. tp_catcher_task->m_request_d2 = (tp_dispatch_coordinates->m_catcher_wheel_base_limit - tp_dispatch_coordinates->m_catcher_d1_d2_distance)/2;
  2855. tp_catcher_task->m_request_wheelbase = tp_dispatch_coordinates->m_catcher_wheel_base_limit;
  2856. }
  2857. else
  2858. {
  2859. tp_catcher_task->m_request_d1 = (m_wheel_base - tp_dispatch_coordinates->m_catcher_d1_d2_distance)/2;
  2860. tp_catcher_task->m_request_d2 = (m_wheel_base - tp_dispatch_coordinates->m_catcher_d1_d2_distance)/2;
  2861. tp_catcher_task->m_request_wheelbase = m_wheel_base;
  2862. }
  2863. tp_catcher_task->m_request_clamp_motion = Catcher_task::Clamp_motion::E_CLAMP_LOOSE;
  2864. std::cout << " huli test :::: " << " catcher_adjust_from_carrier = " << tp_catcher_task->m_request_key << std::endl;
  2865. std::cout << " huli test :::: " << " catcher_adjust_from_carrier = " << tp_catcher_task->m_respons_key << std::endl;
  2866. std::cout << " huli test :::: " << " tp_catcher_task->m_request_x = " << tp_catcher_task->m_request_x << std::endl;
  2867. std::cout << " huli test :::: " << " tp_catcher_task->m_request_y = " << tp_catcher_task->m_request_y << std::endl;
  2868. std::cout << " huli test :::: " << " tp_catcher->m_catcher_direction = " << tp_catcher->m_catcher_direction << std::endl;
  2869. std::cout << " huli test :::: " << " tp_catcher_task->m_request_b = " << tp_catcher_task->m_request_b << std::endl;
  2870. std::cout << " huli test :::: " << " m_wheel_base = " << m_wheel_base << std::endl;
  2871. std::cout << " huli test :::: " << " tp_catcher_task->m_request_d1 = " << tp_catcher_task->m_request_d1 << std::endl;
  2872. std::cout << " huli test :::: " << " tp_catcher_task->m_request_d2 = " << tp_catcher_task->m_request_d2 << std::endl;
  2873. std::cout << " huli test :::: " << " tp_catcher_task->m_request_wheelbase = " << tp_catcher_task->m_request_wheelbase << std::endl;
  2874. std::cout << " huli test :::: " << " tp_catcher_task->m_request_clamp_motion = " << tp_catcher_task->m_request_clamp_motion << std::endl;
  2875. return Error_code::SUCCESS;
  2876. }
  2877. //搬运器调整到 正常待机的姿态(调节夹杆和轴距)
  2878. Error_manager Dispatch_process::carrier_adjust_to_ready(Dispatch_control_node & dispatch_control_node, Carrier * tp_carrier, Carrier_task * tp_carrier_task, Dispatch_coordinates * tp_dispatch_coordinates)
  2879. {
  2880. std::unique_lock<std::mutex> t_lock2(tp_carrier_task->m_lock);
  2881. char t_key[50] = {0};
  2882. sprintf(t_key, "%s+%d+%d", dispatch_control_node.m_dispatch_control_request_msg.command_key().c_str(), dispatch_control_node.m_dispatch_control_request_msg.dispatch_task_type(), tp_carrier_task->m_step);
  2883. tp_carrier_task->m_request_key = t_key;
  2884. tp_carrier_task->set_task_statu(Task_Base::TASK_CREATED);
  2885. tp_carrier_task->m_request_clamp_motion = Carrier_task::Clamp_motion::E_CLAMP_LOOSE;
  2886. tp_carrier_task->m_request_wheelbase = m_wheel_base;
  2887. tp_carrier_task->m_request_y1 = tp_carrier->m_actual_y + tp_dispatch_coordinates->m_carrier_y_y1_distance;
  2888. tp_carrier_task->m_request_y2 = tp_carrier_task->m_request_y1 - tp_carrier_task->m_request_wheelbase;
  2889. std::cout << " huli test :::: " << " carrier_adjust_to_ready = " << tp_carrier_task->m_request_key << std::endl;
  2890. std::cout << " huli test :::: " << " carrier_adjust_to_ready = " << tp_carrier_task->m_respons_key << std::endl;
  2891. std::cout << " huli test :::: " << " tp_carrier_task->m_request_wheelbase = " << tp_carrier_task->m_request_wheelbase << std::endl;
  2892. std::cout << " huli test :::: " << " tp_carrier_task->m_request_y1 = " << tp_carrier_task->m_request_y1 << std::endl;
  2893. std::cout << " huli test :::: " << " tp_carrier_task->m_request_y2 = " << tp_carrier_task->m_request_y2 << std::endl;
  2894. std::cout << " huli test :::: " << " tp_carrier_task->m_request_clamp_motion = " << tp_carrier_task->m_request_clamp_motion << std::endl;
  2895. return Error_code::SUCCESS;
  2896. }
  2897. //搬运器 移动x
  2898. Error_manager Dispatch_process::carrier_move_x(Dispatch_control_node & dispatch_control_node, Carrier * tp_carrier, Carrier_task * tp_carrier_task, Dispatch_coordinates * tp_dispatch_coordinates, float target)
  2899. {
  2900. std::unique_lock<std::mutex> t_lock2(tp_carrier_task->m_lock);
  2901. char t_key[50] = {0};
  2902. sprintf(t_key, "%s+%d+%d", dispatch_control_node.m_dispatch_control_request_msg.command_key().c_str(), dispatch_control_node.m_dispatch_control_request_msg.dispatch_task_type(), tp_carrier_task->m_step);
  2903. tp_carrier_task->m_request_key = t_key;
  2904. tp_carrier_task->set_task_statu(Task_Base::TASK_CREATED);
  2905. tp_carrier_task->m_request_x = target;
  2906. std::cout << " huli test :::: " << " carrier_move_x = " << tp_carrier_task->m_request_key << std::endl;
  2907. std::cout << " huli test :::: " << " carrier_move_x = " << tp_carrier_task->m_respons_key << std::endl;
  2908. std::cout << " huli test :::: " << " tp_carrier_task->m_request_x = " << tp_carrier_task->m_request_x << std::endl;
  2909. return Error_code::SUCCESS;
  2910. }
  2911. //搬运器 移动y
  2912. Error_manager Dispatch_process::carrier_move_y(Dispatch_control_node & dispatch_control_node, Carrier * tp_carrier, Carrier_task * tp_carrier_task, Dispatch_coordinates * tp_dispatch_coordinates, float target)
  2913. {
  2914. std::unique_lock<std::mutex> t_lock2(tp_carrier_task->m_lock);
  2915. char t_key[50] = {0};
  2916. sprintf(t_key, "%s+%d+%d", dispatch_control_node.m_dispatch_control_request_msg.command_key().c_str(), dispatch_control_node.m_dispatch_control_request_msg.dispatch_task_type(), tp_carrier_task->m_step);
  2917. tp_carrier_task->m_request_key = t_key;
  2918. tp_carrier_task->set_task_statu(Task_Base::TASK_CREATED);
  2919. tp_carrier_task->m_request_y = target;
  2920. tp_carrier_task->m_request_wheelbase = m_wheel_base;
  2921. tp_carrier_task->m_request_y1 = tp_carrier_task->m_request_y + tp_dispatch_coordinates->m_carrier_y_y1_distance;
  2922. tp_carrier_task->m_request_y2 = tp_carrier_task->m_request_y1 - tp_carrier_task->m_request_wheelbase;
  2923. std::cout << " huli test :::: " << " carrier_move_y = " << tp_carrier_task->m_request_key << std::endl;
  2924. std::cout << " huli test :::: " << " carrier_move_y = " << tp_carrier_task->m_respons_key << std::endl;
  2925. std::cout << " huli test :::: " << " tp_carrier_task->m_request_y = " << tp_carrier_task->m_request_y << std::endl;
  2926. std::cout << " huli test :::: " << " tp_carrier_task->m_request_wheelbase = " << tp_carrier_task->m_request_wheelbase << std::endl;
  2927. std::cout << " huli test :::: " << " tp_carrier_task->m_request_y1 = " << tp_carrier_task->m_request_y1 << std::endl;
  2928. std::cout << " huli test :::: " << " tp_carrier_task->m_request_y2 = " << tp_carrier_task->m_request_y2 << std::endl;
  2929. return Error_code::SUCCESS;
  2930. }
  2931. //搬运器 移动z
  2932. Error_manager Dispatch_process::carrier_move_z(Dispatch_control_node & dispatch_control_node, Carrier * tp_carrier, Carrier_task * tp_carrier_task, Dispatch_coordinates * tp_dispatch_coordinates, float target)
  2933. {
  2934. std::unique_lock<std::mutex> t_lock2(tp_carrier_task->m_lock);
  2935. char t_key[50] = {0};
  2936. sprintf(t_key, "%s+%d+%d", dispatch_control_node.m_dispatch_control_request_msg.command_key().c_str(), dispatch_control_node.m_dispatch_control_request_msg.dispatch_task_type(), tp_carrier_task->m_step);
  2937. tp_carrier_task->m_request_key = t_key;
  2938. tp_carrier_task->set_task_statu(Task_Base::TASK_CREATED);
  2939. tp_carrier_task->m_request_z = target;
  2940. std::cout << " huli test :::: " << " carrier_move_z = " << tp_carrier_task->m_request_key << std::endl;
  2941. std::cout << " huli test :::: " << " carrier_move_z = " << tp_carrier_task->m_respons_key << std::endl;
  2942. std::cout << " huli test :::: " << " tp_carrier_task->m_request_z = " << tp_carrier_task->m_request_z << std::endl;
  2943. return Error_code::SUCCESS;
  2944. }
  2945. //搬运器 移动c轴 夹车杆
  2946. Error_manager Dispatch_process::carrier_move_c(Dispatch_control_node & dispatch_control_node, Carrier * tp_carrier, Carrier_task * tp_carrier_task, Dispatch_coordinates * tp_dispatch_coordinates, Carrier_task::Clamp_motion target)
  2947. {
  2948. std::unique_lock<std::mutex> t_lock2(tp_carrier_task->m_lock);
  2949. char t_key[50] = {0};
  2950. sprintf(t_key, "%s+%d+%d", dispatch_control_node.m_dispatch_control_request_msg.command_key().c_str(), dispatch_control_node.m_dispatch_control_request_msg.dispatch_task_type(), tp_carrier_task->m_step);
  2951. tp_carrier_task->m_request_key = t_key;
  2952. tp_carrier_task->set_task_statu(Task_Base::TASK_CREATED);
  2953. tp_carrier_task->m_request_clamp_motion = target;
  2954. std::cout << " huli test :::: " << " carrier_move_c = " << tp_carrier_task->m_request_key << std::endl;
  2955. std::cout << " huli test :::: " << " carrier_move_c = " << tp_carrier_task->m_respons_key << std::endl;
  2956. std::cout << " huli test :::: " << " tp_carrier_task->m_request_clamp_motion = " << tp_carrier_task->m_request_clamp_motion << std::endl;
  2957. return Error_code::SUCCESS;
  2958. }
  2959. //搬运器调整 水平的交接
  2960. Error_manager Dispatch_process::carrier_joint_x(Dispatch_control_node & dispatch_control_node, Carrier * tp_carrier, Carrier_task * tp_carrier_task, Dispatch_coordinates * tp_dispatch_coordinates, Carrier_task::Joint_motion target)
  2961. {
  2962. std::unique_lock<std::mutex> t_lock2(tp_carrier_task->m_lock);
  2963. char t_key[50] = {0};
  2964. sprintf(t_key, "%s+%d+%d", dispatch_control_node.m_dispatch_control_request_msg.command_key().c_str(), dispatch_control_node.m_dispatch_control_request_msg.dispatch_task_type(), tp_carrier_task->m_step);
  2965. tp_carrier_task->m_request_key = t_key;
  2966. tp_carrier_task->set_task_statu(Task_Base::TASK_CREATED);
  2967. tp_carrier_task->m_request_joint_motion_x = target;
  2968. std::cout << " huli test :::: " << " carrier_joint_x = " << tp_carrier_task->m_request_key << std::endl;
  2969. std::cout << " huli test :::: " << " carrier_joint_x = " << tp_carrier_task->m_respons_key << std::endl;
  2970. std::cout << " huli test :::: " << " tp_carrier_task->m_request_joint_motion_x = " << tp_carrier_task->m_request_joint_motion_x << std::endl;
  2971. return Error_code::SUCCESS;
  2972. }
  2973. //搬运器 修正轴距
  2974. Error_manager Dispatch_process::carrier_adjust_wheel_base(Dispatch_control_node & dispatch_control_node, Carrier * tp_carrier, Carrier_task * tp_carrier_task, Dispatch_coordinates * tp_dispatch_coordinates)
  2975. {
  2976. std::unique_lock<std::mutex> t_lock2(tp_carrier_task->m_lock);
  2977. char t_key[50] = {0};
  2978. sprintf(t_key, "%s+%d+%d", dispatch_control_node.m_dispatch_control_request_msg.command_key().c_str(), dispatch_control_node.m_dispatch_control_request_msg.dispatch_task_type(), tp_carrier_task->m_step);
  2979. tp_carrier_task->m_request_key = t_key;
  2980. tp_carrier_task->set_task_statu(Task_Base::TASK_CREATED);
  2981. tp_carrier_task->m_request_wheelbase = m_wheel_base;
  2982. tp_carrier_task->m_request_y1 = tp_carrier->m_actual_y + tp_dispatch_coordinates->m_carrier_y_y1_distance;
  2983. tp_carrier_task->m_request_y2 = tp_carrier_task->m_request_y1 - tp_carrier_task->m_request_wheelbase;
  2984. std::cout << " huli test :::: " << " carrier_adjust_wheel_base = " << tp_carrier_task->m_request_key << std::endl;
  2985. std::cout << " huli test :::: " << " carrier_adjust_wheel_base = " << tp_carrier_task->m_respons_key << std::endl;
  2986. std::cout << " huli test :::: " << " tp_carrier_task->m_request_wheelbase = " << tp_carrier_task->m_request_wheelbase << std::endl;
  2987. std::cout << " huli test :::: " << " tp_carrier_task->m_request_y1 = " << tp_carrier_task->m_request_y1 << std::endl;
  2988. std::cout << " huli test :::: " << " tp_carrier_task->m_request_y2 = " << tp_carrier_task->m_request_y2 << std::endl;
  2989. return Error_code::SUCCESS;
  2990. }