22 #include "driver_thread.h"
24 #include "servo_chain.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/DynamixelServoInterface.h>
31 #include <interfaces/JointInterface.h>
32 #include <interfaces/LedInterface.h>
33 #include <utils/math/angle.h>
34 #include <utils/misc/string_split.h>
54 :
Thread(
"DynamixelDriverThread",
Thread::OPMODE_WAITFORWAKEUP),
57 set_name(
"DynamixelDriverThread(%s)", cfg_name.c_str());
59 cfg_prefix_ = cfg_prefix;
67 cfg_read_timeout_ms_ =
config->
get_uint((cfg_prefix_ +
"read_timeout_ms").c_str());
68 cfg_disc_timeout_ms_ =
config->
get_uint((cfg_prefix_ +
"discover_timeout_ms").c_str());
69 cfg_goto_zero_start_ =
config->
get_bool((cfg_prefix_ +
"goto_zero_start").c_str());
70 cfg_turn_off_ =
config->
get_bool((cfg_prefix_ +
"turn_off").c_str());
71 cfg_cw_compl_margin_ =
config->
get_uint((cfg_prefix_ +
"cw_compl_margin").c_str());
72 cfg_ccw_compl_margin_ =
config->
get_uint((cfg_prefix_ +
"ccw_compl_margin").c_str());
73 cfg_cw_compl_slope_ =
config->
get_uint((cfg_prefix_ +
"cw_compl_slope").c_str());
74 cfg_ccw_compl_slope_ =
config->
get_uint((cfg_prefix_ +
"ccw_compl_slope").c_str());
75 cfg_def_angle_margin_ =
config->
get_float((cfg_prefix_ +
"angle_margin").c_str());
76 cfg_enable_echo_fix_ =
config->
get_bool((cfg_prefix_ +
"enable_echo_fix").c_str());
77 cfg_enable_connection_stability_ =
78 config->
get_bool((cfg_prefix_ +
"enable_connection_stability").c_str());
79 cfg_autorecover_enabled_ =
config->
get_bool((cfg_prefix_ +
"autorecover_enabled").c_str());
80 cfg_autorecover_flags_ =
config->
get_uint((cfg_prefix_ +
"autorecover_flags").c_str());
81 cfg_torque_limit_ =
config->
get_float((cfg_prefix_ +
"torque_limit").c_str());
82 cfg_temperature_limit_ =
config->
get_uint((cfg_prefix_ +
"temperature_limit").c_str());
83 cfg_prevent_alarm_shutdown_ =
config->
get_bool((cfg_prefix_ +
"prevent_alarm_shutdown").c_str());
84 cfg_prevent_alarm_shutdown_threshold_ =
85 config->
get_float((cfg_prefix_ +
"prevent_alarm_shutdown_threshold").c_str());
86 cfg_min_voltage_ =
config->
get_float((cfg_prefix_ +
"min_voltage").c_str());
87 cfg_max_voltage_ =
config->
get_float((cfg_prefix_ +
"max_voltage").c_str());
88 cfg_servos_to_discover_ =
config->
get_uints((cfg_prefix_ +
"servos").c_str());
89 cfg_enable_verbose_output_ =
config->
get_bool((cfg_prefix_ +
"enable_verbose_output").c_str());
94 cfg_enable_connection_stability_,
98 std::list<std::string> found_servos;
99 for (DynamixelChain::DeviceList::iterator i = devl.begin(); i != devl.end(); ++i) {
100 found_servos.push_back(std::to_string(*i));
124 s.move_pending =
false;
125 s.mode_set_pending =
false;
126 s.recover_pending =
false;
128 s.velo_pending =
false;
132 s.led_enable =
false;
133 s.led_disable =
false;
135 s.torque_limit = cfg_torque_limit_ * 0x3ff;
137 s.angle_margin = cfg_def_angle_margin_;
145 fresh_data_mutex_ =
new Mutex();
148 if (servos_.empty()) {
149 throw Exception(
"No servos found in chain %s", cfg_name_.c_str());
156 cfg_cw_compl_margin_,
158 cfg_ccw_compl_margin_,
159 cfg_ccw_compl_slope_);
164 for (
auto &sp : servos_) {
165 unsigned int servo_id = sp.first;
166 Servo & s = sp.second;
175 unsigned int cw_limit, ccw_limit;
178 unsigned char cw_margin, cw_slope, ccw_margin, ccw_slope;
181 s.servo_if->set_model(chain_->
get_model(servo_id));
183 s.servo_if->set_cw_angle_limit(cw_limit);
184 s.servo_if->set_ccw_angle_limit(ccw_limit);
187 s.servo_if->set_mode(cw_limit == ccw_limit && cw_limit == 0 ?
"WHEEL" :
"JOINT");
188 s.servo_if->set_cw_slope(cw_slope);
189 s.servo_if->set_ccw_slope(ccw_slope);
190 s.servo_if->set_cw_margin(cw_margin);
191 s.servo_if->set_ccw_margin(ccw_margin);
192 s.servo_if->set_torque_limit(s.torque_limit);
193 s.servo_if->set_max_velocity(s.max_speed);
194 s.servo_if->set_enable_prevent_alarm_shutdown(cfg_prevent_alarm_shutdown_);
195 s.servo_if->set_autorecover_enabled(cfg_autorecover_enabled_);
198 s.servo_if->set_auto_timestamping(
false);
201 if (cfg_goto_zero_start_) {
202 for (
auto &s : servos_) {
203 goto_angle_timed(s.first, 0., 3.0);
215 for (
auto &s : servos_) {
221 delete chain_rwlock_;
222 delete fresh_data_mutex_;
223 delete update_waitcond_;
226 for (
auto &s : servos_) {
233 name(),
"Failed to turn of servo %s:%u: %s", cfg_name_.c_str(), s.first, e.
what());
251 if (has_fresh_data()) {
252 for (
auto &sp : servos_) {
253 unsigned int servo_id = sp.first;
254 Servo & s = sp.second;
257 float angle = get_angle(servo_id, time);
258 float vel = get_velocity(servo_id);
261 if (fabs(s.last_angle - angle) >=
deg2rad(0.5)) {
262 s.last_angle = angle;
264 angle = s.last_angle;
267 ScopedRWLock lock(chain_rwlock_, ScopedRWLock::LOCK_READ);
269 s.servo_if->set_timestamp(&s.time);
270 s.servo_if->set_position(chain_->
get_position(servo_id));
271 s.servo_if->set_speed(chain_->
get_speed(servo_id));
274 s.servo_if->set_load(chain_->
get_load(servo_id));
275 s.servo_if->set_voltage(chain_->
get_voltage(servo_id));
277 s.servo_if->set_punch(chain_->
get_punch(servo_id));
278 s.servo_if->set_angle(angle);
279 s.servo_if->set_velocity(vel);
281 s.servo_if->set_final(is_final(servo_id));
282 s.servo_if->set_velocity(get_velocity(servo_id));
285 if (s.servo_if->is_enable_prevent_alarm_shutdown()) {
286 if ((chain_->
get_load(servo_id) & 0x3ff)
287 > (cfg_prevent_alarm_shutdown_threshold_ * chain_->
get_torque_limit(servo_id))) {
289 "Servo with ID: %d is in overload condition: torque_limit: %d, load: %d",
292 chain_->
get_load(servo_id) & 0x3ff);
294 if (chain_->
get_load(servo_id) & 0x400) {
295 goto_angle(servo_id, get_angle(servo_id) + 0.001);
297 goto_angle(servo_id, get_angle(servo_id) - 0.001);
302 if (s.servo_if->is_autorecover_enabled()
303 && chain_->
get_error(servo_id) & cfg_autorecover_flags_) {
305 s.recover_pending =
true;
308 unsigned char cur_error = chain_->
get_error(servo_id);
309 s.servo_if->set_error(s.servo_if->error() | cur_error);
315 s.joint_if->set_position(angle);
316 s.joint_if->set_velocity(vel);
326 for (
auto &sp : servos_) {
327 unsigned int servo_id = sp.first;
328 Servo & s = sp.second;
330 s.servo_if->set_final(is_final(servo_id));
332 while (!s.servo_if->msgq_empty()) {
336 goto_angle(servo_id, msg->
angle());
337 s.servo_if->set_msgid(msg->
id());
338 s.servo_if->set_final(
false);
344 s.servo_if->set_msgid(msg->
id());
345 s.servo_if->set_final(
false);
355 if (msg->
velocity() > s.servo_if->max_velocity()) {
357 "Desired velocity %f too high, max is %f",
359 s.servo_if->max_velocity());
361 set_velocity(servo_id, msg->
velocity());
371 s.servo_if->set_error(0);
373 }
else if (s.servo_if
380 set_mode(servo_id, msg->
mode());
384 set_speed(servo_id, msg->
speed());
386 }
else if (s.servo_if
393 s.recover_pending =
true;
397 s.recover_pending =
true;
403 s.servo_if->msgq_pop();
408 bool write_led_if =
false;
409 while (!s.led_if->msgq_empty()) {
413 set_led_enabled(servo_id, (msg->
intensity() >= 0.5));
414 s.led_if->set_intensity((msg->
intensity() >= 0.5) ? LedInterface::ON : LedInterface::OFF);
416 set_led_enabled(servo_id,
true);
417 s.led_if->set_intensity(LedInterface::ON);
419 set_led_enabled(servo_id,
false);
420 s.led_if->set_intensity(LedInterface::OFF);
423 s.led_if->msgq_pop();
433 std::map<unsigned int, Servo>::iterator si =
434 std::find_if(servos_.begin(),
436 [interface](
const std::pair<unsigned int, Servo> &sp) {
437 return (strcmp(sp.second.servo_if->uid(), interface->uid()) == 0);
439 if (si != servos_.end()) {
441 stop_motion(si->first);
444 stop_motion(si->first);
445 if (cfg_enable_verbose_output_) {
446 logger->
log_info(name(),
"Flushing message queue");
448 si->second.servo_if->msgq_flush();
451 if (cfg_enable_verbose_output_) {
452 logger->
log_info(name(),
"Received message of type %s, enqueueing", message->type());
464 DynamixelDriverThread::set_enabled(
unsigned int servo_id,
bool enabled)
466 if (servos_.find(servo_id) == servos_.end()) {
468 "No servo with ID %u in chain %s, cannot set LED",
474 Servo &s = servos_[servo_id];
493 DynamixelDriverThread::set_led_enabled(
unsigned int servo_id,
bool enabled)
495 if (servos_.find(servo_id) == servos_.end()) {
497 "No servo with ID %u in chain %s, cannot set LED",
503 Servo &s = servos_[servo_id];
508 s.led_disable =
false;
510 s.led_enable =
false;
511 s.led_disable =
true;
519 DynamixelDriverThread::stop_motion(
unsigned int servo_id)
521 if (servos_.find(servo_id) == servos_.end()) {
523 "No servo with ID %u in chain %s, cannot set LED",
529 float angle = get_angle(servo_id);
530 goto_angle(servo_id, angle);
537 DynamixelDriverThread::goto_angle(
unsigned int servo_id,
float angle)
539 if (servos_.find(servo_id) == servos_.end()) {
541 "No servo with ID %u in chain %s, cannot set LED",
547 Servo &s = servos_[servo_id];
551 s.target_angle = angle;
552 s.move_pending =
true;
562 DynamixelDriverThread::goto_angle_timed(
unsigned int servo_id,
float angle,
float time_sec)
564 if (servos_.find(servo_id) == servos_.end()) {
566 "No servo with ID %u in chain %s, cannot set LED",
571 Servo &s = servos_[servo_id];
573 s.target_angle = angle;
574 s.move_pending =
true;
576 float cangle = get_angle(servo_id);
577 float angle_diff = fabs(angle - cangle);
578 float req_angle_vel = angle_diff / time_sec;
580 if (req_angle_vel > s.max_speed) {
582 "Requested move to %f in %f sec requires a "
583 "angle speed of %f rad/s, which is greater than the maximum "
584 "of %f rad/s, reducing to max",
589 req_angle_vel = s.max_speed;
591 set_velocity(servo_id, req_angle_vel);
600 DynamixelDriverThread::set_velocity(
unsigned int servo_id,
float vel)
602 if (servos_.find(servo_id) == servos_.end()) {
604 "No servo with ID %u in chain %s, cannot set velocity",
609 Servo &s = servos_[servo_id];
612 set_speed(servo_id, (
unsigned int)velo_tmp);
621 DynamixelDriverThread::set_speed(
unsigned int servo_id,
unsigned int speed)
623 if (servos_.find(servo_id) == servos_.end()) {
625 "No servo with ID %u in chain %s, cannot set speed",
630 Servo &s = servos_[servo_id];
635 s.velo_pending =
true;
638 "Calculated velocity value out of bounds, "
639 "min: 0 max: %u des: %u",
649 DynamixelDriverThread::set_mode(
unsigned int servo_id,
unsigned int mode)
651 if (servos_.find(servo_id) == servos_.end()) {
653 "No servo with ID %u in chain %s, cannot set mode",
658 Servo &s = servos_[servo_id];
661 s.mode_set_pending =
true;
663 s.servo_if->set_mode(mode == DynamixelServoInterface::JOINT ?
"JOINT" :
"WHEEL");
669 DynamixelDriverThread::get_velocity(
unsigned int servo_id)
671 if (servos_.find(servo_id) == servos_.end()) {
673 "No servo with ID %u in chain %s, cannot set velocity",
678 Servo &s = servos_[servo_id];
680 unsigned int velticks = chain_->
get_speed(servo_id);
689 DynamixelDriverThread::set_margin(
unsigned int servo_id,
float angle_margin)
691 if (servos_.find(servo_id) == servos_.end()) {
693 "No servo with ID %u in chain %s, cannot set velocity",
698 Servo &s = servos_[servo_id];
699 if (angle_margin > 0.0)
700 s.angle_margin = angle_margin;
706 DynamixelDriverThread::get_angle(
unsigned int servo_id)
708 if (servos_.find(servo_id) == servos_.end()) {
710 "No servo with ID %u in chain %s, cannot set velocity",
716 ScopedRWLock lock(chain_rwlock_, ScopedRWLock::LOCK_READ);
727 DynamixelDriverThread::get_angle(
unsigned int servo_id,
fawkes::Time &time)
729 if (servos_.find(servo_id) == servos_.end()) {
731 "No servo with ID %u in chain %s, cannot set velocity",
736 Servo &s = servos_[servo_id];
739 return get_angle(servo_id);
746 DynamixelDriverThread::is_final(
unsigned int servo_id)
748 if (servos_.find(servo_id) == servos_.end()) {
750 "No servo with ID %u in chain %s, cannot set velocity",
755 Servo &s = servos_[servo_id];
757 float angle = get_angle(servo_id);
759 ScopedRWLock lock(chain_rwlock_, ScopedRWLock::LOCK_READ);
761 return ((fabs(angle - s.target_angle) <= s.angle_margin) || (!chain_->
is_moving(servo_id)));
768 DynamixelDriverThread::is_enabled(
unsigned int servo_id)
778 DynamixelDriverThread::has_fresh_data()
782 bool rv = fresh_data_;
790 for (
auto &sp : servos_) {
791 unsigned int servo_id = sp.first;
792 Servo & s = sp.second;
794 s.value_rwlock->lock_for_write();
796 s.value_rwlock->unlock();
800 if (s.led_enable || s.led_disable || s.velo_pending || s.move_pending || s.mode_set_pending
801 || s.recover_pending)
803 }
else if (s.disable) {
804 s.value_rwlock->lock_for_write();
806 s.value_rwlock->unlock();
809 if (s.led_enable || s.led_disable || s.velo_pending || s.move_pending || s.mode_set_pending
810 || s.recover_pending)
815 s.value_rwlock->lock_for_write();
816 s.led_enable =
false;
817 s.value_rwlock->unlock();
820 if (s.velo_pending || s.move_pending || s.mode_set_pending || s.recover_pending)
822 }
else if (s.led_disable) {
823 s.value_rwlock->lock_for_write();
824 s.led_disable =
false;
825 s.value_rwlock->unlock();
828 if (s.velo_pending || s.move_pending || s.mode_set_pending || s.recover_pending)
832 if (s.velo_pending) {
833 s.value_rwlock->lock_for_write();
834 s.velo_pending =
false;
835 unsigned int vel = s.vel;
836 s.value_rwlock->unlock();
839 if (s.move_pending || s.mode_set_pending || s.recover_pending)
843 if (s.move_pending) {
844 s.value_rwlock->lock_for_write();
845 s.move_pending =
false;
846 float target_angle = s.target_angle;
847 s.value_rwlock->unlock();
848 exec_goto_angle(servo_id, target_angle);
849 if (s.mode_set_pending || s.recover_pending)
853 if (s.mode_set_pending) {
854 s.value_rwlock->lock_for_write();
855 s.mode_set_pending =
false;
856 exec_set_mode(servo_id, s.new_mode);
857 s.value_rwlock->unlock();
858 if (s.recover_pending)
862 if (s.recover_pending) {
863 s.value_rwlock->lock_for_write();
864 s.recover_pending =
false;
866 s.value_rwlock->unlock();
870 ScopedRWLock lock(chain_rwlock_, ScopedRWLock::LOCK_READ);
893 DynamixelDriverThread::exec_goto_angle(
unsigned int servo_id,
float angle_rad)
895 unsigned int pos_min = 0, pos_max = 0;
901 if ((pos < 0) || ((
unsigned int)pos < pos_min) || ((
unsigned int)pos > pos_max)) {
903 name(),
"Position out of bounds, min: %u max: %u des: %i", pos_min, pos_max, pos);
915 DynamixelDriverThread::exec_set_mode(
unsigned int servo_id,
unsigned int new_mode)
917 if (new_mode == DynamixelServoInterface::JOINT) {
920 }
else if (new_mode == DynamixelServoInterface::WHEEL) {
934 DynamixelDriverThread::wait_for_fresh_data()
936 update_waitcond_->
wait();