23 #include "dp_thread.h"
26 #include <core/threading/mutex_locker.h>
27 #include <interfaces/PanTiltInterface.h>
32 using namespace fawkes;
48 std::string &ptu_cfg_prefix,
49 std::string &ptu_name)
53 set_name(
"PanTiltDirectedPerceptionThread(%s)", ptu_name.c_str());
55 __pantilt_cfg_prefix = pantilt_cfg_prefix;
56 __ptu_cfg_prefix = ptu_cfg_prefix;
57 __ptu_name = ptu_name;
69 __cfg_read_timeout_ms =
config->
get_uint((__ptu_cfg_prefix +
"read_timeout_ms").c_str());
74 std::string bbid =
"PanTilt " + __ptu_name;
77 float min_pan=0, max_pan=0, min_tilt=0, max_tilt=0;
78 __ptu->
get_limits(min_pan, max_pan, min_tilt, max_tilt);
90 __pantilt_if->
write();
92 __wt =
new WorkerThread(__ptu_name,
logger, __ptu);
98 #ifdef USE_TIMETRACKER
101 __ttc_read_sensor = __tt->add_class(
"Read Sensor");
129 if (__wt->has_fresh_data()) {
130 float pan = 0, tilt = 0;
131 __wt->get_pantilt(pan, tilt);
134 __pantilt_if->
set_final(__wt->is_final());
135 __pantilt_if->
write();
143 __pantilt_if->
set_final(__wt->is_final());
152 __wt->goto_pantilt(msg->
pan(), msg->
tilt());
159 __wt->goto_pantilt(0, 0);
194 __pantilt_if->
write();
208 logger->
log_info(name(),
"Flushing message queue");
209 __pantilt_if->msgq_flush();
212 logger->
log_info(name(),
"Received message of type %s, enqueueing", message->type());
233 PanTiltDirectedPerceptionThread::WorkerThread::WorkerThread(std::string ptu_name,
238 set_name(
"SonyDirectedPerceptionWorkerThread(%s)", ptu_name.c_str());
243 __move_mutex =
new Mutex();
246 __move_pending =
false;
247 __reset_pending =
false;
251 __ptu->
get_limits(__pan_min, __pan_max, __tilt_min, __tilt_max);
256 PanTiltDirectedPerceptionThread::WorkerThread::~WorkerThread()
264 PanTiltDirectedPerceptionThread::WorkerThread::stop_motion()
266 float pan = 0, tilt = 0;
267 get_pantilt(pan, tilt);
268 goto_pantilt(pan, tilt);
277 PanTiltDirectedPerceptionThread::WorkerThread::goto_pantilt(
float pan,
float tilt)
281 __target_tilt = tilt;
282 __move_pending =
true;
292 PanTiltDirectedPerceptionThread::WorkerThread::get_pantilt(
float &pan,
float &tilt)
301 PanTiltDirectedPerceptionThread::WorkerThread::reset()
303 __reset_pending =
true;
311 PanTiltDirectedPerceptionThread::WorkerThread::is_final()
314 return ( (fabs(__cur_pan - __target_pan) < 0.01) &&
315 (fabs(__cur_tilt - __target_tilt) < 0.01));
324 PanTiltDirectedPerceptionThread::WorkerThread::has_fresh_data()
326 bool rv = __fresh_data;
327 __fresh_data =
false;
333 PanTiltDirectedPerceptionThread::WorkerThread::loop()
335 if (__move_pending) {
336 __move_mutex->lock();
337 exec_goto_pantilt(__target_pan, __target_tilt);
338 __move_mutex->unlock();
341 if (__reset_pending) {
342 __move_mutex->lock();
343 __reset_pending =
false;
344 __move_mutex->unlock();
349 __ptu->get_pan_tilt_rad(__cur_pan, __cur_tilt);
352 __logger->log_warn(name(),
"Failed to get new pan/tilt data, exception follows");
353 __logger->log_warn(name(), e);
368 PanTiltDirectedPerceptionThread::WorkerThread::exec_goto_pantilt(
float pan_rad,
float tilt_rad)
370 if ( (pan_rad < __pan_min) || (pan_rad > __pan_max) ) {
371 __logger->log_warn(name(),
"Pan value out of bounds, min: %f max: %f des: %f",
372 __pan_min, __pan_max, pan_rad);
375 if ( (tilt_rad < __tilt_min) || (tilt_rad > __tilt_max) ) {
376 __logger->log_warn(name(),
"Tilt value out of bounds, min: %f max: %f des: %f",
377 __tilt_min, __tilt_max, tilt_rad);
381 __ptu->set_pan_tilt_rad(pan_rad, tilt_rad);
382 __move_pending =
false;
float pan() const
Get pan value.
Base class for all messages passed through interfaces in Fawkes BlackBoard.
bool msgq_empty()
Check if queue is empty.
virtual bool bb_interface_message_received(fawkes::Interface *interface, fawkes::Message *message)
BlackBoard message received notification.
void set_msgid(const uint32_t new_msgid)
Set msgid value.
PanTiltDirectedPerceptionThread(std::string &pantilt_cfg_prefix, std::string &ptu_cfg_prefix, std::string &ptu_name)
Constructor.
SetEnabledMessage Fawkes BlackBoard Interface Message.
virtual void unregister_listener(BlackBoardInterfaceListener *listener)
Unregister BB interface listener.
void update_sensor_values()
Update sensor values as necessary.
Thread class encapsulation of pthreads.
DirectedPerception PTU implementation.
void write()
Write from local copy into BlackBoard memory.
unsigned int id() const
Get message ID.
Base class for all Fawkes BlackBoard interfaces.
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.
virtual void get_limits(float &pan_min, float &pan_max, float &tilt_min, float &tilt_max)
Get position limits in radians.
void set_min_pan(const float new_min_pan)
Set min_pan value.
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.
Message * msgq_first()
Get the first message from the message queue.
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.
virtual void init()
Initialize the thread.
const char * name() const
Get name of thread.
virtual void finalize()
Finalize the thread.
virtual void log_info(const char *component, const char *format,...)
Log informational message.
float tilt() const
Get tilt value.
void set_pan(const float new_pan)
Set pan value.
CalibrateMessage Fawkes BlackBoard Interface Message.
PanTiltInterface Fawkes BlackBoard Interface.
void set_max_pan(const float new_max_pan)
Set max_pan value.
virtual void loop()
Code to execute in the thread.
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.
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.
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
virtual void close(Interface *interface)=0
Close interface.