Fawkes API  Fawkes Development Version
rx28_thread.cpp
00001 
00002 /***************************************************************************
00003  *  rx28_thread.cpp - RX28 pan/tilt unit act thread
00004  *
00005  *  Created: Thu Jun 18 09:53:49 2009
00006  *  Copyright  2006-2011  Tim Niemueller [www.niemueller.de]
00007  ****************************************************************************/
00008 
00009 /*  This program is free software; you can redistribute it and/or modify
00010  *  it under the terms of the GNU General Public License as published by
00011  *  the Free Software Foundation; either version 2 of the License, or
00012  *  (at your option) any later version.
00013  *
00014  *  This program is distributed in the hope that it will be useful,
00015  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00016  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00017  *  GNU Library General Public License for more details.
00018  *
00019  *  Read the full text in the LICENSE.GPL file in the doc directory.
00020  */
00021 
00022 #include "rx28_thread.h"
00023 #include "rx28.h"
00024 
00025 #include <utils/math/angle.h>
00026 #include <core/threading/mutex_locker.h>
00027 #include <core/threading/read_write_lock.h>
00028 #include <core/threading/scoped_rwlock.h>
00029 #include <core/threading/wait_condition.h>
00030 #include <interfaces/PanTiltInterface.h>
00031 #include <interfaces/LedInterface.h>
00032 
00033 #include <cstdarg>
00034 #include <cmath>
00035 #include <unistd.h>
00036 
00037 using namespace fawkes;
00038 
00039 /** @class PanTiltRX28Thread "rx28_thread.h"
00040  * PanTilt act thread for RX28 PTU.
00041  * This thread integrates into the Fawkes main loop at the ACT_EXEC hook and
00042  * interacts with the controller of the RX28 PTU.
00043  * @author Tim Niemueller
00044  */
00045 
00046 /** Constructor.
00047  * @param pantilt_cfg_prefix pantilt plugin configuration prefix
00048  * @param ptu_cfg_prefix configuration prefix specific for the PTU
00049  * @param ptu_name name of the PTU configuration
00050  */
00051 PanTiltRX28Thread::PanTiltRX28Thread(std::string &pantilt_cfg_prefix,
00052                                      std::string &ptu_cfg_prefix,
00053                                      std::string &ptu_name)
00054   : PanTiltActThread("PanTiltRX28Thread"),
00055 #ifdef HAVE_TF
00056     TransformAspect(TransformAspect::ONLY_PUBLISHER,
00057                     (std::string("PTU ") + ptu_name).c_str()),
00058 #endif
00059     BlackBoardInterfaceListener("PanTiltRX28Thread(%s)", ptu_name.c_str())
00060 {
00061   set_name("PanTiltRX28Thread(%s)", ptu_name.c_str());
00062 
00063   __pantilt_cfg_prefix = pantilt_cfg_prefix;
00064   __ptu_cfg_prefix     = ptu_cfg_prefix;
00065   __ptu_name           = ptu_name;
00066 
00067   __rx28 = NULL;
00068 }
00069 
00070 
00071 void
00072 PanTiltRX28Thread::init()
00073 {
00074   __last_pan = __last_tilt = 0.f;
00075 
00076   // Note: due to the use of auto_ptr and RefPtr resources are automatically
00077   // freed on destruction, therefore no special handling is necessary in init()
00078   // itself!
00079 
00080   __cfg_device           = config->get_string((__ptu_cfg_prefix + "device").c_str());
00081   __cfg_read_timeout_ms  = config->get_uint((__ptu_cfg_prefix + "read_timeout_ms").c_str());
00082   __cfg_disc_timeout_ms  = config->get_uint((__ptu_cfg_prefix + "discover_timeout_ms").c_str());
00083   __cfg_pan_servo_id     = config->get_uint((__ptu_cfg_prefix + "pan_servo_id").c_str());
00084   __cfg_tilt_servo_id    = config->get_uint((__ptu_cfg_prefix + "tilt_servo_id").c_str());
00085   __cfg_pan_offset       = deg2rad(config->get_float((__ptu_cfg_prefix + "pan_offset").c_str()));
00086   __cfg_tilt_offset      = deg2rad(config->get_float((__ptu_cfg_prefix + "tilt_offset").c_str()));
00087   __cfg_goto_zero_start  = config->get_bool((__ptu_cfg_prefix + "goto_zero_start").c_str());
00088   __cfg_turn_off         = config->get_bool((__ptu_cfg_prefix + "turn_off").c_str());
00089   __cfg_cw_compl_margin  = config->get_uint((__ptu_cfg_prefix + "cw_compl_margin").c_str());
00090   __cfg_ccw_compl_margin = config->get_uint((__ptu_cfg_prefix + "ccw_compl_margin").c_str());
00091   __cfg_cw_compl_slope   = config->get_uint((__ptu_cfg_prefix + "cw_compl_slope").c_str());
00092   __cfg_ccw_compl_slope  = config->get_uint((__ptu_cfg_prefix + "ccw_compl_slope").c_str());
00093   __cfg_pan_min          = config->get_float((__ptu_cfg_prefix + "pan_min").c_str());
00094   __cfg_pan_max          = config->get_float((__ptu_cfg_prefix + "pan_max").c_str());
00095   __cfg_tilt_min         = config->get_float((__ptu_cfg_prefix + "tilt_min").c_str());
00096   __cfg_tilt_max         = config->get_float((__ptu_cfg_prefix + "tilt_max").c_str());
00097   __cfg_pan_margin       = config->get_float((__ptu_cfg_prefix + "pan_margin").c_str());
00098   __cfg_tilt_margin      = config->get_float((__ptu_cfg_prefix + "tilt_margin").c_str());
00099   __cfg_pan_start        = config->get_float((__ptu_cfg_prefix + "pan_start").c_str());
00100   __cfg_tilt_start       = config->get_float((__ptu_cfg_prefix + "tilt_start").c_str());
00101 
00102 #ifdef HAVE_TF
00103   float pan_trans_x  =
00104     config->get_float((__ptu_cfg_prefix + "pan_trans_x").c_str());
00105   float pan_trans_y  =
00106     config->get_float((__ptu_cfg_prefix + "pan_trans_y").c_str());
00107   float pan_trans_z  =
00108     config->get_float((__ptu_cfg_prefix + "pan_trans_z").c_str());
00109   float tilt_trans_x =
00110     config->get_float((__ptu_cfg_prefix + "tilt_trans_x").c_str());
00111   float tilt_trans_y =
00112     config->get_float((__ptu_cfg_prefix + "tilt_trans_y").c_str());
00113   float tilt_trans_z =
00114     config->get_float((__ptu_cfg_prefix + "tilt_trans_z").c_str());
00115 
00116   
00117   std::string frame_id_prefix = std::string("/") + __ptu_name;
00118   try {
00119     frame_id_prefix =
00120       config->get_string((__ptu_cfg_prefix + "frame_id_prefix").c_str());
00121   } catch (Exception &e) {} // ignore, use default
00122 
00123   __cfg_base_frame = frame_id_prefix + "/base";
00124   __cfg_pan_link   = frame_id_prefix + "/pan";
00125   __cfg_tilt_link  = frame_id_prefix + "/tilt";
00126 
00127   __translation_pan.setValue(pan_trans_x, pan_trans_y, pan_trans_z);
00128   __translation_tilt.setValue(tilt_trans_x, tilt_trans_y, tilt_trans_z);
00129 #endif
00130 
00131   bool pan_servo_found = false, tilt_servo_found = false;
00132 
00133   __rx28 = new RobotisRX28(__cfg_device.c_str(), __cfg_read_timeout_ms);
00134   RobotisRX28::DeviceList devl = __rx28->discover();
00135   for (RobotisRX28::DeviceList::iterator i = devl.begin(); i != devl.end(); ++i) {
00136     if (__cfg_pan_servo_id == *i) {
00137       pan_servo_found  = true;
00138     } else if (__cfg_tilt_servo_id == *i) {
00139       tilt_servo_found = true;
00140     } else {
00141       logger->log_warn(name(), "Servo %u in PTU servo chain, but neither "
00142                        "configured as pan nor as tilt servo", *i);
00143     }
00144   }
00145 
00146   // We only want responses to be sent on explicit READ to speed up communication
00147   __rx28->set_status_return_level(RobotisRX28::BROADCAST_ID, RobotisRX28::SRL_RESPOND_READ);
00148   // set compliance values
00149   __rx28->set_compliance_values(RobotisRX28::BROADCAST_ID,
00150                                 __cfg_cw_compl_margin, __cfg_cw_compl_slope,
00151                                 __cfg_ccw_compl_margin, __cfg_ccw_compl_slope);
00152   __rx28->set_led_enabled(__cfg_pan_servo_id, false);
00153 
00154 
00155   if (! (pan_servo_found && tilt_servo_found)) {
00156     throw Exception("Pan and/or tilt servo not found: pan: %i  tilt: %i",
00157                     pan_servo_found, tilt_servo_found);
00158   }
00159 
00160   // If you have more than one interface: catch exception and close them!
00161   std::string bbid = "PanTilt " + __ptu_name;
00162   __pantilt_if = blackboard->open_for_writing<PanTiltInterface>(bbid.c_str());
00163   __pantilt_if->set_calibrated(true);
00164   __pantilt_if->set_min_pan(__cfg_pan_min);
00165   __pantilt_if->set_max_pan(__cfg_pan_max);
00166   __pantilt_if->set_min_tilt(__cfg_tilt_min);
00167   __pantilt_if->set_max_tilt(__cfg_tilt_max);
00168   __pantilt_if->set_pan_margin(__cfg_pan_margin);
00169   __pantilt_if->set_tilt_margin(__cfg_tilt_margin);
00170   __pantilt_if->set_max_pan_velocity(__rx28->get_max_supported_speed(__cfg_pan_servo_id));
00171   __pantilt_if->set_max_tilt_velocity(__rx28->get_max_supported_speed(__cfg_tilt_servo_id));
00172   __pantilt_if->set_pan_velocity(__rx28->get_max_supported_speed(__cfg_pan_servo_id));
00173   __pantilt_if->set_tilt_velocity(__rx28->get_max_supported_speed(__cfg_tilt_servo_id));
00174   __pantilt_if->write();
00175 
00176   __led_if = blackboard->open_for_writing<LedInterface>(bbid.c_str());
00177 
00178   __wt = new WorkerThread(__ptu_name, logger, __rx28,
00179                           __cfg_pan_servo_id, __cfg_tilt_servo_id,
00180                           __cfg_pan_min, __cfg_pan_max, __cfg_tilt_min, __cfg_tilt_max,
00181                           __cfg_pan_offset, __cfg_tilt_offset);
00182   __wt->set_margins(__cfg_pan_margin, __cfg_tilt_margin);
00183   __wt->start();
00184   __wt->set_enabled(true);
00185   if ( __cfg_goto_zero_start ) {
00186     __wt->goto_pantilt_timed(__cfg_pan_start, __cfg_tilt_start, 3.0);
00187   }
00188 
00189   bbil_add_message_interface(__pantilt_if);
00190   blackboard->register_listener(this);
00191 
00192 #ifdef USE_TIMETRACKER
00193   __tt.reset(new TimeTracker());
00194   __tt_count = 0;
00195   __ttc_read_sensor = __tt->add_class("Read Sensor");
00196 #endif  
00197 
00198 }
00199 
00200 
00201 bool
00202 PanTiltRX28Thread::prepare_finalize_user()
00203 {
00204   if (__cfg_turn_off) {
00205     logger->log_info(name(), "Moving to park position");
00206     __wt->goto_pantilt_timed(0, __cfg_tilt_max, 2.0);
00207     // we need to wait twice, because the first wakeup is likely to happen
00208     // before the command is actually send
00209     __wt->wait_for_fresh_data();
00210     __wt->wait_for_fresh_data();
00211 
00212     while (! __wt->is_final()) {
00213       //__wt->wakeup();
00214       __wt->wait_for_fresh_data();
00215     }
00216   }
00217   return true;
00218 }
00219 
00220 void
00221 PanTiltRX28Thread::finalize()
00222 {
00223   blackboard->unregister_listener(this);
00224   blackboard->close(__pantilt_if);
00225   blackboard->close(__led_if);
00226 
00227   __wt->cancel();
00228   __wt->join();
00229   delete __wt;
00230 
00231   if (__cfg_turn_off) {
00232     logger->log_info(name(), "Turning off PTU");
00233     try {
00234       __rx28->set_led_enabled(__cfg_pan_servo_id,  false);
00235       __rx28->set_led_enabled(__cfg_tilt_servo_id, false);
00236       __rx28->set_torques_enabled(false, 2, __cfg_pan_servo_id, __cfg_tilt_servo_id);
00237     } catch (Exception &e) {
00238       logger->log_warn(name(), "Failed to turn of PTU: %s", e.what());
00239     }
00240   }
00241   
00242   // Setting to NULL deletes instance (RefPtr)
00243   __rx28 = NULL;
00244 }
00245 
00246 
00247 /** Update sensor values as necessary.
00248  * To be called only from PanTiltSensorThread. Writes the current pan/tilt
00249  * data into the interface.
00250  */
00251 void
00252 PanTiltRX28Thread::update_sensor_values()
00253 {
00254   if (__wt->has_fresh_data()) {
00255     float pan = 0, tilt = 0, panvel=0, tiltvel=0;
00256     fawkes::Time time;
00257     __wt->get_pantilt(pan, tilt, time);
00258     __wt->get_velocities(panvel, tiltvel);
00259 
00260     // poor man's filter: only update if we get a change of least half a degree
00261     if (fabs(__last_pan - pan) >= 0.009 || fabs(__last_tilt - tilt) >= 0.009) {
00262       __last_pan  = pan;
00263       __last_tilt = tilt;
00264     } else {
00265       pan  = __last_pan;
00266       tilt = __last_tilt;
00267     }
00268 
00269     __pantilt_if->set_pan(pan);
00270     __pantilt_if->set_tilt(tilt);
00271     __pantilt_if->set_pan_velocity(panvel);
00272     __pantilt_if->set_tilt_velocity(tiltvel);
00273     __pantilt_if->set_enabled(__wt->is_enabled());
00274     __pantilt_if->set_final(__wt->is_final());
00275     __pantilt_if->write();
00276 
00277 #ifdef HAVE_TF
00278     // Always publish updated transforms
00279     tf::Quaternion pr;  pr.setEulerZYX(pan, 0, 0);
00280     tf::Transform ptr(pr, __translation_pan);
00281     tf_publisher->send_transform(ptr, time, __cfg_base_frame, __cfg_pan_link);
00282 
00283     tf::Quaternion tr; tr.setEulerZYX(0, tilt, 0);
00284     tf::Transform ttr(tr, __translation_tilt);
00285     tf_publisher->send_transform(ttr, time, __cfg_pan_link, __cfg_tilt_link);
00286 #endif
00287   }
00288 }
00289 
00290 
00291 void
00292 PanTiltRX28Thread::loop()
00293 {
00294   __pantilt_if->set_final(__wt->is_final());
00295 
00296   while (! __pantilt_if->msgq_empty() ) {
00297     if (__pantilt_if->msgq_first_is<PanTiltInterface::CalibrateMessage>()) {
00298       // ignored
00299 
00300     } else if (__pantilt_if->msgq_first_is<PanTiltInterface::GotoMessage>()) {
00301       PanTiltInterface::GotoMessage *msg = __pantilt_if->msgq_first(msg);
00302 
00303       __wt->goto_pantilt(msg->pan(), msg->tilt());
00304       __pantilt_if->set_msgid(msg->id());
00305       __pantilt_if->set_final(false);
00306 
00307     } else if (__pantilt_if->msgq_first_is<PanTiltInterface::TimedGotoMessage>()) {
00308       PanTiltInterface::TimedGotoMessage *msg = __pantilt_if->msgq_first(msg);
00309 
00310       __wt->goto_pantilt_timed(msg->pan(), msg->tilt(), msg->time_sec());
00311       __pantilt_if->set_msgid(msg->id());
00312       __pantilt_if->set_final(false);
00313 
00314     } else if (__pantilt_if->msgq_first_is<PanTiltInterface::ParkMessage>()) {
00315       PanTiltInterface::ParkMessage *msg = __pantilt_if->msgq_first(msg);
00316 
00317       __wt->goto_pantilt(0, 0);
00318       __pantilt_if->set_msgid(msg->id());
00319       __pantilt_if->set_final(false);
00320 
00321     } else if (__pantilt_if->msgq_first_is<PanTiltInterface::SetEnabledMessage>()) {
00322       PanTiltInterface::SetEnabledMessage *msg = __pantilt_if->msgq_first(msg);
00323 
00324       __wt->set_enabled(msg->is_enabled());
00325 
00326     } else if (__pantilt_if->msgq_first_is<PanTiltInterface::SetVelocityMessage>()) {
00327       PanTiltInterface::SetVelocityMessage *msg = __pantilt_if->msgq_first(msg);
00328 
00329       if (msg->pan_velocity() > __pantilt_if->max_pan_velocity()) {
00330         logger->log_warn(name(), "Desired pan velocity %f too high, max is %f",
00331                          msg->pan_velocity(), __pantilt_if->max_pan_velocity());
00332       } else if (msg->tilt_velocity() > __pantilt_if->max_tilt_velocity()) {
00333         logger->log_warn(name(), "Desired tilt velocity %f too high, max is %f",
00334                          msg->tilt_velocity(), __pantilt_if->max_tilt_velocity());
00335       } else {
00336         __wt->set_velocities(msg->pan_velocity(), msg->tilt_velocity());
00337       }
00338 
00339     } else if (__pantilt_if->msgq_first_is<PanTiltInterface::SetMarginMessage>()) {
00340       PanTiltInterface::SetMarginMessage *msg = __pantilt_if->msgq_first(msg);
00341 
00342       __wt->set_margins(msg->pan_margin(), msg->tilt_margin());
00343       __pantilt_if->set_pan_margin(msg->pan_margin());
00344       __pantilt_if->set_tilt_margin(msg->tilt_margin());
00345 
00346     } else {
00347       logger->log_warn(name(), "Unknown message received");
00348     }
00349 
00350     __pantilt_if->msgq_pop();
00351   }
00352 
00353   __pantilt_if->write();
00354 
00355   bool write_led_if = false;
00356   while (! __led_if->msgq_empty() ) {
00357     write_led_if = true;
00358     if (__led_if->msgq_first_is<LedInterface::SetIntensityMessage>()) {
00359       LedInterface::SetIntensityMessage *msg = __led_if->msgq_first(msg);
00360       __wt->set_led_enabled((msg->intensity() >= 0.5));
00361       __led_if->set_intensity((msg->intensity() >= 0.5) ? LedInterface::ON : LedInterface::OFF);
00362     } else if (__led_if->msgq_first_is<LedInterface::TurnOnMessage>()) {
00363       __wt->set_led_enabled(true);
00364       __led_if->set_intensity(LedInterface::ON);
00365     } else if (__led_if->msgq_first_is<LedInterface::TurnOffMessage>()) {
00366       __wt->set_led_enabled(false);
00367       __led_if->set_intensity(LedInterface::OFF);
00368     }
00369 
00370     __led_if->msgq_pop();
00371   }
00372   if (write_led_if)  __led_if->write();
00373 
00374   //__wt->wakeup();
00375 }
00376 
00377 
00378 bool
00379 PanTiltRX28Thread::bb_interface_message_received(Interface *interface,
00380                                                  Message *message) throw()
00381 {
00382   if (message->is_of_type<PanTiltInterface::StopMessage>()) {
00383     __wt->stop_motion();
00384     return false; // do not enqueue StopMessage
00385   } else if (message->is_of_type<PanTiltInterface::FlushMessage>()) {
00386     __wt->stop_motion();
00387     logger->log_info(name(), "Flushing message queue");
00388     __pantilt_if->msgq_flush();
00389     return false;
00390   } else {
00391     logger->log_info(name(), "Received message of type %s, enqueueing", message->type());
00392     return true;
00393   }
00394 }
00395 
00396 
00397 /** @class PanTiltRX28Thread::WorkerThread "robotis/rx28_thread.h"
00398  * Worker thread for the PanTiltRX28Thread.
00399  * This continuous thread issues commands to the RX28 chain. In each loop it
00400  * will first execute pending operations, and then update the sensor data (lengthy
00401  * operation). Sensor data will only be updated while either a servo in the chain
00402  * is still moving or torque is disabled (so the motor can be move manually).
00403  * @author Tim Niemueller
00404  */
00405 
00406 
00407 /** Constructor.
00408  * @param ptu_name name of the pan/tilt unit
00409  * @param logger logger
00410  * @param rx28 RX28 chain
00411  * @param pan_servo_id servo ID of the pan servo
00412  * @param tilt_servo_id servo ID of the tilt servo
00413  * @param pan_min minimum pan in rad
00414  * @param pan_max maximum pan in rad
00415  * @param tilt_min minimum tilt in rad
00416  * @param tilt_max maximum tilt in rad
00417  * @param pan_offset pan offset from zero in servo ticks
00418  * @param tilt_offset tilt offset from zero in servo ticks
00419  */
00420 PanTiltRX28Thread::WorkerThread::WorkerThread(std::string ptu_name,
00421                                               fawkes::Logger *logger,
00422                                               fawkes::RefPtr<RobotisRX28> rx28,
00423                                               unsigned char pan_servo_id,
00424                                               unsigned char tilt_servo_id,
00425                                               float &pan_min, float &pan_max,
00426                                               float &tilt_min, float &tilt_max,
00427                                               float &pan_offset, float &tilt_offset)
00428   : Thread("", Thread::OPMODE_WAITFORWAKEUP)
00429 {
00430   set_name("RX28WorkerThread(%s)", ptu_name.c_str());
00431   set_coalesce_wakeups(true);
00432 
00433   __logger           = logger;
00434 
00435   __value_rwlock     = new ReadWriteLock();
00436   __rx28_rwlock      = new ReadWriteLock();
00437   __fresh_data_mutex = new Mutex();
00438   __update_waitcond  = new WaitCondition();
00439 
00440   __rx28 = rx28;
00441   __move_pending     = false;
00442   __target_pan       = 0;
00443   __target_tilt      = 0;
00444   __pan_servo_id     = pan_servo_id;
00445   __tilt_servo_id    = tilt_servo_id;
00446 
00447   __pan_min          = pan_min;
00448   __pan_max          = pan_max;
00449   __tilt_min         = tilt_min;
00450   __tilt_max         = tilt_max;
00451   __pan_offset       = pan_offset;
00452   __tilt_offset      = tilt_offset;
00453   __enable           = false;
00454   __disable          = false;
00455   __led_enable       = false;
00456   __led_disable      = false;
00457 
00458   __max_pan_speed    = __rx28->get_max_supported_speed(__pan_servo_id);
00459   __max_tilt_speed   = __rx28->get_max_supported_speed(__tilt_servo_id);
00460 }
00461 
00462 
00463 /** Destructor. */
00464 PanTiltRX28Thread::WorkerThread::~WorkerThread()
00465 {
00466   delete __value_rwlock;
00467   delete __rx28_rwlock;
00468   delete __fresh_data_mutex;
00469   delete __update_waitcond;
00470 }
00471 
00472 
00473 /** Enable or disable servo.
00474  * @param enabled true to enable servos, false to turn them off
00475  */
00476 void
00477 PanTiltRX28Thread::WorkerThread::set_enabled(bool enabled)
00478 {
00479   ScopedRWLock lock(__value_rwlock);
00480   if (enabled) {
00481     __enable  = true;
00482     __disable = false;
00483   } else {
00484     __enable  = false;
00485     __disable = true;
00486   }
00487   wakeup();
00488 }
00489 
00490 
00491 /** Enable or disable LED.
00492  * @param enabled true to enable LED, false to turn it off
00493  */
00494 void
00495 PanTiltRX28Thread::WorkerThread::set_led_enabled(bool enabled)
00496 {
00497   ScopedRWLock lock(__value_rwlock);
00498   if (enabled) {
00499     __led_enable  = true;
00500     __led_disable = false;
00501   } else {
00502     __led_enable  = false;
00503     __led_disable = true;
00504   }
00505   wakeup();
00506 }
00507 
00508 
00509 /** Stop currently running motion. */
00510 void
00511 PanTiltRX28Thread::WorkerThread::stop_motion()
00512 {
00513   float pan = 0, tilt = 0;
00514   get_pantilt(pan, tilt);
00515   goto_pantilt(pan, tilt);
00516 }
00517 
00518 
00519 /** Goto desired pan/tilt values.
00520  * @param pan pan in radians
00521  * @param tilt tilt in radians
00522  */
00523 void
00524 PanTiltRX28Thread::WorkerThread::goto_pantilt(float pan, float tilt)
00525 {
00526   ScopedRWLock lock(__value_rwlock);
00527   __target_pan   = pan;
00528   __target_tilt  = tilt;
00529   __move_pending = true;
00530   wakeup();
00531 }
00532 
00533 
00534 /** Goto desired pan/tilt values in a specified time.
00535  * @param pan pan in radians
00536  * @param tilt tilt in radians
00537  * @param time_sec time when to reach the desired pan/tilt values
00538  */
00539 void
00540 PanTiltRX28Thread::WorkerThread::goto_pantilt_timed(float pan, float tilt, float time_sec)
00541 {
00542   __target_pan   = pan;
00543   __target_tilt  = tilt;
00544   __move_pending = true;
00545 
00546   float cpan=0, ctilt=0;
00547   get_pantilt(cpan, ctilt);
00548 
00549   float pan_diff  = fabs(pan - cpan);
00550   float tilt_diff = fabs(tilt - ctilt);
00551 
00552   float req_pan_vel  = pan_diff  / time_sec;
00553   float req_tilt_vel = tilt_diff / time_sec;
00554 
00555   //__logger->log_debug(name(), "Current: %f/%f Des: %f/%f  Time: %f  Diff: %f/%f  ReqVel: %f/%f",
00556   //                  cpan, ctilt, pan, tilt, time_sec, pan_diff, tilt_diff, req_pan_vel, req_tilt_vel);
00557 
00558 
00559   if (req_pan_vel > __max_pan_speed) {
00560     __logger->log_warn(name(), "Requested move to (%f, %f) in %f sec requires a "
00561                        "pan speed of %f rad/s, which is greater than the maximum "
00562                        "of %f rad/s, reducing to max", pan, tilt, time_sec,
00563                        req_pan_vel, __max_pan_speed);
00564     req_pan_vel = __max_pan_speed;
00565   }
00566 
00567   if (req_tilt_vel > __max_tilt_speed) {
00568     __logger->log_warn(name(), "Requested move to (%f, %f) in %f sec requires a "
00569                        "tilt speed of %f rad/s, which is greater than the maximum of "
00570                        "%f rad/s, reducing to max", pan, tilt, time_sec,
00571                        req_tilt_vel, __max_tilt_speed);
00572     req_tilt_vel = __max_tilt_speed;
00573   }
00574 
00575   set_velocities(req_pan_vel, req_tilt_vel);
00576 
00577   wakeup();
00578 }
00579 
00580 
00581 /** Set desired velocities.
00582  * @param pan_vel pan velocity
00583  * @param tilt_vel tilt velocity
00584  */
00585 void
00586 PanTiltRX28Thread::WorkerThread::set_velocities(float pan_vel, float tilt_vel)
00587 {
00588   ScopedRWLock lock(__value_rwlock);
00589   float pan_tmp  = roundf((pan_vel  / __max_pan_speed)  * RobotisRX28::MAX_SPEED);
00590   float tilt_tmp = roundf((tilt_vel / __max_tilt_speed) * RobotisRX28::MAX_SPEED);
00591 
00592   //__logger->log_debug(name(), "old speed: %u/%u new speed: %f/%f", __pan_vel,
00593   //                  __tilt_vel, pan_tmp, tilt_tmp);
00594 
00595   if ((pan_tmp >= 0) && (pan_tmp <= RobotisRX28::MAX_SPEED)) {
00596     __pan_vel = (unsigned int)pan_tmp;
00597     __velo_pending = true;
00598   } else {
00599     __logger->log_warn(name(), "Calculated pan value out of bounds, min: 0  max: %u  des: %u",
00600                        RobotisRX28::MAX_SPEED, (unsigned int)pan_tmp);
00601   }
00602 
00603   if ((tilt_tmp >= 0) && (tilt_tmp <= RobotisRX28::MAX_SPEED)) {
00604     __tilt_vel = (unsigned int)tilt_tmp;
00605     __velo_pending = true;
00606   } else {
00607     __logger->log_warn(name(), "Calculated tilt value out of bounds, min: 0  max: %u  des: %u",
00608                        RobotisRX28::MAX_SPEED, (unsigned int)tilt_tmp);
00609   }
00610 }
00611 
00612 
00613 /** Get current velocities.
00614  * @param pan_vel upon return contains current pan velocity
00615  * @param tilt_vel upon return contains current tilt velocity
00616  */
00617 void
00618 PanTiltRX28Thread::WorkerThread::get_velocities(float &pan_vel, float &tilt_vel)
00619 {
00620   unsigned int pan_velticks  = __rx28->get_goal_speed(__pan_servo_id);
00621   unsigned int tilt_velticks = __rx28->get_goal_speed(__tilt_servo_id);
00622 
00623   pan_velticks  = (unsigned int)(((float)pan_velticks  / (float)RobotisRX28::MAX_SPEED) * __max_pan_speed);
00624   tilt_velticks = (unsigned int)(((float)tilt_velticks / (float)RobotisRX28::MAX_SPEED) * __max_tilt_speed);
00625 }
00626 
00627 
00628 /** Set desired velocities.
00629  * @param pan_margin pan margin
00630  * @param tilt_margin tilt margin
00631  */
00632 void
00633 PanTiltRX28Thread::WorkerThread::set_margins(float pan_margin, float tilt_margin)
00634 {
00635   if (pan_margin  > 0.0)  __pan_margin  = pan_margin;
00636   if (tilt_margin > 0.0)  __tilt_margin = tilt_margin;
00637   //__logger->log_warn(name(), "Margins set to %f, %f", __pan_margin, __tilt_margin);
00638 }
00639 
00640 
00641 /** Get pan/tilt value.
00642  * @param pan upon return contains the current pan value
00643  * @param tilt upon return contains the current tilt value
00644  */
00645 void
00646 PanTiltRX28Thread::WorkerThread::get_pantilt(float &pan, float &tilt)
00647 {
00648   ScopedRWLock lock(__rx28_rwlock, ScopedRWLock::LOCK_READ);
00649 
00650   int pan_ticks  = ((int)__rx28->get_position(__pan_servo_id)  - (int)RobotisRX28::CENTER_POSITION);
00651   int tilt_ticks = ((int)__rx28->get_position(__tilt_servo_id) - (int)RobotisRX28::CENTER_POSITION);
00652 
00653   pan  = pan_ticks *  RobotisRX28::RAD_PER_POS_TICK + __pan_offset;
00654   tilt = tilt_ticks * RobotisRX28::RAD_PER_POS_TICK + __tilt_offset;
00655 }
00656 
00657 
00658 /** Get pan/tilt value with time.
00659  * @param pan upon return contains the current pan value
00660  * @param tilt upon return contains the current tilt value
00661  * @param time upon return contains the time the pan and tilt values were read
00662  */
00663 void
00664 PanTiltRX28Thread::WorkerThread::get_pantilt(float &pan, float &tilt,
00665                                              fawkes::Time &time)
00666 {
00667   get_pantilt(pan, tilt);
00668   time = __pantilt_time;
00669 }
00670 
00671 
00672 /** Check if motion is final.
00673  * @return true if motion is final, false otherwise
00674  */
00675 bool
00676 PanTiltRX28Thread::WorkerThread::is_final()
00677 {
00678   float pan, tilt;
00679   get_pantilt(pan, tilt);
00680 
00681   /*
00682   __logger->log_debug(name(), "P: %f  T: %f  TP: %f  TT: %f  PM: %f  TM: %f  PMov: %i  TMov: %i  Final: %s",
00683                       pan, tilt, __target_pan, __target_tilt, __pan_margin, __tilt_margin,
00684                       __rx28->is_moving(__pan_servo_id), __rx28->is_moving(__tilt_servo_id),
00685                       (( (fabs(pan  - __target_pan)  <= __pan_margin) &&
00686                          (fabs(tilt - __target_tilt) <= __tilt_margin) ) ||
00687                        (! __rx28->is_moving(__pan_servo_id) &&
00688                         ! __rx28->is_moving(__tilt_servo_id))) ? "YES" : "NO");
00689   */
00690 
00691   ScopedRWLock lock(__rx28_rwlock, ScopedRWLock::LOCK_READ);
00692 
00693   return  ( (fabs(pan  - __target_pan)  <= __pan_margin) &&
00694             (fabs(tilt - __target_tilt) <= __tilt_margin) ) ||
00695           (! __rx28->is_moving(__pan_servo_id) &&
00696            ! __rx28->is_moving(__tilt_servo_id));
00697 }
00698 
00699 
00700 /** Check if PTU is enabled.
00701  * @return true if torque is enabled for both servos, false otherwise
00702  */
00703 bool
00704 PanTiltRX28Thread::WorkerThread::is_enabled()
00705 {
00706   return (__rx28->is_torque_enabled(__pan_servo_id) &&
00707           __rx28->is_torque_enabled(__tilt_servo_id));
00708 }
00709 
00710 
00711 /** Check is fresh sensor data is available.
00712  * Note that this method will return true at once per sensor update cycle.
00713  * @return true if fresh data is available, false otherwise
00714  */
00715 bool
00716 PanTiltRX28Thread::WorkerThread::has_fresh_data()
00717 {
00718   MutexLocker lock(__fresh_data_mutex);
00719 
00720   bool rv = __fresh_data;
00721   __fresh_data = false;
00722   return rv;
00723 }
00724 
00725 
00726 void
00727 PanTiltRX28Thread::WorkerThread::loop()
00728 {
00729   if (__enable) {
00730     __value_rwlock->lock_for_write();
00731     __enable  = false;
00732     __value_rwlock->unlock();
00733     ScopedRWLock lock(__rx28_rwlock);
00734     __rx28->set_led_enabled(__tilt_servo_id, true);
00735     __rx28->set_torques_enabled(true, 2, __pan_servo_id, __tilt_servo_id);
00736   } else if (__disable) {
00737     __value_rwlock->lock_for_write();
00738     __disable = false;
00739     __value_rwlock->unlock();
00740     ScopedRWLock lock(__rx28_rwlock);
00741     if (__led_enable || __led_disable || __velo_pending || __move_pending) usleep(3000);
00742   }
00743 
00744   if (__led_enable) {
00745     __value_rwlock->lock_for_write();
00746     __led_enable = false;
00747     __value_rwlock->unlock();    
00748     ScopedRWLock lock(__rx28_rwlock);
00749     __rx28->set_led_enabled(__pan_servo_id, true);
00750     if (__velo_pending || __move_pending) usleep(3000);
00751   } else if (__led_disable) {
00752     __value_rwlock->lock_for_write();
00753     __led_disable = false;
00754     __value_rwlock->unlock();    
00755     ScopedRWLock lock(__rx28_rwlock);
00756     __rx28->set_led_enabled(__pan_servo_id, false);    
00757     if (__velo_pending || __move_pending) usleep(3000);
00758   }
00759 
00760   if (__velo_pending) {
00761     __value_rwlock->lock_for_write();
00762     __velo_pending = false;
00763     unsigned int pan_vel  = __pan_vel;
00764     unsigned int tilt_vel = __tilt_vel;
00765     __value_rwlock->unlock();
00766     ScopedRWLock lock(__rx28_rwlock);
00767     __rx28->set_goal_speeds(2, __pan_servo_id, pan_vel, __tilt_servo_id, tilt_vel);
00768     if (__move_pending) usleep(3000);
00769   }
00770 
00771   if (__move_pending) {
00772     __value_rwlock->lock_for_write();
00773     __move_pending    = false;
00774     float target_pan  = __target_pan;
00775     float target_tilt = __target_tilt;
00776     __value_rwlock->unlock();
00777     exec_goto_pantilt(target_pan, target_tilt);
00778   }
00779 
00780   try {
00781     ScopedRWLock lock(__rx28_rwlock, ScopedRWLock::LOCK_READ);
00782     __rx28->read_table_values(__pan_servo_id);
00783     __rx28->read_table_values(__tilt_servo_id);
00784     {
00785       MutexLocker lock_fresh_data(__fresh_data_mutex);
00786       __fresh_data = true;
00787       __pantilt_time.stamp();
00788     }
00789   } catch (Exception &e) {
00790     // usually just a timeout, too noisy
00791     //__logger->log_warn(name(), "Error while reading table values from servos, exception follows");
00792     //__logger->log_warn(name(), e);
00793   }
00794 
00795   //if (! is_final() ||
00796   //    ! __rx28->is_torque_enabled(__pan_servo_id) ||
00797   //    ! __rx28->is_torque_enabled(__tilt_servo_id)) {
00798     // while moving, and while the motor is off, wake us up to get new servo
00799     // position data
00800     //wakeup();
00801     //}
00802 
00803   __update_waitcond->wake_all();
00804 
00805   // Wakeup ourselves for faster updates
00806   wakeup();
00807 }
00808 
00809 
00810 /** Execute pan/tilt motion.
00811  * @param pan_rad pan in rad to move to
00812  * @param tilt_rad tilt in rad to move to
00813  */
00814 void
00815 PanTiltRX28Thread::WorkerThread::exec_goto_pantilt(float pan_rad, float tilt_rad)
00816 {
00817   if ( (pan_rad < __pan_min) || (pan_rad > __pan_max) ) {
00818     __logger->log_warn(name(), "Pan value out of bounds, min: %f  max: %f  des: %f",
00819                        __pan_min, __pan_max, pan_rad);
00820     return;
00821   }
00822   if ( (tilt_rad < __tilt_min) || (tilt_rad > __tilt_max) ) {
00823     __logger->log_warn(name(), "Tilt value out of bounds, min: %f  max: %f  des: %f",
00824                        __tilt_min, __tilt_max, tilt_rad);
00825     return;
00826   }
00827 
00828   unsigned int pan_min = 0, pan_max = 0, tilt_min = 0, tilt_max = 0;
00829 
00830   __rx28->get_angle_limits(__pan_servo_id, pan_min, pan_max);
00831   __rx28->get_angle_limits(__tilt_servo_id, tilt_min, tilt_max);
00832 
00833 
00834   int pan_pos  = (int)roundf(RobotisRX28::POS_TICKS_PER_RAD * (pan_rad - __pan_offset))
00835                  + RobotisRX28::CENTER_POSITION;
00836   int tilt_pos = (int)roundf(RobotisRX28::POS_TICKS_PER_RAD * (tilt_rad - __tilt_offset))
00837                  + RobotisRX28::CENTER_POSITION;
00838 
00839   if ( (pan_pos < 0) || ((unsigned int)pan_pos < pan_min) || ((unsigned int)pan_pos > pan_max) ) {
00840     __logger->log_warn(name(), "Pan position out of bounds, min: %u  max: %u  des: %i",
00841                        pan_min, pan_max, pan_pos);
00842     return;
00843   }
00844 
00845   if ( (tilt_pos < 0) || ((unsigned int)tilt_pos < tilt_min) || ((unsigned int)tilt_pos > tilt_max) ) {
00846     __logger->log_warn(name(), "Tilt position out of bounds, min: %u  max: %u  des: %i",
00847                        tilt_min, tilt_max, tilt_pos);
00848     return;
00849   }
00850 
00851   ScopedRWLock lock(__rx28_rwlock);
00852   __rx28->goto_positions(2, __pan_servo_id, pan_pos, __tilt_servo_id, tilt_pos);
00853 }
00854 
00855 
00856 /** Wait for fresh data to be received.
00857  * Blocks the calling thread.
00858  */
00859 void
00860 PanTiltRX28Thread::WorkerThread::wait_for_fresh_data()
00861 {
00862   __update_waitcond->wait();
00863 }