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 }
fawkes::PanTiltInterface::max_tilt_velocity
float max_tilt_velocity() const
Get max_tilt_velocity value.
Definition: PanTiltInterface.cpp:507
Visca::VISCA_ZOOM_VALUE_DIG_40X
static const unsigned int VISCA_ZOOM_VALUE_DIG_40X
Zoom value: 40x.
Definition: visca.h:94
fawkes::CameraControlInterface::SetEffectMessage::effect
Effect effect() const
Get effect value.
Definition: CameraControlInterface.cpp:440
fawkes::Exception::print_backtrace
void print_backtrace() const
Prints a backtrace.
Definition: exception.cpp:547
fawkes::Interface::msgq_first_is
bool msgq_first_is()
Check if first message has desired type.
Definition: interface.h:314
Visca::VISCA_ZOOM_VALUE_DIG_30X
static const unsigned int VISCA_ZOOM_VALUE_DIG_30X
Zoom value: 30x.
Definition: visca.h:92
fawkes::PanTiltInterface::set_max_tilt
void set_max_tilt(const float new_max_tilt)
Set max_tilt value.
Definition: PanTiltInterface.cpp:467
fawkes::Interface::msgq_pop
void msgq_pop()
Erase first message from queue.
Definition: interface.cpp:1182
SonyEviD100PVisca::MIN_PAN_RAD
static const float MIN_PAN_RAD
Min pan in rad.
Definition: evid100p.h:59
fawkes::SwitchInterface::SetMessage
SetMessage Fawkes BlackBoard Interface Message.
Definition: SwitchInterface.h:81
Visca::VISCA_ZOOM_VALUE_3X
static const unsigned int VISCA_ZOOM_VALUE_3X
Zoom value: 3x.
Definition: visca.h:74
fawkes::Interface::msgq_empty
bool msgq_empty()
Check if queue is empty.
Definition: interface.cpp:1029
SonyEviD100PVisca::MIN_TILT_RAD
static const float MIN_TILT_RAD
Min tilt in rad.
Definition: evid100p.h:61
fawkes::PanTiltInterface::set_min_tilt
void set_min_tilt(const float new_min_tilt)
Set min_tilt value.
Definition: PanTiltInterface.cpp:437
fawkes::BlackBoard::register_listener
virtual void register_listener(BlackBoardInterfaceListener *listener, ListenerRegisterFlag flag=BBIL_FLAG_ALL)
Register BB event listener.
Definition: blackboard.cpp:185
fawkes::PanTiltInterface::StopMessage
StopMessage Fawkes BlackBoard Interface Message.
Definition: PanTiltInterface.h:84
Visca::NONBLOCKING_PANTILT
static const unsigned int NONBLOCKING_PANTILT
Non-blocking pan/tilt item.
Definition: visca.h:60
fawkes::Mutex
Mutex mutual exclusion lock.
Definition: mutex.h:33
fawkes::SwitchInterface
SwitchInterface Fawkes BlackBoard Interface.
Definition: SwitchInterface.h:34
fawkes::PanTiltInterface::SetVelocityMessage::pan_velocity
float pan_velocity() const
Get pan_velocity value.
Definition: PanTiltInterface.cpp:1323
fawkes::PanTiltInterface
PanTiltInterface Fawkes BlackBoard Interface.
Definition: PanTiltInterface.h:34
fawkes::Message
Base class for all messages passed through interfaces in Fawkes BlackBoard.
Definition: message.h:45
fawkes::RefPtr< SonyEviD100PVisca >
fawkes::PanTiltInterface::set_calibrated
void set_calibrated(const bool new_calibrated)
Set calibrated value.
Definition: PanTiltInterface.cpp:347
fawkes::CameraControlInterface::SetMirrorMessage::is_mirror
bool is_mirror() const
Get mirror value.
Definition: CameraControlInterface.cpp:640
Visca::VISCA_ZOOM_VALUE_7X
static const unsigned int VISCA_ZOOM_VALUE_7X
Zoom value: 7x.
Definition: visca.h:82
Visca::NONBLOCKING_ZOOM
static const unsigned int NONBLOCKING_ZOOM
Non-blocking zoom item.
Definition: visca.h:61
fawkes::CameraControlInterface::SetEffectMessage
SetEffectMessage Fawkes BlackBoard Interface Message.
Definition: CameraControlInterface.h:75
SonyEviD100PVisca::MAX_PAN_RAD
static const float MAX_PAN_RAD
Max pan in rad.
Definition: evid100p.h:58
fawkes::Configuration::get_bool
virtual bool get_bool(const char *path)=0
Get value from configuration which is of type bool.
fawkes::JointInterface::set_position
void set_position(const float new_position)
Set position value.
Definition: JointInterface.cpp:94
fawkes::CameraControlInterface::set_zoom_min
void set_zoom_min(const uint32_t new_zoom_min)
Set zoom_min value.
Definition: CameraControlInterface.cpp:269
Visca::set_power
void set_power(bool powered)
Set power state.
Definition: visca.cpp:562
fawkes::CameraControlInterface::SetMirrorMessage
SetMirrorMessage Fawkes BlackBoard Interface Message.
Definition: CameraControlInterface.h:127
Visca::is_powered
bool is_powered()
Check if camera is powered.
Definition: visca.cpp:582
fawkes::MutexLocker
Mutex locking helper.
Definition: mutex_locker.h:34
SonyEviD100PVisca
Sony EviD100P Visca controller.
Definition: evid100p.h:33
fawkes::BlackBoard::unregister_listener
virtual void unregister_listener(BlackBoardInterfaceListener *listener)
Unregister BB interface listener.
Definition: blackboard.cpp:212
fawkes::BlackBoardInterfaceListener
BlackBoard interface listener.
Definition: interface_listener.h:42
PanTiltSonyEviD100PThread::init
virtual void init()
Initialize the thread.
Definition: evid100p_thread.cpp:64
fawkes::SwitchInterface::DisableSwitchMessage
DisableSwitchMessage Fawkes BlackBoard Interface Message.
Definition: SwitchInterface.h:136
fawkes::Thread::name
const char * name() const
Get name of thread.
Definition: thread.h:100
fawkes::JointInterface::set_velocity
void set_velocity(const float new_velocity)
Set velocity value.
Definition: JointInterface.cpp:128
fawkes::CameraControlInterface::is_mirror
bool is_mirror() const
Get mirror value.
Definition: CameraControlInterface.cpp:279
fawkes::CameraControlInterface::SetZoomMessage
SetZoomMessage Fawkes BlackBoard Interface Message.
Definition: CameraControlInterface.h:101
Visca::VISCA_ZOOM_VALUE_9X
static const unsigned int VISCA_ZOOM_VALUE_9X
Zoom value: 9x.
Definition: visca.h:86
fawkes::CameraControlInterface::effect
Effect effect() const
Get effect value.
Definition: CameraControlInterface.cpp:99
fawkes::PanTiltInterface::set_max_pan_velocity
void set_max_pan_velocity(const float new_max_pan_velocity)
Set max_pan_velocity value.
Definition: PanTiltInterface.cpp:497
fawkes::CameraControlInterface::set_mirror
void set_mirror(const bool new_mirror)
Set mirror value.
Definition: CameraControlInterface.cpp:299
fawkes::CameraControlInterface::set_zoom_supported
void set_zoom_supported(const bool new_zoom_supported)
Set zoom_supported value.
Definition: CameraControlInterface.cpp:209
fawkes::SwitchInterface::SetMessage::is_enabled
bool is_enabled() const
Get enabled value.
Definition: SwitchInterface.cpp:401
PanTiltSonyEviD100PThread::loop
virtual void loop()
Code to execute in the thread.
Definition: evid100p_thread.cpp:229
fawkes::PanTiltInterface::SetVelocityMessage
SetVelocityMessage Fawkes BlackBoard Interface Message.
Definition: PanTiltInterface.h:252
Visca::VISCA_ZOOM_VALUE_6X
static const unsigned int VISCA_ZOOM_VALUE_6X
Zoom value: 6x.
Definition: visca.h:80
fawkes::SwitchInterface::EnableSwitchMessage
EnableSwitchMessage Fawkes BlackBoard Interface Message.
Definition: SwitchInterface.h:116
fawkes::PanTiltInterface::GotoMessage::tilt
float tilt() const
Get tilt value.
Definition: PanTiltInterface.cpp:979
fawkes::CameraControlInterface::zoom
uint32_t zoom() const
Get zoom value.
Definition: CameraControlInterface.cpp:159
fawkes::LoggingAspect::logger
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:41
PanTiltActThread
Pan/tilt act thread.
Definition: act_thread.h:41
fawkes::PanTiltInterface::set_max_pan
void set_max_pan(const float new_max_pan)
Set max_pan value.
Definition: PanTiltInterface.cpp:407
fawkes::BlackBoard::close
virtual void close(Interface *interface)=0
Close interface.
fawkes::Thread::set_coalesce_wakeups
void set_coalesce_wakeups(bool coalesce=true)
Set wakeup coalescing.
Definition: thread.cpp:729
fawkes::Logger
Interface for logging.
Definition: logger.h:42
fawkes::SwitchInterface::set_enabled
void set_enabled(const bool new_enabled)
Set enabled value.
Definition: SwitchInterface.cpp:105
fawkes::PanTiltInterface::GotoMessage
GotoMessage Fawkes BlackBoard Interface Message.
Definition: PanTiltInterface.h:164
fawkes
Fawkes library namespace.
SonyEviD100PVisca::get_speed_limits
void get_speed_limits(float &pan_min, float &pan_max, float &tilt_min, float &tilt_max)
Get speed limits.
Definition: evid100p.cpp:239
fawkes::PanTiltInterface::set_enabled
void set_enabled(const bool new_enabled)
Set enabled value.
Definition: PanTiltInterface.cpp:317
fawkes::PanTiltInterface::set_pan_velocity
void set_pan_velocity(const float new_pan_velocity)
Set pan_velocity value.
Definition: PanTiltInterface.cpp:557
fawkes::PanTiltInterface::set_pan
void set_pan(const float new_pan)
Set pan value.
Definition: PanTiltInterface.cpp:159
fawkes::SwitchInterface::SetMessage::value
float value() const
Get value value.
Definition: SwitchInterface.cpp:437
fawkes::Logger::log_warn
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
fawkes::PanTiltInterface::ParkMessage
ParkMessage Fawkes BlackBoard Interface Message.
Definition: PanTiltInterface.h:144
fawkes::BlackBoardInterfaceListener::bbil_add_message_interface
void bbil_add_message_interface(Interface *interface)
Add an interface to the message received watch list.
Definition: interface_listener.cpp:241
fawkes::Interface
Base class for all Fawkes BlackBoard interfaces.
Definition: interface.h:79
SonyEviD100PVisca::MAX_TILT_RAD
static const float MAX_TILT_RAD
Max tilt in rad.
Definition: evid100p.h:60
fawkes::PanTiltInterface::set_msgid
void set_msgid(const uint32_t new_msgid)
Set msgid value.
Definition: PanTiltInterface.cpp:221
fawkes::Exception::print_trace
void print_trace()
Prints trace to stderr.
Definition: exception.cpp:601
fawkes::PanTiltInterface::FlushMessage
FlushMessage Fawkes BlackBoard Interface Message.
Definition: PanTiltInterface.h:104
fawkes::PanTiltInterface::SetEnabledMessage
SetEnabledMessage Fawkes BlackBoard Interface Message.
Definition: PanTiltInterface.h:227
fawkes::ConfigurableAspect::config
Configuration * config
This is the Configuration member used to access the configuration.
Definition: configurable.h:41
fawkes::CameraControlInterface::set_zoom_max
void set_zoom_max(const uint32_t new_zoom_max)
Set zoom_max value.
Definition: CameraControlInterface.cpp:239
fawkes::Message::type
const char * type() const
Get message type.
Definition: message.cpp:346
fawkes::JointInterface
JointInterface Fawkes BlackBoard Interface.
Definition: JointInterface.h:34
Visca::VISCA_ZOOM_VALUE_DIG_20X
static const unsigned int VISCA_ZOOM_VALUE_DIG_20X
Zoom value: 20x.
Definition: visca.h:90
fawkes::CameraControlInterface::set_effect_supported
void set_effect_supported(const bool new_effect_supported)
Set effect_supported value.
Definition: CameraControlInterface.cpp:149
Visca::VISCA_ZOOM_VALUE_10X
static const unsigned int VISCA_ZOOM_VALUE_10X
Zoom value: 10x.
Definition: visca.h:88
fawkes::PanTiltInterface::CalibrateMessage
CalibrateMessage Fawkes BlackBoard Interface Message.
Definition: PanTiltInterface.h:124
Visca::VISCA_ZOOM_VALUE_2X
static const unsigned int VISCA_ZOOM_VALUE_2X
Zoom value: 2x.
Definition: visca.h:72
fawkes::PanTiltInterface::GotoMessage::pan
float pan() const
Get pan value.
Definition: PanTiltInterface.cpp:949
fawkes::CameraControlInterface
CameraControlInterface Fawkes BlackBoard Interface.
Definition: CameraControlInterface.h:34
fawkes::TimeTracker
Time tracking utility.
Definition: tracker.h:37
Visca::VISCA_ZOOM_VALUE_1X
static const unsigned int VISCA_ZOOM_VALUE_1X
Zoom value: 1x.
Definition: visca.h:70
fawkes::Thread
Thread class encapsulation of pthreads.
Definition: thread.h:46
fawkes::BlackBoardAspect::blackboard
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
Definition: blackboard.h:44
Visca::VISCA_ZOOM_VALUE_5X
static const unsigned int VISCA_ZOOM_VALUE_5X
Zoom value: 5x.
Definition: visca.h:78
Visca::VISCA_ZOOM_VALUE_4X
static const unsigned int VISCA_ZOOM_VALUE_4X
Zoom value: 4x.
Definition: visca.h:76
fawkes::Configuration::get_uint
virtual unsigned int get_uint(const char *path)=0
Get value from configuration which is of type unsigned int.
fawkes::Configuration::get_string
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
fawkes::Message::id
unsigned int id() const
Get message ID.
Definition: message.cpp:180
fawkes::CameraControlInterface::set_effect
void set_effect(const Effect new_effect)
Set effect value.
Definition: CameraControlInterface.cpp:119
fawkes::PanTiltInterface::max_pan_velocity
float max_pan_velocity() const
Get max_pan_velocity value.
Definition: PanTiltInterface.cpp:477
fawkes::SwitchInterface::is_enabled
bool is_enabled() const
Get enabled value.
Definition: SwitchInterface.cpp:83
fawkes::PanTiltInterface::set_tilt_velocity
void set_tilt_velocity(const float new_tilt_velocity)
Set tilt_velocity value.
Definition: PanTiltInterface.cpp:587
fawkes::PanTiltInterface::SetVelocityMessage::tilt_velocity
float tilt_velocity() const
Get tilt_velocity value.
Definition: PanTiltInterface.cpp:1353
fawkes::CameraControlInterface::SetZoomMessage::zoom
uint32_t zoom() const
Get zoom value.
Definition: CameraControlInterface.cpp:540
Visca::VISCA_ZOOM_VALUE_WIDE
static const unsigned int VISCA_ZOOM_VALUE_WIDE
Zoom value: wide.
Definition: visca.h:68
Visca::set_mirror
void set_mirror(bool mirror)
Sett mirror sate.
Definition: visca.cpp:1240
PanTiltSonyEviD100PThread::bb_interface_message_received
virtual bool bb_interface_message_received(fawkes::Interface *interface, fawkes::Message *message)
BlackBoard message received notification.
Definition: evid100p_thread.cpp:338
fawkes::Interface::msgq_first
Message * msgq_first()
Get the first message from the message queue.
Definition: interface.cpp:1167
fawkes::PanTiltInterface::set_min_pan
void set_min_pan(const float new_min_pan)
Set min_pan value.
Definition: PanTiltInterface.cpp:377
fawkes::Interface::write
void write()
Write from local copy into BlackBoard memory.
Definition: interface.cpp:494
fawkes::PanTiltInterface::set_final
void set_final(const bool new_final)
Set final value.
Definition: PanTiltInterface.cpp:253
fawkes::BlackBoard::open_for_writing
virtual Interface * open_for_writing(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for writing.
fawkes::CameraControlInterface::Effect
Effect
Enumeration defining the possible effects.
Definition: CameraControlInterface.h:45
PanTiltSonyEviD100PThread::update_sensor_values
void update_sensor_values()
Update sensor values as necessary.
Definition: evid100p_thread.cpp:204
PanTiltSonyEviD100PThread::PanTiltSonyEviD100PThread
PanTiltSonyEviD100PThread(std::string &pantilt_cfg_prefix, std::string &ptu_cfg_prefix, std::string &ptu_name)
Constructor.
Definition: evid100p_thread.cpp:48
fawkes::Thread::set_name
void set_name(const char *format,...)
Set name of thread.
Definition: thread.cpp:748
Visca::VISCA_ZOOM_VALUE_8X
static const unsigned int VISCA_ZOOM_VALUE_8X
Zoom value: 8x.
Definition: visca.h:84
fawkes::PanTiltInterface::set_tilt
void set_tilt(const float new_tilt)
Set tilt value.
Definition: PanTiltInterface.cpp:189
fawkes::CameraControlInterface::set_zoom
void set_zoom(const uint32_t new_zoom)
Set zoom value.
Definition: CameraControlInterface.cpp:179
PanTiltSonyEviD100PThread::finalize
virtual void finalize()
Finalize the thread.
Definition: evid100p_thread.cpp:174
fawkes::PanTiltInterface::set_max_tilt_velocity
void set_max_tilt_velocity(const float new_max_tilt_velocity)
Set max_tilt_velocity value.
Definition: PanTiltInterface.cpp:527
fawkes::CameraControlInterface::set_mirror_supported
void set_mirror_supported(const bool new_mirror_supported)
Set mirror_supported value.
Definition: CameraControlInterface.cpp:329
fawkes::MultiLogger::log_info
virtual void log_info(const char *component, const char *format,...)
Log informational message.
Definition: multi.cpp:195
fawkes::Exception
Base class for exceptions in Fawkes.
Definition: exception.h:36