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