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;
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;