22 #include "rx28_thread.h"
25 #include <utils/math/angle.h>
26 #include <core/threading/mutex_locker.h>
27 #include <core/threading/read_write_lock.h>
28 #include <core/threading/scoped_rwlock.h>
29 #include <core/threading/wait_condition.h>
30 #include <interfaces/PanTiltInterface.h>
31 #include <interfaces/LedInterface.h>
37 using namespace fawkes;
52 std::string &ptu_cfg_prefix,
53 std::string &ptu_name)
57 (std::string(
"PTU ") + ptu_name).c_str()),
61 set_name(
"PanTiltRX28Thread(%s)", ptu_name.c_str());
63 __pantilt_cfg_prefix = pantilt_cfg_prefix;
64 __ptu_cfg_prefix = ptu_cfg_prefix;
65 __ptu_name = ptu_name;
74 __last_pan = __last_tilt = 0.f;
81 __cfg_read_timeout_ms =
config->
get_uint((__ptu_cfg_prefix +
"read_timeout_ms").c_str());
82 __cfg_disc_timeout_ms =
config->
get_uint((__ptu_cfg_prefix +
"discover_timeout_ms").c_str());
83 __cfg_pan_servo_id =
config->
get_uint((__ptu_cfg_prefix +
"pan_servo_id").c_str());
84 __cfg_tilt_servo_id =
config->
get_uint((__ptu_cfg_prefix +
"tilt_servo_id").c_str());
87 __cfg_goto_zero_start =
config->
get_bool((__ptu_cfg_prefix +
"goto_zero_start").c_str());
88 __cfg_turn_off =
config->
get_bool((__ptu_cfg_prefix +
"turn_off").c_str());
89 __cfg_cw_compl_margin =
config->
get_uint((__ptu_cfg_prefix +
"cw_compl_margin").c_str());
90 __cfg_ccw_compl_margin =
config->
get_uint((__ptu_cfg_prefix +
"ccw_compl_margin").c_str());
91 __cfg_cw_compl_slope =
config->
get_uint((__ptu_cfg_prefix +
"cw_compl_slope").c_str());
92 __cfg_ccw_compl_slope =
config->
get_uint((__ptu_cfg_prefix +
"ccw_compl_slope").c_str());
93 __cfg_pan_min =
config->
get_float((__ptu_cfg_prefix +
"pan_min").c_str());
94 __cfg_pan_max =
config->
get_float((__ptu_cfg_prefix +
"pan_max").c_str());
95 __cfg_tilt_min =
config->
get_float((__ptu_cfg_prefix +
"tilt_min").c_str());
96 __cfg_tilt_max =
config->
get_float((__ptu_cfg_prefix +
"tilt_max").c_str());
97 __cfg_pan_margin =
config->
get_float((__ptu_cfg_prefix +
"pan_margin").c_str());
98 __cfg_tilt_margin =
config->
get_float((__ptu_cfg_prefix +
"tilt_margin").c_str());
99 __cfg_pan_start =
config->
get_float((__ptu_cfg_prefix +
"pan_start").c_str());
100 __cfg_tilt_start =
config->
get_float((__ptu_cfg_prefix +
"tilt_start").c_str());
117 std::string frame_id_prefix = std::string(
"/") + __ptu_name;
123 __cfg_base_frame = frame_id_prefix +
"/base";
124 __cfg_pan_link = frame_id_prefix +
"/pan";
125 __cfg_tilt_link = frame_id_prefix +
"/tilt";
127 __translation_pan.setValue(pan_trans_x, pan_trans_y, pan_trans_z);
128 __translation_tilt.setValue(tilt_trans_x, tilt_trans_y, tilt_trans_z);
131 bool pan_servo_found =
false, tilt_servo_found =
false;
133 __rx28 =
new RobotisRX28(__cfg_device.c_str(), __cfg_read_timeout_ms);
135 for (RobotisRX28::DeviceList::iterator i = devl.begin(); i != devl.end(); ++i) {
136 if (__cfg_pan_servo_id == *i) {
137 pan_servo_found =
true;
138 }
else if (__cfg_tilt_servo_id == *i) {
139 tilt_servo_found =
true;
142 "configured as pan nor as tilt servo", *i);
150 __cfg_cw_compl_margin, __cfg_cw_compl_slope,
151 __cfg_ccw_compl_margin, __cfg_ccw_compl_slope);
155 if (! (pan_servo_found && tilt_servo_found)) {
156 throw Exception(
"Pan and/or tilt servo not found: pan: %i tilt: %i",
157 pan_servo_found, tilt_servo_found);
161 std::string bbid =
"PanTilt " + __ptu_name;
174 __pantilt_if->
write();
178 __wt =
new WorkerThread(__ptu_name,
logger, __rx28,
179 __cfg_pan_servo_id, __cfg_tilt_servo_id,
180 __cfg_pan_min, __cfg_pan_max, __cfg_tilt_min, __cfg_tilt_max,
181 __cfg_pan_offset, __cfg_tilt_offset);
182 __wt->set_margins(__cfg_pan_margin, __cfg_tilt_margin);
184 __wt->set_enabled(
true);
185 if ( __cfg_goto_zero_start ) {
186 __wt->goto_pantilt_timed(__cfg_pan_start, __cfg_tilt_start, 3.0);
192 #ifdef USE_TIMETRACKER
195 __ttc_read_sensor = __tt->add_class(
"Read Sensor");
204 if (__cfg_turn_off) {
206 __wt->goto_pantilt_timed(0, __cfg_tilt_max, 2.0);
209 __wt->wait_for_fresh_data();
210 __wt->wait_for_fresh_data();
212 while (! __wt->is_final()) {
214 __wt->wait_for_fresh_data();
231 if (__cfg_turn_off) {
254 if (__wt->has_fresh_data()) {
255 float pan = 0, tilt = 0, panvel=0, tiltvel=0;
257 __wt->get_pantilt(pan, tilt, time);
258 __wt->get_velocities(panvel, tiltvel);
261 if (fabs(__last_pan - pan) >= 0.009 || fabs(__last_tilt - tilt) >= 0.009) {
274 __pantilt_if->
set_final(__wt->is_final());
275 __pantilt_if->
write();
279 tf::Quaternion pr; pr.setEulerZYX(pan, 0, 0);
280 tf::Transform ptr(pr, __translation_pan);
281 tf_publisher->send_transform(ptr, time, __cfg_base_frame, __cfg_pan_link);
283 tf::Quaternion tr; tr.setEulerZYX(0, tilt, 0);
284 tf::Transform ttr(tr, __translation_tilt);
285 tf_publisher->send_transform(ttr, time, __cfg_pan_link, __cfg_tilt_link);
294 __pantilt_if->
set_final(__wt->is_final());
303 __wt->goto_pantilt(msg->
pan(), msg->
tilt());
317 __wt->goto_pantilt(0, 0);
353 __pantilt_if->
write();
355 bool write_led_if =
false;
360 __wt->set_led_enabled((msg->
intensity() >= 0.5));
363 __wt->set_led_enabled(
true);
366 __wt->set_led_enabled(
false);
372 if (write_led_if) __led_if->
write();
387 logger->
log_info(name(),
"Flushing message queue");
388 __pantilt_if->msgq_flush();
391 logger->
log_info(name(),
"Received message of type %s, enqueueing", message->type());
420 PanTiltRX28Thread::WorkerThread::WorkerThread(std::string ptu_name,
423 unsigned char pan_servo_id,
424 unsigned char tilt_servo_id,
425 float &pan_min,
float &pan_max,
426 float &tilt_min,
float &tilt_max,
427 float &pan_offset,
float &tilt_offset)
430 set_name(
"RX28WorkerThread(%s)", ptu_name.c_str());
437 __fresh_data_mutex =
new Mutex();
441 __move_pending =
false;
444 __pan_servo_id = pan_servo_id;
445 __tilt_servo_id = tilt_servo_id;
449 __tilt_min = tilt_min;
450 __tilt_max = tilt_max;
451 __pan_offset = pan_offset;
452 __tilt_offset = tilt_offset;
455 __led_enable =
false;
456 __led_disable =
false;
464 PanTiltRX28Thread::WorkerThread::~WorkerThread()
466 delete __value_rwlock;
467 delete __rx28_rwlock;
468 delete __fresh_data_mutex;
469 delete __update_waitcond;
477 PanTiltRX28Thread::WorkerThread::set_enabled(
bool enabled)
495 PanTiltRX28Thread::WorkerThread::set_led_enabled(
bool enabled)
500 __led_disable =
false;
502 __led_enable =
false;
503 __led_disable =
true;
511 PanTiltRX28Thread::WorkerThread::stop_motion()
513 float pan = 0, tilt = 0;
514 get_pantilt(pan, tilt);
515 goto_pantilt(pan, tilt);
524 PanTiltRX28Thread::WorkerThread::goto_pantilt(
float pan,
float tilt)
528 __target_tilt = tilt;
529 __move_pending =
true;
540 PanTiltRX28Thread::WorkerThread::goto_pantilt_timed(
float pan,
float tilt,
float time_sec)
543 __target_tilt = tilt;
544 __move_pending =
true;
546 float cpan=0, ctilt=0;
547 get_pantilt(cpan, ctilt);
549 float pan_diff = fabs(pan - cpan);
550 float tilt_diff = fabs(tilt - ctilt);
552 float req_pan_vel = pan_diff / time_sec;
553 float req_tilt_vel = tilt_diff / time_sec;
559 if (req_pan_vel > __max_pan_speed) {
560 __logger->log_warn(name(),
"Requested move to (%f, %f) in %f sec requires a "
561 "pan speed of %f rad/s, which is greater than the maximum "
562 "of %f rad/s, reducing to max", pan, tilt, time_sec,
563 req_pan_vel, __max_pan_speed);
564 req_pan_vel = __max_pan_speed;
567 if (req_tilt_vel > __max_tilt_speed) {
568 __logger->log_warn(name(),
"Requested move to (%f, %f) in %f sec requires a "
569 "tilt speed of %f rad/s, which is greater than the maximum of "
570 "%f rad/s, reducing to max", pan, tilt, time_sec,
571 req_tilt_vel, __max_tilt_speed);
572 req_tilt_vel = __max_tilt_speed;
575 set_velocities(req_pan_vel, req_tilt_vel);
586 PanTiltRX28Thread::WorkerThread::set_velocities(
float pan_vel,
float tilt_vel)
596 __pan_vel = (
unsigned int)pan_tmp;
597 __velo_pending =
true;
599 __logger->log_warn(name(),
"Calculated pan value out of bounds, min: 0 max: %u des: %u",
604 __tilt_vel = (
unsigned int)tilt_tmp;
605 __velo_pending =
true;
607 __logger->log_warn(name(),
"Calculated tilt value out of bounds, min: 0 max: %u des: %u",
618 PanTiltRX28Thread::WorkerThread::get_velocities(
float &pan_vel,
float &tilt_vel)
620 unsigned int pan_velticks = __rx28->get_goal_speed(__pan_servo_id);
621 unsigned int tilt_velticks = __rx28->get_goal_speed(__tilt_servo_id);
624 tilt_velticks = (
unsigned int)(((
float)tilt_velticks / (float)RobotisRX28::MAX_SPEED) * __max_tilt_speed);
633 PanTiltRX28Thread::WorkerThread::set_margins(
float pan_margin,
float tilt_margin)
635 if (pan_margin > 0.0) __pan_margin = pan_margin;
636 if (tilt_margin > 0.0) __tilt_margin = tilt_margin;
646 PanTiltRX28Thread::WorkerThread::get_pantilt(
float &pan,
float &tilt)
648 ScopedRWLock lock(__rx28_rwlock, ScopedRWLock::LOCK_READ);
651 int tilt_ticks = ((int)__rx28->get_position(__tilt_servo_id) - (int)RobotisRX28::CENTER_POSITION);
664 PanTiltRX28Thread::WorkerThread::get_pantilt(
float &pan,
float &tilt,
667 get_pantilt(pan, tilt);
668 time = __pantilt_time;
676 PanTiltRX28Thread::WorkerThread::is_final()
679 get_pantilt(pan, tilt);
691 ScopedRWLock lock(__rx28_rwlock, ScopedRWLock::LOCK_READ);
693 return ( (fabs(pan - __target_pan) <= __pan_margin) &&
694 (fabs(tilt - __target_tilt) <= __tilt_margin) ) ||
695 (! __rx28->is_moving(__pan_servo_id) &&
696 ! __rx28->is_moving(__tilt_servo_id));
704 PanTiltRX28Thread::WorkerThread::is_enabled()
706 return (__rx28->is_torque_enabled(__pan_servo_id) &&
707 __rx28->is_torque_enabled(__tilt_servo_id));
716 PanTiltRX28Thread::WorkerThread::has_fresh_data()
720 bool rv = __fresh_data;
721 __fresh_data =
false;
727 PanTiltRX28Thread::WorkerThread::loop()
730 __value_rwlock->lock_for_write();
732 __value_rwlock->unlock();
734 __rx28->set_led_enabled(__tilt_servo_id,
true);
735 __rx28->set_torques_enabled(
true, 2, __pan_servo_id, __tilt_servo_id);
736 }
else if (__disable) {
737 __value_rwlock->lock_for_write();
739 __value_rwlock->unlock();
741 if (__led_enable || __led_disable || __velo_pending || __move_pending) usleep(3000);
745 __value_rwlock->lock_for_write();
746 __led_enable =
false;
747 __value_rwlock->unlock();
749 __rx28->set_led_enabled(__pan_servo_id,
true);
750 if (__velo_pending || __move_pending) usleep(3000);
751 }
else if (__led_disable) {
752 __value_rwlock->lock_for_write();
753 __led_disable =
false;
754 __value_rwlock->unlock();
756 __rx28->set_led_enabled(__pan_servo_id,
false);
757 if (__velo_pending || __move_pending) usleep(3000);
760 if (__velo_pending) {
761 __value_rwlock->lock_for_write();
762 __velo_pending =
false;
763 unsigned int pan_vel = __pan_vel;
764 unsigned int tilt_vel = __tilt_vel;
765 __value_rwlock->unlock();
767 __rx28->set_goal_speeds(2, __pan_servo_id, pan_vel, __tilt_servo_id, tilt_vel);
768 if (__move_pending) usleep(3000);
771 if (__move_pending) {
772 __value_rwlock->lock_for_write();
773 __move_pending =
false;
774 float target_pan = __target_pan;
775 float target_tilt = __target_tilt;
776 __value_rwlock->unlock();
777 exec_goto_pantilt(target_pan, target_tilt);
781 ScopedRWLock lock(__rx28_rwlock, ScopedRWLock::LOCK_READ);
782 __rx28->read_table_values(__pan_servo_id);
783 __rx28->read_table_values(__tilt_servo_id);
787 __pantilt_time.stamp();
803 __update_waitcond->wake_all();
815 PanTiltRX28Thread::WorkerThread::exec_goto_pantilt(
float pan_rad,
float tilt_rad)
817 if ( (pan_rad < __pan_min) || (pan_rad > __pan_max) ) {
818 __logger->log_warn(name(),
"Pan value out of bounds, min: %f max: %f des: %f",
819 __pan_min, __pan_max, pan_rad);
822 if ( (tilt_rad < __tilt_min) || (tilt_rad > __tilt_max) ) {
823 __logger->log_warn(name(),
"Tilt value out of bounds, min: %f max: %f des: %f",
824 __tilt_min, __tilt_max, tilt_rad);
828 unsigned int pan_min = 0, pan_max = 0, tilt_min = 0, tilt_max = 0;
830 __rx28->get_angle_limits(__pan_servo_id, pan_min, pan_max);
831 __rx28->get_angle_limits(__tilt_servo_id, tilt_min, tilt_max);
839 if ( (pan_pos < 0) || ((
unsigned int)pan_pos < pan_min) || ((
unsigned int)pan_pos > pan_max) ) {
840 __logger->log_warn(name(),
"Pan position out of bounds, min: %u max: %u des: %i",
841 pan_min, pan_max, pan_pos);
845 if ( (tilt_pos < 0) || ((
unsigned int)tilt_pos < tilt_min) || ((
unsigned int)tilt_pos > tilt_max) ) {
846 __logger->log_warn(name(),
"Tilt position out of bounds, min: %u max: %u des: %i",
847 tilt_min, tilt_max, tilt_pos);
852 __rx28->goto_positions(2, __pan_servo_id, pan_pos, __tilt_servo_id, tilt_pos);
860 PanTiltRX28Thread::WorkerThread::wait_for_fresh_data()
862 __update_waitcond->wait();
virtual bool bb_interface_message_received(fawkes::Interface *interface, fawkes::Message *message)
BlackBoard message received notification.
TurnOffMessage Fawkes BlackBoard Interface Message.
virtual void loop()
Code to execute in the thread.
float tilt() const
Get tilt value.
std::list< unsigned char > DeviceList
List of servo IDs.
float tilt_margin() const
Get tilt_margin value.
float pan() const
Get pan value.
TimedGotoMessage Fawkes BlackBoard Interface Message.
Wait until a given condition holds.
Base class for all messages passed through interfaces in Fawkes BlackBoard.
bool msgq_empty()
Check if queue is empty.
virtual void log_info(const char *component, const char *format,...)=0
Log informational message.
void update_sensor_values()
Update sensor values as necessary.
float tilt_velocity() const
Get tilt_velocity value.
void set_pan_velocity(const float new_pan_velocity)
Set pan_velocity value.
virtual bool get_bool(const char *path)=0
Get value from configuration which is of type bool.
float get_max_supported_speed(unsigned char id, bool refresh=false)
Get maximum supported speed.
void set_msgid(const uint32_t new_msgid)
Set msgid value.
void set_led_enabled(unsigned char id, bool enabled)
Turn LED on or off.
void set_max_pan_velocity(const float new_max_pan_velocity)
Set max_pan_velocity value.
SetEnabledMessage Fawkes BlackBoard Interface Message.
float pan_margin() const
Get pan_margin value.
A class for handling time.
virtual void unregister_listener(BlackBoardInterfaceListener *listener)
Unregister BB interface listener.
Thread class encapsulation of pthreads.
void write()
Write from local copy into BlackBoard memory.
unsigned int id() const
Get message ID.
Base class for all Fawkes BlackBoard interfaces.
float pan_velocity() const
Get pan_velocity value.
PanTiltRX28Thread(std::string &pantilt_cfg_prefix, std::string &ptu_cfg_prefix, std::string &ptu_name)
Constructor.
void set_pan_margin(const float new_pan_margin)
Set pan_margin value.
Logger * logger
This is the Logger member used to access the logger.
virtual Interface * open_for_writing(const char *interface_type, const char *identifier)=0
Open interface for writing.
SetVelocityMessage Fawkes BlackBoard Interface Message.
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.
void set_intensity(const float new_intensity)
Set intensity value.
ParkMessage Fawkes BlackBoard Interface Message.
void set_name(const char *format,...)
Set name of thread.
virtual const char * what() const
Get primary string.
DeviceList discover(unsigned int total_timeout_ms=50)
Discover devices on the bus.
Base class for exceptions in Fawkes.
Message * msgq_first()
Get the first message from the message queue.
virtual void init()
Initialize the thread.
static const float POS_TICKS_PER_RAD
POS_TICKS_PER_RAD.
void set_tilt_margin(const float new_tilt_margin)
Set tilt_margin value.
FlushMessage Fawkes BlackBoard Interface Message.
static const unsigned int MAX_SPEED
MAX_SPEED.
void set_torques_enabled(bool enabled, unsigned char num_servos,...)
Enable or disable torque for multiple (selected) servos at once.
void set_min_tilt(const float new_min_tilt)
Set min_tilt value.
Read/write lock to allow multiple readers but only a single writer on the resource at a time...
void set_tilt(const float new_tilt)
Set tilt value.
SetIntensityMessage Fawkes BlackBoard Interface Message.
bool msgq_first_is()
Check if first message has desired type.
void set_compliance_values(unsigned char id, unsigned char cw_margin, unsigned char cw_slope, unsigned char ccw_margin, unsigned char ccw_slope)
Set compliance values.
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
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.
float pan() const
Get pan value.
const char * name() const
Get name of thread.
float max_pan_velocity() const
Get max_pan_velocity value.
virtual void log_info(const char *component, const char *format,...)
Log informational message.
float max_tilt_velocity() const
Get max_tilt_velocity value.
Class to access a chain of Robotis RX28 servos.
void set_max_tilt_velocity(const float new_max_tilt_velocity)
Set max_tilt_velocity value.
LedInterface Fawkes BlackBoard Interface.
void set_status_return_level(unsigned char id, unsigned char status_return_level)
Set status return level.
float tilt() const
Get tilt value.
SetMarginMessage Fawkes BlackBoard Interface Message.
void set_pan(const float new_pan)
Set pan value.
CalibrateMessage Fawkes BlackBoard Interface Message.
static const unsigned int CENTER_POSITION
CENTER_POSITION.
virtual bool prepare_finalize_user()
Prepare finalization user implementation.
TurnOnMessage Fawkes BlackBoard Interface Message.
float intensity() const
Get intensity value.
PanTiltInterface Fawkes BlackBoard Interface.
void set_max_pan(const float new_max_pan)
Set max_pan value.
static const float RAD_PER_POS_TICK
RAD_PER_POS_TICK.
static const unsigned char SRL_RESPOND_READ
SRL_RESPOND_READ.
float time_sec() const
Get time_sec value.
static const unsigned char BROADCAST_ID
BROADCAST_ID.
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.
float deg2rad(float deg)
Convert an angle given in degrees to radians.
void set_tilt_velocity(const float new_tilt_velocity)
Set tilt_velocity value.
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 void finalize()
Finalize the thread.
virtual float get_float(const char *path)=0
Get value from configuration which is of type float.
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
bool is_enabled() const
Get enabled 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.