23 #include "dp_thread.h" 26 #include <core/threading/mutex_locker.h> 27 #include <interfaces/PanTiltInterface.h> 28 #include <interfaces/JointInterface.h> 49 std::string &ptu_cfg_prefix,
50 std::string &ptu_name)
54 set_name(
"PanTiltDirectedPerceptionThread(%s)", ptu_name.c_str());
56 __pantilt_cfg_prefix = pantilt_cfg_prefix;
57 __ptu_cfg_prefix = ptu_cfg_prefix;
58 __ptu_name = ptu_name;
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);
91 __pantilt_if->
write();
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");
151 if (__wt->has_fresh_data()) {
152 float pan = 0, tilt = 0;
153 __wt->get_pantilt(pan, tilt);
156 __pantilt_if->
set_final(__wt->is_final());
157 __pantilt_if->
write();
160 __panjoint_if->
write();
163 __tiltjoint_if->
write();
171 __pantilt_if->
set_final(__wt->is_final());
180 __wt->goto_pantilt(msg->
pan(), msg->
tilt());
187 __wt->goto_pantilt(0, 0);
226 __pantilt_if->
write();
265 PanTiltDirectedPerceptionThread::WorkerThread::WorkerThread(std::string ptu_name,
268 :
Thread(
"", Thread::OPMODE_WAITFORWAKEUP)
270 set_name(
"SonyDirectedPerceptionWorkerThread(%s)", ptu_name.c_str());
275 __move_mutex =
new Mutex();
278 __move_pending =
false;
279 __reset_pending =
false;
283 __ptu->
get_limits(__pan_min, __pan_max, __tilt_min, __tilt_max);
288 PanTiltDirectedPerceptionThread::WorkerThread::~WorkerThread()
296 PanTiltDirectedPerceptionThread::WorkerThread::stop_motion()
298 float pan = 0, tilt = 0;
299 get_pantilt(pan, tilt);
300 goto_pantilt(pan, tilt);
309 PanTiltDirectedPerceptionThread::WorkerThread::goto_pantilt(
float pan,
float tilt)
313 __target_tilt = tilt;
314 __move_pending =
true;
324 PanTiltDirectedPerceptionThread::WorkerThread::get_pantilt(
float &pan,
float &tilt)
333 PanTiltDirectedPerceptionThread::WorkerThread::reset()
335 __reset_pending =
true;
343 PanTiltDirectedPerceptionThread::WorkerThread::is_final()
346 return ( (fabs(__cur_pan - __target_pan) < 0.01) &&
347 (fabs(__cur_tilt - __target_tilt) < 0.01));
356 PanTiltDirectedPerceptionThread::WorkerThread::has_fresh_data()
358 bool rv = __fresh_data;
359 __fresh_data =
false;
365 PanTiltDirectedPerceptionThread::WorkerThread::loop()
367 if (__move_pending) {
368 __move_mutex->lock();
369 exec_goto_pantilt(__target_pan, __target_tilt);
370 __move_mutex->unlock();
373 if (__reset_pending) {
374 __move_mutex->lock();
375 __reset_pending =
false;
376 __move_mutex->unlock();
384 __logger->log_warn(
name(),
"Failed to get new pan/tilt data, exception follows");
385 __logger->log_warn(
name(), e);
400 PanTiltDirectedPerceptionThread::WorkerThread::exec_goto_pantilt(
float pan_rad,
float tilt_rad)
402 if ( (pan_rad < __pan_min) || (pan_rad > __pan_max) ) {
403 __logger->log_warn(
name(),
"Pan value out of bounds, min: %f max: %f des: %f",
404 __pan_min, __pan_max, pan_rad);
407 if ( (tilt_rad < __tilt_min) || (tilt_rad > __tilt_max) ) {
408 __logger->log_warn(
name(),
"Tilt value out of bounds, min: %f max: %f des: %f",
409 __tilt_min, __tilt_max, tilt_rad);
414 __move_pending =
false;
Thread(const char *name)
Constructor.
Base class for all messages passed through interfaces in Fawkes BlackBoard.
unsigned int id() const
Get message ID.
bool msgq_empty()
Check if queue is empty.
virtual void log_info(const char *component, const char *format,...)=0
Log informational message.
virtual bool bb_interface_message_received(fawkes::Interface *interface, fawkes::Message *message)
BlackBoard message received notification.
Fawkes library namespace.
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.
DirectedPerception PTU implementation.
void write()
Write from local copy into BlackBoard memory.
Base class for all Fawkes BlackBoard interfaces.
Logger * logger
This is the Logger member used to access the logger.
SetVelocityMessage Fawkes BlackBoard Interface Message.
virtual void get_pan_tilt_rad(float &pan, float &tilt)
Get pan/tilt in radians.
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.
virtual void set_pan_tilt_rad(float pan, float tilt)
Set pan and tilt in radians.
void wakeup()
Wake up thread.
ParkMessage Fawkes BlackBoard Interface Message.
void set_name(const char *format,...)
Set name of thread.
virtual void reset()
Reset the PTU.
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.
const char * name() const
Get name of thread.
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_position(const float new_position)
Set position value.
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.
virtual void finalize()
Finalize the thread.
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.
float tilt() const
Get tilt value.
virtual void loop()
Code to execute in the thread.
void msgq_flush()
Flush all messages.
void set_velocity(const float new_velocity)
Set velocity value.
JointInterface Fawkes BlackBoard Interface.
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 Interface * open_for_writing(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for writing.
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
float pan() const
Get pan value.
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.