Fawkes API  Fawkes Development Version
evid100p_thread.cpp
00001 
00002 /***************************************************************************
00003  *  evid100p_thread.h - Sony EviD100P pan/tilt unit act thread
00004  *
00005  *  Created: Sun Jun 21 12:38:34 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 "evid100p_thread.h"
00024 #include "evid100p.h"
00025 
00026 #include <core/threading/mutex_locker.h>
00027 #include <interfaces/PanTiltInterface.h>
00028 
00029 #include <cstdarg>
00030 #include <cmath>
00031 
00032 using namespace fawkes;
00033 
00034 /** @class PanTiltSonyEviD100PThread "evid100p_thread.h"
00035  * PanTilt act thread for the PTU part of the Sony EviD100P camera.
00036  * This thread integrates into the Fawkes main loop at the ACT_EXEC hook and
00037  * interacts via the Visca protocol with the controller of the Sony EviD100P.
00038  * @author Tim Niemueller
00039  */
00040 
00041 /** Constructor.
00042  * @param pantilt_cfg_prefix pantilt plugin configuration prefix
00043  * @param ptu_cfg_prefix configuration prefix specific for the PTU
00044  * @param ptu_name name of the PTU configuration
00045  */
00046 PanTiltSonyEviD100PThread::PanTiltSonyEviD100PThread(std::string &pantilt_cfg_prefix,
00047                                                      std::string &ptu_cfg_prefix,
00048                                                      std::string &ptu_name)
00049   : PanTiltActThread("PanTiltSonyEviD100PThread"),
00050     BlackBoardInterfaceListener("PanTiltSonyEviD100PThread")
00051 {
00052   set_name("PanTiltSonyEviD100PThread(%s)", ptu_name.c_str());
00053 
00054   __pantilt_cfg_prefix = pantilt_cfg_prefix;
00055   __ptu_cfg_prefix     = ptu_cfg_prefix;
00056   __ptu_name           = ptu_name;
00057 
00058   __cam = NULL;
00059 }
00060 
00061 
00062 void
00063 PanTiltSonyEviD100PThread::init()
00064 {
00065   // Note: due to the use of auto_ptr and RefPtr resources are automatically
00066   // freed on destruction, therefore no special handling is necessary in init()
00067   // itself!
00068 
00069   __cfg_device           = config->get_string((__ptu_cfg_prefix + "device").c_str());
00070   __cfg_read_timeout_ms  = config->get_uint((__ptu_cfg_prefix + "read_timeout_ms").c_str());
00071 
00072   try {
00073     __cam = new SonyEviD100PVisca(__cfg_device.c_str(), __cfg_read_timeout_ms);
00074   } catch (Exception &e) {
00075     e.print_trace();
00076     e.print_backtrace();
00077     throw;
00078   }
00079 
00080   // If you have more than one interface: catch exception and close them!
00081   std::string bbid = "PanTilt " + __ptu_name;
00082   __pantilt_if = blackboard->open_for_writing<PanTiltInterface>(bbid.c_str());
00083   __pantilt_if->set_calibrated(true);
00084   __pantilt_if->set_min_pan(SonyEviD100PVisca::MIN_PAN_RAD);
00085   __pantilt_if->set_max_pan(SonyEviD100PVisca::MAX_PAN_RAD);
00086   __pantilt_if->set_min_tilt(SonyEviD100PVisca::MIN_TILT_RAD);
00087   __pantilt_if->set_max_tilt(SonyEviD100PVisca::MAX_TILT_RAD);
00088   __pantilt_if->set_enabled(true); // Cannot be turned off
00089 
00090   float pan_smin, pan_smax, tilt_smin, tilt_smax;
00091   __cam->get_speed_limits(pan_smin, pan_smax, tilt_smin, tilt_smax);
00092   __pantilt_if->set_max_pan_velocity(pan_smax);
00093   __pantilt_if->set_max_tilt_velocity(tilt_smax);
00094   __pantilt_if->set_pan_velocity(pan_smax);
00095   __pantilt_if->set_tilt_velocity(tilt_smax);
00096   __pantilt_if->write();
00097 
00098   __wt = new WorkerThread(__ptu_name, logger, __cam,
00099                           SonyEviD100PVisca::MIN_PAN_RAD, SonyEviD100PVisca::MAX_PAN_RAD,
00100                           SonyEviD100PVisca::MIN_TILT_RAD, SonyEviD100PVisca::MAX_TILT_RAD);
00101   __wt->start();
00102 
00103   __wt->set_velocities(pan_smax, tilt_smax);
00104 
00105   bbil_add_message_interface(__pantilt_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 PanTiltSonyEviD100PThread::finalize()
00119 {
00120   blackboard->unregister_listener(this);
00121   blackboard->close(__pantilt_if);
00122 
00123   __wt->cancel();
00124   __wt->join();
00125   delete __wt;
00126 
00127   // Setting to NULL deletes instance (RefPtr)
00128   __cam = NULL;
00129 }
00130 
00131 
00132 /** Update sensor values as necessary.
00133  * To be called only from PanTiltSensorThread. Writes the current pan/tilt
00134  * data into the interface.
00135  */
00136 void
00137 PanTiltSonyEviD100PThread::update_sensor_values()
00138 {
00139   if (__wt->has_fresh_data()) {
00140     float pan = 0, tilt = 0;
00141     __wt->get_pantilt(pan, tilt);
00142     __pantilt_if->set_pan(pan);
00143     __pantilt_if->set_tilt(tilt);
00144     __pantilt_if->set_final(__wt->is_final());
00145     __pantilt_if->write();
00146   }
00147 }
00148 
00149 
00150 void
00151 PanTiltSonyEviD100PThread::loop()
00152 {
00153   __pantilt_if->set_final(__wt->is_final());
00154 
00155   while (! __pantilt_if->msgq_empty() ) {
00156     if (__pantilt_if->msgq_first_is<PanTiltInterface::CalibrateMessage>()) {
00157       // ignored
00158 
00159     } else if (__pantilt_if->msgq_first_is<PanTiltInterface::GotoMessage>()) {
00160       PanTiltInterface::GotoMessage *msg = __pantilt_if->msgq_first(msg);
00161 
00162       __wt->goto_pantilt(msg->pan(), msg->tilt());
00163       __pantilt_if->set_msgid(msg->id());
00164       __pantilt_if->set_final(false);
00165 
00166     } else if (__pantilt_if->msgq_first_is<PanTiltInterface::ParkMessage>()) {
00167       PanTiltInterface::ParkMessage *msg = __pantilt_if->msgq_first(msg);
00168 
00169       __wt->goto_pantilt(0, 0);
00170       __pantilt_if->set_msgid(msg->id());
00171       __pantilt_if->set_final(false);
00172 
00173     } else if (__pantilt_if->msgq_first_is<PanTiltInterface::SetEnabledMessage>()) {
00174       PanTiltInterface::SetEnabledMessage *msg = __pantilt_if->msgq_first(msg);
00175 
00176       logger->log_warn(name(), "SetEnabledMessage ignored for Sony EviD100P");
00177 
00178     } else if (__pantilt_if->msgq_first_is<PanTiltInterface::SetVelocityMessage>()) {
00179       PanTiltInterface::SetVelocityMessage *msg = __pantilt_if->msgq_first(msg);
00180 
00181       logger->log_warn(name(), "SetVelocityMessage ignored for Sony EviD100P");
00182 
00183       if ((msg->pan_velocity() < 0) || (msg->tilt_velocity() < 0) ) {
00184         logger->log_warn(name(), "Ignoring pan/tilt velocities %f/%f, at least one "
00185                          " is negative", msg->pan_velocity(), msg->tilt_velocity());
00186       } else if (msg->pan_velocity() > __pantilt_if->max_pan_velocity()) {
00187         logger->log_warn(name(), "Desired pan velocity %f too high, max is %f",
00188                          msg->pan_velocity(), __pantilt_if->max_pan_velocity());
00189       } else if (msg->tilt_velocity() > __pantilt_if->max_tilt_velocity()) {
00190         logger->log_warn(name(), "Desired tilt velocity %f too high, max is %f",
00191                          msg->tilt_velocity(), __pantilt_if->max_tilt_velocity());
00192       } else {
00193         __wt->set_velocities(msg->pan_velocity(), msg->tilt_velocity());
00194         __pantilt_if->set_pan_velocity(msg->pan_velocity());
00195         __pantilt_if->set_tilt_velocity(msg->tilt_velocity());
00196       }
00197 
00198     } else {
00199       logger->log_warn(name(), "Unknown message received");
00200     }
00201 
00202     __pantilt_if->msgq_pop();
00203   }
00204 
00205   __pantilt_if->write();
00206 
00207 }
00208 
00209 
00210 bool
00211 PanTiltSonyEviD100PThread::bb_interface_message_received(Interface *interface,
00212                                                  Message *message) throw()
00213 {
00214   if (message->is_of_type<PanTiltInterface::StopMessage>()) {
00215     __wt->stop_motion();
00216     return false; // do not enqueue StopMessage
00217   } else if (message->is_of_type<PanTiltInterface::FlushMessage>()) {
00218     __wt->stop_motion();
00219     logger->log_info(name(), "Flushing message queue");
00220     __pantilt_if->msgq_flush();
00221     return false;
00222   } else {
00223     logger->log_info(name(), "Received message of type %s, enqueueing", message->type());
00224     return true;
00225   }
00226 }
00227 
00228 
00229 /** @class PanTiltSonyEviD100PThread::WorkerThread "sony/evid100p_thread.h"
00230  * Worker thread for the PanTiltSonyEviD100PThread.
00231  * This continuous thread issues commands to the camera. In each loop it
00232  * will first execute pending operations, and then update the sensor data (lengthy
00233  * operation). Sensor data will only be updated while either a servo in the chain
00234  * is still moving or torque is disabled (so the motor can be move manually).
00235  * @author Tim Niemueller
00236  */
00237 
00238 
00239 /** Constructor.
00240  * @param ptu_name name of the pan/tilt unit
00241  * @param logger logger
00242  * @param cam Visca controller object
00243  * @param pan_min minimum pan in rad
00244  * @param pan_min maximum pan in rad
00245  * @param tilt_min minimum tilt in rad
00246  * @param tilt_max maximum tilt in rad
00247  */
00248 PanTiltSonyEviD100PThread::WorkerThread::WorkerThread(std::string ptu_name,
00249                                                       fawkes::Logger *logger,
00250                                                       fawkes::RefPtr<SonyEviD100PVisca> cam,
00251                                                       const float &pan_min,
00252                                                       const float &pan_max,
00253                                                       const float &tilt_min,
00254                                                       const float &tilt_max)
00255   : Thread("", Thread::OPMODE_WAITFORWAKEUP)
00256 {
00257   set_name("SonyEviD100PWorkerThread(%s)", ptu_name.c_str());
00258   set_coalesce_wakeups(true);
00259 
00260   __logger           = logger;
00261 
00262   __move_mutex       = new Mutex();
00263 
00264   __cam              = cam;
00265   __move_pending     = false;
00266   __target_pan       = 0;
00267   __target_tilt      = 0;
00268 
00269   __velo_pending     = false;
00270   __pan_vel          = 0;
00271   __tilt_vel         = 0;
00272 
00273   __pan_min          = pan_min;
00274   __pan_max          = pan_max;
00275   __tilt_min         = tilt_min;
00276   __tilt_max         = tilt_max;
00277 }
00278 
00279 
00280 /** Destructor. */
00281 PanTiltSonyEviD100PThread::WorkerThread::~WorkerThread()
00282 {
00283   delete __move_mutex;
00284 }
00285 
00286 
00287 /** Stop currently running motion. */
00288 void
00289 PanTiltSonyEviD100PThread::WorkerThread::stop_motion()
00290 {
00291   float pan = 0, tilt = 0;
00292   get_pantilt(pan, tilt);
00293   goto_pantilt(pan, tilt);
00294 }
00295 
00296 
00297 /** Goto desired pan/tilt values.
00298  * @param pan pan in radians
00299  * @param tilt tilt in radians
00300  */
00301 void
00302 PanTiltSonyEviD100PThread::WorkerThread::goto_pantilt(float pan, float tilt)
00303 {
00304   MutexLocker lock(__move_mutex);
00305   __target_pan   = pan;
00306   __target_tilt  = tilt;
00307   __move_pending = true;
00308   wakeup();
00309 }
00310 
00311 
00312 /** Get pan/tilt value.
00313  * @param pan upon return contains the current pan value
00314  * @param tilt upon return contains the current tilt value
00315  */
00316 void
00317 PanTiltSonyEviD100PThread::WorkerThread::get_pantilt(float &pan, float &tilt)
00318 {
00319   pan  = __cur_pan;
00320   tilt = __cur_tilt;
00321 }
00322 
00323 
00324 /** Set desired velocities.
00325  * @param pan_vel pan velocity
00326  * @param tilt_vel tilt velocity
00327  */
00328 void
00329 PanTiltSonyEviD100PThread::WorkerThread::set_velocities(float pan_vel, float tilt_vel)
00330 {
00331   __pan_vel      = pan_vel;
00332   __tilt_vel     = tilt_vel;
00333   __velo_pending = true;
00334 }
00335 
00336 
00337 /** Check if motion is final.
00338  * @return true if motion is final, false otherwise
00339  */
00340 bool
00341 PanTiltSonyEviD100PThread::WorkerThread::is_final()
00342 {
00343   MutexLocker lock(__move_mutex);
00344   return __cam->is_nonblocking_finished(SonyEviD100PVisca::NONBLOCKING_PANTILT);
00345 }
00346 
00347 
00348 /** Check is fresh sensor data is available.
00349  * Note that this method will return true at once per sensor update cycle.
00350  * @return true if fresh data is available, false otherwise
00351  */
00352 bool
00353 PanTiltSonyEviD100PThread::WorkerThread::has_fresh_data()
00354 {
00355   bool rv = __fresh_data;
00356   __fresh_data = false;
00357   return rv;
00358 }
00359 
00360 
00361 void
00362 PanTiltSonyEviD100PThread::WorkerThread::once()
00363 {
00364   // do some process cycles to process data returning back from set_address()
00365   // and clear calls
00366   for (int i = 0; i < 20; ++i) {
00367     try {
00368       __cam->process();
00369     } catch (Exception &e) { /* ignored */ }
00370   }
00371 }
00372 
00373 
00374 void
00375 PanTiltSonyEviD100PThread::WorkerThread::loop()
00376 {
00377   try {
00378     __cam->process();
00379   } catch (Exception &e) {
00380     __logger->log_warn(name(), "Data processing failed, exception follows");
00381     __logger->log_warn(name(), e);
00382   }
00383 
00384   if (__velo_pending) {
00385     try {
00386       __cam->set_speed_radsec(__pan_vel, __tilt_vel);
00387     } catch (Exception &e) {
00388       __logger->log_warn(name(), "Setting pan/tilt values failed, exception follows");
00389       __logger->log_warn(name(), e);
00390     }
00391     __velo_pending = false;
00392   }
00393 
00394   if (__move_pending) {
00395     __move_mutex->lock();
00396     __logger->log_debug(name(), "Executing goto to %f, %f", __target_pan, __target_tilt);
00397     exec_goto_pantilt(__target_pan, __target_tilt);
00398     __move_mutex->unlock();
00399   }
00400 
00401   //__cam->start_get_pan_tilt();
00402   try {
00403     __cam->get_pan_tilt_rad(__cur_pan, __cur_tilt);
00404     __fresh_data = true;
00405   } catch (Exception &e) {
00406     __logger->log_warn(name(), "Failed to get new pan/tilt data, exception follows");
00407     __logger->log_warn(name(), e);
00408   }
00409 
00410   if (! is_final() || ! __fresh_data) {
00411     // while moving or if data reception failed wake us up to get new servo data
00412     wakeup();
00413   }
00414 }
00415 
00416 
00417 /** Execute pan/tilt motion.
00418  * @param pan_rad pan in rad to move to
00419  * @param tilt_rad tilt in rad to move to
00420  */
00421 void
00422 PanTiltSonyEviD100PThread::WorkerThread::exec_goto_pantilt(float pan_rad, float tilt_rad)
00423 {
00424   if ( (pan_rad < __pan_min) || (pan_rad > __pan_max) ) {
00425     __logger->log_warn(name(), "Pan value out of bounds, min: %f  max: %f  des: %f",
00426                        __pan_min, __pan_max, pan_rad);
00427     return;
00428   }
00429   if ( (tilt_rad < __tilt_min) || (tilt_rad > __tilt_max) ) {
00430     __logger->log_warn(name(), "Tilt value out of bounds, min: %f  max: %f  des: %f",
00431                        __tilt_min, __tilt_max, tilt_rad);
00432     return;
00433   }
00434 
00435   try {
00436     __cam->set_pan_tilt_rad(pan_rad, tilt_rad);
00437   } catch (Exception &e) {
00438     __logger->log_warn(name(), "Failed to execute pan/tilt to %f, %f, exception "
00439                        "follows", pan_rad, tilt_rad);
00440     __logger->log_warn(name(), e);
00441   }
00442   __move_pending = false;
00443 }