00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
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
00035
00036
00037
00038
00039
00040
00041
00042
00043
00044
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
00066
00067
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
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);
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
00128 __cam = NULL;
00129 }
00130
00131
00132
00133
00134
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
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;
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
00230
00231
00232
00233
00234
00235
00236
00237
00238
00239
00240
00241
00242
00243
00244
00245
00246
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
00281 PanTiltSonyEviD100PThread::WorkerThread::~WorkerThread()
00282 {
00283 delete __move_mutex;
00284 }
00285
00286
00287
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
00298
00299
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
00313
00314
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
00325
00326
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
00338
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
00349
00350
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
00365
00366 for (int i = 0; i < 20; ++i) {
00367 try {
00368 __cam->process();
00369 } catch (Exception &e) { }
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
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
00412 wakeup();
00413 }
00414 }
00415
00416
00417
00418
00419
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 }