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 #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 }