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