22 #include "rx28_thread.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/JointInterface.h>
31 #include <interfaces/LedInterface.h>
32 #include <interfaces/PanTiltInterface.h>
33 #include <utils/math/angle.h>
54 std::string &ptu_cfg_prefix,
55 std::string &ptu_name)
62 set_name(
"PanTiltRX28Thread(%s)", ptu_name.c_str());
64 pantilt_cfg_prefix_ = pantilt_cfg_prefix;
65 ptu_cfg_prefix_ = ptu_cfg_prefix;
74 last_pan_ = last_tilt_ = 0.f;
75 float init_pan_velocity = 0.f;
76 float init_tilt_velocity = 0.f;
83 cfg_read_timeout_ms_ =
config->
get_uint((ptu_cfg_prefix_ +
"read_timeout_ms").c_str());
84 cfg_disc_timeout_ms_ =
config->
get_uint((ptu_cfg_prefix_ +
"discover_timeout_ms").c_str());
85 cfg_pan_servo_id_ =
config->
get_uint((ptu_cfg_prefix_ +
"pan_servo_id").c_str());
86 cfg_tilt_servo_id_ =
config->
get_uint((ptu_cfg_prefix_ +
"tilt_servo_id").c_str());
89 cfg_goto_zero_start_ =
config->
get_bool((ptu_cfg_prefix_ +
"goto_zero_start").c_str());
90 cfg_turn_off_ =
config->
get_bool((ptu_cfg_prefix_ +
"turn_off").c_str());
91 cfg_cw_compl_margin_ =
config->
get_uint((ptu_cfg_prefix_ +
"cw_compl_margin").c_str());
92 cfg_ccw_compl_margin_ =
config->
get_uint((ptu_cfg_prefix_ +
"ccw_compl_margin").c_str());
93 cfg_cw_compl_slope_ =
config->
get_uint((ptu_cfg_prefix_ +
"cw_compl_slope").c_str());
94 cfg_ccw_compl_slope_ =
config->
get_uint((ptu_cfg_prefix_ +
"ccw_compl_slope").c_str());
95 cfg_pan_min_ =
config->
get_float((ptu_cfg_prefix_ +
"pan_min").c_str());
96 cfg_pan_max_ =
config->
get_float((ptu_cfg_prefix_ +
"pan_max").c_str());
97 cfg_tilt_min_ =
config->
get_float((ptu_cfg_prefix_ +
"tilt_min").c_str());
98 cfg_tilt_max_ =
config->
get_float((ptu_cfg_prefix_ +
"tilt_max").c_str());
99 cfg_pan_margin_ =
config->
get_float((ptu_cfg_prefix_ +
"pan_margin").c_str());
100 cfg_tilt_margin_ =
config->
get_float((ptu_cfg_prefix_ +
"tilt_margin").c_str());
101 cfg_pan_start_ =
config->
get_float((ptu_cfg_prefix_ +
"pan_start").c_str());
102 cfg_tilt_start_ =
config->
get_float((ptu_cfg_prefix_ +
"tilt_start").c_str());
104 cfg_publish_transforms_ =
config->
get_bool((ptu_cfg_prefix_ +
"publish_transforms").c_str());
108 if (cfg_publish_transforms_) {
109 float pan_trans_x =
config->
get_float((ptu_cfg_prefix_ +
"pan_trans_x").c_str());
110 float pan_trans_y =
config->
get_float((ptu_cfg_prefix_ +
"pan_trans_y").c_str());
111 float pan_trans_z =
config->
get_float((ptu_cfg_prefix_ +
"pan_trans_z").c_str());
112 float tilt_trans_x =
config->
get_float((ptu_cfg_prefix_ +
"tilt_trans_x").c_str());
113 float tilt_trans_y =
config->
get_float((ptu_cfg_prefix_ +
"tilt_trans_y").c_str());
114 float tilt_trans_z =
config->
get_float((ptu_cfg_prefix_ +
"tilt_trans_z").c_str());
116 std::string frame_id_prefix = std::string(
"") + ptu_name_;
118 frame_id_prefix =
config->
get_string((ptu_cfg_prefix_ +
"frame_id_prefix").c_str());
122 cfg_base_frame_ = frame_id_prefix +
"/base";
123 cfg_pan_link_ = frame_id_prefix +
"/pan";
124 cfg_tilt_link_ = frame_id_prefix +
"/tilt";
126 translation_pan_.setValue(pan_trans_x, pan_trans_y, pan_trans_z);
127 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 "Servo %u in PTU servo chain, but neither "
143 "configured as pan nor as tilt servo",
152 cfg_cw_compl_margin_,
154 cfg_ccw_compl_margin_,
155 cfg_ccw_compl_slope_);
158 if (!(pan_servo_found && tilt_servo_found)) {
159 throw Exception(
"Pan and/or tilt servo not found: pan: %i tilt: %i",
165 std::string bbid =
"PanTilt " + ptu_name_;
178 pantilt_if_->
write();
182 std::string panid = ptu_name_ +
" pan";
186 panjoint_if_->
write();
188 std::string tiltid = ptu_name_ +
" tilt";
192 tiltjoint_if_->
write();
194 wt_ =
new WorkerThread(ptu_name_,
205 wt_->set_margins(cfg_pan_margin_, cfg_tilt_margin_);
207 wt_->set_enabled(
true);
208 if (cfg_goto_zero_start_) {
209 wt_->goto_pantilt_timed(cfg_pan_start_, cfg_tilt_start_, 3.0);
217 #ifdef USE_TIMETRACKER
220 ttc_read_sensor_ = tt_->add_class(
"Read Sensor");
229 wt_->goto_pantilt_timed(0, cfg_tilt_max_, 2.0);
232 wt_->wait_for_fresh_data();
233 wt_->wait_for_fresh_data();
235 while (!wt_->is_final()) {
237 wt_->wait_for_fresh_data();
278 if (wt_->has_fresh_data()) {
279 float pan = 0, tilt = 0, panvel = 0, tiltvel = 0;
281 wt_->get_pantilt(pan, tilt, time);
282 wt_->get_velocities(panvel, tiltvel);
285 if (fabs(last_pan_ - pan) >= 0.009 || fabs(last_tilt_ - tilt) >= 0.009) {
299 pantilt_if_->
write();
303 panjoint_if_->
write();
307 tiltjoint_if_->
write();
310 if (cfg_publish_transforms_) {
313 pr.setEulerZYX(pan, 0, 0);
314 tf::Transform ptr(pr, translation_pan_);
315 tf_publisher->send_transform(ptr, time, cfg_base_frame_, cfg_pan_link_);
318 tr.setEulerZYX(0, tilt, 0);
319 tf::Transform ttr(tr, translation_tilt_);
320 tf_publisher->send_transform(ttr, time, cfg_pan_link_, cfg_tilt_link_);
338 wt_->goto_pantilt(msg->
pan(), msg->
tilt());
352 wt_->goto_pantilt(0, 0);
366 "Desired pan velocity %f too high, max is %f",
371 "Desired tilt velocity %f too high, max is %f",
392 pantilt_if_->
write();
394 bool write_led_if =
false;
399 wt_->set_led_enabled((msg->
intensity() >= 0.5));
402 wt_->set_led_enabled(
true);
405 wt_->set_led_enabled(
false);
425 logger->
log_info(name(),
"Flushing message queue");
426 pantilt_if_->msgq_flush();
429 logger->
log_info(name(),
"Received message of type %s, enqueueing", message->type());
456 PanTiltRX28Thread::WorkerThread::WorkerThread(std::string ptu_name,
459 unsigned char pan_servo_id,
460 unsigned char tilt_servo_id,
469 set_name(
"RX28WorkerThread(%s)", ptu_name.c_str());
476 fresh_data_mutex_ =
new Mutex();
480 move_pending_ =
false;
483 pan_servo_id_ = pan_servo_id;
484 tilt_servo_id_ = tilt_servo_id;
488 tilt_min_ = tilt_min;
489 tilt_max_ = tilt_max;
490 pan_offset_ = pan_offset;
491 tilt_offset_ = tilt_offset;
495 led_disable_ =
false;
502 PanTiltRX28Thread::WorkerThread::~WorkerThread()
504 delete value_rwlock_;
506 delete fresh_data_mutex_;
507 delete update_waitcond_;
514 PanTiltRX28Thread::WorkerThread::set_enabled(
bool enabled)
531 PanTiltRX28Thread::WorkerThread::set_led_enabled(
bool enabled)
536 led_disable_ =
false;
546 PanTiltRX28Thread::WorkerThread::stop_motion()
548 float pan = 0, tilt = 0;
549 get_pantilt(pan, tilt);
550 goto_pantilt(pan, tilt);
558 PanTiltRX28Thread::WorkerThread::goto_pantilt(
float pan,
float tilt)
563 move_pending_ =
true;
573 PanTiltRX28Thread::WorkerThread::goto_pantilt_timed(
float pan,
float tilt,
float time_sec)
577 move_pending_ =
true;
579 float cpan = 0, ctilt = 0;
580 get_pantilt(cpan, ctilt);
582 float pan_diff = fabs(pan - cpan);
583 float tilt_diff = fabs(tilt - ctilt);
585 float req_pan_vel = pan_diff / time_sec;
586 float req_tilt_vel = tilt_diff / time_sec;
591 if (req_pan_vel > max_pan_speed_) {
592 logger_->log_warn(name(),
593 "Requested move to (%f, %f) in %f sec requires a "
594 "pan speed of %f rad/s, which is greater than the maximum "
595 "of %f rad/s, reducing to max",
601 req_pan_vel = max_pan_speed_;
604 if (req_tilt_vel > max_tilt_speed_) {
605 logger_->log_warn(name(),
606 "Requested move to (%f, %f) in %f sec requires a "
607 "tilt speed of %f rad/s, which is greater than the maximum of "
608 "%f rad/s, reducing to max",
614 req_tilt_vel = max_tilt_speed_;
617 set_velocities(req_pan_vel, req_tilt_vel);
627 PanTiltRX28Thread::WorkerThread::set_velocities(
float pan_vel,
float tilt_vel)
637 pan_vel_ = (
unsigned int)pan_tmp;
638 velo_pending_ =
true;
640 logger_->log_warn(name(),
641 "Calculated pan value out of bounds, min: 0 max: %u des: %u",
643 (
unsigned int)pan_tmp);
647 tilt_vel_ = (
unsigned int)tilt_tmp;
648 velo_pending_ =
true;
650 logger_->log_warn(name(),
651 "Calculated tilt value out of bounds, min: 0 max: %u des: %u",
653 (
unsigned int)tilt_tmp);
662 PanTiltRX28Thread::WorkerThread::get_velocities(
float &pan_vel,
float &tilt_vel)
664 unsigned int pan_velticks = rx28_->get_goal_speed(pan_servo_id_);
665 unsigned int tilt_velticks = rx28_->get_goal_speed(tilt_servo_id_);
678 PanTiltRX28Thread::WorkerThread::set_margins(
float pan_margin,
float tilt_margin)
680 if (pan_margin > 0.0)
681 pan_margin_ = pan_margin;
682 if (tilt_margin > 0.0)
683 tilt_margin_ = tilt_margin;
692 PanTiltRX28Thread::WorkerThread::get_pantilt(
float &pan,
float &tilt)
694 ScopedRWLock lock(rx28_rwlock_, ScopedRWLock::LOCK_READ);
709 PanTiltRX28Thread::WorkerThread::get_pantilt(
float &pan,
float &tilt,
fawkes::Time &time)
711 get_pantilt(pan, tilt);
712 time = pantilt_time_;
719 PanTiltRX28Thread::WorkerThread::is_final()
722 get_pantilt(pan, tilt);
734 ScopedRWLock lock(rx28_rwlock_, ScopedRWLock::LOCK_READ);
736 return ((fabs(pan - target_pan_) <= pan_margin_) && (fabs(tilt - target_tilt_) <= tilt_margin_))
737 || (!rx28_->is_moving(pan_servo_id_) && !rx28_->is_moving(tilt_servo_id_));
744 PanTiltRX28Thread::WorkerThread::is_enabled()
746 return (rx28_->is_torque_enabled(pan_servo_id_) && rx28_->is_torque_enabled(tilt_servo_id_));
754 PanTiltRX28Thread::WorkerThread::has_fresh_data()
758 bool rv = fresh_data_;
764 PanTiltRX28Thread::WorkerThread::loop()
767 value_rwlock_->lock_for_write();
769 value_rwlock_->unlock();
771 rx28_->set_led_enabled(tilt_servo_id_,
true);
772 rx28_->set_torques_enabled(
true, 2, pan_servo_id_, tilt_servo_id_);
773 }
else if (disable_) {
774 value_rwlock_->lock_for_write();
776 value_rwlock_->unlock();
778 if (led_enable_ || led_disable_ || velo_pending_ || move_pending_)
783 value_rwlock_->lock_for_write();
785 value_rwlock_->unlock();
787 rx28_->set_led_enabled(pan_servo_id_,
true);
788 if (velo_pending_ || move_pending_)
790 }
else if (led_disable_) {
791 value_rwlock_->lock_for_write();
792 led_disable_ =
false;
793 value_rwlock_->unlock();
795 rx28_->set_led_enabled(pan_servo_id_,
false);
796 if (velo_pending_ || move_pending_)
801 value_rwlock_->lock_for_write();
802 velo_pending_ =
false;
803 unsigned int pan_vel = pan_vel_;
804 unsigned int tilt_vel = tilt_vel_;
805 value_rwlock_->unlock();
807 rx28_->set_goal_speeds(2, pan_servo_id_, pan_vel, tilt_servo_id_, tilt_vel);
813 value_rwlock_->lock_for_write();
814 move_pending_ =
false;
815 float target_pan = target_pan_;
816 float target_tilt = target_tilt_;
817 value_rwlock_->unlock();
818 exec_goto_pantilt(target_pan, target_tilt);
822 ScopedRWLock lock(rx28_rwlock_, ScopedRWLock::LOCK_READ);
823 rx28_->read_table_values(pan_servo_id_);
824 rx28_->read_table_values(tilt_servo_id_);
828 pantilt_time_.stamp();
844 update_waitcond_->wake_all();
855 PanTiltRX28Thread::WorkerThread::exec_goto_pantilt(
float pan_rad,
float tilt_rad)
857 if ((pan_rad < pan_min_) || (pan_rad > pan_max_)) {
859 name(),
"Pan value out of bounds, min: %f max: %f des: %f", pan_min_, pan_max_, pan_rad);
862 if ((tilt_rad < tilt_min_) || (tilt_rad > tilt_max_)) {
863 logger_->log_warn(name(),
864 "Tilt value out of bounds, min: %f max: %f des: %f",
871 unsigned int pan_min = 0, pan_max = 0, tilt_min = 0, tilt_max = 0;
873 rx28_->get_angle_limits(pan_servo_id_, pan_min, pan_max);
874 rx28_->get_angle_limits(tilt_servo_id_, tilt_min, tilt_max);
881 if ((pan_pos < 0) || ((
unsigned int)pan_pos < pan_min) || ((
unsigned int)pan_pos > pan_max)) {
883 name(),
"Pan position out of bounds, min: %u max: %u des: %i", pan_min, pan_max, pan_pos);
887 if ((tilt_pos < 0) || ((
unsigned int)tilt_pos < tilt_min)
888 || ((
unsigned int)tilt_pos > tilt_max)) {
889 logger_->log_warn(name(),
890 "Tilt position out of bounds, min: %u max: %u des: %i",
898 rx28_->goto_positions(2, pan_servo_id_, pan_pos, tilt_servo_id_, tilt_pos);
905 PanTiltRX28Thread::WorkerThread::wait_for_fresh_data()
907 update_waitcond_->wait();
virtual bool bb_interface_message_received(fawkes::Interface *interface, fawkes::Message *message)
BlackBoard message received notification.
void update_sensor_values()
Update sensor values as necessary.
PanTiltRX28Thread(std::string &pantilt_cfg_prefix, std::string &ptu_cfg_prefix, std::string &ptu_name)
Constructor.
virtual void loop()
Code to execute in the thread.
virtual void init()
Initialize the thread.
virtual void finalize()
Finalize the thread.
virtual bool prepare_finalize_user()
Prepare finalization user implementation.
Class to access a chain of Robotis RX28 servos.
std::list< unsigned char > DeviceList
List of servo IDs.
static const unsigned int MAX_SPEED
MAX_SPEED.
float get_max_supported_speed(unsigned char id, bool refresh=false)
Get maximum supported speed.
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.
static const float RAD_PER_POS_TICK
RAD_PER_POS_TICK.
void set_status_return_level(unsigned char id, unsigned char status_return_level)
Set status return level.
static const float POS_TICKS_PER_RAD
POS_TICKS_PER_RAD.
static const unsigned char BROADCAST_ID
BROADCAST_ID.
void set_torques_enabled(bool enabled, unsigned int num_servos,...)
Enable or disable torque for multiple (selected) servos at once.
static const unsigned char SRL_RESPOND_READ
SRL_RESPOND_READ.
void set_led_enabled(unsigned char id, bool enabled)
Turn LED on or off.
static const unsigned int CENTER_POSITION
CENTER_POSITION.
DeviceList discover(unsigned int total_timeout_ms=50)
Discover devices on the bus.
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 bool get_bool(const char *path)=0
Get value from configuration which is of type bool.
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.
Base class for exceptions in Fawkes.
virtual const char * what() const
Get primary string.
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.
SetIntensityMessage Fawkes BlackBoard Interface Message.
float intensity() const
Get intensity value.
TurnOffMessage Fawkes BlackBoard Interface Message.
TurnOnMessage Fawkes BlackBoard Interface Message.
LedInterface Fawkes BlackBoard Interface.
void set_intensity(const float new_intensity)
Set intensity value.
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
virtual void log_info(const char *component, const char *format,...)=0
Log informational 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.
bool is_enabled() const
Get enabled value.
SetMarginMessage Fawkes BlackBoard Interface Message.
float tilt_margin() const
Get tilt_margin value.
float pan_margin() const
Get pan_margin value.
SetVelocityMessage Fawkes BlackBoard Interface Message.
float tilt_velocity() const
Get tilt_velocity value.
float pan_velocity() const
Get pan_velocity value.
StopMessage Fawkes BlackBoard Interface Message.
TimedGotoMessage Fawkes BlackBoard Interface Message.
float time_sec() const
Get time_sec value.
float tilt() const
Get tilt value.
float pan() const
Get pan value.
PanTiltInterface Fawkes BlackBoard Interface.
void set_enabled(const bool new_enabled)
Set enabled value.
float max_tilt_velocity() const
Get max_tilt_velocity value.
void set_tilt_velocity(const float new_tilt_velocity)
Set tilt_velocity 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_velocity(const float new_max_tilt_velocity)
Set max_tilt_velocity 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_pan_velocity(const float new_pan_velocity)
Set pan_velocity value.
void set_pan_margin(const float new_pan_margin)
Set pan_margin value.
void set_tilt(const float new_tilt)
Set tilt value.
void set_max_pan_velocity(const float new_max_pan_velocity)
Set max_pan_velocity value.
void set_calibrated(const bool new_calibrated)
Set calibrated value.
float max_pan_velocity() const
Get max_pan_velocity value.
void set_tilt_margin(const float new_tilt_margin)
Set tilt_margin value.
void set_pan(const float new_pan)
Set pan value.
Read/write lock to allow multiple readers but only a single writer on the resource at a time.
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.
A class for handling time.
Wait until a given condition holds.
Fawkes library namespace.
float deg2rad(float deg)
Convert an angle given in degrees to radians.