Fawkes API  Fawkes Development Version
goto_thread.cpp
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 }