22 #include "evid100p_thread.h" 25 #include <core/threading/mutex_locker.h> 26 #include <interfaces/PanTiltInterface.h> 27 #include <interfaces/JointInterface.h> 28 #include <interfaces/SwitchInterface.h> 48 std::string &ptu_cfg_prefix,
49 std::string &ptu_name)
53 set_name(
"PanTiltSonyEviD100PThread(%s)", ptu_name.c_str());
55 __pantilt_cfg_prefix = pantilt_cfg_prefix;
56 __ptu_cfg_prefix = ptu_cfg_prefix;
57 __ptu_name = ptu_name;
71 __cfg_read_timeout_ms =
config->
get_uint((__ptu_cfg_prefix +
"read_timeout_ms").c_str());
84 power_up =
config->
get_bool((__ptu_cfg_prefix +
"power-up").c_str());
89 float init_tilt = 0.f;
90 float init_pan_velocity = 0.f;
91 float init_tilt_velocity = 0.f;
94 std::string bbid =
"PanTilt " + __ptu_name;
103 float pan_smin, pan_smax, tilt_smin, tilt_smax;
109 __pantilt_if->
write();
111 std::string panid = __ptu_name +
" pan";
115 __panjoint_if->
write();
117 std::string tiltid = __ptu_name +
" tilt";
121 __tiltjoint_if->
write();
124 __camctrl_if->
set_effect(CameraControlInterface::EFF_NONE);
144 __camctrl_if->
write();
146 __wt =
new WorkerThread(__ptu_name,
logger, __cam,
153 __wt->set_velocities(pan_smax, tilt_smax);
158 #ifdef USE_TIMETRACKER 161 __ttc_read_sensor = __tt->add_class(
"Read Sensor");
181 bool power_down =
true;
183 power_down =
config->
get_bool((__ptu_cfg_prefix +
"power-down").c_str());
199 if (__wt->has_fresh_data()) {
200 float pan = 0, tilt = 0;
201 __wt->get_pantilt(pan, tilt);
204 __pantilt_if->
set_final(__wt->is_final());
205 __pantilt_if->
write();
208 __panjoint_if->
write();
211 __tiltjoint_if->
write();
213 unsigned int zoom = __wt->get_zoom();
214 if (__camctrl_if->
zoom() != zoom) {
216 __camctrl_if->
write();
225 __pantilt_if->
set_final(__wt->is_final());
234 __wt->goto_pantilt(msg->
pan(), msg->
tilt());
241 __wt->goto_pantilt(0, 0);
267 __panjoint_if->
write();
269 __tiltjoint_if->
write();
278 __pantilt_if->
write();
285 __camctrl_if->
write();
288 __wt->set_effect(msg->
effect());
290 __camctrl_if->
write();
293 __wt->set_zoom(msg->
zoom());
304 __wt->set_mirror(__camctrl_if->
is_mirror());
305 __wt->set_effect(__camctrl_if->
effect());
306 __wt->set_power(
true);
310 __wt->set_power(
false);
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)
374 set_name(
"SonyEviD100PWorkerThread(%s)", ptu_name.c_str());
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();
386 __move_pending =
false;
389 __fresh_data =
false;
391 __velo_pending =
false;
397 __tilt_min = tilt_min;
398 __tilt_max = tilt_max;
400 __zoom_pending =
false;
403 __mirror_pending =
false;
404 __power_pending =
false;
405 __effect_pending =
false;
412 PanTiltSonyEviD100PThread::WorkerThread::~WorkerThread()
416 delete __effect_mutex;
417 delete __mirror_mutex;
418 delete __power_mutex;
424 PanTiltSonyEviD100PThread::WorkerThread::stop_motion()
427 float pan = 0, tilt = 0;
428 get_pantilt(pan, tilt);
429 goto_pantilt(pan, tilt);
439 PanTiltSonyEviD100PThread::WorkerThread::goto_pantilt(
float pan,
float tilt)
443 __target_tilt = tilt;
444 __move_pending =
true;
452 PanTiltSonyEviD100PThread::WorkerThread::set_zoom(
unsigned int zoom_value)
455 __zoom_pending =
true;
457 switch (zoom_value) {
473 __logger->log_warn(
name(),
"Illegal zoom value %u ignored", zoom_value);
474 __zoom_pending =
false;
487 __target_effect = effect;
488 __effect_pending =
true;
496 PanTiltSonyEviD100PThread::WorkerThread::set_mirror(
bool enabled)
499 __target_mirror = enabled;
500 __mirror_pending =
true;
509 PanTiltSonyEviD100PThread::WorkerThread::set_power(
bool powered)
512 __power_desired = powered;
513 __power_pending =
true;
522 PanTiltSonyEviD100PThread::WorkerThread::get_pantilt(
float &pan,
float &tilt)
533 PanTiltSonyEviD100PThread::WorkerThread::get_zoom()
535 switch (__cur_zoom) {
559 PanTiltSonyEviD100PThread::WorkerThread::set_velocities(
float pan_vel,
float tilt_vel)
562 __tilt_vel = tilt_vel;
563 __velo_pending =
true;
571 PanTiltSonyEviD100PThread::WorkerThread::is_final()
584 PanTiltSonyEviD100PThread::WorkerThread::has_fresh_data()
586 bool rv = __fresh_data;
587 __fresh_data =
false;
593 PanTiltSonyEviD100PThread::WorkerThread::once()
597 for (
int i = 0; i < 20; ++i) {
606 PanTiltSonyEviD100PThread::WorkerThread::loop()
611 __logger->log_warn(
name(),
"Data processing failed, exception follows");
612 __logger->log_warn(
name(), e);
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;
620 __powered = __power_desired;
621 __power_mutex->unlock();
624 if (__velo_pending) {
628 __logger->log_warn(
name(),
"Setting pan/tilt values failed, exception follows");
629 __logger->log_warn(
name(), e);
631 __velo_pending =
false;
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();
642 if (__zoom_pending) {
643 __zoom_mutex->lock();
644 if (__powered) exec_set_zoom(__target_zoom);
645 __zoom_pending =
false;
646 __zoom_mutex->unlock();
649 if (__effect_pending) {
650 __effect_mutex->lock();
651 if (__powered) exec_set_effect(__target_effect);
652 __effect_pending =
false;
653 __effect_mutex->unlock();
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();
671 __logger->log_warn(
name(),
"Failed to get new pan/tilt data, exception follows");
672 __logger->log_warn(
name(), e);
677 unsigned int new_zoom = 0;
679 if (new_zoom != __cur_zoom) {
680 __cur_zoom = new_zoom;
685 __logger->log_warn(
name(),
"Failed to get new zoom data, exception follows");
686 __logger->log_warn(
name(), e);
689 if (__powered && (! is_final() || ! __fresh_data)) {
701 PanTiltSonyEviD100PThread::WorkerThread::exec_goto_pantilt(
float pan_rad,
float tilt_rad)
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);
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);
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);
727 PanTiltSonyEviD100PThread::WorkerThread::exec_set_zoom(
unsigned int zoom)
732 __logger->log_warn(
name(),
"Failed to execute zoom to %u, exception " 734 __logger->log_warn(
name(), e);
742 PanTiltSonyEviD100PThread::WorkerThread::exec_set_mirror(
bool mirror)
747 __logger->log_warn(
name(),
"Failed to %sabling mirror mod, exception follows",
748 mirror ?
"En" :
"Dis");
749 __logger->log_warn(
name(), e);
762 case CameraControlInterface::EFF_NEGATIVE:
764 case CameraControlInterface::EFF_PASTEL:
766 case CameraControlInterface::EFF_BW:
768 case CameraControlInterface::EFF_SOLARIZE:
774 __logger->log_warn(
name(),
"Failed to set effect, exception follows");
775 __logger->log_warn(
name(), e);
static const unsigned int VISCA_ZOOM_VALUE_WIDE
Zoom value: wide.
Thread(const char *name)
Constructor.
static const unsigned int VISCA_ZOOM_VALUE_DIG_30X
Zoom value: 30x.
static const unsigned int VISCA_ZOOM_VALUE_3X
Zoom value: 3x.
static const float MIN_TILT_RAD
Min tilt in rad.
bool is_mirror() const
Get mirror value.
static const unsigned int NONBLOCKING_PANTILT
Non-blocking pan/tilt item.
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.
Base class for all messages passed through interfaces in Fawkes BlackBoard.
unsigned int id() const
Get message ID.
void apply_effect_solarize()
Apply solarize effect.
SetMessage Fawkes BlackBoard Interface Message.
bool msgq_empty()
Check if queue is empty.
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.
void set_effect(const Effect new_effect)
Set effect value.
static const float MIN_PAN_RAD
Min pan in rad.
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.
void set_pan_velocity(const float new_pan_velocity)
Set pan_velocity value.
void apply_effect_pastel()
Apply pastel effect.
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.
void set_msgid(const uint32_t new_msgid)
Set msgid value.
float tilt_velocity() const
Get tilt_velocity value.
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.
static const unsigned int NONBLOCKING_ZOOM
Non-blocking zoom item.
float value() const
Get value value.
virtual void unregister_listener(BlackBoardInterfaceListener *listener)
Unregister BB interface listener.
bool is_powered()
Check if camera is powered.
void write()
Write from local copy into BlackBoard memory.
void reset_effect()
Reset effects.
CameraControlInterface Fawkes BlackBoard Interface.
Base class for all Fawkes BlackBoard interfaces.
Logger * logger
This is the Logger member used to access the logger.
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.
void set_pan_tilt_rad(float pan, float tilt)
Set pan/tilt in radians.
void set_min_pan(const float new_min_pan)
Set min_pan value.
static const unsigned int VISCA_ZOOM_VALUE_9X
Zoom value: 9x.
static const unsigned int VISCA_ZOOM_VALUE_6X
Zoom value: 6x.
void get_speed_limits(float &pan_min, float &pan_max, float &tilt_min, float &tilt_max)
Get speed limits.
virtual void register_listener(BlackBoardInterfaceListener *listener, ListenerRegisterFlag flag=BBIL_FLAG_ALL)
Register BB event listener.
void msgq_pop()
Erase first message from queue.
SwitchInterface Fawkes BlackBoard Interface.
SetEffectMessage Fawkes BlackBoard Interface Message.
void wakeup()
Wake up thread.
ParkMessage Fawkes BlackBoard Interface Message.
void set_name(const char *format,...)
Set name of thread.
Base class for exceptions in Fawkes.
Message * msgq_first()
Get the first message from the message queue.
void set_effect_supported(const bool new_effect_supported)
Set effect_supported value.
uint32_t zoom() const
Get zoom 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.
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.
float max_pan_velocity() const
Get max_pan_velocity value.
bool is_nonblocking_finished(unsigned int item) const
Check if a non-blocking operation has been finished.
DisableSwitchMessage Fawkes BlackBoard Interface Message.
void set_mirror(const bool new_mirror)
Set mirror value.
const char * name() const
Get name of thread.
bool msgq_first_is()
Check if first message has desired type.
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.
bool is_enabled() const
Get enabled value.
void set_calibrated(const bool new_calibrated)
Set calibrated value.
GotoMessage Fawkes BlackBoard Interface Message.
Effect
Enumeration defining the possible effects.
void set_coalesce_wakeups(bool coalesce=true)
Set wakeup coalescing.
Effect effect() const
Get effect value.
static const unsigned int VISCA_ZOOM_VALUE_10X
Zoom value: 10x.
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.
void print_trace()
Prints trace to stderr.
void apply_effect_neg_art()
Apply negative art effect.
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.
void set_pan(const float new_pan)
Set pan value.
static const unsigned int VISCA_ZOOM_VALUE_5X
Zoom value: 5x.
CalibrateMessage Fawkes BlackBoard Interface Message.
static const float MAX_TILT_RAD
Max tilt in rad.
Sony EviD100P Visca controller.
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.
static const unsigned int VISCA_ZOOM_VALUE_8X
Zoom value: 8x.
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.
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.
void set_enabled(const bool new_enabled)
Set enabled value.
Mutex mutual exclusion lock.
void set_tilt_velocity(const float new_tilt_velocity)
Set tilt_velocity value.
void apply_effect_bnw()
Apply B/W effect.
Configuration * config
This is the Configuration member used to access the configuration.
void bbil_add_message_interface(Interface *interface)
Add an interface to the message received watch list.
void set_mirror(bool mirror)
Sett mirror sate.
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.
bool is_mirror() const
Get mirror value.
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.
void process()
Process incoming data.
virtual void close(Interface *interface)=0
Close interface.