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;
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();