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();
373PanTiltSonyEviD100PThread::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_;
420PanTiltSonyEviD100PThread::WorkerThread::~WorkerThread()
424 delete effect_mutex_;
425 delete mirror_mutex_;
431PanTiltSonyEviD100PThread::WorkerThread::stop_motion()
434 float pan = 0, tilt = 0;
435 get_pantilt(pan, tilt);
436 goto_pantilt(pan, tilt);
445PanTiltSonyEviD100PThread::WorkerThread::goto_pantilt(
float pan,
float tilt)
450 move_pending_ =
true;
459PanTiltSonyEviD100PThread::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;
504PanTiltSonyEviD100PThread::WorkerThread::set_mirror(
bool enabled)
507 target_mirror_ = enabled;
508 mirror_pending_ =
true;
517PanTiltSonyEviD100PThread::WorkerThread::set_power(
bool powered)
520 power_desired_ = powered;
521 power_pending_ =
true;
530PanTiltSonyEviD100PThread::WorkerThread::get_pantilt(
float &pan,
float &tilt)
540PanTiltSonyEviD100PThread::WorkerThread::get_zoom()
565PanTiltSonyEviD100PThread::WorkerThread::set_velocities(
float pan_vel,
float tilt_vel)
568 tilt_vel_ = tilt_vel;
569 velo_pending_ =
true;
576PanTiltSonyEviD100PThread::WorkerThread::is_final()
588PanTiltSonyEviD100PThread::WorkerThread::has_fresh_data()
590 bool rv = fresh_data_;
596PanTiltSonyEviD100PThread::WorkerThread::once()
600 for (
int i = 0; i < 20; ++i) {
609PanTiltSonyEviD100PThread::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_)) {
708PanTiltSonyEviD100PThread::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);
740PanTiltSonyEviD100PThread::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);
757PanTiltSonyEviD100PThread::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);
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.
static const float MIN_TILT_RAD
Min tilt in rad.
static const float MAX_TILT_RAD
Max tilt in rad.
static const float MIN_PAN_RAD
Min pan in rad.
void get_speed_limits(float &pan_min, float &pan_max, float &tilt_min, float &tilt_max)
Get speed limits.
static const float MAX_PAN_RAD
Max pan in rad.
void set_mirror(bool mirror)
Sett mirror sate.
static const unsigned int VISCA_ZOOM_VALUE_1X
Zoom value: 1x.
static const unsigned int VISCA_ZOOM_VALUE_8X
Zoom value: 8x.
static const unsigned int NONBLOCKING_ZOOM
Non-blocking zoom item.
static const unsigned int VISCA_ZOOM_VALUE_10X
Zoom value: 10x.
bool is_powered()
Check if camera is powered.
static const unsigned int VISCA_ZOOM_VALUE_WIDE
Zoom value: wide.
static const unsigned int VISCA_ZOOM_VALUE_2X
Zoom value: 2x.
static const unsigned int VISCA_ZOOM_VALUE_DIG_40X
Zoom value: 40x.
static const unsigned int VISCA_ZOOM_VALUE_4X
Zoom value: 4x.
static const unsigned int VISCA_ZOOM_VALUE_5X
Zoom value: 5x.
static const unsigned int VISCA_ZOOM_VALUE_9X
Zoom value: 9x.
static const unsigned int VISCA_ZOOM_VALUE_3X
Zoom value: 3x.
static const unsigned int VISCA_ZOOM_VALUE_7X
Zoom value: 7x.
void set_power(bool powered)
Set power state.
static const unsigned int VISCA_ZOOM_VALUE_6X
Zoom value: 6x.
static const unsigned int NONBLOCKING_PANTILT
Non-blocking pan/tilt item.
static const unsigned int VISCA_ZOOM_VALUE_DIG_20X
Zoom value: 20x.
static const unsigned int VISCA_ZOOM_VALUE_DIG_30X
Zoom value: 30x.
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
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.
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.
virtual void close(Interface *interface)=0
Close interface.
SetEffectMessage Fawkes BlackBoard Interface Message.
Effect effect() const
Get effect value.
SetMirrorMessage Fawkes BlackBoard Interface Message.
bool is_mirror() const
Get mirror value.
SetZoomMessage Fawkes BlackBoard Interface Message.
uint32_t zoom() const
Get zoom value.
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.
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.
void print_trace() noexcept
Prints trace to stderr.
void print_backtrace() const noexcept
Prints a backtrace.
Base class for all Fawkes BlackBoard interfaces.
bool msgq_first_is()
Check if first message has desired type.
void msgq_pop()
Erase first message from queue.
Message * msgq_first()
Get the first message from the message queue.
void write()
Write from local copy into BlackBoard memory.
bool msgq_empty()
Check if queue is empty.
JointInterface Fawkes BlackBoard Interface.
void set_position(const float new_position)
Set position value.
void set_velocity(const float new_velocity)
Set velocity value.
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.
Base class for all messages passed through interfaces in Fawkes BlackBoard.
const char * type() const
Get message type.
unsigned int id() const
Get message ID.
virtual void log_info(const char *component, const char *format,...)
Log informational message.
Mutex mutual exclusion lock.
CalibrateMessage Fawkes BlackBoard Interface Message.
FlushMessage Fawkes BlackBoard Interface Message.
GotoMessage Fawkes BlackBoard Interface Message.
float pan() const
Get pan value.
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.
const char * name() const
Get name of thread.
void set_name(const char *format,...)
Set name of thread.
void set_coalesce_wakeups(bool coalesce=true)
Set wakeup coalescing.
Fawkes library namespace.