dp_thread.cpp

00001 
00002 /***************************************************************************
00003  *  dp_thread.h - DirectedPerception pan/tilt unit act thread
00004  *
00005  *  Created: Sun Jun 21 17:31:50 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 "dp_thread.h"
00024 #include "dp_ptu.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 PanTiltDirectedPerceptionThread "dp_thread.h"
00035  * PanTilt act thread for PTUs from DirectedPerception employing the ASCII
00036  * protocol.
00037  * This thread integrates into the Fawkes main loop at the ACT_EXEC hook and
00038  * interacts via the Visca protocol with the controller of the Sony EviD100P.
00039  * @author Tim Niemueller
00040  */
00041 
00042 /** Constructor.
00043  * @param pantilt_cfg_prefix pantilt plugin configuration prefix
00044  * @param ptu_cfg_prefix configuration prefix specific for the PTU
00045  * @param ptu_name name of the PTU configuration
00046  */
00047 PanTiltDirectedPerceptionThread::PanTiltDirectedPerceptionThread(std::string &pantilt_cfg_prefix,
00048                                                                  std::string &ptu_cfg_prefix,
00049                                                                  std::string &ptu_name)
00050   : PanTiltActThread("PanTiltDirectedPerceptionThread"),
00051     BlackBoardInterfaceListener("PanTiltDirectedPerceptionThread")
00052 {
00053   set_name("PanTiltDirectedPerceptionThread(%s)", ptu_name.c_str());
00054 
00055   __pantilt_cfg_prefix = pantilt_cfg_prefix;
00056   __ptu_cfg_prefix     = ptu_cfg_prefix;
00057   __ptu_name           = ptu_name;
00058 }
00059 
00060 
00061 void
00062 PanTiltDirectedPerceptionThread::init()
00063 {
00064   // Note: due to the use of auto_ptr and RefPtr resources are automatically
00065   // freed on destruction, therefore no special handling is necessary in init()
00066   // itself!
00067 
00068   __cfg_device           = config->get_string((__ptu_cfg_prefix + "device").c_str());
00069   __cfg_read_timeout_ms  = config->get_uint((__ptu_cfg_prefix + "read_timeout_ms").c_str());
00070 
00071   __ptu = new DirectedPerceptionPTU(__cfg_device.c_str(), __cfg_read_timeout_ms);
00072 
00073   // If you have more than one interface: catch exception and close them!
00074   std::string bbid = "PanTilt " + __ptu_name;
00075   __pantilt_if = blackboard->open_for_writing<PanTiltInterface>(bbid.c_str());
00076 
00077   float min_pan=0, max_pan=0, min_tilt=0, max_tilt=0;
00078   __ptu->get_limits(min_pan, max_pan, min_tilt, max_tilt);
00079 
00080   __pantilt_if->set_calibrated(true);
00081   __pantilt_if->set_min_pan(min_pan);
00082   __pantilt_if->set_max_pan(max_pan);
00083   __pantilt_if->set_min_tilt(min_tilt);
00084   __pantilt_if->set_max_tilt(max_tilt);
00085   __pantilt_if->set_enabled(true); // Cannot be turned off
00086   //__pantilt_if->set_max_pan_velocity(0);
00087   //__pantilt_if->set_max_tilt_velocity(0);
00088   //__pantilt_if->set_pan_velocity(0);
00089   //__pantilt_if->set_tilt_velocity(0);
00090   __pantilt_if->write();
00091 
00092   __wt = new WorkerThread(__ptu_name, logger, __ptu);
00093   __wt->start();
00094 
00095   bbil_add_message_interface(__pantilt_if);
00096   blackboard->register_listener(this, BlackBoard::BBIL_FLAG_MESSAGES);
00097 
00098 #ifdef USE_TIMETRACKER
00099   __tt.reset(new TimeTracker());
00100   __tt_count = 0;
00101   __ttc_read_sensor = __tt->add_class("Read Sensor");
00102 #endif  
00103 
00104 }
00105 
00106 
00107 void
00108 PanTiltDirectedPerceptionThread::finalize()
00109 {
00110   blackboard->unregister_listener(this);
00111   blackboard->close(__pantilt_if);
00112 
00113   __wt->cancel();
00114   __wt->join();
00115   delete __wt;
00116 
00117   // Setting to NULL deletes instance (RefPtr)
00118   __ptu = NULL;
00119 }
00120 
00121 
00122 /** Update sensor values as necessary.
00123  * To be called only from PanTiltSensorThread. Writes the current pan/tilt
00124  * data into the interface.
00125  */
00126 void
00127 PanTiltDirectedPerceptionThread::update_sensor_values()
00128 {
00129   if (__wt->has_fresh_data()) {
00130     float pan = 0, tilt = 0;
00131     __wt->get_pantilt(pan, tilt);
00132     __pantilt_if->set_pan(pan);
00133     __pantilt_if->set_tilt(tilt);
00134     __pantilt_if->set_final(__wt->is_final());
00135     __pantilt_if->write();
00136   }
00137 }
00138 
00139 
00140 void
00141 PanTiltDirectedPerceptionThread::loop()
00142 {
00143   __pantilt_if->set_final(__wt->is_final());
00144 
00145   while (! __pantilt_if->msgq_empty() ) {
00146     if (__pantilt_if->msgq_first_is<PanTiltInterface::CalibrateMessage>()) {
00147       __wt->reset();
00148 
00149     } else if (__pantilt_if->msgq_first_is<PanTiltInterface::GotoMessage>()) {
00150       PanTiltInterface::GotoMessage *msg = __pantilt_if->msgq_first(msg);
00151 
00152       __wt->goto_pantilt(msg->pan(), msg->tilt());
00153       __pantilt_if->set_msgid(msg->id());
00154       __pantilt_if->set_final(false);
00155 
00156     } else if (__pantilt_if->msgq_first_is<PanTiltInterface::ParkMessage>()) {
00157       PanTiltInterface::ParkMessage *msg = __pantilt_if->msgq_first(msg);
00158 
00159       __wt->goto_pantilt(0, 0);
00160       __pantilt_if->set_msgid(msg->id());
00161       __pantilt_if->set_final(false);
00162 
00163     } else if (__pantilt_if->msgq_first_is<PanTiltInterface::SetEnabledMessage>()) {
00164       PanTiltInterface::SetEnabledMessage *msg = __pantilt_if->msgq_first(msg);
00165 
00166       logger->log_warn(name(), "SetEnabledMessage ignored for Sony EviD100P");
00167 
00168     } else if (__pantilt_if->msgq_first_is<PanTiltInterface::SetVelocityMessage>()) {
00169       PanTiltInterface::SetVelocityMessage *msg = __pantilt_if->msgq_first(msg);
00170 
00171       logger->log_warn(name(), "SetVelocityMessage ignored for Sony EviD100P");
00172 
00173       /* ignored for now
00174       if (msg->pan_velocity() > __pantilt_if->max_pan_velocity()) {
00175         logger->log_warn(name(), "Desired pan velocity %f too high, max is %f",
00176                          msg->pan_velocity(), __pantilt_if->max_pan_velocity());
00177       } else if (msg->tilt_velocity() > __pantilt_if->max_tilt_velocity()) {
00178         logger->log_warn(name(), "Desired tilt velocity %f too high, max is %f",
00179                          msg->tilt_velocity(), __pantilt_if->max_tilt_velocity());
00180       } else {
00181         __wt->set_velocities(msg->pan_velocity(), msg->tilt_velocity());
00182         __pantilt_if->set_pan_velocity(msg->pan_velocity());
00183         __pantilt_if->set_tilt_velocity(msg->tilt_velocity());
00184       }
00185       */
00186 
00187     } else {
00188       logger->log_warn(name(), "Unknown message received");
00189     }
00190 
00191     __pantilt_if->msgq_pop();
00192   }
00193 
00194   __pantilt_if->write();
00195 
00196 }
00197 
00198 
00199 bool
00200 PanTiltDirectedPerceptionThread::bb_interface_message_received(Interface *interface,
00201                                                  Message *message) throw()
00202 {
00203   if (message->is_of_type<PanTiltInterface::StopMessage>()) {
00204     __wt->stop_motion();
00205     return false; // do not enqueue StopMessage
00206   } else if (message->is_of_type<PanTiltInterface::FlushMessage>()) {
00207     __wt->stop_motion();
00208     logger->log_info(name(), "Flushing message queue");
00209     __pantilt_if->msgq_flush();
00210     return false;
00211   } else {
00212     logger->log_info(name(), "Received message of type %s, enqueueing", message->type());
00213     return true;
00214   }
00215 }
00216 
00217 
00218 /** @class PanTiltDirectedPerceptionThread::WorkerThread "sony/evid100p_thread.h"
00219  * Worker thread for the PanTiltDirectedPerceptionThread.
00220  * This continuous thread issues commands to the camera. In each loop it
00221  * will first execute pending operations, and then update the sensor data (lengthy
00222  * operation). Sensor data will only be updated while either a servo in the chain
00223  * is still moving or torque is disabled (so the motor can be move manually).
00224  * @author Tim Niemueller
00225  */
00226 
00227 
00228 /** Constructor.
00229  * @param ptu_name name of the pan/tilt unit
00230  * @param logger logger
00231  * @param ptu ptu controller
00232  */
00233 PanTiltDirectedPerceptionThread::WorkerThread::WorkerThread(std::string ptu_name,
00234                                                       fawkes::Logger *logger,
00235                                                       fawkes::RefPtr<DirectedPerceptionPTU> ptu)
00236   : Thread("", Thread::OPMODE_WAITFORWAKEUP)
00237 {
00238   set_name("SonyDirectedPerceptionWorkerThread(%s)", ptu_name.c_str());
00239   set_coalesce_wakeups(true);
00240 
00241   __logger           = logger;
00242 
00243   __move_mutex       = new Mutex();
00244 
00245   __ptu              = ptu;
00246   __move_pending     = false;
00247   __reset_pending    = false;
00248   __target_pan       = 0;
00249   __target_tilt      = 0;
00250 
00251   __ptu->get_limits(__pan_min, __pan_max, __tilt_min, __tilt_max);
00252 }
00253 
00254 
00255 /** Destructor. */
00256 PanTiltDirectedPerceptionThread::WorkerThread::~WorkerThread()
00257 {
00258   delete __move_mutex;
00259 }
00260 
00261 
00262 /** Stop currently running motion. */
00263 void
00264 PanTiltDirectedPerceptionThread::WorkerThread::stop_motion()
00265 {
00266   float pan = 0, tilt = 0;
00267   get_pantilt(pan, tilt);
00268   goto_pantilt(pan, tilt);
00269 }
00270 
00271 
00272 /** Goto desired pan/tilt values.
00273  * @param pan pan in radians
00274  * @param tilt tilt in radians
00275  */
00276 void
00277 PanTiltDirectedPerceptionThread::WorkerThread::goto_pantilt(float pan, float tilt)
00278 {
00279   MutexLocker lock(__move_mutex);
00280   __target_pan   = pan;
00281   __target_tilt  = tilt;
00282   __move_pending = true;
00283   wakeup();
00284 }
00285 
00286 
00287 /** Get pan/tilt value.
00288  * @param pan upon return contains the current pan value
00289  * @param tilt upon return contains the current tilt value
00290  */
00291 void
00292 PanTiltDirectedPerceptionThread::WorkerThread::get_pantilt(float &pan, float &tilt)
00293 {
00294   pan  = __cur_pan;
00295   tilt = __cur_tilt;
00296 }
00297 
00298 
00299 /** Trigger a reset of the PTU. */
00300 void
00301 PanTiltDirectedPerceptionThread::WorkerThread::reset()
00302 {
00303   __reset_pending = true;
00304 }
00305 
00306 
00307 /** Check if motion is final.
00308  * @return true if motion is final, false otherwise
00309  */
00310 bool
00311 PanTiltDirectedPerceptionThread::WorkerThread::is_final()
00312 {
00313   MutexLocker lock(__move_mutex);
00314   return ( (fabs(__cur_pan  - __target_pan)  < 0.01) &&
00315            (fabs(__cur_tilt - __target_tilt) < 0.01));
00316 }
00317 
00318 
00319 /** Check is fresh sensor data is available.
00320  * Note that this method will return true at once per sensor update cycle.
00321  * @return true if fresh data is available, false otherwise
00322  */
00323 bool
00324 PanTiltDirectedPerceptionThread::WorkerThread::has_fresh_data()
00325 {
00326   bool rv = __fresh_data;
00327   __fresh_data = false;
00328   return rv;
00329 }
00330 
00331 
00332 void
00333 PanTiltDirectedPerceptionThread::WorkerThread::loop()
00334 {
00335   if (__move_pending) {
00336     __move_mutex->lock();
00337     exec_goto_pantilt(__target_pan, __target_tilt);
00338     __move_mutex->unlock();
00339   }
00340 
00341   if (__reset_pending) {
00342     __move_mutex->lock();
00343     __reset_pending = false;
00344     __move_mutex->unlock();
00345     __ptu->reset();
00346   }
00347 
00348   try {
00349     __ptu->get_pan_tilt_rad(__cur_pan, __cur_tilt);
00350     __fresh_data = true;
00351   } catch (Exception &e) {
00352     __logger->log_warn(name(), "Failed to get new pan/tilt data, exception follows");
00353     __logger->log_warn(name(), e);
00354   }
00355 
00356   if (! is_final()) {
00357     // while moving wake us up to get new servo position data
00358     wakeup();
00359   }
00360 }
00361 
00362 
00363 /** Execute pan/tilt motion.
00364  * @param pan_rad pan in rad to move to
00365  * @param tilt_rad tilt in rad to move to
00366  */
00367 void
00368 PanTiltDirectedPerceptionThread::WorkerThread::exec_goto_pantilt(float pan_rad, float tilt_rad)
00369 {
00370   if ( (pan_rad < __pan_min) || (pan_rad > __pan_max) ) {
00371     __logger->log_warn(name(), "Pan value out of bounds, min: %f  max: %f  des: %f",
00372                        __pan_min, __pan_max, pan_rad);
00373     return;
00374   }
00375   if ( (tilt_rad < __tilt_min) || (tilt_rad > __tilt_max) ) {
00376     __logger->log_warn(name(), "Tilt value out of bounds, min: %f  max: %f  des: %f",
00377                        __tilt_min, __tilt_max, tilt_rad);
00378     return;
00379   }
00380 
00381   __ptu->set_pan_tilt_rad(pan_rad, tilt_rad);
00382   __move_pending = false;
00383 }

Generated on Tue Feb 22 13:31:29 2011 for Fawkes API by  doxygen 1.4.7