22 #include "evid100p_thread.h"
26 #include <core/threading/mutex_locker.h>
27 #include <interfaces/JointInterface.h>
28 #include <interfaces/PanTiltInterface.h>
29 #include <interfaces/SwitchInterface.h>
49 std::string &ptu_cfg_prefix,
50 std::string &ptu_name)
54 set_name(
"PanTiltSonyEviD100PThread(%s)", ptu_name.c_str());
56 pantilt_cfg_prefix_ = pantilt_cfg_prefix;
57 ptu_cfg_prefix_ = ptu_cfg_prefix;
71 cfg_read_timeout_ms_ =
config->
get_uint((ptu_cfg_prefix_ +
"read_timeout_ms").c_str());
85 power_up =
config->
get_bool((ptu_cfg_prefix_ +
"power-up").c_str());
92 float init_tilt = 0.f;
93 float init_pan_velocity = 0.f;
94 float init_tilt_velocity = 0.f;
97 std::string bbid =
"PanTilt " + ptu_name_;
106 float pan_smin, pan_smax, tilt_smin, tilt_smax;
112 pantilt_if_->
write();
114 std::string panid = ptu_name_ +
" pan";
118 panjoint_if_->
write();
120 std::string tiltid = ptu_name_ +
" tilt";
124 tiltjoint_if_->
write();
127 camctrl_if_->
set_effect(CameraControlInterface::EFF_NONE);
148 camctrl_if_->
write();
150 wt_ =
new WorkerThread(ptu_name_,
161 wt_->set_velocities(pan_smax, tilt_smax);
166 #ifdef USE_TIMETRACKER
169 ttc_read_sensor_ = tt_->add_class(
"Read Sensor");
187 bool power_down =
true;
189 power_down =
config->
get_bool((ptu_cfg_prefix_ +
"power-down").c_str());
206 if (wt_->has_fresh_data()) {
207 float pan = 0, tilt = 0;
208 wt_->get_pantilt(pan, tilt);
212 pantilt_if_->
write();
215 panjoint_if_->
write();
218 tiltjoint_if_->
write();
220 unsigned int zoom = wt_->get_zoom();
221 if (camctrl_if_->
zoom() != zoom) {
223 camctrl_if_->
write();
240 wt_->goto_pantilt(msg->
pan(), msg->
tilt());
247 wt_->goto_pantilt(0, 0);
261 "Ignoring pan/tilt velocities %f/%f, at least one "
267 "Desired pan velocity %f too high, max is %f",
272 "Desired tilt velocity %f too high, max is %f",
280 panjoint_if_->
write();
282 tiltjoint_if_->
write();
291 pantilt_if_->
write();
298 camctrl_if_->
write();
301 wt_->set_effect(msg->
effect());
303 camctrl_if_->
write();
306 wt_->set_zoom(msg->
zoom());
316 wt_->set_mirror(camctrl_if_->
is_mirror());
317 wt_->set_effect(camctrl_if_->
effect());
318 wt_->set_power(
true);
322 wt_->set_power(
false);
346 logger->
log_info(name(),
"Flushing message queue");
347 pantilt_if_->msgq_flush();
373 PanTiltSonyEviD100PThread::WorkerThread::WorkerThread(std::string ptu_name,
376 const float & pan_min,
377 const float & pan_max,
378 const float & tilt_min,
379 const float & tilt_max)
382 set_name(
"SonyEviD100PWorkerThread(%s)", ptu_name.c_str());
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();
394 move_pending_ =
false;
399 velo_pending_ =
false;
405 tilt_min_ = tilt_min;
406 tilt_max_ = tilt_max;
408 zoom_pending_ =
false;
411 mirror_pending_ =
false;
412 power_pending_ =
false;
413 effect_pending_ =
false;
416 power_desired_ = powered_;
420 PanTiltSonyEviD100PThread::WorkerThread::~WorkerThread()
424 delete effect_mutex_;
425 delete mirror_mutex_;
431 PanTiltSonyEviD100PThread::WorkerThread::stop_motion()
434 float pan = 0, tilt = 0;
435 get_pantilt(pan, tilt);
436 goto_pantilt(pan, tilt);
445 PanTiltSonyEviD100PThread::WorkerThread::goto_pantilt(
float pan,
float tilt)
450 move_pending_ =
true;
459 PanTiltSonyEviD100PThread::WorkerThread::set_zoom(
unsigned int zoom_value)
462 zoom_pending_ =
true;
464 switch (zoom_value) {
480 logger_->log_warn(name(),
"Illegal zoom value %u ignored", zoom_value);
481 zoom_pending_ =
false;
494 target_effect_ = effect;
495 effect_pending_ =
true;
504 PanTiltSonyEviD100PThread::WorkerThread::set_mirror(
bool enabled)
507 target_mirror_ = enabled;
508 mirror_pending_ =
true;
517 PanTiltSonyEviD100PThread::WorkerThread::set_power(
bool powered)
520 power_desired_ = powered;
521 power_pending_ =
true;
530 PanTiltSonyEviD100PThread::WorkerThread::get_pantilt(
float &pan,
float &tilt)
540 PanTiltSonyEviD100PThread::WorkerThread::get_zoom()
565 PanTiltSonyEviD100PThread::WorkerThread::set_velocities(
float pan_vel,
float tilt_vel)
568 tilt_vel_ = tilt_vel;
569 velo_pending_ =
true;
576 PanTiltSonyEviD100PThread::WorkerThread::is_final()
588 PanTiltSonyEviD100PThread::WorkerThread::has_fresh_data()
590 bool rv = fresh_data_;
596 PanTiltSonyEviD100PThread::WorkerThread::once()
600 for (
int i = 0; i < 20; ++i) {
609 PanTiltSonyEviD100PThread::WorkerThread::loop()
614 logger_->log_warn(name(),
"Data processing failed, exception follows");
615 logger_->log_warn(name(), e);
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();
630 cam_->set_speed_radsec(pan_vel_, tilt_vel_);
632 logger_->log_warn(name(),
"Setting pan/tilt values failed, exception follows");
633 logger_->log_warn(name(), e);
635 velo_pending_ =
false;
640 logger_->log_debug(name(),
"Executing goto to %f, %f", target_pan_, target_tilt_);
642 exec_goto_pantilt(target_pan_, target_tilt_);
643 move_pending_ =
false;
644 move_mutex_->unlock();
650 exec_set_zoom(target_zoom_);
651 zoom_pending_ =
false;
652 zoom_mutex_->unlock();
655 if (effect_pending_) {
656 effect_mutex_->lock();
658 exec_set_effect(target_effect_);
659 effect_pending_ =
false;
660 effect_mutex_->unlock();
663 if (mirror_pending_) {
664 mirror_mutex_->lock();
665 logger_->log_debug(name(),
"%sabling mirroring", target_mirror_ ?
"En" :
"Dis");
667 exec_set_mirror(target_mirror_);
668 mirror_pending_ =
false;
669 mirror_mutex_->unlock();
675 cam_->get_pan_tilt_rad(cur_pan_, cur_tilt_);
679 logger_->log_warn(name(),
"Failed to get new pan/tilt data, exception follows");
680 logger_->log_warn(name(), e);
685 unsigned int new_zoom = 0;
686 cam_->get_zoom(new_zoom);
687 if (new_zoom != cur_zoom_) {
688 cur_zoom_ = new_zoom;
693 logger_->log_warn(name(),
"Failed to get new zoom data, exception follows");
694 logger_->log_warn(name(), e);
697 if (powered_ && (!is_final() || !fresh_data_)) {
708 PanTiltSonyEviD100PThread::WorkerThread::exec_goto_pantilt(
float pan_rad,
float tilt_rad)
710 if ((pan_rad < pan_min_) || (pan_rad > pan_max_)) {
712 name(),
"Pan value out of bounds, min: %f max: %f des: %f", pan_min_, pan_max_, pan_rad);
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",
725 cam_->set_pan_tilt_rad(pan_rad, tilt_rad);
727 logger_->log_warn(name(),
728 "Failed to execute pan/tilt to %f, %f, exception "
732 logger_->log_warn(name(), e);
740 PanTiltSonyEviD100PThread::WorkerThread::exec_set_zoom(
unsigned int zoom)
743 cam_->set_zoom(zoom);
745 logger_->log_warn(name(),
746 "Failed to execute zoom to %u, exception "
749 logger_->log_warn(name(), e);
757 PanTiltSonyEviD100PThread::WorkerThread::exec_set_mirror(
bool mirror)
760 cam_->set_mirror(mirror);
762 logger_->log_warn(name(),
763 "Failed to %sabling mirror mod, exception follows",
764 mirror ?
"En" :
"Dis");
765 logger_->log_warn(name(), e);
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;
784 logger_->log_warn(name(),
"Failed to set effect, exception follows");
785 logger_->log_warn(name(), e);