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> 32 #include <interfaces/JointInterface.h> 53 std::string &ptu_cfg_prefix,
54 std::string &ptu_name)
58 (
std::string(
"PTU ") + ptu_name).c_str()),
62 set_name(
"PanTiltRX28Thread(%s)", ptu_name.c_str());
64 __pantilt_cfg_prefix = pantilt_cfg_prefix;
65 __ptu_cfg_prefix = ptu_cfg_prefix;
66 __ptu_name = ptu_name;
75 __last_pan = __last_tilt = 0.f;
76 float init_pan_velocity = 0.f;
77 float init_tilt_velocity = 0.f;
84 __cfg_read_timeout_ms =
config->
get_uint((__ptu_cfg_prefix +
"read_timeout_ms").c_str());
85 __cfg_disc_timeout_ms =
config->
get_uint((__ptu_cfg_prefix +
"discover_timeout_ms").c_str());
86 __cfg_pan_servo_id =
config->
get_uint((__ptu_cfg_prefix +
"pan_servo_id").c_str());
87 __cfg_tilt_servo_id =
config->
get_uint((__ptu_cfg_prefix +
"tilt_servo_id").c_str());
90 __cfg_goto_zero_start =
config->
get_bool((__ptu_cfg_prefix +
"goto_zero_start").c_str());
91 __cfg_turn_off =
config->
get_bool((__ptu_cfg_prefix +
"turn_off").c_str());
92 __cfg_cw_compl_margin =
config->
get_uint((__ptu_cfg_prefix +
"cw_compl_margin").c_str());
93 __cfg_ccw_compl_margin =
config->
get_uint((__ptu_cfg_prefix +
"ccw_compl_margin").c_str());
94 __cfg_cw_compl_slope =
config->
get_uint((__ptu_cfg_prefix +
"cw_compl_slope").c_str());
95 __cfg_ccw_compl_slope =
config->
get_uint((__ptu_cfg_prefix +
"ccw_compl_slope").c_str());
96 __cfg_pan_min =
config->
get_float((__ptu_cfg_prefix +
"pan_min").c_str());
97 __cfg_pan_max =
config->
get_float((__ptu_cfg_prefix +
"pan_max").c_str());
98 __cfg_tilt_min =
config->
get_float((__ptu_cfg_prefix +
"tilt_min").c_str());
99 __cfg_tilt_max =
config->
get_float((__ptu_cfg_prefix +
"tilt_max").c_str());
100 __cfg_pan_margin =
config->
get_float((__ptu_cfg_prefix +
"pan_margin").c_str());
101 __cfg_tilt_margin =
config->
get_float((__ptu_cfg_prefix +
"tilt_margin").c_str());
102 __cfg_pan_start =
config->
get_float((__ptu_cfg_prefix +
"pan_start").c_str());
103 __cfg_tilt_start =
config->
get_float((__ptu_cfg_prefix +
"tilt_start").c_str());
105 __cfg_publish_transforms=
config->
get_bool((__ptu_cfg_prefix +
"publish_transforms").c_str());
109 if (__cfg_publish_transforms) {
124 std::string frame_id_prefix = std::string(
"") + __ptu_name;
130 __cfg_base_frame = frame_id_prefix +
"/base";
131 __cfg_pan_link = frame_id_prefix +
"/pan";
132 __cfg_tilt_link = frame_id_prefix +
"/tilt";
134 __translation_pan.setValue(pan_trans_x, pan_trans_y, pan_trans_z);
135 __translation_tilt.setValue(tilt_trans_x, tilt_trans_y, tilt_trans_z);
139 bool pan_servo_found =
false, tilt_servo_found =
false;
141 __rx28 =
new RobotisRX28(__cfg_device.c_str(), __cfg_read_timeout_ms);
143 for (RobotisRX28::DeviceList::iterator i = devl.begin(); i != devl.end(); ++i) {
144 if (__cfg_pan_servo_id == *i) {
145 pan_servo_found =
true;
146 }
else if (__cfg_tilt_servo_id == *i) {
147 tilt_servo_found =
true;
150 "configured as pan nor as tilt servo", *i);
158 __cfg_cw_compl_margin, __cfg_cw_compl_slope,
159 __cfg_ccw_compl_margin, __cfg_ccw_compl_slope);
163 if (! (pan_servo_found && tilt_servo_found)) {
164 throw Exception(
"Pan and/or tilt servo not found: pan: %i tilt: %i",
165 pan_servo_found, tilt_servo_found);
169 std::string bbid =
"PanTilt " + __ptu_name;
182 __pantilt_if->
write();
186 std::string panid = __ptu_name +
" pan";
190 __panjoint_if->
write();
192 std::string tiltid = __ptu_name +
" tilt";
196 __tiltjoint_if->
write();
198 __wt =
new WorkerThread(__ptu_name,
logger, __rx28,
199 __cfg_pan_servo_id, __cfg_tilt_servo_id,
200 __cfg_pan_min, __cfg_pan_max, __cfg_tilt_min, __cfg_tilt_max,
201 __cfg_pan_offset, __cfg_tilt_offset);
202 __wt->set_margins(__cfg_pan_margin, __cfg_tilt_margin);
204 __wt->set_enabled(
true);
205 if ( __cfg_goto_zero_start ) {
206 __wt->goto_pantilt_timed(__cfg_pan_start, __cfg_tilt_start, 3.0);
214 #ifdef USE_TIMETRACKER 217 __ttc_read_sensor = __tt->add_class(
"Read Sensor");
226 if (__cfg_turn_off) {
228 __wt->goto_pantilt_timed(0, __cfg_tilt_max, 2.0);
231 __wt->wait_for_fresh_data();
232 __wt->wait_for_fresh_data();
234 while (! __wt->is_final()) {
236 __wt->wait_for_fresh_data();
255 if (__cfg_turn_off) {
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) {
298 __pantilt_if->
set_final(__wt->is_final());
299 __pantilt_if->
write();
303 __panjoint_if->
write();
307 __tiltjoint_if->
write();
310 if (__cfg_publish_transforms) {
312 tf::Quaternion pr; pr.setEulerZYX(pan, 0, 0);
313 tf::Transform ptr(pr, __translation_pan);
314 tf_publisher->send_transform(ptr, time, __cfg_base_frame, __cfg_pan_link);
316 tf::Quaternion tr; tr.setEulerZYX(0, tilt, 0);
317 tf::Transform ttr(tr, __translation_tilt);
318 tf_publisher->send_transform(ttr, time, __cfg_pan_link, __cfg_tilt_link);
328 __pantilt_if->
set_final(__wt->is_final());
337 __wt->goto_pantilt(msg->
pan(), msg->
tilt());
351 __wt->goto_pantilt(0, 0);
387 __pantilt_if->
write();
389 bool write_led_if =
false;
394 __wt->set_led_enabled((msg->
intensity() >= 0.5));
397 __wt->set_led_enabled(
true);
400 __wt->set_led_enabled(
false);
406 if (write_led_if) __led_if->
write();
454 PanTiltRX28Thread::WorkerThread::WorkerThread(std::string ptu_name,
457 unsigned char pan_servo_id,
458 unsigned char tilt_servo_id,
459 float &pan_min,
float &pan_max,
460 float &tilt_min,
float &tilt_max,
461 float &pan_offset,
float &tilt_offset)
462 :
Thread(
"", Thread::OPMODE_WAITFORWAKEUP)
464 set_name(
"RX28WorkerThread(%s)", ptu_name.c_str());
471 __fresh_data_mutex =
new Mutex();
475 __move_pending =
false;
478 __pan_servo_id = pan_servo_id;
479 __tilt_servo_id = tilt_servo_id;
483 __tilt_min = tilt_min;
484 __tilt_max = tilt_max;
485 __pan_offset = pan_offset;
486 __tilt_offset = tilt_offset;
489 __led_enable =
false;
490 __led_disable =
false;
498 PanTiltRX28Thread::WorkerThread::~WorkerThread()
500 delete __value_rwlock;
501 delete __rx28_rwlock;
502 delete __fresh_data_mutex;
503 delete __update_waitcond;
511 PanTiltRX28Thread::WorkerThread::set_enabled(
bool enabled)
529 PanTiltRX28Thread::WorkerThread::set_led_enabled(
bool enabled)
534 __led_disable =
false;
536 __led_enable =
false;
537 __led_disable =
true;
545 PanTiltRX28Thread::WorkerThread::stop_motion()
547 float pan = 0, tilt = 0;
548 get_pantilt(pan, tilt);
549 goto_pantilt(pan, tilt);
558 PanTiltRX28Thread::WorkerThread::goto_pantilt(
float pan,
float tilt)
562 __target_tilt = tilt;
563 __move_pending =
true;
574 PanTiltRX28Thread::WorkerThread::goto_pantilt_timed(
float pan,
float tilt,
float time_sec)
577 __target_tilt = tilt;
578 __move_pending =
true;
580 float cpan=0, ctilt=0;
581 get_pantilt(cpan, ctilt);
583 float pan_diff = fabs(pan - cpan);
584 float tilt_diff = fabs(tilt - ctilt);
586 float req_pan_vel = pan_diff / time_sec;
587 float req_tilt_vel = tilt_diff / time_sec;
593 if (req_pan_vel > __max_pan_speed) {
594 __logger->log_warn(
name(),
"Requested move to (%f, %f) in %f sec requires a " 595 "pan speed of %f rad/s, which is greater than the maximum " 596 "of %f rad/s, reducing to max", pan, tilt, time_sec,
597 req_pan_vel, __max_pan_speed);
598 req_pan_vel = __max_pan_speed;
601 if (req_tilt_vel > __max_tilt_speed) {
602 __logger->log_warn(
name(),
"Requested move to (%f, %f) in %f sec requires a " 603 "tilt speed of %f rad/s, which is greater than the maximum of " 604 "%f rad/s, reducing to max", pan, tilt, time_sec,
605 req_tilt_vel, __max_tilt_speed);
606 req_tilt_vel = __max_tilt_speed;
609 set_velocities(req_pan_vel, req_tilt_vel);
620 PanTiltRX28Thread::WorkerThread::set_velocities(
float pan_vel,
float tilt_vel)
624 float tilt_tmp = roundf((tilt_vel / __max_tilt_speed) * RobotisRX28::MAX_SPEED);
629 if ((pan_tmp >= 0) && (pan_tmp <= RobotisRX28::MAX_SPEED)) {
630 __pan_vel = (
unsigned int)pan_tmp;
631 __velo_pending =
true;
633 __logger->log_warn(
name(),
"Calculated pan value out of bounds, min: 0 max: %u des: %u",
634 RobotisRX28::MAX_SPEED, (
unsigned int)pan_tmp);
637 if ((tilt_tmp >= 0) && (tilt_tmp <= RobotisRX28::MAX_SPEED)) {
638 __tilt_vel = (
unsigned int)tilt_tmp;
639 __velo_pending =
true;
641 __logger->log_warn(
name(),
"Calculated tilt value out of bounds, min: 0 max: %u des: %u",
642 RobotisRX28::MAX_SPEED, (
unsigned int)tilt_tmp);
652 PanTiltRX28Thread::WorkerThread::get_velocities(
float &pan_vel,
float &tilt_vel)
654 unsigned int pan_velticks = __rx28->
get_goal_speed(__pan_servo_id);
655 unsigned int tilt_velticks = __rx28->
get_goal_speed(__tilt_servo_id);
658 tilt_velticks = (
unsigned int)(((
float)tilt_velticks / (float)RobotisRX28::MAX_SPEED) * __max_tilt_speed);
667 PanTiltRX28Thread::WorkerThread::set_margins(
float pan_margin,
float tilt_margin)
669 if (pan_margin > 0.0) __pan_margin = pan_margin;
670 if (tilt_margin > 0.0) __tilt_margin = tilt_margin;
680 PanTiltRX28Thread::WorkerThread::get_pantilt(
float &pan,
float &tilt)
682 ScopedRWLock lock(__rx28_rwlock, ScopedRWLock::LOCK_READ);
685 int tilt_ticks = ((int)__rx28->
get_position(__tilt_servo_id) - (int)RobotisRX28::CENTER_POSITION);
698 PanTiltRX28Thread::WorkerThread::get_pantilt(
float &pan,
float &tilt,
701 get_pantilt(pan, tilt);
702 time = __pantilt_time;
710 PanTiltRX28Thread::WorkerThread::is_final()
713 get_pantilt(pan, tilt);
725 ScopedRWLock lock(__rx28_rwlock, ScopedRWLock::LOCK_READ);
727 return ( (fabs(pan - __target_pan) <= __pan_margin) &&
728 (fabs(tilt - __target_tilt) <= __tilt_margin) ) ||
738 PanTiltRX28Thread::WorkerThread::is_enabled()
750 PanTiltRX28Thread::WorkerThread::has_fresh_data()
754 bool rv = __fresh_data;
755 __fresh_data =
false;
761 PanTiltRX28Thread::WorkerThread::loop()
764 __value_rwlock->lock_for_write();
766 __value_rwlock->unlock();
770 }
else if (__disable) {
771 __value_rwlock->lock_for_write();
773 __value_rwlock->unlock();
775 if (__led_enable || __led_disable || __velo_pending || __move_pending) usleep(3000);
779 __value_rwlock->lock_for_write();
780 __led_enable =
false;
781 __value_rwlock->unlock();
784 if (__velo_pending || __move_pending) usleep(3000);
785 }
else if (__led_disable) {
786 __value_rwlock->lock_for_write();
787 __led_disable =
false;
788 __value_rwlock->unlock();
791 if (__velo_pending || __move_pending) usleep(3000);
794 if (__velo_pending) {
795 __value_rwlock->lock_for_write();
796 __velo_pending =
false;
797 unsigned int pan_vel = __pan_vel;
798 unsigned int tilt_vel = __tilt_vel;
799 __value_rwlock->unlock();
801 __rx28->
set_goal_speeds(2, __pan_servo_id, pan_vel, __tilt_servo_id, tilt_vel);
802 if (__move_pending) usleep(3000);
805 if (__move_pending) {
806 __value_rwlock->lock_for_write();
807 __move_pending =
false;
808 float target_pan = __target_pan;
809 float target_tilt = __target_tilt;
810 __value_rwlock->unlock();
811 exec_goto_pantilt(target_pan, target_tilt);
815 ScopedRWLock lock(__rx28_rwlock, ScopedRWLock::LOCK_READ);
821 __pantilt_time.stamp();
837 __update_waitcond->wake_all();
849 PanTiltRX28Thread::WorkerThread::exec_goto_pantilt(
float pan_rad,
float tilt_rad)
851 if ( (pan_rad < __pan_min) || (pan_rad > __pan_max) ) {
852 __logger->log_warn(
name(),
"Pan value out of bounds, min: %f max: %f des: %f",
853 __pan_min, __pan_max, pan_rad);
856 if ( (tilt_rad < __tilt_min) || (tilt_rad > __tilt_max) ) {
857 __logger->log_warn(
name(),
"Tilt value out of bounds, min: %f max: %f des: %f",
858 __tilt_min, __tilt_max, tilt_rad);
862 unsigned int pan_min = 0, pan_max = 0, tilt_min = 0, tilt_max = 0;
873 if ( (pan_pos < 0) || ((
unsigned int)pan_pos < pan_min) || ((
unsigned int)pan_pos > pan_max) ) {
874 __logger->log_warn(
name(),
"Pan position out of bounds, min: %u max: %u des: %i",
875 pan_min, pan_max, pan_pos);
879 if ( (tilt_pos < 0) || ((
unsigned int)tilt_pos < tilt_min) || ((
unsigned int)tilt_pos > tilt_max) ) {
880 __logger->log_warn(
name(),
"Tilt position out of bounds, min: %u max: %u des: %i",
881 tilt_min, tilt_max, tilt_pos);
886 __rx28->
goto_positions(2, __pan_servo_id, pan_pos, __tilt_servo_id, tilt_pos);
894 PanTiltRX28Thread::WorkerThread::wait_for_fresh_data()
896 __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.
Thread(const char *name)
Constructor.
std::list< unsigned char > DeviceList
List of servo IDs.
TimedGotoMessage Fawkes BlackBoard Interface Message.
Wait until a given condition holds.
unsigned int get_goal_speed(unsigned char id, bool refresh=false)
Get goal speed.
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.
bool is_moving(unsigned char id, bool refresh=false)
Check if servo is moving.
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.
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.
bool is_torque_enabled(unsigned char id, bool refresh=false)
Check if torque is enabled.
void set_intensity(const float new_intensity)
Set intensity value.
void set_goal_speeds(unsigned int num_servos,...)
Set goal speeds for multiple servos.
void get_angle_limits(unsigned char id, unsigned int &cw_limit, unsigned int &ccw_limit, bool refresh=false)
Get angle limits.
void wakeup()
Wake up thread.
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.
unsigned int get_position(unsigned char id, bool refresh=false)
Get current position.
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.
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.
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.
void msgq_flush()
Flush all messages.
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.
void goto_positions(unsigned int num_positions,...)
Move several servos to specified positions.
float time_sec() const
Get time_sec value.
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.
void read_table_values(unsigned char id)
Read all table values for given servo.
virtual void close(Interface *interface)=0
Close interface.