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;
DirectedPerception PTU implementation.
virtual void get_limits(float &pan_min, float &pan_max, float &tilt_min, float &tilt_max)
Get position limits in radians.
virtual void loop()
Code to execute in the thread.
PanTiltDirectedPerceptionThread(std::string &pantilt_cfg_prefix, std::string &ptu_cfg_prefix, std::string &ptu_name)
Constructor.
virtual bool bb_interface_message_received(fawkes::Interface *interface, fawkes::Message *message)
BlackBoard message received notification.
virtual void finalize()
Finalize the thread.
virtual void init()
Initialize the thread.
void update_sensor_values()
Update sensor values as necessary.
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
BlackBoard interface listener.
void bbil_add_message_interface(Interface *interface)
Add an interface to the message received watch list.
virtual void unregister_listener(BlackBoardInterfaceListener *listener)
Unregister BB interface listener.
virtual Interface * open_for_writing(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for writing.
virtual void register_listener(BlackBoardInterfaceListener *listener, ListenerRegisterFlag flag=BBIL_FLAG_ALL)
Register BB event listener.
virtual void close(Interface *interface)=0
Close interface.
Configuration * config
This is the Configuration member used to access the configuration.
virtual unsigned int get_uint(const char *path)=0
Get value from configuration which is of type unsigned int.
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
Base class for exceptions in Fawkes.
Base class for all Fawkes BlackBoard interfaces.
bool msgq_first_is()
Check if first message has desired type.
void msgq_pop()
Erase first message from queue.
Message * msgq_first()
Get the first message from the message queue.
void write()
Write from local copy into BlackBoard memory.
bool msgq_empty()
Check if queue is empty.
JointInterface Fawkes BlackBoard Interface.
void set_position(const float new_position)
Set position value.
void set_velocity(const float new_velocity)
Set velocity value.
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
Logger * logger
This is the Logger member used to access the logger.
Base class for all messages passed through interfaces in Fawkes BlackBoard.
unsigned int id() const
Get message ID.
virtual void log_info(const char *component, const char *format,...)
Log informational message.
Mutex mutual exclusion lock.
CalibrateMessage Fawkes BlackBoard Interface Message.
FlushMessage Fawkes BlackBoard Interface Message.
GotoMessage Fawkes BlackBoard Interface Message.
float pan() const
Get pan value.
float tilt() const
Get tilt value.
ParkMessage Fawkes BlackBoard Interface Message.
SetEnabledMessage Fawkes BlackBoard Interface Message.
SetVelocityMessage Fawkes BlackBoard Interface Message.
StopMessage Fawkes BlackBoard Interface Message.
PanTiltInterface Fawkes BlackBoard Interface.
void set_enabled(const bool new_enabled)
Set enabled value.
void set_min_pan(const float new_min_pan)
Set min_pan value.
void set_final(const bool new_final)
Set final value.
void set_min_tilt(const float new_min_tilt)
Set min_tilt value.
void set_max_tilt(const float new_max_tilt)
Set max_tilt value.
void set_msgid(const uint32_t new_msgid)
Set msgid value.
void set_max_pan(const float new_max_pan)
Set max_pan value.
void set_tilt(const float new_tilt)
Set tilt value.
void set_calibrated(const bool new_calibrated)
Set calibrated value.
void set_pan(const float new_pan)
Set pan value.
Thread class encapsulation of pthreads.
const char * name() const
Get name of thread.
void set_name(const char *format,...)
Set name of thread.
void set_coalesce_wakeups(bool coalesce=true)
Set wakeup coalescing.
Fawkes library namespace.