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 {
433  std::map<unsigned int, Servo>::iterator si =
434  std::find_if(servos_.begin(),
435  servos_.end(),
436  [interface](const std::pair<unsigned int, Servo> &sp) {
437  return (strcmp(sp.second.servo_if->uid(), interface->uid()) == 0);
438  });
439  if (si != servos_.end()) {
440  if (message->is_of_type<DynamixelServoInterface::StopMessage>()) {
441  stop_motion(si->first);
442  return false; // do not enqueue StopMessage
443  } else if (message->is_of_type<DynamixelServoInterface::FlushMessage>()) {
444  stop_motion(si->first);
445  if (cfg_enable_verbose_output_) {
446  logger->log_info(name(), "Flushing message queue");
447  }
448  si->second.servo_if->msgq_flush();
449  return false;
450  } else {
451  if (cfg_enable_verbose_output_) {
452  logger->log_info(name(), "Received message of type %s, enqueueing", message->type());
453  }
454  return true;
455  }
456  }
457  return true;
458 }
459 
460 /** Enable or disable servo.
461  * @param enabled true to enable servos, false to turn them off
462  */
463 void
464 DynamixelDriverThread::set_enabled(unsigned int servo_id, bool enabled)
465 {
466  if (servos_.find(servo_id) == servos_.end()) {
467  logger->log_warn(name(),
468  "No servo with ID %u in chain %s, cannot set LED",
469  servo_id,
470  cfg_name_.c_str());
471  return;
472  }
473 
474  Servo &s = servos_[servo_id];
475 
476  logger->log_debug(name(), "Lock %d", __LINE__);
477  ScopedRWLock lock(s.value_rwlock);
478  if (enabled) {
479  s.enable = true;
480  s.disable = false;
481  } else {
482  s.enable = false;
483  s.disable = true;
484  }
485  wakeup();
486  logger->log_debug(name(), "UNLock %d", __LINE__);
487 }
488 
489 /** Enable or disable LED.
490  * @param enabled true to enable LED, false to turn it off
491  */
492 void
493 DynamixelDriverThread::set_led_enabled(unsigned int servo_id, bool enabled)
494 {
495  if (servos_.find(servo_id) == servos_.end()) {
496  logger->log_warn(name(),
497  "No servo with ID %u in chain %s, cannot set LED",
498  servo_id,
499  cfg_name_.c_str());
500  return;
501  }
502 
503  Servo &s = servos_[servo_id];
504  logger->log_debug(name(), "Lock %d", __LINE__);
505  ScopedRWLock lock(s.value_rwlock);
506  if (enabled) {
507  s.led_enable = true;
508  s.led_disable = false;
509  } else {
510  s.led_enable = false;
511  s.led_disable = true;
512  }
513  wakeup();
514  logger->log_debug(name(), "UNLock %d", __LINE__);
515 }
516 
517 /** Stop currently running motion. */
518 void
519 DynamixelDriverThread::stop_motion(unsigned int servo_id)
520 {
521  if (servos_.find(servo_id) == servos_.end()) {
522  logger->log_warn(name(),
523  "No servo with ID %u in chain %s, cannot set LED",
524  servo_id,
525  cfg_name_.c_str());
526  return;
527  }
528 
529  float angle = get_angle(servo_id);
530  goto_angle(servo_id, angle);
531 }
532 
533 /** Goto desired angle value.
534  * @param angle in radians
535  */
536 void
537 DynamixelDriverThread::goto_angle(unsigned int servo_id, float angle)
538 {
539  if (servos_.find(servo_id) == servos_.end()) {
540  logger->log_warn(name(),
541  "No servo with ID %u in chain %s, cannot set LED",
542  servo_id,
543  cfg_name_.c_str());
544  return;
545  }
546 
547  Servo &s = servos_[servo_id];
548 
549  logger->log_debug(name(), "Lock %d", __LINE__);
550  ScopedRWLock lock(s.value_rwlock);
551  s.target_angle = angle;
552  s.move_pending = true;
553  wakeup();
554  logger->log_debug(name(), "UNLock %d", __LINE__);
555 }
556 
557 /** Goto desired angle value in a specified time.
558  * @param angle in radians
559  * @param time_sec time when to reach the desired angle value
560  */
561 void
562 DynamixelDriverThread::goto_angle_timed(unsigned int servo_id, float angle, float time_sec)
563 {
564  if (servos_.find(servo_id) == servos_.end()) {
565  logger->log_warn(name(),
566  "No servo with ID %u in chain %s, cannot set LED",
567  servo_id,
568  cfg_name_.c_str());
569  return;
570  }
571  Servo &s = servos_[servo_id];
572 
573  s.target_angle = angle;
574  s.move_pending = true;
575 
576  float cangle = get_angle(servo_id);
577  float angle_diff = fabs(angle - cangle);
578  float req_angle_vel = angle_diff / time_sec;
579 
580  if (req_angle_vel > s.max_speed) {
581  logger->log_warn(name(),
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",
585  angle,
586  time_sec,
587  req_angle_vel,
588  s.max_speed);
589  req_angle_vel = s.max_speed;
590  }
591  set_velocity(servo_id, req_angle_vel);
592 
593  wakeup();
594 }
595 
596 /** Set desired velocity.
597  * @param vel the desired velocity in rad/s
598  */
599 void
600 DynamixelDriverThread::set_velocity(unsigned int servo_id, float vel)
601 {
602  if (servos_.find(servo_id) == servos_.end()) {
603  logger->log_warn(name(),
604  "No servo with ID %u in chain %s, cannot set velocity",
605  servo_id,
606  cfg_name_.c_str());
607  return;
608  }
609  Servo &s = servos_[servo_id];
610 
611  float velo_tmp = roundf((vel / s.max_speed) * DynamixelChain::MAX_SPEED);
612  set_speed(servo_id, (unsigned int)velo_tmp);
613 }
614 
615 /** Set desired speed.
616  * When the servo is set to wheel mode, bit 10 of the speed value is used
617  * to either move cw (1) or ccw (0).
618  * @param speed the speed
619  */
620 void
621 DynamixelDriverThread::set_speed(unsigned int servo_id, unsigned int speed)
622 {
623  if (servos_.find(servo_id) == servos_.end()) {
624  logger->log_warn(name(),
625  "No servo with ID %u in chain %s, cannot set speed",
626  servo_id,
627  cfg_name_.c_str());
628  return;
629  }
630  Servo &s = servos_[servo_id];
631 
632  ScopedRWLock lock(s.value_rwlock);
633  if (speed <= DynamixelChain::MAX_SPEED) {
634  s.vel = speed;
635  s.velo_pending = true;
636  } else {
637  logger->log_warn(name(),
638  "Calculated velocity value out of bounds, "
639  "min: 0 max: %u des: %u",
641  speed);
642  }
643 }
644 
645 /** Set desired mode.
646  * @param mode, either DynamixelServoInterface.JOINT or DynamixelServoInterface.WHEEL
647  */
648 void
649 DynamixelDriverThread::set_mode(unsigned int servo_id, unsigned int mode)
650 {
651  if (servos_.find(servo_id) == servos_.end()) {
652  logger->log_warn(name(),
653  "No servo with ID %u in chain %s, cannot set mode",
654  servo_id,
655  cfg_name_.c_str());
656  return;
657  }
658  Servo &s = servos_[servo_id];
659 
660  ScopedRWLock(s.value_rwlock);
661  s.mode_set_pending = true;
662  s.new_mode = mode;
663  s.servo_if->set_mode(mode == DynamixelServoInterface::JOINT ? "JOINT" : "WHEEL");
664 }
665 
666 /** Get current velocity.
667  */
668 float
669 DynamixelDriverThread::get_velocity(unsigned int servo_id)
670 {
671  if (servos_.find(servo_id) == servos_.end()) {
672  logger->log_warn(name(),
673  "No servo with ID %u in chain %s, cannot set velocity",
674  servo_id,
675  cfg_name_.c_str());
676  return 0.;
677  }
678  Servo &s = servos_[servo_id];
679 
680  unsigned int velticks = chain_->get_speed(servo_id);
681 
682  return (((float)velticks / (float)DynamixelChain::MAX_SPEED) * s.max_speed);
683 }
684 
685 /** Set desired angle margin.
686  * @param angle_margin the desired angle_margin
687  */
688 void
689 DynamixelDriverThread::set_margin(unsigned int servo_id, float angle_margin)
690 {
691  if (servos_.find(servo_id) == servos_.end()) {
692  logger->log_warn(name(),
693  "No servo with ID %u in chain %s, cannot set velocity",
694  servo_id,
695  cfg_name_.c_str());
696  return;
697  }
698  Servo &s = servos_[servo_id];
699  if (angle_margin > 0.0)
700  s.angle_margin = angle_margin;
701 }
702 
703 /** Get angle - the position from -2.62 to + 2.62 (-150 to +150 degrees)
704  */
705 float
706 DynamixelDriverThread::get_angle(unsigned int servo_id)
707 {
708  if (servos_.find(servo_id) == servos_.end()) {
709  logger->log_warn(name(),
710  "No servo with ID %u in chain %s, cannot set velocity",
711  servo_id,
712  cfg_name_.c_str());
713  return 0.;
714  }
715 
716  ScopedRWLock lock(chain_rwlock_, ScopedRWLock::LOCK_READ);
717 
718  int ticks = ((int)chain_->get_position(servo_id) - (int)DynamixelChain::CENTER_POSITION);
719 
720  return ticks * DynamixelChain::RAD_PER_POS_TICK;
721 }
722 
723 /** Get angle value with time.
724  * @param time upon return contains the time the angle value was read
725  */
726 float
727 DynamixelDriverThread::get_angle(unsigned int servo_id, fawkes::Time &time)
728 {
729  if (servos_.find(servo_id) == servos_.end()) {
730  logger->log_warn(name(),
731  "No servo with ID %u in chain %s, cannot set velocity",
732  servo_id,
733  cfg_name_.c_str());
734  return 0.;
735  }
736  Servo &s = servos_[servo_id];
737 
738  time = s.time;
739  return get_angle(servo_id);
740 }
741 
742 /** Check if motion is final.
743  * @return true if motion is final, false otherwise
744  */
745 bool
746 DynamixelDriverThread::is_final(unsigned int servo_id)
747 {
748  if (servos_.find(servo_id) == servos_.end()) {
749  logger->log_warn(name(),
750  "No servo with ID %u in chain %s, cannot set velocity",
751  servo_id,
752  cfg_name_.c_str());
753  return 0.;
754  }
755  Servo &s = servos_[servo_id];
756 
757  float angle = get_angle(servo_id);
758 
759  ScopedRWLock lock(chain_rwlock_, ScopedRWLock::LOCK_READ);
760 
761  return ((fabs(angle - s.target_angle) <= s.angle_margin) || (!chain_->is_moving(servo_id)));
762 }
763 
764 /** Check if servo is enabled.
765  * @return true if torque is enabled for both servos, false otherwise
766  */
767 bool
768 DynamixelDriverThread::is_enabled(unsigned int servo_id)
769 {
770  return chain_->is_torque_enabled(servo_id);
771 }
772 
773 /** Check is fresh sensor data is available.
774  * Note that this method will return true at once per sensor update cycle.
775  * @return true if fresh data is available, false otherwise
776  */
777 bool
778 DynamixelDriverThread::has_fresh_data()
779 {
780  MutexLocker lock(fresh_data_mutex_);
781 
782  bool rv = fresh_data_;
783  fresh_data_ = false;
784  return rv;
785 }
786 
787 void
789 {
790  for (auto &sp : servos_) {
791  unsigned int servo_id = sp.first;
792  Servo & s = sp.second;
793  if (s.enable) {
794  s.value_rwlock->lock_for_write();
795  s.enable = false;
796  s.value_rwlock->unlock();
797  ScopedRWLock lock(chain_rwlock_);
798  chain_->set_led_enabled(servo_id, true);
799  chain_->set_torque_enabled(servo_id, true);
800  if (s.led_enable || s.led_disable || s.velo_pending || s.move_pending || s.mode_set_pending
801  || s.recover_pending)
802  usleep(3000);
803  } else if (s.disable) {
804  s.value_rwlock->lock_for_write();
805  s.disable = false;
806  s.value_rwlock->unlock();
807  ScopedRWLock lock(chain_rwlock_);
808  chain_->set_torque_enabled(servo_id, false);
809  if (s.led_enable || s.led_disable || s.velo_pending || s.move_pending || s.mode_set_pending
810  || s.recover_pending)
811  usleep(3000);
812  }
813 
814  if (s.led_enable) {
815  s.value_rwlock->lock_for_write();
816  s.led_enable = false;
817  s.value_rwlock->unlock();
818  ScopedRWLock lock(chain_rwlock_);
819  chain_->set_led_enabled(servo_id, true);
820  if (s.velo_pending || s.move_pending || s.mode_set_pending || s.recover_pending)
821  usleep(3000);
822  } else if (s.led_disable) {
823  s.value_rwlock->lock_for_write();
824  s.led_disable = false;
825  s.value_rwlock->unlock();
826  ScopedRWLock lock(chain_rwlock_);
827  chain_->set_led_enabled(servo_id, false);
828  if (s.velo_pending || s.move_pending || s.mode_set_pending || s.recover_pending)
829  usleep(3000);
830  }
831 
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();
837  ScopedRWLock lock(chain_rwlock_);
838  chain_->set_goal_speed(servo_id, vel);
839  if (s.move_pending || s.mode_set_pending || s.recover_pending)
840  usleep(3000);
841  }
842 
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)
850  usleep(3000);
851  }
852 
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)
859  usleep(3000);
860  }
861 
862  if (s.recover_pending) {
863  s.value_rwlock->lock_for_write();
864  s.recover_pending = false;
865  chain_->set_torque_limit(servo_id, s.torque_limit);
866  s.value_rwlock->unlock();
867  }
868 
869  try {
870  ScopedRWLock lock(chain_rwlock_, ScopedRWLock::LOCK_READ);
871  chain_->read_table_values(servo_id);
872 
873  MutexLocker lock_fresh_data(fresh_data_mutex_);
874  fresh_data_ = true;
875  s.time.stamp();
876  } catch (Exception &e) {
877  // usually just a timeout, too noisy
878  //logger_->log_warn(name(), "Error while reading table values from servos, exception follows");
879  //logger_->log_warn(name(), e);
880  }
881  }
882 
883  update_waitcond_->wake_all();
884 
885  // Wakeup ourselves for faster updates
886  wakeup();
887 }
888 
889 /** Execute angle motion.
890  * @param angle_rad angle in rad to move to
891  */
892 void
893 DynamixelDriverThread::exec_goto_angle(unsigned int servo_id, float angle_rad)
894 {
895  unsigned int pos_min = 0, pos_max = 0;
896  chain_->get_angle_limits(servo_id, pos_min, pos_max);
897 
898  int pos =
900 
901  if ((pos < 0) || ((unsigned int)pos < pos_min) || ((unsigned int)pos > pos_max)) {
902  logger->log_warn(
903  name(), "Position out of bounds, min: %u max: %u des: %i", pos_min, pos_max, pos);
904  return;
905  }
906 
907  ScopedRWLock lock(chain_rwlock_);
908  chain_->goto_position(servo_id, pos);
909 }
910 
911 /** Execute set mode.
912  * @param new_mode - either DynamixelServoInterface::JOINT or DynamixelServoInterface::WHEEL
913  */
914 void
915 DynamixelDriverThread::exec_set_mode(unsigned int servo_id, unsigned int new_mode)
916 {
917  if (new_mode == DynamixelServoInterface::JOINT) {
918  ScopedRWLock lock(chain_rwlock_);
919  chain_->set_angle_limits(servo_id, 0, 1023);
920  } else if (new_mode == DynamixelServoInterface::WHEEL) {
921  ScopedRWLock lock(chain_rwlock_);
922  chain_->set_angle_limits(servo_id, 0, 0);
923  } else {
924  logger->log_error(name(), "Mode %d cannot be set - unknown", new_mode);
925  }
926 
927  return;
928 }
929 
930 /** Wait for fresh data to be received.
931  * Blocks the calling thread.
932  */
933 void
934 DynamixelDriverThread::wait_for_fresh_data()
935 {
936  update_waitcond_->wait();
937 }
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.
void exec_act()
Process commands.
virtual bool bb_interface_message_received(fawkes::Interface *interface, fawkes::Message *message)
BlackBoard message received notification.
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
Get primary string.
Definition: exception.cpp:639
Base class for all Fawkes BlackBoard interfaces.
Definition: interface.h:79
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:45
unsigned int id() const
Get message ID.
Definition: message.cpp:180
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