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;