Fawkes API
Fawkes Development Version
|
00001 00002 /*************************************************************************** 00003 * goto_thread.cpp - Katana goto one-time thread 00004 * 00005 * Created: Wed Jun 10 11:45:31 2009 00006 * Copyright 2006-2009 Tim Niemueller [www.niemueller.de] 00007 * 00008 ****************************************************************************/ 00009 00010 /* This program is free software; you can redistribute it and/or modify 00011 * it under the terms of the GNU General Public License as published by 00012 * the Free Software Foundation; either version 2 of the License, or 00013 * (at your option) any later version. 00014 * 00015 * This program is distributed in the hope that it will be useful, 00016 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00017 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00018 * GNU Library General Public License for more details. 00019 * 00020 * Read the full text in the LICENSE.GPL file in the doc directory. 00021 */ 00022 00023 #include "goto_thread.h" 00024 #include "controller.h" 00025 #include "exception.h" 00026 00027 #include <cstdlib> 00028 00029 /** @class KatanaGotoThread "goto_thread.h" 00030 * Katana linear goto thread. 00031 * This thread moves the arm into a specified position. 00032 * @author Tim Niemueller 00033 */ 00034 00035 /** Constructor. 00036 * @param katana katana controller base class 00037 * @param logger logger 00038 * @param poll_interval_ms interval in ms between two checks if the 00039 * final position has been reached 00040 */ 00041 KatanaGotoThread::KatanaGotoThread(fawkes::RefPtr<fawkes::KatanaController> katana, 00042 fawkes::Logger *logger, 00043 unsigned int poll_interval_ms) 00044 : KatanaMotionThread("KatanaGotoThread", katana, logger) 00045 { 00046 __poll_interval_usec = poll_interval_ms * 1000; 00047 } 00048 00049 00050 /** Set target position. 00051 * @param x X coordinate relative to base 00052 * @param y Y coordinate relative to base 00053 * @param z Z coordinate relative to base 00054 * @param phi Phi Euler angle of tool 00055 * @param theta Theta Euler angle of tool 00056 * @param psi Psi Euler angle of tool 00057 */ 00058 void 00059 KatanaGotoThread::set_target(float x, float y, float z, 00060 float phi, float theta, float psi) 00061 { 00062 __x = x; 00063 __y = y; 00064 __z = z; 00065 __phi = phi; 00066 __theta = theta; 00067 __psi = psi; 00068 } 00069 00070 void 00071 KatanaGotoThread::once() 00072 { 00073 try { 00074 // non-blocking call 00075 _katana->move_to(__x, __y, __z, __phi, __theta, __psi); 00076 } catch (fawkes::KatanaNoSolutionException &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 (fawkes::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 // check if final 00089 bool final = false; 00090 short num_errors = 0; 00091 while ( !final ) { 00092 usleep(__poll_interval_usec); 00093 try { 00094 _katana->read_sensor_data(); 00095 _katana->read_motor_data(); 00096 } catch (fawkes::Exception &e) { 00097 if (++num_errors <= 10) { 00098 _logger->log_warn("KatanaMotorControlThread", "Reading sensor/motor data failed, retrying"); 00099 continue; 00100 } else { 00101 _logger->log_warn("KatanaMotorControlThread", "Receiving sensor/motor data failed too often, aborting"); 00102 _error_code = fawkes::KatanaInterface::ERROR_COMMUNICATION; 00103 break; 00104 } 00105 } 00106 00107 try { 00108 final = _katana->final(); 00109 } catch (fawkes::KatanaMotorCrashedException &e) { 00110 _logger->log_warn("KatanaMotorControlTrhead", e.what()); 00111 _error_code = fawkes::KatanaInterface::ERROR_MOTOR_CRASHED; 00112 break; 00113 } 00114 } 00115 00116 _logger->log_debug(name(), "Position (%f,%f,%f, %f,%f,%f) reached", 00117 __x, __y, __z, __phi, __theta, __psi); 00118 00119 _finished = true; 00120 }