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.
TurnOffMessage Fawkes BlackBoard Interface Message.
virtual void loop()
Code to execute in the thread.
std::list< unsigned char > DeviceList
List of servo IDs.
TimedGotoMessage Fawkes BlackBoard Interface Message.
Wait until a given condition holds.
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.
void update_sensor_values()
Update sensor values as necessary.
float intensity() const
Get intensity value.
void set_pan_velocity(const float new_pan_velocity)
Set pan_velocity value.
Fawkes library namespace.
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.
float tilt_velocity() const
Get tilt_velocity 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.
A class for handling time.
float tilt() const
Get tilt value.
virtual const char * what() const
Get primary string.
virtual void unregister_listener(BlackBoardInterfaceListener *listener)
Unregister BB interface listener.
Thread class encapsulation of pthreads.
void write()
Write from local copy into BlackBoard memory.
Base class for all Fawkes BlackBoard interfaces.
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.
float tilt_margin() const
Get tilt_margin value.
SetVelocityMessage Fawkes BlackBoard Interface Message.
float pan_velocity() const
Get pan_velocity value.
float max_tilt_velocity() const
Get max_tilt_velocity value.
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.
float pan() const
Get pan value.
void set_name(const char *format,...)
Set name of thread.
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_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.
float max_pan_velocity() const
Get max_pan_velocity value.
bool is_enabled() const
Get enabled value.
SetIntensityMessage Fawkes BlackBoard Interface Message.
const char * name() const
Get name of thread.
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_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 log_info(const char *component, const char *format,...)
Log informational message.
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.
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.
PanTiltInterface Fawkes BlackBoard Interface.
void set_max_pan(const float new_max_pan)
Set max_pan value.
float tilt() const
Get tilt value.
static const float RAD_PER_POS_TICK
RAD_PER_POS_TICK.
void set_velocity(const float new_velocity)
Set velocity value.
static const unsigned char SRL_RESPOND_READ
SRL_RESPOND_READ.
JointInterface Fawkes BlackBoard Interface.
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.
float pan_margin() const
Get pan_margin 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 Interface * open_for_writing(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for writing.
virtual float get_float(const char *path)=0
Get value from configuration which is of type float.
float time_sec() const
Get time_sec value.
void set_torques_enabled(bool enabled, unsigned int num_servos,...)
Enable or disable torque for multiple (selected) servos at once.
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.