Fawkes API
Fawkes Development Version
|
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);
float max_tilt_velocity() const
Get max_tilt_velocity value.
static const unsigned int VISCA_ZOOM_VALUE_DIG_40X
Zoom value: 40x.
Effect effect() const
Get effect value.
void print_backtrace() const
Prints a backtrace.
bool msgq_first_is()
Check if first message has desired type.
static const unsigned int VISCA_ZOOM_VALUE_DIG_30X
Zoom value: 30x.
void set_max_tilt(const float new_max_tilt)
Set max_tilt value.
void msgq_pop()
Erase first message from queue.
static const float MIN_PAN_RAD
Min pan in rad.
SetMessage Fawkes BlackBoard Interface Message.
static const unsigned int VISCA_ZOOM_VALUE_3X
Zoom value: 3x.
bool msgq_empty()
Check if queue is empty.
static const float MIN_TILT_RAD
Min tilt in rad.
void set_min_tilt(const float new_min_tilt)
Set min_tilt value.
virtual void register_listener(BlackBoardInterfaceListener *listener, ListenerRegisterFlag flag=BBIL_FLAG_ALL)
Register BB event listener.
StopMessage Fawkes BlackBoard Interface Message.
static const unsigned int NONBLOCKING_PANTILT
Non-blocking pan/tilt item.
Mutex mutual exclusion lock.
SwitchInterface Fawkes BlackBoard Interface.
float pan_velocity() const
Get pan_velocity value.
PanTiltInterface Fawkes BlackBoard Interface.
Base class for all messages passed through interfaces in Fawkes BlackBoard.
void set_calibrated(const bool new_calibrated)
Set calibrated value.
bool is_mirror() const
Get mirror value.
static const unsigned int VISCA_ZOOM_VALUE_7X
Zoom value: 7x.
static const unsigned int NONBLOCKING_ZOOM
Non-blocking zoom item.
SetEffectMessage Fawkes BlackBoard Interface Message.
static const float MAX_PAN_RAD
Max pan in rad.
virtual bool get_bool(const char *path)=0
Get value from configuration which is of type bool.
void set_position(const float new_position)
Set position value.
void set_zoom_min(const uint32_t new_zoom_min)
Set zoom_min value.
void set_power(bool powered)
Set power state.
SetMirrorMessage Fawkes BlackBoard Interface Message.
bool is_powered()
Check if camera is powered.
Sony EviD100P Visca controller.
virtual void unregister_listener(BlackBoardInterfaceListener *listener)
Unregister BB interface listener.
BlackBoard interface listener.
virtual void init()
Initialize the thread.
DisableSwitchMessage Fawkes BlackBoard Interface Message.
const char * name() const
Get name of thread.
void set_velocity(const float new_velocity)
Set velocity value.
bool is_mirror() const
Get mirror value.
SetZoomMessage Fawkes BlackBoard Interface Message.
static const unsigned int VISCA_ZOOM_VALUE_9X
Zoom value: 9x.
Effect effect() const
Get effect value.
void set_max_pan_velocity(const float new_max_pan_velocity)
Set max_pan_velocity value.
void set_mirror(const bool new_mirror)
Set mirror value.
void set_zoom_supported(const bool new_zoom_supported)
Set zoom_supported value.
bool is_enabled() const
Get enabled value.
virtual void loop()
Code to execute in the thread.
SetVelocityMessage Fawkes BlackBoard Interface Message.
static const unsigned int VISCA_ZOOM_VALUE_6X
Zoom value: 6x.
EnableSwitchMessage Fawkes BlackBoard Interface Message.
float tilt() const
Get tilt value.
uint32_t zoom() const
Get zoom value.
Logger * logger
This is the Logger member used to access the logger.
void set_max_pan(const float new_max_pan)
Set max_pan value.
virtual void close(Interface *interface)=0
Close interface.
void set_coalesce_wakeups(bool coalesce=true)
Set wakeup coalescing.
void set_enabled(const bool new_enabled)
Set enabled value.
GotoMessage Fawkes BlackBoard Interface Message.
Fawkes library namespace.
void get_speed_limits(float &pan_min, float &pan_max, float &tilt_min, float &tilt_max)
Get speed limits.
void set_enabled(const bool new_enabled)
Set enabled value.
void set_pan_velocity(const float new_pan_velocity)
Set pan_velocity value.
void set_pan(const float new_pan)
Set pan value.
float value() const
Get value value.
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
ParkMessage Fawkes BlackBoard Interface Message.
void bbil_add_message_interface(Interface *interface)
Add an interface to the message received watch list.
Base class for all Fawkes BlackBoard interfaces.
static const float MAX_TILT_RAD
Max tilt in rad.
void set_msgid(const uint32_t new_msgid)
Set msgid value.
void print_trace()
Prints trace to stderr.
FlushMessage Fawkes BlackBoard Interface Message.
SetEnabledMessage Fawkes BlackBoard Interface Message.
Configuration * config
This is the Configuration member used to access the configuration.
void set_zoom_max(const uint32_t new_zoom_max)
Set zoom_max value.
const char * type() const
Get message type.
JointInterface Fawkes BlackBoard Interface.
static const unsigned int VISCA_ZOOM_VALUE_DIG_20X
Zoom value: 20x.
void set_effect_supported(const bool new_effect_supported)
Set effect_supported value.
static const unsigned int VISCA_ZOOM_VALUE_10X
Zoom value: 10x.
CalibrateMessage Fawkes BlackBoard Interface Message.
static const unsigned int VISCA_ZOOM_VALUE_2X
Zoom value: 2x.
float pan() const
Get pan value.
CameraControlInterface Fawkes BlackBoard Interface.
static const unsigned int VISCA_ZOOM_VALUE_1X
Zoom value: 1x.
Thread class encapsulation of pthreads.
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
static const unsigned int VISCA_ZOOM_VALUE_5X
Zoom value: 5x.
static const unsigned int VISCA_ZOOM_VALUE_4X
Zoom value: 4x.
virtual unsigned int get_uint(const char *path)=0
Get value from configuration which is of type unsigned int.
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
unsigned int id() const
Get message ID.
void set_effect(const Effect new_effect)
Set effect value.
float max_pan_velocity() const
Get max_pan_velocity value.
bool is_enabled() const
Get enabled value.
void set_tilt_velocity(const float new_tilt_velocity)
Set tilt_velocity value.
float tilt_velocity() const
Get tilt_velocity value.
uint32_t zoom() const
Get zoom value.
static const unsigned int VISCA_ZOOM_VALUE_WIDE
Zoom value: wide.
void set_mirror(bool mirror)
Sett mirror sate.
virtual bool bb_interface_message_received(fawkes::Interface *interface, fawkes::Message *message)
BlackBoard message received notification.
Message * msgq_first()
Get the first message from the message queue.
void set_min_pan(const float new_min_pan)
Set min_pan value.
void write()
Write from local copy into BlackBoard memory.
void set_final(const bool new_final)
Set final value.
virtual Interface * open_for_writing(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for writing.
Effect
Enumeration defining the possible effects.
void update_sensor_values()
Update sensor values as necessary.
PanTiltSonyEviD100PThread(std::string &pantilt_cfg_prefix, std::string &ptu_cfg_prefix, std::string &ptu_name)
Constructor.
void set_name(const char *format,...)
Set name of thread.
static const unsigned int VISCA_ZOOM_VALUE_8X
Zoom value: 8x.
void set_tilt(const float new_tilt)
Set tilt value.
void set_zoom(const uint32_t new_zoom)
Set zoom value.
virtual void finalize()
Finalize the thread.
void set_max_tilt_velocity(const float new_max_tilt_velocity)
Set max_tilt_velocity value.
void set_mirror_supported(const bool new_mirror_supported)
Set mirror_supported value.
virtual void log_info(const char *component, const char *format,...)
Log informational message.
Base class for exceptions in Fawkes.