Fawkes API
Fawkes Development Version
|
00001 00002 /*************************************************************************** 00003 * act_thread.cpp - Katana plugin act thread 00004 * 00005 * Created: Mon Jun 08 18:00:56 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 "act_thread.h" 00024 00025 #include <core/threading/mutex_locker.h> 00026 #include <interfaces/KatanaInterface.h> 00027 00028 #include <algorithm> 00029 #include <cstdarg> 00030 #include <kniBase.h> 00031 00032 using namespace fawkes; 00033 00034 /** @class KatanaActThread "act_thread.h" 00035 * Katana act thread. 00036 * This thread integrates into the Fawkes main loop at the ACT_EXEC hook and 00037 * interacts with the controller of the Katana arm via the KNI library. 00038 * @author Tim Niemueller 00039 */ 00040 00041 /** Constructor. */ 00042 KatanaActThread::KatanaActThread() 00043 : Thread("KatanaActThread", Thread::OPMODE_WAITFORWAKEUP), 00044 BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_ACT_EXEC), 00045 BlackBoardInterfaceListener("KatanaActThread") 00046 { 00047 } 00048 00049 00050 void 00051 KatanaActThread::init() 00052 { 00053 // Note: due to the use of auto_ptr and RefPtr resources are automatically 00054 // freed on destruction, therefore no special handling is necessary in init() 00055 // itself! 00056 00057 __cfg_device = config->get_string("/hardware/katana/device"); 00058 __cfg_kni_conffile = config->get_string("/hardware/katana/kni_conffile"); 00059 __cfg_auto_calibrate = config->get_bool("/hardware/katana/auto_calibrate"); 00060 __cfg_defmax_speed = config->get_uint("/hardware/katana/default_max_speed"); 00061 __cfg_read_timeout = config->get_uint("/hardware/katana/read_timeout_msec"); 00062 __cfg_write_timeout = config->get_uint("/hardware/katana/write_timeout_msec"); 00063 __cfg_gripper_pollint = config->get_uint("/hardware/katana/gripper_pollint_msec"); 00064 __cfg_goto_pollint = config->get_uint("/hardware/katana/goto_pollint_msec"); 00065 00066 __cfg_park_x = config->get_float("/hardware/katana/park_x"); 00067 __cfg_park_y = config->get_float("/hardware/katana/park_y"); 00068 __cfg_park_z = config->get_float("/hardware/katana/park_z"); 00069 __cfg_park_phi = config->get_float("/hardware/katana/park_phi"); 00070 __cfg_park_theta = config->get_float("/hardware/katana/park_theta"); 00071 __cfg_park_psi = config->get_float("/hardware/katana/park_psi"); 00072 00073 try { 00074 TCdlCOMDesc ccd = {0, 57600, 8, 'N', 1, __cfg_read_timeout, __cfg_write_timeout}; 00075 __device.reset(new CCdlCOM(ccd, __cfg_device.c_str())); 00076 00077 __protocol.reset(new CCplSerialCRC()); 00078 __protocol->init(__device.get()); 00079 00080 __katana = RefPtr<CLMBase>(new CLMBase()); 00081 __katana->create(__cfg_kni_conffile.c_str(), __protocol.get()); 00082 __katbase = __katana->GetBase(); 00083 __sensor_ctrl = &__katbase->GetSCT()->arr[0]; 00084 00085 __katana->setRobotVelocityLimit(__cfg_defmax_speed); 00086 00087 __katbase->recvECH(); 00088 logger->log_debug(name(), "Katana successfully initialized"); 00089 00090 } catch (/*KNI*/::Exception &e) { 00091 throw fawkes::Exception(e.what()); 00092 } 00093 00094 // If you have more than one interface: catch exception and close them! 00095 __katana_if = blackboard->open_for_writing<KatanaInterface>("Katana"); 00096 00097 __sensacq_thread.reset(new KatanaSensorAcquisitionThread(__katana, logger)); 00098 __calib_thread = new KatanaCalibrationThread(__katana, logger); 00099 __goto_thread = new KatanaGotoThread(__katana, logger, __cfg_goto_pollint); 00100 __gripper_thread = new KatanaGripperThread(__katana, logger, 00101 __cfg_gripper_pollint); 00102 00103 __sensacq_thread->start(); 00104 00105 bbil_add_message_interface(__katana_if); 00106 blackboard->register_listener(this, BlackBoard::BBIL_FLAG_MESSAGES); 00107 00108 #ifdef USE_TIMETRACKER 00109 __tt.reset(new TimeTracker()); 00110 __tt_count = 0; 00111 __ttc_read_sensor = __tt->add_class("Read Sensor"); 00112 #endif 00113 00114 } 00115 00116 00117 void 00118 KatanaActThread::finalize() 00119 { 00120 if ( __actmot_thread ) { 00121 __actmot_thread->cancel(); 00122 __actmot_thread->join(); 00123 __actmot_thread = NULL; 00124 } 00125 __sensacq_thread->cancel(); 00126 __sensacq_thread->join(); 00127 __sensacq_thread.reset(); 00128 00129 // Setting to NULL also deletes instance (RefPtr) 00130 __calib_thread = NULL; 00131 __goto_thread = NULL; 00132 __gripper_thread = NULL; 00133 00134 __katana->freezeRobot(); 00135 __katana = NULL; 00136 00137 __device.reset(); 00138 __protocol.reset(); 00139 00140 blackboard->unregister_listener(this); 00141 blackboard->close(__katana_if); 00142 } 00143 00144 00145 void 00146 KatanaActThread::once() 00147 { 00148 if ( __cfg_auto_calibrate ) { 00149 start_motion(__calib_thread, 0, "Auto calibration enabled, calibrating"); 00150 } 00151 } 00152 00153 00154 /** Update position data in BB interface. 00155 * @param refresh recv new data from arm 00156 */ 00157 void 00158 KatanaActThread::update_position(bool refresh) 00159 { 00160 double x, y, z, phi, theta, psi; 00161 try { 00162 __katana->getCoordinates(x, y, z, phi, theta, psi, refresh); 00163 __katana_if->set_x(x); 00164 __katana_if->set_y(y); 00165 __katana_if->set_z(z); 00166 __katana_if->set_phi(phi); 00167 __katana_if->set_theta(theta); 00168 __katana_if->set_psi(psi); 00169 } catch (/*KNI*/::Exception &e) { 00170 logger->log_warn(name(), "Updating position values failed: %s", e.what()); 00171 } 00172 } 00173 00174 00175 /** Update sensor values as necessary. 00176 * To be called only from KatanaSensorThread. Makes the local decision whether 00177 * sensor can be written (calibration is not running) and whether the data 00178 * needs to be refreshed (no active motion). 00179 */ 00180 void 00181 KatanaActThread::update_sensor_values() 00182 { 00183 MutexLocker lock(loop_mutex); 00184 if ( __actmot_thread != __calib_thread ) { 00185 update_sensors(! __actmot_thread); 00186 } 00187 } 00188 00189 00190 /** Update sensor value in BB interface. 00191 * @param refresh recv new data from arm 00192 */ 00193 void 00194 KatanaActThread::update_sensors(bool refresh) 00195 { 00196 try { 00197 const TSctDAT *sensor_data = __sensor_ctrl->GetDAT(); 00198 00199 unsigned char sensor_values[__katana_if->maxlenof_sensor_value()]; 00200 const int num_sensors = std::min((size_t)sensor_data->cnt, __katana_if->maxlenof_sensor_value()); 00201 for (int i = 0; i < num_sensors; ++i) { 00202 if (sensor_data->arr[i] <= 0) { 00203 sensor_values[i] = 0; 00204 } else if (sensor_data->arr[i] >= 255) { 00205 sensor_values[i] = 255; 00206 } else { 00207 sensor_values[i] = sensor_data->arr[i]; 00208 } 00209 } 00210 00211 __katana_if->set_sensor_value(sensor_values); 00212 } catch (/*KNI*/::Exception &e) { 00213 logger->log_warn(name(), "Updating sensor values failed: %s", e.what()); 00214 } 00215 00216 if (refresh) __sensacq_thread->wakeup(); 00217 } 00218 00219 /** Start a motion. 00220 * @param motion_thread motion thread to start 00221 * @param msgid BB message ID of message that caused the motion 00222 * @param logmsg message to print, format for following arguments 00223 */ 00224 void 00225 KatanaActThread::start_motion(RefPtr<KatanaMotionThread> motion_thread, 00226 unsigned int msgid, const char *logmsg, ...) 00227 { 00228 va_list arg; 00229 va_start(arg, logmsg); 00230 logger->vlog_debug(name(), logmsg, arg); 00231 __sensacq_thread->set_enabled(false); 00232 __actmot_thread = motion_thread; 00233 __actmot_thread->start(/* wait */ false); 00234 __katana_if->set_msgid(msgid); 00235 __katana_if->set_final(false); 00236 va_end(arg); 00237 } 00238 00239 00240 /** Stop any running motion. */ 00241 void 00242 KatanaActThread::stop_motion() 00243 { 00244 logger->log_info(name(), "Stopping arm movement"); 00245 loop_mutex->lock(); 00246 if (__actmot_thread) { 00247 __actmot_thread->cancel(); 00248 __actmot_thread->join(); 00249 __actmot_thread = NULL; 00250 } 00251 try { 00252 __katana->freezeRobot(); 00253 } catch (/*KNI*/::Exception &e) { 00254 logger->log_warn(name(), "Failed to freeze robot on stop: %s", e.what()); 00255 } 00256 loop_mutex->unlock(); 00257 } 00258 00259 00260 void 00261 KatanaActThread::loop() 00262 { 00263 if ( __actmot_thread ) { 00264 update_position(/* refresh */ false); 00265 __katana_if->write(); 00266 if (! __actmot_thread->finished()) { 00267 return; 00268 } else { 00269 logger->log_debug(name(), "Motion thread %s finished, collecting", __actmot_thread->name()); 00270 __actmot_thread->join(); 00271 __katana_if->set_final(true); 00272 __katana_if->set_error_code(__actmot_thread->error_code()); 00273 if (__actmot_thread == __calib_thread) { 00274 __katana_if->set_calibrated(true); 00275 } 00276 __actmot_thread->reset(); 00277 __actmot_thread = NULL; 00278 logger->log_debug(name(), "Motion thread collected"); 00279 __sensacq_thread->set_enabled(true); 00280 } 00281 } 00282 00283 while (! __katana_if->msgq_empty() && ! __actmot_thread ) { 00284 if (__katana_if->msgq_first_is<KatanaInterface::CalibrateMessage>()) { 00285 KatanaInterface::CalibrateMessage *msg = __katana_if->msgq_first(msg); 00286 start_motion(__calib_thread, msg->id(), "Calibrating arm"); 00287 00288 } else if (__katana_if->msgq_first_is<KatanaInterface::LinearGotoMessage>()) { 00289 KatanaInterface::LinearGotoMessage *msg = __katana_if->msgq_first(msg); 00290 00291 __goto_thread->set_target(msg->x(), msg->y(), msg->z(), 00292 msg->phi(), msg->theta(), msg->psi()); 00293 start_motion(__goto_thread, msg->id(), 00294 "Linear movement to (%f,%f,%f, %f,%f,%f)", 00295 msg->x(), msg->y(), msg->z(), 00296 msg->phi(), msg->theta(), msg->psi()); 00297 00298 } else if (__katana_if->msgq_first_is<KatanaInterface::ParkMessage>()) { 00299 KatanaInterface::ParkMessage *msg = __katana_if->msgq_first(msg); 00300 00301 __goto_thread->set_target(__cfg_park_x, __cfg_park_y, __cfg_park_z, 00302 __cfg_park_phi, __cfg_park_theta, __cfg_park_psi); 00303 start_motion(__goto_thread, msg->id(), "Parking arm"); 00304 00305 } else if (__katana_if->msgq_first_is<KatanaInterface::OpenGripperMessage>()) { 00306 KatanaInterface::OpenGripperMessage *msg = __katana_if->msgq_first(msg); 00307 __gripper_thread->set_mode(KatanaGripperThread::OPEN_GRIPPER); 00308 start_motion(__gripper_thread, msg->id(), "Opening gripper"); 00309 00310 } else if (__katana_if->msgq_first_is<KatanaInterface::CloseGripperMessage>()) { 00311 KatanaInterface::CloseGripperMessage *msg = __katana_if->msgq_first(msg); 00312 __gripper_thread->set_mode(KatanaGripperThread::CLOSE_GRIPPER); 00313 start_motion(__gripper_thread, msg->id(), "Closing gripper"); 00314 00315 } else if (__katana_if->msgq_first_is<KatanaInterface::SetEnabledMessage>()) { 00316 KatanaInterface::SetEnabledMessage *msg = __katana_if->msgq_first(msg); 00317 00318 try { 00319 if (msg->is_enabled()) { 00320 logger->log_debug(name(), "Turning ON the arm"); 00321 __katana->switchRobotOn(); 00322 update_position(/* refresh */ true); 00323 } else { 00324 logger->log_debug(name(), "Turning OFF the arm"); 00325 __katana->switchRobotOff(); 00326 } 00327 __katana_if->set_enabled(msg->is_enabled()); 00328 } catch (/*KNI*/::Exception &e) { 00329 logger->log_warn(name(), "Failed enable/disable arm: %s", e.what()); 00330 } 00331 00332 } else if (__katana_if->msgq_first_is<KatanaInterface::SetMaxVelocityMessage>()) { 00333 KatanaInterface::SetMaxVelocityMessage *msg = __katana_if->msgq_first(msg); 00334 00335 unsigned int max_vel = msg->max_velocity(); 00336 if ( max_vel == 0 ) max_vel = __cfg_defmax_speed; 00337 00338 __katana->setRobotVelocityLimit(max_vel); 00339 __katana_if->set_max_velocity(max_vel); 00340 00341 } else { 00342 logger->log_warn(name(), "Unknown message received"); 00343 } 00344 00345 __katana_if->msgq_pop(); 00346 } 00347 00348 __katana_if->write(); 00349 00350 #ifdef USE_TIMETRACKER 00351 if (++__tt_count > 100) { 00352 __tt_count = 0; 00353 __tt->print_to_stdout(); 00354 } 00355 #endif 00356 } 00357 00358 00359 bool 00360 KatanaActThread::bb_interface_message_received(Interface *interface, 00361 Message *message) throw() 00362 { 00363 if (message->is_of_type<KatanaInterface::StopMessage>()) { 00364 stop_motion(); 00365 return false; // do not enqueue StopMessage 00366 } else if (message->is_of_type<KatanaInterface::FlushMessage>()) { 00367 stop_motion(); 00368 logger->log_info(name(), "Flushing message queue"); 00369 __katana_if->msgq_flush(); 00370 return false; 00371 } else { 00372 logger->log_info(name(), "Received message of type %s, enqueueing", message->type()); 00373 return true; 00374 } 00375 }