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