Fawkes API Fawkes Development Version

act_thread.cpp

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 }
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends