Fawkes API  Fawkes Development Version
gripper_thread.cpp
00001 
00002 /***************************************************************************
00003  *  gripper_thread.cpp - Katana gripper one-time thread
00004  *
00005  *  Created: Thu Jun 11 11:59:38 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 "gripper_thread.h"
00024 #include "controller.h"
00025 #include "exception.h"
00026 
00027 
00028 /** @class KatanaGripperThread "gripper_thread.h"
00029  * Katana gripper thread.
00030  * This thread opens or closes the gripper when started.
00031  * @author Tim Niemueller
00032  */
00033 
00034 /** Constructor.
00035  * @param katana katana controller base class
00036  * @param logger logger
00037  * @param poll_interval_ms interval in ms between two checks if the
00038  * final position has been reached
00039  */
00040 KatanaGripperThread::KatanaGripperThread(fawkes::RefPtr<fawkes::KatanaController> katana,
00041                                          fawkes::Logger *logger,
00042                                          unsigned int poll_interval_ms)
00043   : KatanaMotionThread("KatanaGripperThread", katana, logger)
00044 {
00045   __mode               = OPEN_GRIPPER;
00046   __poll_interval_usec = poll_interval_ms * 1000;
00047 }
00048 
00049 
00050 /** Set mode.
00051  * @param mode open, either open or close
00052  */
00053 void
00054 KatanaGripperThread::set_mode(gripper_mode_t mode)
00055 {
00056   __mode = mode;
00057 }
00058 
00059 
00060 void
00061 KatanaGripperThread::once()
00062 {
00063   try {
00064     // non-blocking call
00065     if (__mode == CLOSE_GRIPPER) {
00066       _katana->gripper_close(/* wait */ false);
00067     } else {
00068       _katana->gripper_open(/* wait */ false);
00069     }
00070 
00071   } catch (fawkes::Exception &e) {
00072     _logger->log_warn("KatanaGripperThread", "Starting gripper motion failed (ignoring): %s", e.what());
00073     _finished = true;
00074     _error_code = fawkes::KatanaInterface::ERROR_CMD_START_FAILED;
00075     return;
00076   }
00077 
00078   // check if final
00079   bool final = false;
00080   short num_errors  = 0;
00081   while ( !final ) {
00082     usleep(__poll_interval_usec);
00083     try {
00084       _katana->read_sensor_data();
00085       _katana->read_motor_data();
00086     } catch (fawkes::Exception &e) {
00087       if (++num_errors <= 10) {
00088         _logger->log_warn("KatanaMotorControlThread", "Reading sensor/motor data failed, retrying");
00089         continue;
00090       } else {
00091         _logger->log_warn("KatanaMotorControlThread", "Receiving sensor/motor data failed too often, aborting");
00092         _error_code = fawkes::KatanaInterface::ERROR_COMMUNICATION;
00093         break;
00094       }
00095     }
00096 
00097     try {
00098       final = _katana->final();
00099     } catch (fawkes::KatanaMotorCrashedException &e) {
00100       _logger->log_warn("KatanaMotorControlTrhead", e.what());
00101       _error_code = fawkes::KatanaInterface::ERROR_MOTOR_CRASHED;
00102       break;
00103     }
00104   }
00105 
00106   _logger->log_debug("KatanaGripperThread", "Gripper motion finished");
00107 
00108   _finished = true;
00109 }