00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
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
00035
00036
00037
00038
00039
00040
00041
00042
00043
00044
00045
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
00065
00066
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
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);
00086
00087
00088
00089
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
00118 __ptu = NULL;
00119 }
00120
00121
00122
00123
00124
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
00174
00175
00176
00177
00178
00179
00180
00181
00182
00183
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;
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
00219
00220
00221
00222
00223
00224
00225
00226
00227
00228
00229
00230
00231
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
00256 PanTiltDirectedPerceptionThread::WorkerThread::~WorkerThread()
00257 {
00258 delete __move_mutex;
00259 }
00260
00261
00262
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
00273
00274
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
00288
00289
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
00300 void
00301 PanTiltDirectedPerceptionThread::WorkerThread::reset()
00302 {
00303 __reset_pending = true;
00304 }
00305
00306
00307
00308
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
00320
00321
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
00358 wakeup();
00359 }
00360 }
00361
00362
00363
00364
00365
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 }