goto_thread.cpp
00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023 #include "goto_thread.h"
00024
00025 #include <cstdlib>
00026 #include <kniBase.h>
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040 KatanaGotoThread::KatanaGotoThread(fawkes::RefPtr<CLMBase> katana,
00041 fawkes::Logger *logger,
00042 unsigned int poll_interval_ms)
00043 : KatanaMotionThread("KatanaGotoThread", katana, logger)
00044 {
00045 __poll_interval_usec = poll_interval_ms * 1000;
00046 }
00047
00048
00049
00050
00051
00052
00053
00054
00055
00056
00057 void
00058 KatanaGotoThread::set_target(float x, float y, float z,
00059 float phi, float theta, float psi)
00060 {
00061 __x = x;
00062 __y = y;
00063 __z = z;
00064 __phi = phi;
00065 __theta = theta;
00066 __psi = psi;
00067 }
00068
00069
00070 void
00071 KatanaGotoThread::once()
00072 {
00073 try {
00074
00075 _katana->moveRobotTo(__x, __y, __z, __phi, __theta, __psi);
00076 } catch (KNI::NoSolutionException &e) {
00077 _logger->log_warn("KatanaGotoThread", "Initiating goto failed (no solution, ignoring): %s", e.what());
00078 _finished = true;
00079 _error_code = fawkes::KatanaInterface::ERROR_NO_SOLUTION;
00080 return;
00081 } catch (::Exception &e) {
00082 _logger->log_warn("KatanaGotoThread", "Initiating goto failed (ignoring): %s", e.what());
00083 _finished = true;
00084 _error_code = fawkes::KatanaInterface::ERROR_CMD_START_FAILED;
00085 return;
00086 }
00087
00088
00089 bool final = false;
00090 short num_motors = _katana->getNumberOfMotors();
00091 CKatBase *base = _katana->GetBase();
00092 const TKatMOT *mot = base->GetMOT();
00093 short num_errors = 0;
00094
00095 while (! final) {
00096 usleep(__poll_interval_usec);
00097 final = true;
00098 try {
00099 base->GetSCT()->arr[0].recvDAT();
00100 base->recvMPS();
00101 base->recvGMS();
00102 } catch (::Exception &e) {
00103 if (++num_errors <= 10) {
00104 _logger->log_warn("KatanaGripperThread", "Receiving MPS/GMS failed, retrying");
00105 continue;
00106 } else {
00107 _logger->log_warn("KatanaGripperThread", "Receiving MPS/GMS failed too often, aborting");
00108 _error_code = fawkes::KatanaInterface::ERROR_COMMUNICATION;
00109 break;
00110 }
00111 }
00112
00113 for (int i=0; i < num_motors; ++i) {
00114 if (mot->arr[i].GetPVP()->msf == MSF_MOTCRASHED) {
00115 _error_code = fawkes::KatanaInterface::ERROR_MOTOR_CRASHED;
00116 break;
00117 }
00118
00119 final &= std::abs(mot->arr[i].GetTPS()->tarpos - mot->arr[i].GetPVP()->pos) < 100;
00120 }
00121 }
00122
00123 _logger->log_debug(name(), "Position (%f,%f,%f, %f,%f,%f) reached",
00124 __x, __y, __z, __phi, __theta, __psi);
00125
00126 _finished = true;
00127 }