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 }
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.
TurnOffMessage Fawkes BlackBoard Interface Message.
Definition: LedInterface.h:106
std::list< unsigned char > DeviceList
List of servo IDs.
Definition: servo_chain.h:40
unsigned int get_speed(unsigned char id, bool refresh=false)
Get current speed.
Class to access a chain of Robotis dynamixel servos.
Definition: servo_chain.h:36
unsigned char get_alarm_shutdown(unsigned char id, bool refresh=false)
Get shutdown on alarm state.
Wait until a given condition holds.
Base class for all messages passed through interfaces in Fawkes BlackBoard.
Definition: message.h:41
unsigned int id() const
Get message ID.
Definition: message.cpp:190
SetAutorecoverEnabledMessage Fawkes BlackBoard Interface Message.
bool is_torque_enabled(unsigned char id, bool refresh=false)
Check if torque is enabled.
virtual void init()
Initialize the thread.
virtual void log_info(const char *component, const char *format,...)=0
Log informational message.
static const unsigned char BROADCAST_ID
BROADCAST_ID.
Definition: servo_chain.h:148
float get_max_supported_speed(unsigned char id, bool refresh=false)
Get maximum supported speed.
void set_temperature_limit(unsigned char id, unsigned char temp_limit)
Set temperature limit.
float intensity() const
Get intensity value.
Fawkes library namespace.
virtual bool get_bool(const char *path)=0
Get value from configuration which is of type bool.
const char * get_model(unsigned char id, bool refresh=false)
Get model string.
unsigned int get_position(unsigned char id, bool refresh=false)
Get current position.
void wake_all()
Wake up all waiting threads.
virtual bool bb_interface_message_received(fawkes::Interface *interface, fawkes::Message *message)
BlackBoard message received notification.
Mutex locking helper.
Definition: mutex_locker.h:33
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
unsigned int get_max_torque(unsigned char id, bool refresh=false)
Get maximum torque.
SetTorqueLimitMessage Fawkes BlackBoard Interface Message.
unsigned int get_model_number(unsigned char id, bool refresh=false)
Get model.
Scoped read/write lock.
Definition: scoped_rwlock.h:33
unsigned int get_goal_speed(unsigned char id, bool refresh=false)
Get goal speed.
A class for handling time.
Definition: time.h:92
virtual void finalize()
Finalize the thread.
unsigned char get_error(unsigned char id)
Get error flags set by the servo.
virtual const char * what() const
Get primary string.
Definition: exception.cpp:639
unsigned char get_voltage(unsigned char id, bool refresh=false)
Get current voltage.
virtual void unregister_listener(BlackBoardInterfaceListener *listener)
Unregister BB interface listener.
Definition: blackboard.cpp:212
Thread class encapsulation of pthreads.
Definition: thread.h:45
void set_angle_limits(unsigned char id, unsigned int cw_limit, unsigned int ccw_limit)
Set angle limits.
SetMarginMessage Fawkes BlackBoard Interface Message.
Base class for all Fawkes BlackBoard interfaces.
Definition: interface.h:78
static const float RAD_PER_POS_TICK
RAD_PER_POS_TICK.
Definition: servo_chain.h:153
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:41
DeviceList discover(unsigned int total_timeout_ms=50, const std::vector< unsigned int > &servos=std::vector< unsigned int >())
Discover devices on the bus.
ResetRawErrorMessage Fawkes BlackBoard Interface Message.
void exec_act()
Process commands.
StopMessage Fawkes BlackBoard Interface Message.
SetModeMessage Fawkes BlackBoard Interface Message.
void get_angle_limits(unsigned char id, unsigned int &cw_limit, unsigned int &ccw_limit, bool refresh=false)
Get angle limits.
unsigned int get_goal_position(unsigned char id, bool refresh=false)
Get goal position.
FlushMessage Fawkes BlackBoard Interface Message.
void read_table_values(unsigned char id)
Read all table values for given servo.
virtual void register_listener(BlackBoardInterfaceListener *listener, ListenerRegisterFlag flag=BBIL_FLAG_ALL)
Register BB event listener.
Definition: blackboard.cpp:185
bool is_moving(unsigned char id, bool refresh=false)
Check if servo is moving.
void goto_position(unsigned char id, unsigned int value)
Move servo to specified position.
void wakeup()
Wake up thread.
Definition: thread.cpp:995
void set_name(const char *format,...)
Set name of thread.
Definition: thread.cpp:748
Base class for exceptions in Fawkes.
Definition: exception.h:35
static const unsigned char SRL_RESPOND_READ
SRL_RESPOND_READ.
Definition: servo_chain.h:145
void set_autorecover_enabled(const bool new_autorecover_enabled)
Set autorecover_enabled value.
virtual std::vector< unsigned int > get_uints(const char *path)=0
Get list of values from configuration which is of type unsigned int.
bool is_enable_prevent_alarm_shutdown() const
Get enable_prevent_alarm_shutdown value.
static const float POS_TICKS_PER_RAD
POS_TICKS_PER_RAD.
Definition: servo_chain.h:154
Read/write lock to allow multiple readers but only a single writer on the resource at a time.
void set_led_enabled(unsigned char id, bool enabled)
Turn LED on or off.
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
TimedGotoMessage Fawkes BlackBoard Interface Message.
SetIntensityMessage Fawkes BlackBoard Interface Message.
Definition: LedInterface.h:55
const char * name() const
Get name of thread.
Definition: thread.h:100
unsigned int get_load(unsigned char id, bool refresh=false)
Get current load.
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.
SetEnabledMessage Fawkes BlackBoard Interface 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.
unsigned int get_punch(unsigned char id, bool refresh=false)
Get punch.
void wait()
Wait for the condition forever.
virtual void log_info(const char *component, const char *format,...)
Log informational message.
Definition: multi.cpp:195
void set_status_return_level(unsigned char id, unsigned char status_return_level)
Set status return level.
LedInterface Fawkes BlackBoard Interface.
Definition: LedInterface.h:33
static const unsigned int CENTER_POSITION
CENTER_POSITION.
Definition: servo_chain.h:150
virtual void loop()
Code to execute in the thread.
SetSpeedMessage Fawkes BlackBoard Interface Message.
void set_enable_prevent_alarm_shutdown(const bool new_enable_prevent_alarm_shutdown)
Set enable_prevent_alarm_shutdown value.
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
void exec_sensor()
Update sensor values as necessary.
GotoMessage Fawkes BlackBoard Interface Message.
TurnOnMessage Fawkes BlackBoard Interface Message.
Definition: LedInterface.h:86
DynamixelServoInterface Fawkes BlackBoard Interface.
void set_torque_enabled(unsigned char id, bool enabled)
Enable or disable torque.
JointInterface Fawkes BlackBoard Interface.
virtual unsigned int get_uint(const char *path)=0
Get value from configuration which is of type unsigned int.
Mutex mutual exclusion lock.
Definition: mutex.h:32
float deg2rad(float deg)
Convert an angle given in degrees to radians.
Definition: angle.h:36
void set_goal_speed(unsigned char id, unsigned int goal_speed)
Set goal speed.
unsigned char get_temperature_limit(unsigned char id, bool refresh=false)
Get temperature limit.
unsigned int get_torque_limit(unsigned char id, bool refresh=false)
Get torque limit.
DynamixelDriverThread(std::string &cfg_name, std::string &cfg_prefix)
Constructor.
void set_torque_limit(unsigned char id, unsigned int torque_limit)
Set torque limit.
Configuration * config
This is the Configuration member used to access the configuration.
Definition: configurable.h:41
RecoverMessage Fawkes BlackBoard Interface Message.
void bbil_add_message_interface(Interface *interface)
Add an interface to the message received watch list.
virtual float get_float(const char *path)=0
Get value from configuration which is of type float.
static const unsigned int MAX_SPEED
MAX_SPEED.
Definition: servo_chain.h:157
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
BlackBoard interface listener.
unsigned char get_temperature(unsigned char id, bool refresh=false)
Get temperature.
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
Definition: blackboard.h:44
SetVelocityMessage Fawkes BlackBoard Interface Message.
SetPreventAlarmShutdownMessage Fawkes BlackBoard Interface Message.
virtual void close(Interface *interface)=0
Close interface.