dispatch_process.cpp 125 KB

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