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
34using 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
63void
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();
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
173void
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 */
203void
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
228void
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)) {
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()) {
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()) {
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()) {
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>()) {
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
337bool
339 Message * message) noexcept
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 */
373PanTiltSonyEviD100PThread::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());
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. */
420PanTiltSonyEviD100PThread::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. */
430void
431PanTiltSonyEviD100PThread::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 */
444void
445PanTiltSonyEviD100PThread::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 */
458void
459PanTiltSonyEviD100PThread::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 */
490void
491PanTiltSonyEviD100PThread::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 */
503void
504PanTiltSonyEviD100PThread::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 */
516void
517PanTiltSonyEviD100PThread::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 */
529void
530PanTiltSonyEviD100PThread::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 */
539unsigned int
540PanTiltSonyEviD100PThread::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 */
564void
565PanTiltSonyEviD100PThread::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 */
575bool
576PanTiltSonyEviD100PThread::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 */
587bool
588PanTiltSonyEviD100PThread::WorkerThread::has_fresh_data()
589{
590 bool rv = fresh_data_;
591 fresh_data_ = false;
592 return rv;
593}
594
595void
596PanTiltSonyEviD100PThread::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
608void
609PanTiltSonyEviD100PThread::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 */
707void
708PanTiltSonyEviD100PThread::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 */
739void
740PanTiltSonyEviD100PThread::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 */
756void
757PanTiltSonyEviD100PThread::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 */
772void
773PanTiltSonyEviD100PThread::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 void finalize()
Finalize the thread.
virtual void loop()
Code to execute in the thread.
virtual bool bb_interface_message_received(fawkes::Interface *interface, fawkes::Message *message) noexcept
BlackBoard message received notification.
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_trace() noexcept
Prints trace to stderr.
Definition: exception.cpp:601
void print_backtrace() const noexcept
Prints a backtrace.
Definition: exception.cpp:547
Base class for all Fawkes BlackBoard interfaces.
Definition: interface.h:80
bool msgq_first_is()
Check if first message has desired type.
Definition: interface.h:351
void msgq_pop()
Erase first message from queue.
Definition: interface.cpp:1215
Message * msgq_first()
Get the first message from the message queue.
Definition: interface.cpp:1200
void write()
Write from local copy into BlackBoard memory.
Definition: interface.cpp:501
bool msgq_empty()
Check if queue is empty.
Definition: interface.cpp:1062
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:44
const char * type() const
Get message type.
Definition: message.cpp:381
unsigned int id() const
Get message ID.
Definition: message.cpp:181
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.