Fawkes API  Fawkes Development Version
driver_thread.cpp
1 
2 /***************************************************************************
3  * driver_thread.cpp - Robotis dynamixel servo driver thread
4  *
5  * Created: Mon Mar 23 20:37:32 2015 (based on pantilt plugin)
6  * Copyright 2006-2015 Tim Niemueller [www.niemueller.de]
7  ****************************************************************************/
8 
9 /* This program is free software; you can redistribute it and/or modify
10  * it under the terms of the GNU General Public License as published by
11  * the Free Software Foundation; either version 2 of the License, or
12  * (at your option) any later version.
13  *
14  * This program is distributed in the hope that it will be useful,
15  * but WITHOUT ANY WARRANTY; without even the implied warranty of
16  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
17  * GNU Library General Public License for more details.
18  *
19  * Read the full text in the LICENSE.GPL file in the doc directory.
20  */
21 
22 #include "driver_thread.h"
23 
24 #include "servo_chain.h"
25 
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>
35 
36 #include <algorithm>
37 #include <cmath>
38 #include <cstdarg>
39 #include <cstring>
40 #include <unistd.h>
41 
42 using namespace fawkes;
43 
44 /** @class DynamixelDriverThread "driver_thread.h"
45  * Driver thread for Robotis dynamixel servos.
46  * @author Tim Niemueller
47  */
48 
49 /** Constructor.
50  * @param cfg_prefix configuration prefix specific for the servo chain
51  * @param cfg_name name of the servo configuration
52  */
53 DynamixelDriverThread::DynamixelDriverThread(std::string &cfg_name, std::string &cfg_prefix)
54 : Thread("DynamixelDriverThread", Thread::OPMODE_WAITFORWAKEUP),
55  BlackBoardInterfaceListener("DynamixelDriverThread(%s)", cfg_name.c_str())
56 {
57  set_name("DynamixelDriverThread(%s)", cfg_name.c_str());
58 
59  cfg_prefix_ = cfg_prefix;
60  cfg_name_ = cfg_name;
61 }
62 
63 void
65 {
66  cfg_device_ = config->get_string((cfg_prefix_ + "device").c_str());
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());
90 
91  chain_ = new DynamixelChain(cfg_device_.c_str(),
92  cfg_read_timeout_ms_,
93  cfg_enable_echo_fix_,
94  cfg_enable_connection_stability_,
95  cfg_min_voltage_,
96  cfg_max_voltage_);
97  DynamixelChain::DeviceList devl = chain_->discover(cfg_disc_timeout_ms_, cfg_servos_to_discover_);
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));
101  Servo s;
102  s.servo_if = NULL;
103  s.led_if = NULL;
104  s.joint_if = NULL;
105 
106  try {
107  s.servo_if = blackboard->open_for_writing_f<DynamixelServoInterface>("/dynamixel/%s/%u",
108  cfg_name_.c_str(),
109  *i);
110  s.led_if =
111  blackboard->open_for_writing_f<LedInterface>("/dynamixel/%s/%u", cfg_name_.c_str(), *i);
112  s.joint_if =
113  blackboard->open_for_writing_f<JointInterface>("/dynamixel/%s/%u", cfg_name_.c_str(), *i);
114 
115  bbil_add_message_interface(s.servo_if);
116 
117  } catch (Exception &e) {
118  blackboard->close(s.servo_if);
119  blackboard->close(s.led_if);
120  blackboard->close(s.joint_if);
121  throw;
122  }
123 
124  s.move_pending = false;
125  s.mode_set_pending = false;
126  s.recover_pending = false;
127  s.target_angle = 0;
128  s.velo_pending = false;
129  s.vel = 0.;
130  s.enable = false;
131  s.disable = false;
132  s.led_enable = false;
133  s.led_disable = false;
134  s.last_angle = 0.f;
135  s.torque_limit = cfg_torque_limit_ * 0x3ff;
136  s.value_rwlock = new ReadWriteLock();
137  s.angle_margin = cfg_def_angle_margin_;
138 
139  servos_[*i] = s;
140  }
141 
142  logger->log_info(name(), "Found servos [%s]", str_join(found_servos, ",").c_str());
143 
144  chain_rwlock_ = new ReadWriteLock();
145  fresh_data_mutex_ = new Mutex();
146  update_waitcond_ = new WaitCondition();
147 
148  if (servos_.empty()) {
149  throw Exception("No servos found in chain %s", cfg_name_.c_str());
150  }
151 
152  // We only want responses to be sent on explicit READ to speed up communication
154  // set compliance values
156  cfg_cw_compl_margin_,
157  cfg_cw_compl_slope_,
158  cfg_ccw_compl_margin_,
159  cfg_ccw_compl_slope_);
160 
161  // set temperature limit
162  chain_->set_temperature_limit(DynamixelChain::BROADCAST_ID, cfg_temperature_limit_);
163 
164  for (auto &sp : servos_) {
165  unsigned int servo_id = sp.first;
166  Servo & s = sp.second;
167 
168  chain_->set_led_enabled(servo_id, false);
169  chain_->set_torque_enabled(servo_id, true);
170 
171  chain_->read_table_values(servo_id);
172 
173  s.max_speed = chain_->get_max_supported_speed(servo_id);
174 
175  unsigned int cw_limit, ccw_limit;
176  chain_->get_angle_limits(servo_id, cw_limit, ccw_limit);
177 
178  unsigned char cw_margin, cw_slope, ccw_margin, ccw_slope;
179  chain_->get_compliance_values(servo_id, cw_margin, cw_slope, ccw_margin, ccw_slope);
180 
181  s.servo_if->set_model(chain_->get_model(servo_id));
182  s.servo_if->set_model_number(chain_->get_model_number(servo_id));
183  s.servo_if->set_cw_angle_limit(cw_limit);
184  s.servo_if->set_ccw_angle_limit(ccw_limit);
185  s.servo_if->set_temperature_limit(chain_->get_temperature_limit(servo_id));
186  s.servo_if->set_max_torque(chain_->get_max_torque(servo_id));
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_);
196  s.servo_if->write();
197 
198  s.servo_if->set_auto_timestamping(false);
199  }
200 
201  if (cfg_goto_zero_start_) {
202  for (auto &s : servos_) {
203  goto_angle_timed(s.first, 0., 3.0);
204  }
205  }
206 
208 }
209 
210 void
212 {
214 
215  for (auto &s : servos_) {
216  blackboard->close(s.second.servo_if);
217  blackboard->close(s.second.led_if);
218  blackboard->close(s.second.joint_if);
219  }
220 
221  delete chain_rwlock_;
222  delete fresh_data_mutex_;
223  delete update_waitcond_;
224 
225  if (cfg_turn_off_) {
226  for (auto &s : servos_) {
227  try {
228  logger->log_debug(name(), "Turning off servo %s:%u", cfg_name_.c_str(), s.first);
229  chain_->set_led_enabled(s.first, false);
230  chain_->set_torque_enabled(s.first, false);
231  } catch (Exception &e) {
232  logger->log_warn(
233  name(), "Failed to turn of servo %s:%u: %s", cfg_name_.c_str(), s.first, e.what());
234  }
235  }
236  // Give some time for shutdown comands to get through
237  usleep(10000);
238  }
239 
240  // Setting to NULL deletes instance (RefPtr)
241  chain_ = NULL;
242 }
243 
244 /** Update sensor values as necessary.
245  * To be called only from DynamixelSensorThread. Writes the current servo
246  * data into the interface.
247  */
248 void
250 {
251  if (has_fresh_data()) {
252  for (auto &sp : servos_) {
253  unsigned int servo_id = sp.first;
254  Servo & s = sp.second;
255 
256  fawkes::Time time;
257  float angle = get_angle(servo_id, time);
258  float vel = get_velocity(servo_id);
259 
260  // poor man's filter: only update if we get a change of least half a degree
261  if (fabs(s.last_angle - angle) >= deg2rad(0.5)) {
262  s.last_angle = angle;
263  } else {
264  angle = s.last_angle;
265  }
266 
267  ScopedRWLock lock(chain_rwlock_, ScopedRWLock::LOCK_READ);
268 
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));
272  s.servo_if->set_goal_position(chain_->get_goal_position(servo_id));
273  s.servo_if->set_goal_speed(chain_->get_goal_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));
276  s.servo_if->set_temperature(chain_->get_temperature(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);
280  s.servo_if->set_enabled(chain_->is_torque_enabled(servo_id));
281  s.servo_if->set_final(is_final(servo_id));
282  s.servo_if->set_velocity(get_velocity(servo_id));
283  s.servo_if->set_alarm_shutdown(chain_->get_alarm_shutdown(servo_id));
284 
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))) {
288  logger->log_warn(name(),
289  "Servo with ID: %d is in overload condition: torque_limit: %d, load: %d",
290  servo_id,
291  chain_->get_torque_limit(servo_id),
292  chain_->get_load(servo_id) & 0x3ff);
293  // is the current load cw or ccw?
294  if (chain_->get_load(servo_id) & 0x400) {
295  goto_angle(servo_id, get_angle(servo_id) + 0.001);
296  } else {
297  goto_angle(servo_id, get_angle(servo_id) - 0.001);
298  }
299  }
300  }
301 
302  if (s.servo_if->is_autorecover_enabled()
303  && chain_->get_error(servo_id) & cfg_autorecover_flags_) {
304  logger->log_warn(name(), "Recovery for servo with ID: %d pending", servo_id);
305  s.recover_pending = true;
306  }
307 
308  unsigned char cur_error = chain_->get_error(servo_id);
309  s.servo_if->set_error(s.servo_if->error() | cur_error);
310  if (cur_error) {
311  logger->log_error(name(), "Servo with ID: %d has error-flag: %d", servo_id, cur_error);
312  }
313  s.servo_if->write();
314 
315  s.joint_if->set_position(angle);
316  s.joint_if->set_velocity(vel);
317  s.joint_if->write();
318  }
319  }
320 }
321 
322 /** Process commands. */
323 void
325 {
326  for (auto &sp : servos_) {
327  unsigned int servo_id = sp.first;
328  Servo & s = sp.second;
329 
330  s.servo_if->set_final(is_final(servo_id));
331 
332  while (!s.servo_if->msgq_empty()) {
333  if (s.servo_if->msgq_first_is<DynamixelServoInterface::GotoMessage>()) {
334  DynamixelServoInterface::GotoMessage *msg = s.servo_if->msgq_first(msg);
335 
336  goto_angle(servo_id, msg->angle());
337  s.servo_if->set_msgid(msg->id());
338  s.servo_if->set_final(false);
339 
340  } else if (s.servo_if->msgq_first_is<DynamixelServoInterface::TimedGotoMessage>()) {
341  DynamixelServoInterface::TimedGotoMessage *msg = s.servo_if->msgq_first(msg);
342 
343  goto_angle_timed(servo_id, msg->angle(), msg->time_sec());
344  s.servo_if->set_msgid(msg->id());
345  s.servo_if->set_final(false);
346 
347  } else if (s.servo_if->msgq_first_is<DynamixelServoInterface::SetEnabledMessage>()) {
348  DynamixelServoInterface::SetEnabledMessage *msg = s.servo_if->msgq_first(msg);
349 
350  set_enabled(servo_id, msg->is_enabled());
351 
352  } else if (s.servo_if->msgq_first_is<DynamixelServoInterface::SetVelocityMessage>()) {
353  DynamixelServoInterface::SetVelocityMessage *msg = s.servo_if->msgq_first(msg);
354 
355  if (msg->velocity() > s.servo_if->max_velocity()) {
356  logger->log_warn(name(),
357  "Desired velocity %f too high, max is %f",
358  msg->velocity(),
359  s.servo_if->max_velocity());
360  } else {
361  set_velocity(servo_id, msg->velocity());
362  }
363 
364  } else if (s.servo_if->msgq_first_is<DynamixelServoInterface::SetMarginMessage>()) {
365  DynamixelServoInterface::SetMarginMessage *msg = s.servo_if->msgq_first(msg);
366 
367  set_margin(servo_id, msg->angle_margin());
368  s.servo_if->set_angle_margin(msg->angle_margin());
369 
370  } else if (s.servo_if->msgq_first_is<DynamixelServoInterface::ResetRawErrorMessage>()) {
371  s.servo_if->set_error(0);
372 
373  } else if (s.servo_if
375  DynamixelServoInterface::SetPreventAlarmShutdownMessage *msg = s.servo_if->msgq_first(msg);
377 
378  } else if (s.servo_if->msgq_first_is<DynamixelServoInterface::SetModeMessage>()) {
379  DynamixelServoInterface::SetModeMessage *msg = s.servo_if->msgq_first(msg);
380  set_mode(servo_id, msg->mode());
381 
382  } else if (s.servo_if->msgq_first_is<DynamixelServoInterface::SetSpeedMessage>()) {
383  DynamixelServoInterface::SetSpeedMessage *msg = s.servo_if->msgq_first(msg);
384  set_speed(servo_id, msg->speed());
385 
386  } else if (s.servo_if
388  DynamixelServoInterface::SetAutorecoverEnabledMessage *msg = s.servo_if->msgq_first(msg);
390 
391  } else if (s.servo_if->msgq_first_is<DynamixelServoInterface::SetTorqueLimitMessage>()) {
392  DynamixelServoInterface::SetTorqueLimitMessage *msg = s.servo_if->msgq_first(msg);
393  s.recover_pending = true;
394  s.torque_limit = msg->torque_limit();
395 
396  } else if (s.servo_if->msgq_first_is<DynamixelServoInterface::RecoverMessage>()) {
397  s.recover_pending = true;
398 
399  } else {
400  logger->log_warn(name(), "Unknown message received");
401  }
402 
403  s.servo_if->msgq_pop();
404  }
405 
406  s.servo_if->write();
407 
408  bool write_led_if = false;
409  while (!s.led_if->msgq_empty()) {
410  write_led_if = true;
411  if (s.led_if->msgq_first_is<LedInterface::SetIntensityMessage>()) {
412  LedInterface::SetIntensityMessage *msg = s.led_if->msgq_first(msg);
413  set_led_enabled(servo_id, (msg->intensity() >= 0.5));
414  s.led_if->set_intensity((msg->intensity() >= 0.5) ? LedInterface::ON : LedInterface::OFF);
415  } else if (s.led_if->msgq_first_is<LedInterface::TurnOnMessage>()) {
416  set_led_enabled(servo_id, true);
417  s.led_if->set_intensity(LedInterface::ON);
418  } else if (s.led_if->msgq_first_is<LedInterface::TurnOffMessage>()) {
419  set_led_enabled(servo_id, false);
420  s.led_if->set_intensity(LedInterface::OFF);
421  }
422 
423  s.led_if->msgq_pop();
424  }
425  if (write_led_if)
426  s.led_if->write();
427  }
428 }
429 
430 bool
432  Message * message) noexcept
433 {
434  std::map<unsigned int, Servo>::iterator si =
435  std::find_if(servos_.begin(),
436  servos_.end(),
437  [interface](const std::pair<unsigned int, Servo> &sp) {
438  return (strcmp(sp.second.servo_if->uid(), interface->uid()) == 0);
439  });
440  if (si != servos_.end()) {
441  if (message->is_of_type<DynamixelServoInterface::StopMessage>()) {
442  stop_motion(si->first);
443  return false; // do not enqueue StopMessage
444  } else if (message->is_of_type<DynamixelServoInterface::FlushMessage>()) {
445  stop_motion(si->first);
446  if (cfg_enable_verbose_output_) {
447  logger->log_info(name(), "Flushing message queue");
448  }
449  si->second.servo_if->msgq_flush();
450  return false;
451  } else {
452  if (cfg_enable_verbose_output_) {
453  logger->log_info(name(), "Received message of type %s, enqueueing", message->type());
454  }
455  return true;
456  }
457  }
458  return true;
459 }
460 
461 /** Enable or disable servo.
462  * @param enabled true to enable servos, false to turn them off
463  */
464 void
465 DynamixelDriverThread::set_enabled(unsigned int servo_id, bool enabled)
466 {
467  if (servos_.find(servo_id) == servos_.end()) {
468  logger->log_warn(name(),
469  "No servo with ID %u in chain %s, cannot set LED",
470  servo_id,
471  cfg_name_.c_str());
472  return;
473  }
474 
475  Servo &s = servos_[servo_id];
476 
477  logger->log_debug(name(), "Lock %d", __LINE__);
478  ScopedRWLock lock(s.value_rwlock);
479  if (enabled) {
480  s.enable = true;
481  s.disable = false;
482  } else {
483  s.enable = false;
484  s.disable = true;
485  }
486  wakeup();
487  logger->log_debug(name(), "UNLock %d", __LINE__);
488 }
489 
490 /** Enable or disable LED.
491  * @param enabled true to enable LED, false to turn it off
492  */
493 void
494 DynamixelDriverThread::set_led_enabled(unsigned int servo_id, bool enabled)
495 {
496  if (servos_.find(servo_id) == servos_.end()) {
497  logger->log_warn(name(),
498  "No servo with ID %u in chain %s, cannot set LED",
499  servo_id,
500  cfg_name_.c_str());
501  return;
502  }
503 
504  Servo &s = servos_[servo_id];
505  logger->log_debug(name(), "Lock %d", __LINE__);
506  ScopedRWLock lock(s.value_rwlock);
507  if (enabled) {
508  s.led_enable = true;
509  s.led_disable = false;
510  } else {
511  s.led_enable = false;
512  s.led_disable = true;
513  }
514  wakeup();
515  logger->log_debug(name(), "UNLock %d", __LINE__);
516 }
517 
518 /** Stop currently running motion. */
519 void
520 DynamixelDriverThread::stop_motion(unsigned int servo_id)
521 {
522  if (servos_.find(servo_id) == servos_.end()) {
523  logger->log_warn(name(),
524  "No servo with ID %u in chain %s, cannot set LED",
525  servo_id,
526  cfg_name_.c_str());
527  return;
528  }
529 
530  float angle = get_angle(servo_id);
531  goto_angle(servo_id, angle);
532 }
533 
534 /** Goto desired angle value.
535  * @param angle in radians
536  */
537 void
538 DynamixelDriverThread::goto_angle(unsigned int servo_id, float angle)
539 {
540  if (servos_.find(servo_id) == servos_.end()) {
541  logger->log_warn(name(),
542  "No servo with ID %u in chain %s, cannot set LED",
543  servo_id,
544  cfg_name_.c_str());
545  return;
546  }
547 
548  Servo &s = servos_[servo_id];
549 
550  logger->log_debug(name(), "Lock %d", __LINE__);
551  ScopedRWLock lock(s.value_rwlock);
552  s.target_angle = angle;
553  s.move_pending = true;
554  wakeup();
555  logger->log_debug(name(), "UNLock %d", __LINE__);
556 }
557 
558 /** Goto desired angle value in a specified time.
559  * @param angle in radians
560  * @param time_sec time when to reach the desired angle value
561  */
562 void
563 DynamixelDriverThread::goto_angle_timed(unsigned int servo_id, float angle, float time_sec)
564 {
565  if (servos_.find(servo_id) == servos_.end()) {
566  logger->log_warn(name(),
567  "No servo with ID %u in chain %s, cannot set LED",
568  servo_id,
569  cfg_name_.c_str());
570  return;
571  }
572  Servo &s = servos_[servo_id];
573 
574  s.target_angle = angle;
575  s.move_pending = true;
576 
577  float cangle = get_angle(servo_id);
578  float angle_diff = fabs(angle - cangle);
579  float req_angle_vel = angle_diff / time_sec;
580 
581  if (req_angle_vel > s.max_speed) {
582  logger->log_warn(name(),
583  "Requested move to %f in %f sec requires a "
584  "angle speed of %f rad/s, which is greater than the maximum "
585  "of %f rad/s, reducing to max",
586  angle,
587  time_sec,
588  req_angle_vel,
589  s.max_speed);
590  req_angle_vel = s.max_speed;
591  }
592  set_velocity(servo_id, req_angle_vel);
593 
594  wakeup();
595 }
596 
597 /** Set desired velocity.
598  * @param vel the desired velocity in rad/s
599  */
600 void
601 DynamixelDriverThread::set_velocity(unsigned int servo_id, float vel)
602 {
603  if (servos_.find(servo_id) == servos_.end()) {
604  logger->log_warn(name(),
605  "No servo with ID %u in chain %s, cannot set velocity",
606  servo_id,
607  cfg_name_.c_str());
608  return;
609  }
610  Servo &s = servos_[servo_id];
611 
612  float velo_tmp = roundf((vel / s.max_speed) * DynamixelChain::MAX_SPEED);
613  set_speed(servo_id, (unsigned int)velo_tmp);
614 }
615 
616 /** Set desired speed.
617  * When the servo is set to wheel mode, bit 10 of the speed value is used
618  * to either move cw (1) or ccw (0).
619  * @param speed the speed
620  */
621 void
622 DynamixelDriverThread::set_speed(unsigned int servo_id, unsigned int speed)
623 {
624  if (servos_.find(servo_id) == servos_.end()) {
625  logger->log_warn(name(),
626  "No servo with ID %u in chain %s, cannot set speed",
627  servo_id,
628  cfg_name_.c_str());
629  return;
630  }
631  Servo &s = servos_[servo_id];
632 
633  ScopedRWLock lock(s.value_rwlock);
634  if (speed <= DynamixelChain::MAX_SPEED) {
635  s.vel = speed;
636  s.velo_pending = true;
637  } else {
638  logger->log_warn(name(),
639  "Calculated velocity value out of bounds, "
640  "min: 0 max: %u des: %u",
642  speed);
643  }
644 }
645 
646 /** Set desired mode.
647  * @param mode, either DynamixelServoInterface.JOINT or DynamixelServoInterface.WHEEL
648  */
649 void
650 DynamixelDriverThread::set_mode(unsigned int servo_id, unsigned int mode)
651 {
652  if (servos_.find(servo_id) == servos_.end()) {
653  logger->log_warn(name(),
654  "No servo with ID %u in chain %s, cannot set mode",
655  servo_id,
656  cfg_name_.c_str());
657  return;
658  }
659  Servo &s = servos_[servo_id];
660 
661  ScopedRWLock(s.value_rwlock);
662  s.mode_set_pending = true;
663  s.new_mode = mode;
664  s.servo_if->set_mode(mode == DynamixelServoInterface::JOINT ? "JOINT" : "WHEEL");
665 }
666 
667 /** Get current velocity.
668  */
669 float
670 DynamixelDriverThread::get_velocity(unsigned int servo_id)
671 {
672  if (servos_.find(servo_id) == servos_.end()) {
673  logger->log_warn(name(),
674  "No servo with ID %u in chain %s, cannot set velocity",
675  servo_id,
676  cfg_name_.c_str());
677  return 0.;
678  }
679  Servo &s = servos_[servo_id];
680 
681  unsigned int velticks = chain_->get_speed(servo_id);
682 
683  return (((float)velticks / (float)DynamixelChain::MAX_SPEED) * s.max_speed);
684 }
685 
686 /** Set desired angle margin.
687  * @param angle_margin the desired angle_margin
688  */
689 void
690 DynamixelDriverThread::set_margin(unsigned int servo_id, float angle_margin)
691 {
692  if (servos_.find(servo_id) == servos_.end()) {
693  logger->log_warn(name(),
694  "No servo with ID %u in chain %s, cannot set velocity",
695  servo_id,
696  cfg_name_.c_str());
697  return;
698  }
699  Servo &s = servos_[servo_id];
700  if (angle_margin > 0.0)
701  s.angle_margin = angle_margin;
702 }
703 
704 /** Get angle - the position from -2.62 to + 2.62 (-150 to +150 degrees)
705  */
706 float
707 DynamixelDriverThread::get_angle(unsigned int servo_id)
708 {
709  if (servos_.find(servo_id) == servos_.end()) {
710  logger->log_warn(name(),
711  "No servo with ID %u in chain %s, cannot set velocity",
712  servo_id,
713  cfg_name_.c_str());
714  return 0.;
715  }
716 
717  ScopedRWLock lock(chain_rwlock_, ScopedRWLock::LOCK_READ);
718 
719  int ticks = ((int)chain_->get_position(servo_id) - (int)DynamixelChain::CENTER_POSITION);
720 
721  return ticks * DynamixelChain::RAD_PER_POS_TICK;
722 }
723 
724 /** Get angle value with time.
725  * @param time upon return contains the time the angle value was read
726  */
727 float
728 DynamixelDriverThread::get_angle(unsigned int servo_id, fawkes::Time &time)
729 {
730  if (servos_.find(servo_id) == servos_.end()) {
731  logger->log_warn(name(),
732  "No servo with ID %u in chain %s, cannot set velocity",
733  servo_id,
734  cfg_name_.c_str());
735  return 0.;
736  }
737  Servo &s = servos_[servo_id];
738 
739  time = s.time;
740  return get_angle(servo_id);
741 }
742 
743 /** Check if motion is final.
744  * @return true if motion is final, false otherwise
745  */
746 bool
747 DynamixelDriverThread::is_final(unsigned int servo_id)
748 {
749  if (servos_.find(servo_id) == servos_.end()) {
750  logger->log_warn(name(),
751  "No servo with ID %u in chain %s, cannot set velocity",
752  servo_id,
753  cfg_name_.c_str());
754  return 0.;
755  }
756  Servo &s = servos_[servo_id];
757 
758  float angle = get_angle(servo_id);
759 
760  ScopedRWLock lock(chain_rwlock_, ScopedRWLock::LOCK_READ);
761 
762  return ((fabs(angle - s.target_angle) <= s.angle_margin) || (!chain_->is_moving(servo_id)));
763 }
764 
765 /** Check if servo is enabled.
766  * @return true if torque is enabled for both servos, false otherwise
767  */
768 bool
769 DynamixelDriverThread::is_enabled(unsigned int servo_id)
770 {
771  return chain_->is_torque_enabled(servo_id);
772 }
773 
774 /** Check is fresh sensor data is available.
775  * Note that this method will return true at once per sensor update cycle.
776  * @return true if fresh data is available, false otherwise
777  */
778 bool
779 DynamixelDriverThread::has_fresh_data()
780 {
781  MutexLocker lock(fresh_data_mutex_);
782 
783  bool rv = fresh_data_;
784  fresh_data_ = false;
785  return rv;
786 }
787 
788 void
790 {
791  for (auto &sp : servos_) {
792  unsigned int servo_id = sp.first;
793  Servo & s = sp.second;
794  if (s.enable) {
795  s.value_rwlock->lock_for_write();
796  s.enable = false;
797  s.value_rwlock->unlock();
798  ScopedRWLock lock(chain_rwlock_);
799  chain_->set_led_enabled(servo_id, true);
800  chain_->set_torque_enabled(servo_id, true);
801  if (s.led_enable || s.led_disable || s.velo_pending || s.move_pending || s.mode_set_pending
802  || s.recover_pending)
803  usleep(3000);
804  } else if (s.disable) {
805  s.value_rwlock->lock_for_write();
806  s.disable = false;
807  s.value_rwlock->unlock();
808  ScopedRWLock lock(chain_rwlock_);
809  chain_->set_torque_enabled(servo_id, false);
810  if (s.led_enable || s.led_disable || s.velo_pending || s.move_pending || s.mode_set_pending
811  || s.recover_pending)
812  usleep(3000);
813  }
814 
815  if (s.led_enable) {
816  s.value_rwlock->lock_for_write();
817  s.led_enable = false;
818  s.value_rwlock->unlock();
819  ScopedRWLock lock(chain_rwlock_);
820  chain_->set_led_enabled(servo_id, true);
821  if (s.velo_pending || s.move_pending || s.mode_set_pending || s.recover_pending)
822  usleep(3000);
823  } else if (s.led_disable) {
824  s.value_rwlock->lock_for_write();
825  s.led_disable = false;
826  s.value_rwlock->unlock();
827  ScopedRWLock lock(chain_rwlock_);
828  chain_->set_led_enabled(servo_id, false);
829  if (s.velo_pending || s.move_pending || s.mode_set_pending || s.recover_pending)
830  usleep(3000);
831  }
832 
833  if (s.velo_pending) {
834  s.value_rwlock->lock_for_write();
835  s.velo_pending = false;
836  unsigned int vel = s.vel;
837  s.value_rwlock->unlock();
838  ScopedRWLock lock(chain_rwlock_);
839  chain_->set_goal_speed(servo_id, vel);
840  if (s.move_pending || s.mode_set_pending || s.recover_pending)
841  usleep(3000);
842  }
843 
844  if (s.move_pending) {
845  s.value_rwlock->lock_for_write();
846  s.move_pending = false;
847  float target_angle = s.target_angle;
848  s.value_rwlock->unlock();
849  exec_goto_angle(servo_id, target_angle);
850  if (s.mode_set_pending || s.recover_pending)
851  usleep(3000);
852  }
853 
854  if (s.mode_set_pending) {
855  s.value_rwlock->lock_for_write();
856  s.mode_set_pending = false;
857  exec_set_mode(servo_id, s.new_mode);
858  s.value_rwlock->unlock();
859  if (s.recover_pending)
860  usleep(3000);
861  }
862 
863  if (s.recover_pending) {
864  s.value_rwlock->lock_for_write();
865  s.recover_pending = false;
866  chain_->set_torque_limit(servo_id, s.torque_limit);
867  s.value_rwlock->unlock();
868  }
869 
870  try {
871  ScopedRWLock lock(chain_rwlock_, ScopedRWLock::LOCK_READ);
872  chain_->read_table_values(servo_id);
873 
874  MutexLocker lock_fresh_data(fresh_data_mutex_);
875  fresh_data_ = true;
876  s.time.stamp();
877  } catch (Exception &e) {
878  // usually just a timeout, too noisy
879  //logger_->log_warn(name(), "Error while reading table values from servos, exception follows");
880  //logger_->log_warn(name(), e);
881  }
882  }
883 
884  update_waitcond_->wake_all();
885 
886  // Wakeup ourselves for faster updates
887  wakeup();
888 }
889 
890 /** Execute angle motion.
891  * @param angle_rad angle in rad to move to
892  */
893 void
894 DynamixelDriverThread::exec_goto_angle(unsigned int servo_id, float angle_rad)
895 {
896  unsigned int pos_min = 0, pos_max = 0;
897  chain_->get_angle_limits(servo_id, pos_min, pos_max);
898 
899  int pos =
901 
902  if ((pos < 0) || ((unsigned int)pos < pos_min) || ((unsigned int)pos > pos_max)) {
903  logger->log_warn(
904  name(), "Position out of bounds, min: %u max: %u des: %i", pos_min, pos_max, pos);
905  return;
906  }
907 
908  ScopedRWLock lock(chain_rwlock_);
909  chain_->goto_position(servo_id, pos);
910 }
911 
912 /** Execute set mode.
913  * @param new_mode - either DynamixelServoInterface::JOINT or DynamixelServoInterface::WHEEL
914  */
915 void
916 DynamixelDriverThread::exec_set_mode(unsigned int servo_id, unsigned int new_mode)
917 {
918  if (new_mode == DynamixelServoInterface::JOINT) {
919  ScopedRWLock lock(chain_rwlock_);
920  chain_->set_angle_limits(servo_id, 0, 1023);
921  } else if (new_mode == DynamixelServoInterface::WHEEL) {
922  ScopedRWLock lock(chain_rwlock_);
923  chain_->set_angle_limits(servo_id, 0, 0);
924  } else {
925  logger->log_error(name(), "Mode %d cannot be set - unknown", new_mode);
926  }
927 
928  return;
929 }
930 
931 /** Wait for fresh data to be received.
932  * Blocks the calling thread.
933  */
934 void
935 DynamixelDriverThread::wait_for_fresh_data()
936 {
937  update_waitcond_->wait();
938 }
Class to access a chain of Robotis dynamixel servos.
Definition: servo_chain.h:37
void set_goal_speed(unsigned char id, unsigned int goal_speed)
Set goal speed.
void set_angle_limits(unsigned char id, unsigned int cw_limit, unsigned int ccw_limit)
Set angle limits.
unsigned char get_voltage(unsigned char id, bool refresh=false)
Get current voltage.
std::list< unsigned char > DeviceList
List of servo IDs.
Definition: servo_chain.h:40
static const unsigned char SRL_RESPOND_READ
SRL_RESPOND_READ.
Definition: servo_chain.h:145
void set_status_return_level(unsigned char id, unsigned char status_return_level)
Set status return level.
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.
unsigned int get_model_number(unsigned char id, bool refresh=false)
Get model.
unsigned int get_goal_speed(unsigned char id, bool refresh=false)
Get goal speed.
DeviceList discover(unsigned int total_timeout_ms=50, const std::vector< unsigned int > &servos=std::vector< unsigned int >())
Discover devices on the bus.
void get_compliance_values(unsigned char id, unsigned char &cw_margin, unsigned char &cw_slope, unsigned char &ccw_margin, unsigned char &ccw_slope, bool refresh=false)
Get compliance values.
void set_torque_limit(unsigned char id, unsigned int torque_limit)
Set torque limit.
unsigned char get_temperature_limit(unsigned char id, bool refresh=false)
Get temperature limit.
unsigned int get_max_torque(unsigned char id, bool refresh=false)
Get maximum torque.
void read_table_values(unsigned char id)
Read all table values for given servo.
static const unsigned int CENTER_POSITION
CENTER_POSITION.
Definition: servo_chain.h:150
unsigned int get_position(unsigned char id, bool refresh=false)
Get current position.
void get_angle_limits(unsigned char id, unsigned int &cw_limit, unsigned int &ccw_limit, bool refresh=false)
Get angle limits.
void goto_position(unsigned char id, unsigned int value)
Move servo to specified position.
unsigned int get_punch(unsigned char id, bool refresh=false)
Get punch.
void set_torque_enabled(unsigned char id, bool enabled)
Enable or disable torque.
float get_max_supported_speed(unsigned char id, bool refresh=false)
Get maximum supported speed.
static const unsigned int MAX_SPEED
MAX_SPEED.
Definition: servo_chain.h:157
static const float POS_TICKS_PER_RAD
POS_TICKS_PER_RAD.
Definition: servo_chain.h:154
static const float RAD_PER_POS_TICK
RAD_PER_POS_TICK.
Definition: servo_chain.h:153
unsigned int get_speed(unsigned char id, bool refresh=false)
Get current speed.
unsigned int get_goal_position(unsigned char id, bool refresh=false)
Get goal position.
unsigned char get_error(unsigned char id)
Get error flags set by the servo.
bool is_moving(unsigned char id, bool refresh=false)
Check if servo is moving.
void set_led_enabled(unsigned char id, bool enabled)
Turn LED on or off.
unsigned char get_temperature(unsigned char id, bool refresh=false)
Get temperature.
static const unsigned char BROADCAST_ID
BROADCAST_ID.
Definition: servo_chain.h:148
void set_temperature_limit(unsigned char id, unsigned char temp_limit)
Set temperature limit.
bool is_torque_enabled(unsigned char id, bool refresh=false)
Check if torque is enabled.
const char * get_model(unsigned char id, bool refresh=false)
Get model string.
unsigned char get_alarm_shutdown(unsigned char id, bool refresh=false)
Get shutdown on alarm state.
unsigned int get_torque_limit(unsigned char id, bool refresh=false)
Get torque limit.
unsigned int get_load(unsigned char id, bool refresh=false)
Get current load.
virtual void finalize()
Finalize the thread.
DynamixelDriverThread(std::string &cfg_name, std::string &cfg_prefix)
Constructor.
void exec_sensor()
Update sensor values as necessary.
virtual void init()
Initialize the thread.
virtual void loop()
Code to execute in the thread.
virtual bool bb_interface_message_received(fawkes::Interface *interface, fawkes::Message *message) noexcept
BlackBoard message received notification.
void exec_act()
Process commands.
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
Definition: blackboard.h:44
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.
Definition: blackboard.cpp:212
virtual Interface * open_for_writing_f(const char *interface_type, const char *identifier,...)
Open interface for writing with identifier format string.
Definition: blackboard.cpp:319
virtual void register_listener(BlackBoardInterfaceListener *listener, ListenerRegisterFlag flag=BBIL_FLAG_ALL)
Register BB event listener.
Definition: blackboard.cpp:185
virtual void close(Interface *interface)=0
Close interface.
Configuration * config
This is the Configuration member used to access the configuration.
Definition: configurable.h:41
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.
virtual std::vector< unsigned int > get_uints(const char *path)=0
Get list of values from configuration which is of type unsigned int.
FlushMessage Fawkes BlackBoard Interface Message.
GotoMessage Fawkes BlackBoard Interface Message.
RecoverMessage Fawkes BlackBoard Interface Message.
ResetRawErrorMessage Fawkes BlackBoard Interface Message.
SetAutorecoverEnabledMessage Fawkes BlackBoard Interface Message.
void set_autorecover_enabled(const bool new_autorecover_enabled)
Set autorecover_enabled value.
SetEnabledMessage Fawkes BlackBoard Interface Message.
SetMarginMessage Fawkes BlackBoard Interface Message.
SetModeMessage Fawkes BlackBoard Interface Message.
SetPreventAlarmShutdownMessage Fawkes BlackBoard Interface Message.
bool is_enable_prevent_alarm_shutdown() const
Get enable_prevent_alarm_shutdown value.
void set_enable_prevent_alarm_shutdown(const bool new_enable_prevent_alarm_shutdown)
Set enable_prevent_alarm_shutdown value.
SetSpeedMessage Fawkes BlackBoard Interface Message.
SetTorqueLimitMessage Fawkes BlackBoard Interface Message.
SetVelocityMessage Fawkes BlackBoard Interface Message.
StopMessage Fawkes BlackBoard Interface Message.
TimedGotoMessage Fawkes BlackBoard Interface Message.
DynamixelServoInterface Fawkes BlackBoard Interface.
Base class for exceptions in Fawkes.
Definition: exception.h:36
virtual const char * what() const noexcept
Get primary string.
Definition: exception.cpp:639
Base class for all Fawkes BlackBoard interfaces.
Definition: interface.h:80
JointInterface Fawkes BlackBoard Interface.
SetIntensityMessage Fawkes BlackBoard Interface Message.
Definition: LedInterface.h:56
float intensity() const
Get intensity value.
TurnOffMessage Fawkes BlackBoard Interface Message.
Definition: LedInterface.h:107
TurnOnMessage Fawkes BlackBoard Interface Message.
Definition: LedInterface.h:87
LedInterface Fawkes BlackBoard Interface.
Definition: LedInterface.h:34
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
virtual void log_error(const char *component, const char *format,...)=0
Log error 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.
Definition: logging.h:41
Base class for all messages passed through interfaces in Fawkes BlackBoard.
Definition: message.h:44
unsigned int id() const
Get message ID.
Definition: message.cpp:181
virtual void log_info(const char *component, const char *format,...)
Log informational message.
Definition: multi.cpp:195
Mutex locking helper.
Definition: mutex_locker.h:34
Mutex mutual exclusion lock.
Definition: mutex.h:33
Read/write lock to allow multiple readers but only a single writer on the resource at a time.
Scoped read/write lock.
Definition: scoped_rwlock.h:34
Thread class encapsulation of pthreads.
Definition: thread.h:46
const char * name() const
Get name of thread.
Definition: thread.h:100
void set_name(const char *format,...)
Set name of thread.
Definition: thread.cpp:748
void wakeup()
Wake up thread.
Definition: thread.cpp:995
A class for handling time.
Definition: time.h:93
Wait until a given condition holds.
void wait()
Wait for the condition forever.
void wake_all()
Wake up all waiting threads.
Fawkes library namespace.
float deg2rad(float deg)
Convert an angle given in degrees to radians.
Definition: angle.h:36
std::string str_join(const InputIterator &first, const InputIterator &last, char delim='/')
Join list of strings string using given delimiter.
Definition: string_split.h:139