00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
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
00035
00036
00037
00038
00039
00040
00041
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
00054
00055
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 (::Exception &e) {
00091 throw fawkes::Exception(e.what());
00092 }
00093
00094
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
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
00155
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 (::Exception &e) {
00170 logger->log_warn(name(), "Updating position values failed: %s", e.what());
00171 }
00172 }
00173
00174
00175
00176
00177
00178
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
00191
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 (::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
00220
00221
00222
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( false);
00234 __katana_if->set_msgid(msgid);
00235 __katana_if->set_final(false);
00236 va_end(arg);
00237 }
00238
00239
00240
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 (::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( 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( 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 (::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;
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 }