Fawkes API  Fawkes Development Version
evid100p_thread.cpp
1 
2 /***************************************************************************
3  * evid100p_thread.h - Sony EviD100P pan/tilt unit act thread
4  *
5  * Created: Sun Jun 21 12:38:34 2009
6  * Copyright 2006-2014 Tim Niemueller [www.niemueller.de]
7  ****************************************************************************/
8 
9 /* This program is free software; you can redistribute it and/or modify
10  * it under the terms of the GNU General Public License as published by
11  * the Free Software Foundation; either version 2 of the License, or
12  * (at your option) any later version.
13  *
14  * This program is distributed in the hope that it will be useful,
15  * but WITHOUT ANY WARRANTY; without even the implied warranty of
16  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
17  * GNU Library General Public License for more details.
18  *
19  * Read the full text in the LICENSE.GPL file in the doc directory.
20  */
21 
22 #include "evid100p_thread.h"
23 
24 #include "evid100p.h"
25 
26 #include <core/threading/mutex_locker.h>
27 #include <interfaces/JointInterface.h>
28 #include <interfaces/PanTiltInterface.h>
29 #include <interfaces/SwitchInterface.h>
30 
31 #include <cmath>
32 #include <cstdarg>
33 
34 using namespace fawkes;
35 
36 /** @class PanTiltSonyEviD100PThread "evid100p_thread.h"
37  * PanTilt act thread for the PTU part of the Sony EviD100P camera.
38  * This thread integrates into the Fawkes main loop at the ACT_EXEC hook and
39  * interacts via the Visca protocol with the controller of the Sony EviD100P.
40  * @author Tim Niemueller
41  */
42 
43 /** Constructor.
44  * @param pantilt_cfg_prefix pantilt plugin configuration prefix
45  * @param ptu_cfg_prefix configuration prefix specific for the PTU
46  * @param ptu_name name of the PTU configuration
47  */
49  std::string &ptu_cfg_prefix,
50  std::string &ptu_name)
51 : PanTiltActThread("PanTiltSonyEviD100PThread"),
52  BlackBoardInterfaceListener("PanTiltSonyEviD100PThread")
53 {
54  set_name("PanTiltSonyEviD100PThread(%s)", ptu_name.c_str());
55 
56  pantilt_cfg_prefix_ = pantilt_cfg_prefix;
57  ptu_cfg_prefix_ = ptu_cfg_prefix;
58  ptu_name_ = ptu_name;
59 
60  cam_ = NULL;
61 }
62 
63 void
65 {
66  // Note: due to the use of auto_ptr and RefPtr resources are automatically
67  // freed on destruction, therefore no special handling is necessary in init()
68  // itself!
69 
70  cfg_device_ = config->get_string((ptu_cfg_prefix_ + "device").c_str());
71  cfg_read_timeout_ms_ = config->get_uint((ptu_cfg_prefix_ + "read_timeout_ms").c_str());
72 
73  try {
74  cam_ = new SonyEviD100PVisca(cfg_device_.c_str(),
75  cfg_read_timeout_ms_,
76  /* blocking */ false);
77  } catch (Exception &e) {
78  e.print_trace();
79  e.print_backtrace();
80  throw;
81  }
82 
83  bool power_up = true;
84  try {
85  power_up = config->get_bool((ptu_cfg_prefix_ + "power-up").c_str());
86  } catch (Exception &e) {
87  } // ignore, use default
88  if (power_up)
89  cam_->set_power(true);
90 
91  float init_pan = 0.f;
92  float init_tilt = 0.f;
93  float init_pan_velocity = 0.f;
94  float init_tilt_velocity = 0.f;
95 
96  // If you have more than one interface: catch exception and close them!
97  std::string bbid = "PanTilt " + ptu_name_;
98  pantilt_if_ = blackboard->open_for_writing<PanTiltInterface>(bbid.c_str());
99  pantilt_if_->set_calibrated(true);
104  pantilt_if_->set_enabled(true); // Cannot be turned off
105 
106  float pan_smin, pan_smax, tilt_smin, tilt_smax;
107  cam_->get_speed_limits(pan_smin, pan_smax, tilt_smin, tilt_smax);
108  pantilt_if_->set_max_pan_velocity(pan_smax);
109  pantilt_if_->set_max_tilt_velocity(tilt_smax);
110  pantilt_if_->set_pan_velocity(init_pan_velocity);
111  pantilt_if_->set_tilt_velocity(init_tilt_velocity);
112  pantilt_if_->write();
113 
114  std::string panid = ptu_name_ + " pan";
115  panjoint_if_ = blackboard->open_for_writing<JointInterface>(panid.c_str());
116  panjoint_if_->set_position(init_pan);
117  panjoint_if_->set_velocity(init_pan_velocity);
118  panjoint_if_->write();
119 
120  std::string tiltid = ptu_name_ + " tilt";
121  tiltjoint_if_ = blackboard->open_for_writing<JointInterface>(tiltid.c_str());
122  tiltjoint_if_->set_position(init_tilt);
123  tiltjoint_if_->set_velocity(init_tilt_velocity);
124  tiltjoint_if_->write();
125 
126  camctrl_if_ = blackboard->open_for_writing<CameraControlInterface>(bbid.c_str());
127  camctrl_if_->set_effect(CameraControlInterface::EFF_NONE);
128  camctrl_if_->set_effect_supported(true);
129  camctrl_if_->set_zoom_supported(true);
130  camctrl_if_->set_zoom_min(0);
131  camctrl_if_->set_zoom_max(13);
132 
133  power_if_ = blackboard->open_for_writing<SwitchInterface>(bbid.c_str());
134  power_if_->set_enabled(cam_->is_powered());
135  power_if_->write();
136 
137  bool mirror = false;
138  try {
139  mirror = config->get_bool((ptu_cfg_prefix_ + "mirror").c_str());
140  } catch (Exception &e) {
141  } // ignore, use default
142  if (power_if_->is_enabled()) {
143  cam_->set_mirror(mirror);
144  }
145 
146  camctrl_if_->set_mirror(mirror);
147  camctrl_if_->set_mirror_supported(true);
148  camctrl_if_->write();
149 
150  wt_ = new WorkerThread(ptu_name_,
151  logger,
152  cam_,
157  wt_->start();
158  // Wakeup once to get values
159  wt_->wakeup();
160 
161  wt_->set_velocities(pan_smax, tilt_smax);
162 
163  bbil_add_message_interface(pantilt_if_);
165 
166 #ifdef USE_TIMETRACKER
167  tt_.reset(new TimeTracker());
168  tt_count_ = 0;
169  ttc_read_sensor_ = tt_->add_class("Read Sensor");
170 #endif
171 }
172 
173 void
175 {
177  blackboard->close(pantilt_if_);
178  blackboard->close(panjoint_if_);
179  blackboard->close(tiltjoint_if_);
180  blackboard->close(camctrl_if_);
181  blackboard->close(power_if_);
182 
183  wt_->cancel();
184  wt_->join();
185  delete wt_;
186 
187  bool power_down = true;
188  try {
189  power_down = config->get_bool((ptu_cfg_prefix_ + "power-down").c_str());
190  } catch (Exception &e) {
191  } // ignore, use default
192  if (power_down)
193  cam_->set_power(false);
194 
195  // Setting to NULL deletes instance (RefPtr)
196  cam_ = NULL;
197 }
198 
199 /** Update sensor values as necessary.
200  * To be called only from PanTiltSensorThread. Writes the current pan/tilt
201  * data into the interface.
202  */
203 void
205 {
206  if (wt_->has_fresh_data()) {
207  float pan = 0, tilt = 0;
208  wt_->get_pantilt(pan, tilt);
209  pantilt_if_->set_pan(pan);
210  pantilt_if_->set_tilt(tilt);
211  pantilt_if_->set_final(wt_->is_final());
212  pantilt_if_->write();
213 
214  panjoint_if_->set_position(pan);
215  panjoint_if_->write();
216 
217  tiltjoint_if_->set_position(tilt);
218  tiltjoint_if_->write();
219 
220  unsigned int zoom = wt_->get_zoom();
221  if (camctrl_if_->zoom() != zoom) {
222  camctrl_if_->set_zoom(zoom);
223  camctrl_if_->write();
224  }
225  }
226 }
227 
228 void
230 {
231  pantilt_if_->set_final(wt_->is_final());
232 
233  while (!pantilt_if_->msgq_empty()) {
235  // ignored
236 
237  } else if (pantilt_if_->msgq_first_is<PanTiltInterface::GotoMessage>()) {
238  PanTiltInterface::GotoMessage *msg = pantilt_if_->msgq_first(msg);
239 
240  wt_->goto_pantilt(msg->pan(), msg->tilt());
241  pantilt_if_->set_msgid(msg->id());
242  pantilt_if_->set_final(false);
243 
244  } else if (pantilt_if_->msgq_first_is<PanTiltInterface::ParkMessage>()) {
245  PanTiltInterface::ParkMessage *msg = pantilt_if_->msgq_first(msg);
246 
247  wt_->goto_pantilt(0, 0);
248  pantilt_if_->set_msgid(msg->id());
249  pantilt_if_->set_final(false);
250 
251  } else if (pantilt_if_->msgq_first_is<PanTiltInterface::SetEnabledMessage>()) {
252  PanTiltInterface::SetEnabledMessage *msg = pantilt_if_->msgq_first(msg);
253 
254  logger->log_warn(name(), "SetEnabledMessage ignored for Sony EviD100P");
255 
256  } else if (pantilt_if_->msgq_first_is<PanTiltInterface::SetVelocityMessage>()) {
257  PanTiltInterface::SetVelocityMessage *msg = pantilt_if_->msgq_first(msg);
258 
259  if ((msg->pan_velocity() < 0) || (msg->tilt_velocity() < 0)) {
260  logger->log_warn(name(),
261  "Ignoring pan/tilt velocities %f/%f, at least one "
262  " is negative",
263  msg->pan_velocity(),
264  msg->tilt_velocity());
265  } else if (msg->pan_velocity() > pantilt_if_->max_pan_velocity()) {
266  logger->log_warn(name(),
267  "Desired pan velocity %f too high, max is %f",
268  msg->pan_velocity(),
269  pantilt_if_->max_pan_velocity());
270  } else if (msg->tilt_velocity() > pantilt_if_->max_tilt_velocity()) {
271  logger->log_warn(name(),
272  "Desired tilt velocity %f too high, max is %f",
273  msg->tilt_velocity(),
274  pantilt_if_->max_tilt_velocity());
275  } else {
276  wt_->set_velocities(msg->pan_velocity(), msg->tilt_velocity());
277  pantilt_if_->set_pan_velocity(msg->pan_velocity());
278  pantilt_if_->set_tilt_velocity(msg->tilt_velocity());
279  panjoint_if_->set_velocity(msg->pan_velocity());
280  panjoint_if_->write();
281  tiltjoint_if_->set_velocity(msg->tilt_velocity());
282  tiltjoint_if_->write();
283  }
284 
285  } else {
286  logger->log_warn(name(), "Unknown message received");
287  }
288 
289  pantilt_if_->msgq_pop();
290  }
291  pantilt_if_->write();
292 
293  while (!camctrl_if_->msgq_empty()) {
295  CameraControlInterface::SetMirrorMessage *msg = camctrl_if_->msgq_first(msg);
296  wt_->set_mirror(msg->is_mirror());
297  camctrl_if_->set_mirror(msg->is_mirror());
298  camctrl_if_->write();
299  } else if (camctrl_if_->msgq_first_is<CameraControlInterface::SetEffectMessage>()) {
300  CameraControlInterface::SetEffectMessage *msg = camctrl_if_->msgq_first(msg);
301  wt_->set_effect(msg->effect());
302  camctrl_if_->set_effect(msg->effect());
303  camctrl_if_->write();
304  } else if (camctrl_if_->msgq_first_is<CameraControlInterface::SetZoomMessage>()) {
305  CameraControlInterface::SetZoomMessage *msg = camctrl_if_->msgq_first(msg);
306  wt_->set_zoom(msg->zoom());
307  } else {
308  logger->log_warn(name(), "Unhandled message %s ignored", camctrl_if_->msgq_first()->type());
309  }
310  camctrl_if_->msgq_pop();
311  }
312 
313  while (!power_if_->msgq_empty()) {
315  // must be re-established
316  wt_->set_mirror(camctrl_if_->is_mirror());
317  wt_->set_effect(camctrl_if_->effect());
318  wt_->set_power(true);
319  power_if_->set_enabled(true);
320  power_if_->write();
321  } else if (power_if_->msgq_first_is<SwitchInterface::DisableSwitchMessage>()) {
322  wt_->set_power(false);
323  power_if_->set_enabled(false);
324  power_if_->write();
325  } else if (power_if_->msgq_first_is<SwitchInterface::SetMessage>()) {
326  SwitchInterface::SetMessage *msg = power_if_->msgq_first(msg);
327  wt_->set_power(msg->is_enabled() || msg->value() > 0.5);
328  power_if_->set_enabled(msg->is_enabled() || msg->value() > 0.5);
329  power_if_->write();
330  } else {
331  logger->log_warn(name(), "Unhandled message %s ignored", power_if_->msgq_first()->type());
332  }
333  power_if_->msgq_pop();
334  }
335 }
336 
337 bool
339  Message * message) throw()
340 {
341  if (message->is_of_type<PanTiltInterface::StopMessage>()) {
342  wt_->stop_motion();
343  return false; // do not enqueue StopMessage
344  } else if (message->is_of_type<PanTiltInterface::FlushMessage>()) {
345  wt_->stop_motion();
346  logger->log_info(name(), "Flushing message queue");
347  pantilt_if_->msgq_flush();
348  return false;
349  } else {
350  //logger->log_info(name(), "Received message of type %s, enqueueing", message->type());
351  return true;
352  }
353 }
354 
355 /** @class PanTiltSonyEviD100PThread::WorkerThread "sony/evid100p_thread.h"
356  * Worker thread for the PanTiltSonyEviD100PThread.
357  * This continuous thread issues commands to the camera. In each loop it
358  * will first execute pending operations, and then update the sensor data (lengthy
359  * operation). Sensor data will only be updated while either a servo in the chain
360  * is still moving or torque is disabled (so the motor can be move manually).
361  * @author Tim Niemueller
362  */
363 
364 /** Constructor.
365  * @param ptu_name name of the pan/tilt unit
366  * @param logger logger
367  * @param cam Visca controller object
368  * @param pan_min minimum pan in rad
369  * @param pan_max maximum pan in rad
370  * @param tilt_min minimum tilt in rad
371  * @param tilt_max maximum tilt in rad
372  */
373 PanTiltSonyEviD100PThread::WorkerThread::WorkerThread(std::string ptu_name,
374  fawkes::Logger * logger,
376  const float & pan_min,
377  const float & pan_max,
378  const float & tilt_min,
379  const float & tilt_max)
380 : Thread("", Thread::OPMODE_WAITFORWAKEUP)
381 {
382  set_name("SonyEviD100PWorkerThread(%s)", ptu_name.c_str());
383  set_coalesce_wakeups(true);
384 
385  logger_ = logger;
386 
387  move_mutex_ = new Mutex();
388  effect_mutex_ = new Mutex();
389  zoom_mutex_ = new Mutex();
390  mirror_mutex_ = new Mutex();
391  power_mutex_ = new Mutex();
392 
393  cam_ = cam;
394  move_pending_ = false;
395  target_pan_ = 0;
396  target_tilt_ = 0;
397  fresh_data_ = false;
398 
399  velo_pending_ = false;
400  pan_vel_ = 0;
401  tilt_vel_ = 0;
402 
403  pan_min_ = pan_min;
404  pan_max_ = pan_max;
405  tilt_min_ = tilt_min;
406  tilt_max_ = tilt_max;
407 
408  zoom_pending_ = false;
409  target_zoom_ = 0;
410 
411  mirror_pending_ = false;
412  power_pending_ = false;
413  effect_pending_ = false;
414 
415  powered_ = cam_->is_powered();
416  power_desired_ = powered_;
417 }
418 
419 /** Destructor. */
420 PanTiltSonyEviD100PThread::WorkerThread::~WorkerThread()
421 {
422  delete move_mutex_;
423  delete zoom_mutex_;
424  delete effect_mutex_;
425  delete mirror_mutex_;
426  delete power_mutex_;
427 }
428 
429 /** Stop currently running motion. */
430 void
431 PanTiltSonyEviD100PThread::WorkerThread::stop_motion()
432 {
433  if (powered_) {
434  float pan = 0, tilt = 0;
435  get_pantilt(pan, tilt);
436  goto_pantilt(pan, tilt);
437  }
438 }
439 
440 /** Goto desired pan/tilt values.
441  * @param pan pan in radians
442  * @param tilt tilt in radians
443  */
444 void
445 PanTiltSonyEviD100PThread::WorkerThread::goto_pantilt(float pan, float tilt)
446 {
447  MutexLocker lock(move_mutex_);
448  target_pan_ = pan;
449  target_tilt_ = tilt;
450  move_pending_ = true;
451  if (powered_)
452  wakeup();
453 }
454 
455 /** Set desired zoom value.
456  * @param zoom_value desired zoom
457  */
458 void
459 PanTiltSonyEviD100PThread::WorkerThread::set_zoom(unsigned int zoom_value)
460 {
461  MutexLocker lock(zoom_mutex_);
462  zoom_pending_ = true;
463 
464  switch (zoom_value) {
465  case 0: target_zoom_ = Visca::VISCA_ZOOM_VALUE_WIDE; break;
466  case 1: target_zoom_ = Visca::VISCA_ZOOM_VALUE_1X; break;
467  case 2: target_zoom_ = Visca::VISCA_ZOOM_VALUE_2X; break;
468  case 3: target_zoom_ = Visca::VISCA_ZOOM_VALUE_3X; break;
469  case 4: target_zoom_ = Visca::VISCA_ZOOM_VALUE_4X; break;
470  case 5: target_zoom_ = Visca::VISCA_ZOOM_VALUE_5X; break;
471  case 6: target_zoom_ = Visca::VISCA_ZOOM_VALUE_6X; break;
472  case 7: target_zoom_ = Visca::VISCA_ZOOM_VALUE_7X; break;
473  case 8: target_zoom_ = Visca::VISCA_ZOOM_VALUE_8X; break;
474  case 9: target_zoom_ = Visca::VISCA_ZOOM_VALUE_9X; break;
475  case 10: target_zoom_ = Visca::VISCA_ZOOM_VALUE_10X; break;
476  case 11: target_zoom_ = Visca::VISCA_ZOOM_VALUE_DIG_20X; break;
477  case 12: target_zoom_ = Visca::VISCA_ZOOM_VALUE_DIG_30X; break;
478  case 13: target_zoom_ = Visca::VISCA_ZOOM_VALUE_DIG_40X; break;
479  default:
480  logger_->log_warn(name(), "Illegal zoom value %u ignored", zoom_value);
481  zoom_pending_ = false;
482  }
483  if (powered_)
484  wakeup();
485 }
486 
487 /** Set desired effect.
488  * @param effect effect value
489  */
490 void
491 PanTiltSonyEviD100PThread::WorkerThread::set_effect(CameraControlInterface::Effect effect)
492 {
493  MutexLocker lock(effect_mutex_);
494  target_effect_ = effect;
495  effect_pending_ = true;
496  if (powered_)
497  wakeup();
498 }
499 
500 /** Set mirror state.
501  * @param enabled true to enable mirroring, false to disable
502  */
503 void
504 PanTiltSonyEviD100PThread::WorkerThread::set_mirror(bool enabled)
505 {
506  MutexLocker lock(effect_mutex_);
507  target_mirror_ = enabled;
508  mirror_pending_ = true;
509  if (powered_)
510  wakeup();
511 }
512 
513 /** Set power for camera.
514  * @param powered true to turn on, false to turn off
515  */
516 void
517 PanTiltSonyEviD100PThread::WorkerThread::set_power(bool powered)
518 {
519  MutexLocker lock(power_mutex_);
520  power_desired_ = powered;
521  power_pending_ = true;
522  wakeup();
523 }
524 
525 /** Get pan/tilt value.
526  * @param pan upon return contains the current pan value
527  * @param tilt upon return contains the current tilt value
528  */
529 void
530 PanTiltSonyEviD100PThread::WorkerThread::get_pantilt(float &pan, float &tilt)
531 {
532  pan = cur_pan_;
533  tilt = cur_tilt_;
534 }
535 
536 /** Get zoom value.
537  * @return current zoom value
538  */
539 unsigned int
540 PanTiltSonyEviD100PThread::WorkerThread::get_zoom()
541 {
542  switch (cur_zoom_) {
543  case Visca::VISCA_ZOOM_VALUE_1X: return 1;
544  case Visca::VISCA_ZOOM_VALUE_2X: return 2;
545  case Visca::VISCA_ZOOM_VALUE_3X: return 3;
546  case Visca::VISCA_ZOOM_VALUE_4X: return 4;
547  case Visca::VISCA_ZOOM_VALUE_5X: return 5;
548  case Visca::VISCA_ZOOM_VALUE_6X: return 6;
549  case Visca::VISCA_ZOOM_VALUE_7X: return 7;
550  case Visca::VISCA_ZOOM_VALUE_8X: return 8;
551  case Visca::VISCA_ZOOM_VALUE_9X: return 9;
552  case Visca::VISCA_ZOOM_VALUE_10X: return 10;
553  case Visca::VISCA_ZOOM_VALUE_DIG_20X: return 11;
554  case Visca::VISCA_ZOOM_VALUE_DIG_30X: return 12;
555  case Visca::VISCA_ZOOM_VALUE_DIG_40X: return 13;
556  default: return 0;
557  }
558 }
559 
560 /** Set desired velocities.
561  * @param pan_vel pan velocity
562  * @param tilt_vel tilt velocity
563  */
564 void
565 PanTiltSonyEviD100PThread::WorkerThread::set_velocities(float pan_vel, float tilt_vel)
566 {
567  pan_vel_ = pan_vel;
568  tilt_vel_ = tilt_vel;
569  velo_pending_ = true;
570 }
571 
572 /** Check if motion is final.
573  * @return true if motion is final, false otherwise
574  */
575 bool
576 PanTiltSonyEviD100PThread::WorkerThread::is_final()
577 {
578  MutexLocker lock(move_mutex_);
579  return powered_ && cam_->is_nonblocking_finished(SonyEviD100PVisca::NONBLOCKING_PANTILT)
580  && cam_->is_nonblocking_finished(SonyEviD100PVisca::NONBLOCKING_ZOOM);
581 }
582 
583 /** Check is fresh sensor data is available.
584  * Note that this method will return true at once per sensor update cycle.
585  * @return true if fresh data is available, false otherwise
586  */
587 bool
588 PanTiltSonyEviD100PThread::WorkerThread::has_fresh_data()
589 {
590  bool rv = fresh_data_;
591  fresh_data_ = false;
592  return rv;
593 }
594 
595 void
596 PanTiltSonyEviD100PThread::WorkerThread::once()
597 {
598  // do some process cycles to process data returning back from set_address()
599  // and clear calls
600  for (int i = 0; i < 20; ++i) {
601  try {
602  cam_->process();
603  } catch (Exception &e) { /* ignored */
604  }
605  }
606 }
607 
608 void
609 PanTiltSonyEviD100PThread::WorkerThread::loop()
610 {
611  try {
612  cam_->process();
613  } catch (Exception &e) {
614  logger_->log_warn(name(), "Data processing failed, exception follows");
615  logger_->log_warn(name(), e);
616  }
617 
618  if (power_pending_) {
619  power_mutex_->lock();
620  logger_->log_debug(name(), "Powering %s the PTU", power_desired_ ? "up" : "down");
621  power_pending_ = false;
622  cam_->set_power(power_desired_);
623  powered_ = power_desired_;
624  power_mutex_->unlock();
625  }
626 
627  if (velo_pending_) {
628  try {
629  if (powered_)
630  cam_->set_speed_radsec(pan_vel_, tilt_vel_);
631  } catch (Exception &e) {
632  logger_->log_warn(name(), "Setting pan/tilt values failed, exception follows");
633  logger_->log_warn(name(), e);
634  }
635  velo_pending_ = false;
636  }
637 
638  if (move_pending_) {
639  move_mutex_->lock();
640  logger_->log_debug(name(), "Executing goto to %f, %f", target_pan_, target_tilt_);
641  if (powered_)
642  exec_goto_pantilt(target_pan_, target_tilt_);
643  move_pending_ = false;
644  move_mutex_->unlock();
645  }
646 
647  if (zoom_pending_) {
648  zoom_mutex_->lock();
649  if (powered_)
650  exec_set_zoom(target_zoom_);
651  zoom_pending_ = false;
652  zoom_mutex_->unlock();
653  }
654 
655  if (effect_pending_) {
656  effect_mutex_->lock();
657  if (powered_)
658  exec_set_effect(target_effect_);
659  effect_pending_ = false;
660  effect_mutex_->unlock();
661  }
662 
663  if (mirror_pending_) {
664  mirror_mutex_->lock();
665  logger_->log_debug(name(), "%sabling mirroring", target_mirror_ ? "En" : "Dis");
666  if (powered_)
667  exec_set_mirror(target_mirror_);
668  mirror_pending_ = false;
669  mirror_mutex_->unlock();
670  }
671 
672  //cam_->start_get_pan_tilt();
673  try {
674  if (powered_) {
675  cam_->get_pan_tilt_rad(cur_pan_, cur_tilt_);
676  fresh_data_ = true;
677  }
678  } catch (Exception &e) {
679  logger_->log_warn(name(), "Failed to get new pan/tilt data, exception follows");
680  logger_->log_warn(name(), e);
681  }
682 
683  try {
684  if (powered_) {
685  unsigned int new_zoom = 0;
686  cam_->get_zoom(new_zoom);
687  if (new_zoom != cur_zoom_) {
688  cur_zoom_ = new_zoom;
689  fresh_data_ = true;
690  }
691  }
692  } catch (Exception &e) {
693  logger_->log_warn(name(), "Failed to get new zoom data, exception follows");
694  logger_->log_warn(name(), e);
695  }
696 
697  if (powered_ && (!is_final() || !fresh_data_)) {
698  // while moving or if data reception failed wake us up to get new servo data
699  wakeup();
700  }
701 }
702 
703 /** Execute pan/tilt motion.
704  * @param pan_rad pan in rad to move to
705  * @param tilt_rad tilt in rad to move to
706  */
707 void
708 PanTiltSonyEviD100PThread::WorkerThread::exec_goto_pantilt(float pan_rad, float tilt_rad)
709 {
710  if ((pan_rad < pan_min_) || (pan_rad > pan_max_)) {
711  logger_->log_warn(
712  name(), "Pan value out of bounds, min: %f max: %f des: %f", pan_min_, pan_max_, pan_rad);
713  return;
714  }
715  if ((tilt_rad < tilt_min_) || (tilt_rad > tilt_max_)) {
716  logger_->log_warn(name(),
717  "Tilt value out of bounds, min: %f max: %f des: %f",
718  tilt_min_,
719  tilt_max_,
720  tilt_rad);
721  return;
722  }
723 
724  try {
725  cam_->set_pan_tilt_rad(pan_rad, tilt_rad);
726  } catch (Exception &e) {
727  logger_->log_warn(name(),
728  "Failed to execute pan/tilt to %f, %f, exception "
729  "follows",
730  pan_rad,
731  tilt_rad);
732  logger_->log_warn(name(), e);
733  }
734 }
735 
736 /** Execute zoom setting.
737  * @param zoom Zoom value to set
738  */
739 void
740 PanTiltSonyEviD100PThread::WorkerThread::exec_set_zoom(unsigned int zoom)
741 {
742  try {
743  cam_->set_zoom(zoom);
744  } catch (Exception &e) {
745  logger_->log_warn(name(),
746  "Failed to execute zoom to %u, exception "
747  "follows",
748  zoom);
749  logger_->log_warn(name(), e);
750  }
751 }
752 
753 /** Execute mirror setting.
754  * @param mirror true to enable monitoring, false to disable
755  */
756 void
757 PanTiltSonyEviD100PThread::WorkerThread::exec_set_mirror(bool mirror)
758 {
759  try {
760  cam_->set_mirror(mirror);
761  } catch (Exception &e) {
762  logger_->log_warn(name(),
763  "Failed to %sabling mirror mod, exception follows",
764  mirror ? "En" : "Dis");
765  logger_->log_warn(name(), e);
766  }
767 }
768 
769 /** Execute effect setting.
770  * @param effect Target effect value to set
771  */
772 void
773 PanTiltSonyEviD100PThread::WorkerThread::exec_set_effect(CameraControlInterface::Effect effect)
774 {
775  try {
776  switch (effect) {
777  case CameraControlInterface::EFF_NEGATIVE: cam_->apply_effect_neg_art(); break;
778  case CameraControlInterface::EFF_PASTEL: cam_->apply_effect_pastel(); break;
779  case CameraControlInterface::EFF_BW: cam_->apply_effect_bnw(); break;
780  case CameraControlInterface::EFF_SOLARIZE: cam_->apply_effect_solarize(); break;
781  default: cam_->reset_effect(); break;
782  }
783  } catch (Exception &e) {
784  logger_->log_warn(name(), "Failed to set effect, exception follows");
785  logger_->log_warn(name(), e);
786  }
787 }
Pan/tilt act thread.
Definition: act_thread.h:41
PanTiltSonyEviD100PThread(std::string &pantilt_cfg_prefix, std::string &ptu_cfg_prefix, std::string &ptu_name)
Constructor.
virtual bool bb_interface_message_received(fawkes::Interface *interface, fawkes::Message *message)
BlackBoard message received notification.
virtual void finalize()
Finalize the thread.
virtual void loop()
Code to execute in the thread.
void update_sensor_values()
Update sensor values as necessary.
virtual void init()
Initialize the thread.
Sony EviD100P Visca controller.
Definition: evid100p.h:33
static const float MIN_TILT_RAD
Min tilt in rad.
Definition: evid100p.h:61
static const float MAX_TILT_RAD
Max tilt in rad.
Definition: evid100p.h:60
static const float MIN_PAN_RAD
Min pan in rad.
Definition: evid100p.h:59
void get_speed_limits(float &pan_min, float &pan_max, float &tilt_min, float &tilt_max)
Get speed limits.
Definition: evid100p.cpp:239
static const float MAX_PAN_RAD
Max pan in rad.
Definition: evid100p.h:58
void set_mirror(bool mirror)
Sett mirror sate.
Definition: visca.cpp:1240
static const unsigned int VISCA_ZOOM_VALUE_1X
Zoom value: 1x.
Definition: visca.h:70
static const unsigned int VISCA_ZOOM_VALUE_8X
Zoom value: 8x.
Definition: visca.h:84
static const unsigned int NONBLOCKING_ZOOM
Non-blocking zoom item.
Definition: visca.h:61
static const unsigned int VISCA_ZOOM_VALUE_10X
Zoom value: 10x.
Definition: visca.h:88
bool is_powered()
Check if camera is powered.
Definition: visca.cpp:582
static const unsigned int VISCA_ZOOM_VALUE_WIDE
Zoom value: wide.
Definition: visca.h:68
static const unsigned int VISCA_ZOOM_VALUE_2X
Zoom value: 2x.
Definition: visca.h:72
static const unsigned int VISCA_ZOOM_VALUE_DIG_40X
Zoom value: 40x.
Definition: visca.h:94
static const unsigned int VISCA_ZOOM_VALUE_4X
Zoom value: 4x.
Definition: visca.h:76
static const unsigned int VISCA_ZOOM_VALUE_5X
Zoom value: 5x.
Definition: visca.h:78
static const unsigned int VISCA_ZOOM_VALUE_9X
Zoom value: 9x.
Definition: visca.h:86
static const unsigned int VISCA_ZOOM_VALUE_3X
Zoom value: 3x.
Definition: visca.h:74
static const unsigned int VISCA_ZOOM_VALUE_7X
Zoom value: 7x.
Definition: visca.h:82
void set_power(bool powered)
Set power state.
Definition: visca.cpp:562
static const unsigned int VISCA_ZOOM_VALUE_6X
Zoom value: 6x.
Definition: visca.h:80
static const unsigned int NONBLOCKING_PANTILT
Non-blocking pan/tilt item.
Definition: visca.h:60
static const unsigned int VISCA_ZOOM_VALUE_DIG_20X
Zoom value: 20x.
Definition: visca.h:90
static const unsigned int VISCA_ZOOM_VALUE_DIG_30X
Zoom value: 30x.
Definition: visca.h:92
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
Definition: blackboard.h:44
BlackBoard interface listener.
void bbil_add_message_interface(Interface *interface)
Add an interface to the message received watch list.
virtual void unregister_listener(BlackBoardInterfaceListener *listener)
Unregister BB interface listener.
Definition: blackboard.cpp:212
virtual Interface * open_for_writing(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for writing.
virtual void register_listener(BlackBoardInterfaceListener *listener, ListenerRegisterFlag flag=BBIL_FLAG_ALL)
Register BB event listener.
Definition: blackboard.cpp:185
virtual void close(Interface *interface)=0
Close interface.
SetEffectMessage Fawkes BlackBoard Interface Message.
SetMirrorMessage Fawkes BlackBoard Interface Message.
SetZoomMessage Fawkes BlackBoard Interface Message.
CameraControlInterface Fawkes BlackBoard Interface.
void set_mirror_supported(const bool new_mirror_supported)
Set mirror_supported value.
Effect effect() const
Get effect value.
void set_mirror(const bool new_mirror)
Set mirror value.
Effect
Enumeration defining the possible effects.
uint32_t zoom() const
Get zoom value.
void set_effect_supported(const bool new_effect_supported)
Set effect_supported value.
void set_zoom_supported(const bool new_zoom_supported)
Set zoom_supported value.
void set_zoom(const uint32_t new_zoom)
Set zoom value.
bool is_mirror() const
Get mirror value.
void set_zoom_min(const uint32_t new_zoom_min)
Set zoom_min value.
void set_zoom_max(const uint32_t new_zoom_max)
Set zoom_max value.
void set_effect(const Effect new_effect)
Set effect value.
Configuration * config
This is the Configuration member used to access the configuration.
Definition: configurable.h:41
virtual unsigned int get_uint(const char *path)=0
Get value from configuration which is of type unsigned int.
virtual bool get_bool(const char *path)=0
Get value from configuration which is of type bool.
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
Base class for exceptions in Fawkes.
Definition: exception.h:36
void print_backtrace() const
Prints a backtrace.
Definition: exception.cpp:547
void print_trace()
Prints trace to stderr.
Definition: exception.cpp:601
Base class for all Fawkes BlackBoard interfaces.
Definition: interface.h:79
bool msgq_first_is()
Check if first message has desired type.
Definition: interface.h:314
void msgq_pop()
Erase first message from queue.
Definition: interface.cpp:1182
Message * msgq_first()
Get the first message from the message queue.
Definition: interface.cpp:1167
void write()
Write from local copy into BlackBoard memory.
Definition: interface.cpp:494
bool msgq_empty()
Check if queue is empty.
Definition: interface.cpp:1029
JointInterface Fawkes BlackBoard Interface.
void set_position(const float new_position)
Set position value.
void set_velocity(const float new_velocity)
Set velocity value.
Interface for logging.
Definition: logger.h:42
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:41
Base class for all messages passed through interfaces in Fawkes BlackBoard.
Definition: message.h:45
const char * type() const
Get message type.
Definition: message.cpp:346
unsigned int id() const
Get message ID.
Definition: message.cpp:180
virtual void log_info(const char *component, const char *format,...)
Log informational message.
Definition: multi.cpp:195
Mutex locking helper.
Definition: mutex_locker.h:34
Mutex mutual exclusion lock.
Definition: mutex.h:33
CalibrateMessage Fawkes BlackBoard Interface Message.
FlushMessage Fawkes BlackBoard Interface Message.
GotoMessage Fawkes BlackBoard Interface Message.
float tilt() const
Get tilt value.
ParkMessage Fawkes BlackBoard Interface Message.
SetEnabledMessage Fawkes BlackBoard Interface Message.
SetVelocityMessage Fawkes BlackBoard Interface Message.
float tilt_velocity() const
Get tilt_velocity value.
float pan_velocity() const
Get pan_velocity value.
StopMessage Fawkes BlackBoard Interface Message.
PanTiltInterface Fawkes BlackBoard Interface.
void set_enabled(const bool new_enabled)
Set enabled value.
float max_tilt_velocity() const
Get max_tilt_velocity value.
void set_tilt_velocity(const float new_tilt_velocity)
Set tilt_velocity value.
void set_min_pan(const float new_min_pan)
Set min_pan value.
void set_final(const bool new_final)
Set final value.
void set_min_tilt(const float new_min_tilt)
Set min_tilt value.
void set_max_tilt_velocity(const float new_max_tilt_velocity)
Set max_tilt_velocity value.
void set_max_tilt(const float new_max_tilt)
Set max_tilt value.
void set_msgid(const uint32_t new_msgid)
Set msgid value.
void set_max_pan(const float new_max_pan)
Set max_pan value.
void set_pan_velocity(const float new_pan_velocity)
Set pan_velocity value.
void set_tilt(const float new_tilt)
Set tilt value.
void set_max_pan_velocity(const float new_max_pan_velocity)
Set max_pan_velocity value.
void set_calibrated(const bool new_calibrated)
Set calibrated value.
float max_pan_velocity() const
Get max_pan_velocity value.
void set_pan(const float new_pan)
Set pan value.
DisableSwitchMessage Fawkes BlackBoard Interface Message.
EnableSwitchMessage Fawkes BlackBoard Interface Message.
SetMessage Fawkes BlackBoard Interface Message.
float value() const
Get value value.
bool is_enabled() const
Get enabled value.
SwitchInterface Fawkes BlackBoard Interface.
void set_enabled(const bool new_enabled)
Set enabled value.
bool is_enabled() const
Get enabled value.
Thread class encapsulation of pthreads.
Definition: thread.h:46
const char * name() const
Get name of thread.
Definition: thread.h:100
void set_name(const char *format,...)
Set name of thread.
Definition: thread.cpp:748
void set_coalesce_wakeups(bool coalesce=true)
Set wakeup coalescing.
Definition: thread.cpp:729
Time tracking utility.
Definition: tracker.h:37
Fawkes library namespace.