Fawkes API
Fawkes Development Version
|
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 }