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 #include "controller_kni.h" 00025 #include "controller_openrave.h" 00026 00027 #include <core/threading/mutex_locker.h> 00028 #include <interfaces/KatanaInterface.h> 00029 #include <utils/math/angle.h> 00030 #include <utils/time/time.h> 00031 00032 #include <algorithm> 00033 #include <cstdarg> 00034 00035 using namespace fawkes; 00036 #ifdef HAVE_TF 00037 using namespace fawkes::tf; 00038 #endif 00039 00040 /** @class KatanaActThread "act_thread.h" 00041 * Katana act thread. 00042 * This thread integrates into the Fawkes main loop at the ACT_EXEC hook and 00043 * interacts with a given controller for the Katana. 00044 * @author Tim Niemueller 00045 */ 00046 00047 /** Constructor. */ 00048 KatanaActThread::KatanaActThread() 00049 : Thread("KatanaActThread", Thread::OPMODE_WAITFORWAKEUP), 00050 BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_ACT_EXEC), 00051 TransformAspect(TransformAspect::BOTH, "Katana"), 00052 BlackBoardInterfaceListener("KatanaActThread") 00053 { 00054 __last_update = new Time(); 00055 } 00056 00057 /** Destructor. */ 00058 KatanaActThread::~KatanaActThread() 00059 { 00060 delete __last_update; 00061 } 00062 00063 00064 void 00065 KatanaActThread::init() 00066 { 00067 // Note: due to the use of auto_ptr and RefPtr resources are automatically 00068 // freed on destruction, therefore no special handling is necessary in init() 00069 // itself! 00070 00071 __cfg_controller = config->get_string("/hardware/katana/controller"); 00072 __cfg_device = config->get_string("/hardware/katana/device"); 00073 __cfg_kni_conffile = config->get_string("/hardware/katana/kni_conffile"); 00074 __cfg_auto_calibrate = config->get_bool("/hardware/katana/auto_calibrate"); 00075 __cfg_defmax_speed = config->get_uint("/hardware/katana/default_max_speed"); 00076 __cfg_read_timeout = config->get_uint("/hardware/katana/read_timeout_msec"); 00077 __cfg_write_timeout = config->get_uint("/hardware/katana/write_timeout_msec"); 00078 __cfg_gripper_pollint = config->get_uint("/hardware/katana/gripper_pollint_msec"); 00079 __cfg_goto_pollint = config->get_uint("/hardware/katana/goto_pollint_msec"); 00080 00081 __cfg_park_x = config->get_float("/hardware/katana/park_x"); 00082 __cfg_park_y = config->get_float("/hardware/katana/park_y"); 00083 __cfg_park_z = config->get_float("/hardware/katana/park_z"); 00084 __cfg_park_phi = config->get_float("/hardware/katana/park_phi"); 00085 __cfg_park_theta = config->get_float("/hardware/katana/park_theta"); 00086 __cfg_park_psi = config->get_float("/hardware/katana/park_psi"); 00087 00088 __cfg_distance_scale = config->get_float("/hardware/katana/distance_scale"); 00089 00090 __cfg_update_interval = config->get_float("/hardware/katana/update_interval"); 00091 00092 __cfg_frame_kni = config->get_string("/plugins/static-transforms/transforms/katana_kni/child_frame"); 00093 __cfg_frame_openrave = config->get_string("/plugins/static-transforms/transforms/openrave/child_frame"); 00094 00095 #ifdef HAVE_OPENRAVE 00096 __cfg_OR_enabled = config->get_bool("/hardware/katana/openrave/enabled"); 00097 __cfg_OR_use_viewer = config->get_bool("/hardware/katana/openrave/use_viewer"); 00098 __cfg_OR_auto_load_ik = config->get_bool("/hardware/katana/openrave/auto_load_ik"); 00099 __cfg_OR_robot_file = config->get_string("/hardware/katana/openrave/robot_file"); 00100 __cfg_OR_arm_model = config->get_string("/hardware/katana/openrave/arm_model"); 00101 #else 00102 __cfg_OR_enabled = false; 00103 #endif 00104 00105 __last_update->set_clock(clock); 00106 __last_update->set_time(0, 0); 00107 00108 // Load katana controller 00109 if( __cfg_controller == "kni") { 00110 KatanaControllerKni* kat_ctrl = new KatanaControllerKni(); 00111 __katana = kat_ctrl; 00112 try { 00113 kat_ctrl->setup(__cfg_device, __cfg_kni_conffile, 00114 __cfg_read_timeout, __cfg_write_timeout); 00115 } catch(fawkes::Exception &e) { 00116 logger->log_warn(name(), "Setup KatanaControllerKni failed. Ex: %s", e.what()); 00117 } 00118 kat_ctrl = NULL; 00119 00120 } else if( __cfg_controller == "openrave") { 00121 #ifdef HAVE_OPENRAVE 00122 if(!__cfg_OR_enabled) { 00123 throw fawkes::Exception("Cannot use controller 'openrave', OpenRAVE is deactivated by config flag!"); 00124 } 00125 __katana = new KatanaControllerOpenrave(openrave); 00126 #else 00127 throw fawkes::Exception("Cannot use controller 'openrave', OpenRAVE not installed!"); 00128 #endif 00129 00130 } else { 00131 throw fawkes::Exception("Invalid controller given: '%s'", __cfg_controller.c_str()); 00132 } 00133 00134 // If you have more than one interface: catch exception and close them! 00135 __katana_if = blackboard->open_for_writing<KatanaInterface>("Katana"); 00136 00137 // Create all other threads 00138 __sensacq_thread.reset(new KatanaSensorAcquisitionThread(__katana, logger)); 00139 __calib_thread = new KatanaCalibrationThread(__katana, logger); 00140 __gripper_thread = new KatanaGripperThread(__katana, logger, 00141 __cfg_gripper_pollint); 00142 __motor_control_thread = new KatanaMotorControlThread(__katana, logger, __cfg_goto_pollint); 00143 __goto_thread = new KatanaGotoThread(__katana, logger, __cfg_goto_pollint); 00144 #ifdef HAVE_OPENRAVE 00145 __goto_openrave_thread = new KatanaGotoOpenRaveThread(__katana, logger, openrave, __cfg_goto_pollint, __cfg_OR_robot_file, 00146 __cfg_OR_arm_model, __cfg_OR_auto_load_ik, __cfg_OR_use_viewer); 00147 if(__cfg_OR_enabled) 00148 {__goto_openrave_thread->init();} 00149 #endif 00150 00151 // Intialize katana controller 00152 try { 00153 __katana->init(); 00154 __katana->set_max_velocity(__cfg_defmax_speed); 00155 logger->log_debug(name(), "Katana successfully initialized"); 00156 } catch(fawkes::Exception &e) { 00157 logger->log_warn(name(), "Initializing controller failed. Ex: %s", e.what()); 00158 finalize(); 00159 throw; // need try-catch anyway? 00160 } 00161 00162 __sensacq_thread->start(); 00163 00164 bbil_add_message_interface(__katana_if); 00165 blackboard->register_listener(this); 00166 00167 #ifdef USE_TIMETRACKER 00168 __tt.reset(new TimeTracker()); 00169 __tt_count = 0; 00170 __ttc_read_sensor = __tt->add_class("Read Sensor"); 00171 #endif 00172 00173 } 00174 00175 00176 void 00177 KatanaActThread::finalize() 00178 { 00179 if ( __actmot_thread ) { 00180 __actmot_thread->cancel(); 00181 __actmot_thread->join(); 00182 __actmot_thread = NULL; 00183 } 00184 __sensacq_thread->cancel(); 00185 __sensacq_thread->join(); 00186 __sensacq_thread.reset(); 00187 00188 // Setting to NULL also deletes instance (RefPtr) 00189 __calib_thread = NULL; 00190 __goto_thread = NULL; 00191 __gripper_thread = NULL; 00192 __motor_control_thread = NULL; 00193 #ifdef HAVE_OPENRAVE 00194 if(__cfg_OR_enabled) 00195 {__goto_openrave_thread->finalize();} 00196 __goto_openrave_thread = NULL; 00197 #endif 00198 00199 try { 00200 __katana->stop(); 00201 } catch(fawkes::Exception &e) { 00202 logger->log_warn(name(), "failed stopping katana. Ex:%s", e.what()); 00203 } 00204 __katana = NULL; 00205 00206 blackboard->unregister_listener(this); 00207 blackboard->close(__katana_if); 00208 } 00209 00210 00211 void 00212 KatanaActThread::once() 00213 { 00214 if ( __cfg_auto_calibrate ) { 00215 start_motion(__calib_thread, 0, "Auto calibration enabled, calibrating"); 00216 __katana_if->set_enabled(true); 00217 __katana_if->write(); 00218 } 00219 } 00220 00221 00222 /** Update position data in BB interface. 00223 * @param refresh recv new data from arm 00224 */ 00225 void 00226 KatanaActThread::update_position(bool refresh) 00227 { 00228 try { 00229 __katana->read_coordinates(refresh); 00230 if( __cfg_controller == "kni") { 00231 __katana_if->set_x(__cfg_distance_scale * __katana->x()); 00232 __katana_if->set_y(__cfg_distance_scale * __katana->y()); 00233 __katana_if->set_z(__cfg_distance_scale * __katana->z()); 00234 } else if( __cfg_controller == "openrave") { 00235 00236 if( !tf_listener->frame_exists(__cfg_frame_openrave) ) { 00237 logger->log_warn(name(), "tf frames not existing: '%s'. Skipping transform to kni coordinates.", 00238 __cfg_frame_openrave.c_str() ); 00239 } else { 00240 Stamped<Point> target; 00241 Stamped<Point> target_local(Point(__katana->x(), __katana->y(), __katana->z()), 00242 fawkes::Time(0,0), __cfg_frame_openrave); 00243 00244 tf_listener->transform_point(__cfg_frame_kni, target_local, target); 00245 00246 __katana_if->set_x(target.getX()); 00247 __katana_if->set_y(target.getY()); 00248 __katana_if->set_z(target.getZ()); 00249 } 00250 } 00251 __katana_if->set_phi(__katana->phi()); 00252 __katana_if->set_theta(__katana->theta()); 00253 __katana_if->set_psi(__katana->psi()); 00254 } catch (fawkes::Exception &e) { 00255 logger->log_warn(name(), "Updating position values failed: %s", e.what()); 00256 } 00257 00258 float *a = __katana_if->angles(); 00259 fawkes::Time now(clock); 00260 00261 static const float p90 = deg2rad(90); 00262 static const float p180 = deg2rad(180); 00263 00264 Transform bs_j1(Quaternion(a[0], 0, 0), Vector3(0, 0, 0.141)); 00265 Transform j1_j2(Quaternion(0, a[1] - p90, 0), Vector3(0, 0, 0.064)); 00266 Transform j2_j3(Quaternion(0, a[2] + p180, 0), Vector3(0, 0, 0.190)); 00267 Transform j3_j4(Quaternion(0, -a[3] - p180, 0), Vector3(0, 0, 0.139)); 00268 Transform j4_j5(Quaternion(-a[4], 0, 0), Vector3(0, 0, 0.120)); 00269 Transform j5_gr(Quaternion(0, -p90, 0), Vector3(0, 0, 0.065)); 00270 00271 tf_publisher->send_transform(bs_j1, now, "/katana/base", "/katana/j1"); 00272 tf_publisher->send_transform(j1_j2, now, "/katana/j1", "/katana/j2"); 00273 tf_publisher->send_transform(j2_j3, now, "/katana/j2", "/katana/j3"); 00274 tf_publisher->send_transform(j3_j4, now, "/katana/j3", "/katana/j4"); 00275 tf_publisher->send_transform(j4_j5, now, "/katana/j4", "/katana/j5"); 00276 tf_publisher->send_transform(j5_gr, now, "/katana/j5", "/katana/gripper"); //remember to adjust name in message-processing on change 00277 } 00278 00279 00280 /** Update sensor values as necessary. 00281 * To be called only from KatanaSensorThread. Makes the local decision whether 00282 * sensor can be written (calibration is not running) and whether the data 00283 * needs to be refreshed (no active motion). 00284 */ 00285 void 00286 KatanaActThread::update_sensor_values() 00287 { 00288 MutexLocker lock(loop_mutex); 00289 if ( __actmot_thread != __calib_thread ) { 00290 update_sensors(! __actmot_thread); 00291 } 00292 } 00293 00294 00295 /** Update sensor value in BB interface. 00296 * @param refresh recv new data from arm 00297 */ 00298 void 00299 KatanaActThread::update_sensors(bool refresh) 00300 { 00301 try { 00302 std::vector<int> sensors; 00303 __katana->get_sensors(sensors, false); 00304 00305 const int num_sensors = std::min(sensors.size(), __katana_if->maxlenof_sensor_value()); 00306 for (int i = 0; i < num_sensors; ++i) { 00307 if (sensors.at(i) <= 0) { 00308 __katana_if->set_sensor_value(i, 0); 00309 } else if (sensors.at(i) >= 255) { 00310 __katana_if->set_sensor_value(i, 255); 00311 } else { 00312 __katana_if->set_sensor_value(i, sensors.at(i)); 00313 } 00314 } 00315 } catch (fawkes::Exception &e) { 00316 logger->log_warn(name(), "Updating sensor values failed: %s", e.what()); 00317 } 00318 00319 if (refresh) __sensacq_thread->wakeup(); 00320 } 00321 00322 00323 /** Update motor encoder and angle data in BB interface. 00324 * @param refresh recv new data from arm 00325 */ 00326 void 00327 KatanaActThread::update_motors(bool refresh) 00328 { 00329 try { 00330 if( __katana->joint_encoders()) { 00331 std::vector<int> encoders; 00332 __katana->get_encoders(encoders, refresh); 00333 for(unsigned int i=0; i<encoders.size(); i++) { 00334 __katana_if->set_encoders(i, encoders.at(i)); 00335 } 00336 } 00337 00338 if( __katana->joint_angles()) { 00339 std::vector<float> angles; 00340 __katana->get_angles(angles, false); 00341 for(unsigned int i=0; i<angles.size(); i++) { 00342 __katana_if->set_angles(i, angles.at(i)); 00343 } 00344 } 00345 } catch (fawkes::Exception &e) { 00346 logger->log_warn(name(), "Updating motor values failed. Ex:%s", e.what()); 00347 } 00348 } 00349 00350 00351 /** Start a motion. 00352 * @param motion_thread motion thread to start 00353 * @param msgid BB message ID of message that caused the motion 00354 * @param logmsg message to print, format for following arguments 00355 */ 00356 void 00357 KatanaActThread::start_motion(RefPtr<KatanaMotionThread> motion_thread, 00358 unsigned int msgid, const char *logmsg, ...) 00359 { 00360 va_list arg; 00361 va_start(arg, logmsg); 00362 logger->vlog_debug(name(), logmsg, arg); 00363 __sensacq_thread->set_enabled(false); 00364 __actmot_thread = motion_thread; 00365 __actmot_thread->start(/* wait */ false); 00366 __katana_if->set_msgid(msgid); 00367 __katana_if->set_final(false); 00368 va_end(arg); 00369 } 00370 00371 00372 /** Stop any running motion. */ 00373 void 00374 KatanaActThread::stop_motion() 00375 { 00376 logger->log_info(name(), "Stopping arm movement"); 00377 loop_mutex->lock(); 00378 if (__actmot_thread) { 00379 __actmot_thread->cancel(); 00380 __actmot_thread->join(); 00381 __actmot_thread = NULL; 00382 } 00383 try { 00384 __katana->stop(); 00385 } catch (fawkes::Exception &e) { 00386 logger->log_warn(name(), "Failed to freeze robot on stop: %s", e.what()); 00387 } 00388 loop_mutex->unlock(); 00389 } 00390 00391 00392 void 00393 KatanaActThread::loop() 00394 { 00395 if ( __actmot_thread ) { 00396 update_motors(/* refresh */ false); 00397 update_position(/* refresh */ false); 00398 __katana_if->write(); 00399 if (! __actmot_thread->finished()) { 00400 return; 00401 } else { 00402 logger->log_debug(name(), "Motion thread %s finished, collecting", __actmot_thread->name()); 00403 __actmot_thread->join(); 00404 __katana_if->set_final(true); 00405 __katana_if->set_error_code(__actmot_thread->error_code()); 00406 if (__actmot_thread == __calib_thread) { 00407 __katana_if->set_calibrated(true); 00408 } 00409 __actmot_thread->reset(); 00410 __actmot_thread = NULL; 00411 logger->log_debug(name(), "Motion thread collected"); 00412 __sensacq_thread->set_enabled(true); 00413 00414 update_motors(/* refresh */ true); 00415 update_position(/* refresh */ true); 00416 00417 #ifdef HAVE_OPENRAVE 00418 if(__cfg_OR_enabled) { __goto_openrave_thread->update_openrave_data(); } 00419 #endif 00420 } 00421 } else if (!__katana_if->is_enabled()) { 00422 update_position(/* refresh */ true); 00423 update_motors(/* refresh */ true); 00424 00425 } else { 00426 // update every once in a while to keep transforms updated 00427 fawkes::Time now(clock); 00428 if ((now - __last_update) >= __cfg_update_interval) { 00429 __last_update->stamp(); 00430 update_position(/* refresh */ false); 00431 update_motors(/* refresh */ false); 00432 } 00433 } 00434 00435 while (! __katana_if->msgq_empty() && ! __actmot_thread ) { 00436 if (__katana_if->msgq_first_is<KatanaInterface::CalibrateMessage>()) { 00437 KatanaInterface::CalibrateMessage *msg = __katana_if->msgq_first(msg); 00438 start_motion(__calib_thread, msg->id(), "Calibrating arm"); 00439 00440 } else if (__katana_if->msgq_first_is<KatanaInterface::LinearGotoMessage>()) { 00441 KatanaInterface::LinearGotoMessage *msg = __katana_if->msgq_first(msg); 00442 00443 bool trans_frame_exists = tf_listener->frame_exists(msg->trans_frame()); 00444 bool rot_frame_exists = tf_listener->frame_exists(msg->rot_frame()); 00445 if( !trans_frame_exists || !rot_frame_exists ) { 00446 logger->log_warn(name(), "tf frames not existing: '%s%s%s'. Skipping message.", 00447 trans_frame_exists ? "" : msg->trans_frame(), 00448 trans_frame_exists ? "" : "', '", 00449 rot_frame_exists ? "" : msg->rot_frame() ); 00450 } else { 00451 Stamped<Point> target; 00452 Stamped<Point> target_local(Point(msg->x(), msg->y(), msg->z()), 00453 fawkes::Time(0,0), msg->trans_frame()); 00454 if( __cfg_OR_enabled ) { 00455 #ifdef HAVE_OPENRAVE 00456 tf_listener->transform_point(__cfg_frame_openrave, target_local, target); 00457 if( msg->offset_xy() != 0.f ) { 00458 Vector3 offset(target.getX(), target.getY(), 0.f); 00459 offset = (offset/offset.length()) * msg->offset_xy(); // Vector3 does not support a set_length method 00460 target += offset; 00461 } 00462 // TODO: how to transform euler rotation to quaternion, to be used for tf?? 00463 if( strcmp(msg->trans_frame(), "/katana/gripper")==0 ) { 00464 __goto_openrave_thread->set_target(msg->x(), msg->y(), msg->z(), 00465 msg->phi(), msg->theta(), msg->psi()); 00466 __goto_openrave_thread->set_arm_extension(true); 00467 } else { 00468 __goto_openrave_thread->set_target(target.getX(), target.getY(), target.getZ(), 00469 msg->phi(), msg->theta(), msg->psi()); 00470 } 00471 __goto_openrave_thread->set_theta_error(msg->theta_error()); 00472 __goto_openrave_thread->set_move_straight(msg->is_straight()); 00473 #ifdef EARLY_PLANNING 00474 __goto_openrave_thread->plan_target(); 00475 #endif 00476 start_motion(__goto_openrave_thread, msg->id(), 00477 "Linear movement to (%f,%f,%f, %f,%f,%f), frame '%s', theta_error:%f, straight:%u", 00478 target.getX(), target.getY(), target.getZ(), 00479 msg->phi(), msg->theta(), msg->psi(), __cfg_frame_openrave.c_str(), msg->theta_error(), msg->is_straight()); 00480 #endif 00481 } else { 00482 tf_listener->transform_point(__cfg_frame_kni, target_local, target); 00483 if( msg->offset_xy() != 0.f ) { 00484 Vector3 offset(target.getX(), target.getY(), 0.f); 00485 offset = (offset/offset.length()) * msg->offset_xy(); // Vector3 does not support a set_length method 00486 target += offset; 00487 } 00488 __goto_thread->set_target(target.getX() / __cfg_distance_scale, 00489 target.getY() / __cfg_distance_scale, 00490 target.getZ() / __cfg_distance_scale, 00491 msg->phi(), msg->theta(), msg->psi()); 00492 start_motion(__goto_thread, msg->id(), 00493 "Linear movement to (%f,%f,%f, %f,%f,%f), frame '%s'", 00494 target.getX(), target.getY(), target.getZ(), 00495 msg->phi(), msg->theta(), msg->psi(), __cfg_frame_kni.c_str()); 00496 } 00497 } 00498 00499 } else if (__katana_if->msgq_first_is<KatanaInterface::LinearGotoKniMessage>()) { 00500 KatanaInterface::LinearGotoKniMessage *msg = __katana_if->msgq_first(msg); 00501 00502 float x = msg->x() * __cfg_distance_scale; 00503 float y = msg->y() * __cfg_distance_scale; 00504 float z = msg->z() * __cfg_distance_scale; 00505 00506 tf::Stamped<Point> target; 00507 tf::Stamped<Point> target_local(tf::Point(x, y, z), 00508 fawkes::Time(0,0), __cfg_frame_kni); 00509 00510 if( __cfg_OR_enabled ) { 00511 #ifdef HAVE_OPENRAVE 00512 tf_listener->transform_point(__cfg_frame_openrave, target_local, target); 00513 __goto_openrave_thread->set_target(target.getX(), target.getY(), target.getZ(), 00514 msg->phi(), msg->theta(), msg->psi()); 00515 #ifdef EARLY_PLANNING 00516 __goto_openrave_thread->plan_target(); 00517 #endif 00518 start_motion(__goto_openrave_thread, msg->id(), 00519 "Linear movement to (%f,%f,%f, %f,%f,%f), frame '%s'", 00520 target.getX(), target.getY(), target.getZ(), 00521 msg->phi(), msg->theta(), msg->psi(), __cfg_frame_openrave.c_str()); 00522 #endif 00523 } else { 00524 __goto_thread->set_target(msg->x(), msg->y(), msg->z(), 00525 msg->phi(), msg->theta(), msg->psi()); 00526 00527 start_motion(__goto_thread, msg->id(), 00528 "Linear movement to (%f,%f,%f, %f,%f,%f), frame '%s'", 00529 x, y, z, 00530 msg->phi(), msg->theta(), msg->psi(), __cfg_frame_kni.c_str()); 00531 } 00532 00533 #ifdef HAVE_OPENRAVE 00534 } else if (__katana_if->msgq_first_is<KatanaInterface::ObjectGotoMessage>() && __cfg_OR_enabled) { 00535 KatanaInterface::ObjectGotoMessage *msg = __katana_if->msgq_first(msg); 00536 00537 float rot_x = 0.f; 00538 if( msg->rot_x() ) 00539 { rot_x = msg->rot_x(); } 00540 00541 __goto_openrave_thread->set_target(msg->object(), rot_x); 00542 #ifdef EARLY_PLANNING 00543 __goto_openrave_thread->plan_target(); 00544 #endif 00545 start_motion(__goto_openrave_thread, msg->id(), 00546 "Linear movement to object (%s, %f)", msg->object(), msg->rot_x()); 00547 #endif 00548 00549 } else if (__katana_if->msgq_first_is<KatanaInterface::ParkMessage>()) { 00550 KatanaInterface::ParkMessage *msg = __katana_if->msgq_first(msg); 00551 00552 if(__cfg_OR_enabled) { 00553 #ifdef HAVE_OPENRAVE 00554 tf::Stamped<Point> target; 00555 tf::Stamped<Point> target_local(tf::Point(__cfg_park_x * __cfg_distance_scale, 00556 __cfg_park_y * __cfg_distance_scale, 00557 __cfg_park_z * __cfg_distance_scale), 00558 fawkes::Time(0,0), __cfg_frame_kni); 00559 tf_listener->transform_point(__cfg_frame_openrave, target_local, target); 00560 00561 __goto_openrave_thread->set_target(target.getX(), target.getY(), target.getZ(), 00562 __cfg_park_phi, __cfg_park_theta, __cfg_park_psi); 00563 #ifdef EARLY_PLANNING 00564 __goto_openrave_thread->plan_target(); 00565 #endif 00566 start_motion(__goto_openrave_thread, msg->id(), "Parking arm"); 00567 #endif 00568 } else { 00569 __goto_thread->set_target(__cfg_park_x, __cfg_park_y, __cfg_park_z, 00570 __cfg_park_phi, __cfg_park_theta, __cfg_park_psi); 00571 start_motion(__goto_thread, msg->id(), "Parking arm"); 00572 } 00573 00574 } else if (__katana_if->msgq_first_is<KatanaInterface::OpenGripperMessage>()) { 00575 KatanaInterface::OpenGripperMessage *msg = __katana_if->msgq_first(msg); 00576 __gripper_thread->set_mode(KatanaGripperThread::OPEN_GRIPPER); 00577 start_motion(__gripper_thread, msg->id(), "Opening gripper"); 00578 00579 } else if (__katana_if->msgq_first_is<KatanaInterface::CloseGripperMessage>()) { 00580 KatanaInterface::CloseGripperMessage *msg = __katana_if->msgq_first(msg); 00581 __gripper_thread->set_mode(KatanaGripperThread::CLOSE_GRIPPER); 00582 start_motion(__gripper_thread, msg->id(), "Closing gripper"); 00583 00584 } else if (__katana_if->msgq_first_is<KatanaInterface::SetEnabledMessage>()) { 00585 KatanaInterface::SetEnabledMessage *msg = __katana_if->msgq_first(msg); 00586 00587 try { 00588 if (msg->is_enabled()) { 00589 logger->log_debug(name(), "Turning ON the arm"); 00590 __katana->turn_on(); 00591 update_position(/* refresh */ true); 00592 update_motors(/* refresh */ true); 00593 #ifdef HAVE_OPENRAVE 00594 if(__cfg_OR_enabled) 00595 {__goto_openrave_thread->update_openrave_data();} 00596 #endif 00597 } else { 00598 logger->log_debug(name(), "Turning OFF the arm"); 00599 __katana->turn_off(); 00600 } 00601 __katana_if->set_enabled(msg->is_enabled()); 00602 } catch (/*KNI*/::Exception &e) { 00603 logger->log_warn(name(), "Failed enable/disable arm: %s", e.what()); 00604 } 00605 00606 } else if (__katana_if->msgq_first_is<KatanaInterface::SetMaxVelocityMessage>()) { 00607 KatanaInterface::SetMaxVelocityMessage *msg = __katana_if->msgq_first(msg); 00608 00609 unsigned int max_vel = msg->max_velocity(); 00610 if ( max_vel == 0 ) max_vel = __cfg_defmax_speed; 00611 00612 try { 00613 __katana->set_max_velocity(max_vel); 00614 } catch (fawkes::Exception &e) { 00615 logger->log_warn(name(), "Failed setting max velocity. Ex:%s", e.what()); 00616 } 00617 __katana_if->set_max_velocity(max_vel); 00618 00619 } else if (__katana_if->msgq_first_is<KatanaInterface::SetPlannerParamsMessage>()) { 00620 KatanaInterface::SetPlannerParamsMessage *msg = __katana_if->msgq_first(msg); 00621 00622 if( __cfg_OR_enabled ) { 00623 #ifdef HAVE_OPENRAVE 00624 __goto_openrave_thread->set_plannerparams(msg->plannerparams()); 00625 #endif 00626 } 00627 00628 } else if (__katana_if->msgq_first_is<KatanaInterface::SetMotorEncoderMessage>()) { 00629 KatanaInterface::SetMotorEncoderMessage *msg = __katana_if->msgq_first(msg); 00630 00631 __motor_control_thread->set_encoder(msg->nr(), msg->enc(), false); 00632 start_motion(__motor_control_thread, msg->id(), "Moving motor"); 00633 00634 } else if (__katana_if->msgq_first_is<KatanaInterface::MoveMotorEncoderMessage>()) { 00635 KatanaInterface::MoveMotorEncoderMessage *msg = __katana_if->msgq_first(msg); 00636 00637 __motor_control_thread->set_encoder(msg->nr(), msg->enc(), true); 00638 start_motion(__motor_control_thread, msg->id(), "Moving motor"); 00639 00640 } else if (__katana_if->msgq_first_is<KatanaInterface::SetMotorAngleMessage>()) { 00641 KatanaInterface::SetMotorAngleMessage *msg = __katana_if->msgq_first(msg); 00642 00643 __motor_control_thread->set_angle(msg->nr(), msg->angle(), false); 00644 start_motion(__motor_control_thread, msg->id(), "Moving motor"); 00645 00646 } else if (__katana_if->msgq_first_is<KatanaInterface::MoveMotorAngleMessage>()) { 00647 KatanaInterface::MoveMotorAngleMessage *msg = __katana_if->msgq_first(msg); 00648 00649 __motor_control_thread->set_angle(msg->nr(), msg->angle(), true); 00650 start_motion(__motor_control_thread, msg->id(), "Moving motor"); 00651 00652 } else { 00653 logger->log_warn(name(), "Unknown message received"); 00654 } 00655 00656 __katana_if->msgq_pop(); 00657 } 00658 00659 __katana_if->write(); 00660 00661 #ifdef USE_TIMETRACKER 00662 if (++__tt_count > 100) { 00663 __tt_count = 0; 00664 __tt->print_to_stdout(); 00665 } 00666 #endif 00667 } 00668 00669 00670 bool 00671 KatanaActThread::bb_interface_message_received(Interface *interface, 00672 Message *message) throw() 00673 { 00674 if (message->is_of_type<KatanaInterface::StopMessage>()) { 00675 stop_motion(); 00676 return false; // do not enqueue StopMessage 00677 } else if (message->is_of_type<KatanaInterface::FlushMessage>()) { 00678 stop_motion(); 00679 logger->log_info(name(), "Flushing message queue"); 00680 __katana_if->msgq_flush(); 00681 return false; 00682 } else { 00683 logger->log_info(name(), "Received message of type %s, enqueueing", message->type()); 00684 return true; 00685 } 00686 }