23 #include "evid100p_thread.h"
26 #include <core/threading/mutex_locker.h>
27 #include <interfaces/PanTiltInterface.h>
32 using namespace fawkes;
47 std::string &ptu_cfg_prefix,
48 std::string &ptu_name)
52 set_name(
"PanTiltSonyEviD100PThread(%s)", ptu_name.c_str());
54 __pantilt_cfg_prefix = pantilt_cfg_prefix;
55 __ptu_cfg_prefix = ptu_cfg_prefix;
56 __ptu_name = ptu_name;
70 __cfg_read_timeout_ms =
config->
get_uint((__ptu_cfg_prefix +
"read_timeout_ms").c_str());
81 std::string bbid =
"PanTilt " + __ptu_name;
90 float pan_smin, pan_smax, tilt_smin, tilt_smax;
96 __pantilt_if->
write();
98 __wt =
new WorkerThread(__ptu_name,
logger, __cam,
103 __wt->set_velocities(pan_smax, tilt_smax);
108 #ifdef USE_TIMETRACKER
111 __ttc_read_sensor = __tt->add_class(
"Read Sensor");
139 if (__wt->has_fresh_data()) {
140 float pan = 0, tilt = 0;
141 __wt->get_pantilt(pan, tilt);
144 __pantilt_if->
set_final(__wt->is_final());
145 __pantilt_if->
write();
153 __pantilt_if->
set_final(__wt->is_final());
162 __wt->goto_pantilt(msg->
pan(), msg->
tilt());
169 __wt->goto_pantilt(0, 0);
205 __pantilt_if->
write();
219 logger->
log_info(name(),
"Flushing message queue");
220 __pantilt_if->msgq_flush();
223 logger->
log_info(name(),
"Received message of type %s, enqueueing", message->type());
248 PanTiltSonyEviD100PThread::WorkerThread::WorkerThread(std::string ptu_name,
251 const float &pan_min,
252 const float &pan_max,
253 const float &tilt_min,
254 const float &tilt_max)
257 set_name(
"SonyEviD100PWorkerThread(%s)", ptu_name.c_str());
262 __move_mutex =
new Mutex();
265 __move_pending =
false;
269 __velo_pending =
false;
275 __tilt_min = tilt_min;
276 __tilt_max = tilt_max;
281 PanTiltSonyEviD100PThread::WorkerThread::~WorkerThread()
289 PanTiltSonyEviD100PThread::WorkerThread::stop_motion()
291 float pan = 0, tilt = 0;
292 get_pantilt(pan, tilt);
293 goto_pantilt(pan, tilt);
302 PanTiltSonyEviD100PThread::WorkerThread::goto_pantilt(
float pan,
float tilt)
306 __target_tilt = tilt;
307 __move_pending =
true;
317 PanTiltSonyEviD100PThread::WorkerThread::get_pantilt(
float &pan,
float &tilt)
329 PanTiltSonyEviD100PThread::WorkerThread::set_velocities(
float pan_vel,
float tilt_vel)
332 __tilt_vel = tilt_vel;
333 __velo_pending =
true;
341 PanTiltSonyEviD100PThread::WorkerThread::is_final()
353 PanTiltSonyEviD100PThread::WorkerThread::has_fresh_data()
355 bool rv = __fresh_data;
356 __fresh_data =
false;
362 PanTiltSonyEviD100PThread::WorkerThread::once()
366 for (
int i = 0; i < 20; ++i) {
375 PanTiltSonyEviD100PThread::WorkerThread::loop()
380 __logger->log_warn(name(),
"Data processing failed, exception follows");
381 __logger->log_warn(name(), e);
384 if (__velo_pending) {
386 __cam->set_speed_radsec(__pan_vel, __tilt_vel);
388 __logger->log_warn(name(),
"Setting pan/tilt values failed, exception follows");
389 __logger->log_warn(name(), e);
391 __velo_pending =
false;
394 if (__move_pending) {
395 __move_mutex->lock();
396 __logger->log_debug(name(),
"Executing goto to %f, %f", __target_pan, __target_tilt);
397 exec_goto_pantilt(__target_pan, __target_tilt);
398 __move_mutex->unlock();
403 __cam->get_pan_tilt_rad(__cur_pan, __cur_tilt);
406 __logger->log_warn(name(),
"Failed to get new pan/tilt data, exception follows");
407 __logger->log_warn(name(), e);
410 if (! is_final() || ! __fresh_data) {
422 PanTiltSonyEviD100PThread::WorkerThread::exec_goto_pantilt(
float pan_rad,
float tilt_rad)
424 if ( (pan_rad < __pan_min) || (pan_rad > __pan_max) ) {
425 __logger->log_warn(name(),
"Pan value out of bounds, min: %f max: %f des: %f",
426 __pan_min, __pan_max, pan_rad);
429 if ( (tilt_rad < __tilt_min) || (tilt_rad > __tilt_max) ) {
430 __logger->log_warn(name(),
"Tilt value out of bounds, min: %f max: %f des: %f",
431 __tilt_min, __tilt_max, tilt_rad);
436 __cam->set_pan_tilt_rad(pan_rad, tilt_rad);
438 __logger->log_warn(name(),
"Failed to execute pan/tilt to %f, %f, exception "
439 "follows", pan_rad, tilt_rad);
440 __logger->log_warn(name(), e);
442 __move_pending =
false;
static const float MIN_TILT_RAD
Min tilt in rad.
float pan() const
Get pan value.
static const unsigned int NONBLOCKING_PANTILT
Non-blocking pan/tilt item.
Base class for all messages passed through interfaces in Fawkes BlackBoard.
bool msgq_empty()
Check if queue is empty.
virtual void finalize()
Finalize the thread.
static const float MIN_PAN_RAD
Min pan in rad.
virtual void loop()
Code to execute in the thread.
float tilt_velocity() const
Get tilt_velocity value.
static const float MAX_PAN_RAD
Max pan in rad.
void set_pan_velocity(const float new_pan_velocity)
Set pan_velocity value.
void set_msgid(const uint32_t new_msgid)
Set msgid 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.
virtual void unregister_listener(BlackBoardInterfaceListener *listener)
Unregister BB interface listener.
Thread class encapsulation of pthreads.
void write()
Write from local copy into BlackBoard memory.
unsigned int id() const
Get message ID.
Base class for all Fawkes BlackBoard interfaces.
float pan_velocity() const
Get pan_velocity value.
Logger * logger
This is the Logger member used to access the logger.
virtual Interface * open_for_writing(const char *interface_type, const char *identifier)=0
Open interface for writing.
SetVelocityMessage Fawkes BlackBoard Interface Message.
void set_min_pan(const float new_min_pan)
Set min_pan value.
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.
ParkMessage Fawkes BlackBoard Interface Message.
void set_name(const char *format,...)
Set name of thread.
Base class for exceptions in Fawkes.
void print_backtrace() const
Prints a backtrace.
FlushMessage 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.
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_final(const bool new_final)
Set final value.
void set_calibrated(const bool new_calibrated)
Set calibrated value.
GotoMessage Fawkes BlackBoard Interface Message.
void set_coalesce_wakeups(bool coalesce=true)
Set wakeup coalescing.
void set_max_tilt(const float new_max_tilt)
Set max_tilt value.
const char * name() const
Get name of thread.
float max_pan_velocity() const
Get max_pan_velocity value.
virtual void log_info(const char *component, const char *format,...)
Log informational message.
void print_trace()
Prints trace to stderr.
float max_tilt_velocity() const
Get max_tilt_velocity value.
void set_max_tilt_velocity(const float new_max_tilt_velocity)
Set max_tilt_velocity value.
float tilt() const
Get tilt value.
void set_pan(const float new_pan)
Set pan value.
CalibrateMessage Fawkes BlackBoard Interface Message.
static const float MAX_TILT_RAD
Max tilt in rad.
Sony EviD100P Visca controller.
PanTiltInterface Fawkes BlackBoard Interface.
void set_max_pan(const float new_max_pan)
Set max_pan value.
virtual bool bb_interface_message_received(fawkes::Interface *interface, fawkes::Message *message)
BlackBoard message received notification.
PanTiltSonyEviD100PThread(std::string &pantilt_cfg_prefix, std::string &ptu_cfg_prefix, std::string &ptu_name)
Constructor.
Message * msgq_first()
Get the first message from the message queue.
virtual unsigned int get_uint(const char *path)=0
Get value from configuration which is of type unsigned int.
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.
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.
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
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.
virtual void close(Interface *interface)=0
Close interface.