23 #include "dp_thread.h"
27 #include <core/threading/mutex_locker.h>
28 #include <interfaces/JointInterface.h>
29 #include <interfaces/PanTiltInterface.h>
50 std::string &ptu_cfg_prefix,
51 std::string &ptu_name)
55 set_name(
"PanTiltDirectedPerceptionThread(%s)", ptu_name.c_str());
57 pantilt_cfg_prefix_ = pantilt_cfg_prefix;
58 ptu_cfg_prefix_ = ptu_cfg_prefix;
70 cfg_read_timeout_ms_ =
config->
get_uint((ptu_cfg_prefix_ +
"read_timeout_ms").c_str());
75 std::string bbid =
"PanTilt " + ptu_name_;
78 float min_pan = 0, max_pan = 0, min_tilt = 0, max_tilt = 0;
79 ptu_->
get_limits(min_pan, max_pan, min_tilt, max_tilt);
94 float init_tilt = 0.f;
95 float init_pan_velocity = 0.f;
96 float init_tilt_velocity = 0.f;
98 std::string panid = ptu_name_ +
" pan";
102 panjoint_if_->
write();
104 std::string tiltid = ptu_name_ +
" tilt";
108 tiltjoint_if_->
write();
110 wt_ =
new WorkerThread(ptu_name_,
logger, ptu_);
118 #ifdef USE_TIMETRACKER
121 ttc_read_sensor_ = tt_->add_class(
"Read Sensor");
148 if (wt_->has_fresh_data()) {
149 float pan = 0, tilt = 0;
150 wt_->get_pantilt(pan, tilt);
154 pantilt_if_->
write();
157 panjoint_if_->
write();
160 tiltjoint_if_->
write();
176 wt_->goto_pantilt(msg->
pan(), msg->
tilt());
183 wt_->goto_pantilt(0, 0);
222 pantilt_if_->
write();
234 logger->
log_info(name(),
"Flushing message queue");
235 pantilt_if_->msgq_flush();
238 logger->
log_info(name(),
"Received message of type %s, enqueueing", message->type());
257 PanTiltDirectedPerceptionThread::WorkerThread::WorkerThread(
258 std::string ptu_name,
263 set_name(
"SonyDirectedPerceptionWorkerThread(%s)", ptu_name.c_str());
268 move_mutex_ =
new Mutex();
271 move_pending_ =
false;
272 reset_pending_ =
false;
276 ptu_->
get_limits(pan_min_, pan_max_, tilt_min_, tilt_max_);
280 PanTiltDirectedPerceptionThread::WorkerThread::~WorkerThread()
287 PanTiltDirectedPerceptionThread::WorkerThread::stop_motion()
289 float pan = 0, tilt = 0;
290 get_pantilt(pan, tilt);
291 goto_pantilt(pan, tilt);
299 PanTiltDirectedPerceptionThread::WorkerThread::goto_pantilt(
float pan,
float tilt)
304 move_pending_ =
true;
313 PanTiltDirectedPerceptionThread::WorkerThread::get_pantilt(
float &pan,
float &tilt)
321 PanTiltDirectedPerceptionThread::WorkerThread::reset()
323 reset_pending_ =
true;
330 PanTiltDirectedPerceptionThread::WorkerThread::is_final()
333 return ((fabs(cur_pan_ - target_pan_) < 0.01) && (fabs(cur_tilt_ - target_tilt_) < 0.01));
341 PanTiltDirectedPerceptionThread::WorkerThread::has_fresh_data()
343 bool rv = fresh_data_;
349 PanTiltDirectedPerceptionThread::WorkerThread::loop()
353 exec_goto_pantilt(target_pan_, target_tilt_);
354 move_mutex_->unlock();
357 if (reset_pending_) {
359 reset_pending_ =
false;
360 move_mutex_->unlock();
365 ptu_->get_pan_tilt_rad(cur_pan_, cur_tilt_);
368 logger_->log_warn(name(),
"Failed to get new pan/tilt data, exception follows");
369 logger_->log_warn(name(), e);
383 PanTiltDirectedPerceptionThread::WorkerThread::exec_goto_pantilt(
float pan_rad,
float tilt_rad)
385 if ((pan_rad < pan_min_) || (pan_rad > pan_max_)) {
387 name(),
"Pan value out of bounds, min: %f max: %f des: %f", pan_min_, pan_max_, pan_rad);
390 if ((tilt_rad < tilt_min_) || (tilt_rad > tilt_max_)) {
391 logger_->log_warn(name(),
392 "Tilt value out of bounds, min: %f max: %f des: %f",
399 ptu_->set_pan_tilt_rad(pan_rad, tilt_rad);
400 move_pending_ =
false;