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 #include "servo_chain.h"
24 
25 #include <utils/math/angle.h>
26 #include <core/threading/mutex_locker.h>
27 #include <core/threading/read_write_lock.h>
28 #include <core/threading/scoped_rwlock.h>
29 #include <core/threading/wait_condition.h>
30 #include <interfaces/DynamixelServoInterface.h>
31 #include <interfaces/LedInterface.h>
32 #include <interfaces/JointInterface.h>
33 #include <utils/misc/string_split.h>
34 
35 #include <cstdarg>
36 #include <cmath>
37 #include <unistd.h>
38 #include <algorithm>
39 #include <cstring>
40 
41 using namespace fawkes;
42 
43 /** @class DynamixelDriverThread "driver_thread.h"
44  * Driver thread for Robotis dynamixel servos.
45  * @author Tim Niemueller
46  */
47 
48 /** Constructor.
49  * @param cfg_prefix configuration prefix specific for the servo chain
50  * @param cfg_name name of the servo configuration
51  */
53  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 
64 void
66 {
67  cfg_device_ = config->get_string((cfg_prefix_ + "device").c_str());
68  cfg_read_timeout_ms_ = config->get_uint((cfg_prefix_ + "read_timeout_ms").c_str());
69  cfg_disc_timeout_ms_ = config->get_uint((cfg_prefix_ + "discover_timeout_ms").c_str());
70  cfg_goto_zero_start_ = config->get_bool((cfg_prefix_ + "goto_zero_start").c_str());
71  cfg_turn_off_ = config->get_bool((cfg_prefix_ + "turn_off").c_str());
72  cfg_cw_compl_margin_ = config->get_uint((cfg_prefix_ + "cw_compl_margin").c_str());
73  cfg_ccw_compl_margin_ = config->get_uint((cfg_prefix_ + "ccw_compl_margin").c_str());
74  cfg_cw_compl_slope_ = config->get_uint((cfg_prefix_ + "cw_compl_slope").c_str());
75  cfg_ccw_compl_slope_ = config->get_uint((cfg_prefix_ + "ccw_compl_slope").c_str());
76  cfg_def_angle_margin_ = config->get_float((cfg_prefix_ + "angle_margin").c_str());
77  cfg_enable_echo_fix_ = config->get_bool((cfg_prefix_ + "enable_echo_fix").c_str());
78  cfg_enable_connection_stability_ = 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_ = config->get_float((cfg_prefix_ + "prevent_alarm_shutdown_threshold").c_str());
85  cfg_min_voltage_ = config->get_float((cfg_prefix_ + "min_voltage").c_str());
86  cfg_max_voltage_ = config->get_float((cfg_prefix_ + "max_voltage").c_str());
87  cfg_servos_to_discover_ = config->get_uints((cfg_prefix_ + "servos").c_str());
88 
89  chain_ = new DynamixelChain(cfg_device_.c_str(), cfg_read_timeout_ms_, cfg_enable_echo_fix_, cfg_enable_connection_stability_, cfg_min_voltage_, cfg_max_voltage_);
90  DynamixelChain::DeviceList devl = chain_->discover(cfg_disc_timeout_ms_, cfg_servos_to_discover_);
91  std::list<std::string> found_servos;
92  for (DynamixelChain::DeviceList::iterator i = devl.begin(); i != devl.end(); ++i) {
93  found_servos.push_back(std::to_string(*i));
94  Servo s;
95  s.servo_if = NULL;
96  s.led_if = NULL;
97  s.joint_if = NULL;
98 
99  try {
100  s.servo_if =
102  cfg_name_.c_str(), *i);
103  s.led_if =
104  blackboard->open_for_writing_f<LedInterface>("/dynamixel/%s/%u",
105  cfg_name_.c_str(), *i);
106  s.joint_if =
107  blackboard->open_for_writing_f<JointInterface>("/dynamixel/%s/%u",
108  cfg_name_.c_str(), *i);
109 
110  bbil_add_message_interface(s.servo_if);
111 
112  } catch (Exception &e) {
113  blackboard->close(s.servo_if);
114  blackboard->close(s.led_if);
115  blackboard->close(s.joint_if);
116  throw;
117  }
118 
119  s.move_pending = false;
120  s.mode_set_pending = false;
121  s.recover_pending = false;
122  s.target_angle = 0;
123  s.velo_pending = false;
124  s.vel = 0.;
125  s.enable = false;
126  s.disable = false;
127  s.led_enable = false;
128  s.led_disable = false;
129  s.last_angle = 0.f;
130  s.torque_limit = cfg_torque_limit_ * 0x3ff;
131  s.value_rwlock = new ReadWriteLock();
132  s.angle_margin = cfg_def_angle_margin_;
133 
134  servos_[*i] = s;
135  }
136 
137  logger->log_info(name(), "Found servos [%s]", str_join(found_servos, ",").c_str());
138 
139  chain_rwlock_ = new ReadWriteLock();
140  fresh_data_mutex_ = new Mutex();
141  update_waitcond_ = new WaitCondition();
142 
143  if (servos_.empty()) {
144  throw Exception("No servos found in chain %s", cfg_name_.c_str());
145  }
146 
147  // We only want responses to be sent on explicit READ to speed up communication
149  // set compliance values
151  cfg_cw_compl_margin_, cfg_cw_compl_slope_,
152  cfg_ccw_compl_margin_, cfg_ccw_compl_slope_);
153 
154  // set temperature limit
155  chain_->set_temperature_limit(DynamixelChain::BROADCAST_ID, cfg_temperature_limit_);
156 
157  for (auto &sp : servos_) {
158  unsigned int servo_id = sp.first;
159  Servo &s = sp.second;
160 
161 
162 
163  chain_->set_led_enabled(servo_id, false);
164  chain_->set_torque_enabled(servo_id, true);
165 
166  chain_->read_table_values(servo_id);
167 
168  s.max_speed = chain_->get_max_supported_speed(servo_id);
169 
170  unsigned int cw_limit, ccw_limit;
171  chain_->get_angle_limits(servo_id, cw_limit, ccw_limit);
172 
173  unsigned char cw_margin, cw_slope, ccw_margin, ccw_slope;
174  chain_->get_compliance_values(servo_id, cw_margin, cw_slope, ccw_margin, ccw_slope);
175 
176  s.servo_if->set_model(chain_->get_model(servo_id));
177  s.servo_if->set_model_number(chain_->get_model_number(servo_id));
178  s.servo_if->set_cw_angle_limit(cw_limit);
179  s.servo_if->set_ccw_angle_limit(ccw_limit);
180  s.servo_if->set_temperature_limit(chain_->get_temperature_limit(servo_id));
181  s.servo_if->set_max_torque(chain_->get_max_torque(servo_id));
182  s.servo_if->set_mode(cw_limit == ccw_limit && cw_limit == 0 ? "WHEEL" : "JOINT");
183  s.servo_if->set_cw_slope(cw_slope);
184  s.servo_if->set_ccw_slope(ccw_slope);
185  s.servo_if->set_cw_margin(cw_margin);
186  s.servo_if->set_ccw_margin(ccw_margin);
187  s.servo_if->set_torque_limit(s.torque_limit);
188  s.servo_if->set_max_velocity(s.max_speed);
189  s.servo_if->set_enable_prevent_alarm_shutdown(cfg_prevent_alarm_shutdown_);
190  s.servo_if->set_autorecover_enabled(cfg_autorecover_enabled_);
191  s.servo_if->write();
192 
193  s.servo_if->set_auto_timestamping(false);
194  }
195 
196  if ( cfg_goto_zero_start_ ) {
197  for (auto &s : servos_) {
198  goto_angle_timed(s.first, 0., 3.0);
199  }
200  }
201 
203 }
204 
205 void
207 {
209 
210  for (auto &s : servos_) {
211  blackboard->close(s.second.servo_if);
212  blackboard->close(s.second.led_if);
213  blackboard->close(s.second.joint_if);
214  }
215 
216  delete chain_rwlock_;
217  delete fresh_data_mutex_;
218  delete update_waitcond_;
219 
220  if (cfg_turn_off_) {
221  for (auto &s : servos_) {
222  try {
223  logger->log_debug(name(), "Turning off servo %s:%u", cfg_name_.c_str(), s.first);
224  chain_->set_led_enabled(s.first, false);
225  chain_->set_torque_enabled(s.first, false);
226  } catch (Exception &e) {
227  logger->log_warn(name(), "Failed to turn of servo %s:%u: %s",
228  cfg_name_.c_str(), s.first, e.what());
229  }
230  }
231  // Give some time for shutdown comands to get through
232  usleep(10000);
233  }
234 
235  // Setting to NULL deletes instance (RefPtr)
236  chain_ = NULL;
237 }
238 
239 
240 /** Update sensor values as necessary.
241  * To be called only from DynamixelSensorThread. Writes the current servo
242  * data into the interface.
243  */
244 void
246 {
247  if (has_fresh_data()) {
248  for (auto &sp : servos_) {
249  unsigned int servo_id = sp.first;
250  Servo &s = sp.second;
251 
252  fawkes::Time time;
253  float angle = get_angle(servo_id, time);
254  float vel = get_velocity(servo_id);
255 
256  // poor man's filter: only update if we get a change of least half a degree
257  if (fabs(s.last_angle - angle) >= deg2rad(0.5)) {
258  s.last_angle = angle;
259  } else {
260  angle = s.last_angle;
261  }
262 
263  ScopedRWLock lock(chain_rwlock_, ScopedRWLock::LOCK_READ);
264 
265  s.servo_if->set_timestamp(&s.time);
266  s.servo_if->set_position(chain_->get_position(servo_id));
267  s.servo_if->set_speed(chain_->get_speed(servo_id));
268  s.servo_if->set_goal_position(chain_->get_goal_position(servo_id));
269  s.servo_if->set_goal_speed(chain_->get_goal_speed(servo_id));
270  s.servo_if->set_load(chain_->get_load(servo_id));
271  s.servo_if->set_voltage(chain_->get_voltage(servo_id));
272  s.servo_if->set_temperature(chain_->get_temperature(servo_id));
273  s.servo_if->set_punch(chain_->get_punch(servo_id));
274  s.servo_if->set_angle(angle);
275  s.servo_if->set_velocity(vel);
276  s.servo_if->set_enabled(chain_->is_torque_enabled(servo_id));
277  s.servo_if->set_final(is_final(servo_id));
278  s.servo_if->set_velocity(get_velocity(servo_id));
279  s.servo_if->set_alarm_shutdown(chain_->get_alarm_shutdown(servo_id));
280 
281  if ((chain_->get_load(servo_id) & 0x3ff) > (cfg_prevent_alarm_shutdown_threshold_ * chain_->get_torque_limit(servo_id))) {
282  logger->log_warn(name(), "Servo with ID: %d is in overload condition: torque_limit: %d, load: %d", servo_id, chain_->get_torque_limit(servo_id), chain_->get_load(servo_id) & 0x3ff);
283  if (s.servo_if->is_enable_prevent_alarm_shutdown()) {
284  // is the current load cw or ccw?
285  if (chain_->get_load(servo_id) & 0x400) {
286  goto_angle(servo_id, get_angle(servo_id) + 0.001);
287  }
288  else {
289  goto_angle(servo_id, get_angle(servo_id) - 0.001);
290  }
291  }
292  }
293 
294  if (s.servo_if->is_autorecover_enabled() && chain_->get_error(servo_id) & cfg_autorecover_flags_) {
295  logger->log_warn(name(), "Recovery for servo with ID: %d pending", servo_id);
296  s.recover_pending = true;
297  }
298 
299  unsigned char cur_error = chain_->get_error(servo_id);
300  s.servo_if->set_error(s.servo_if->error() | cur_error);
301  if (cur_error) {
302  logger->log_error(name(), "Servo with ID: %d has error-flag: %d", servo_id, cur_error);
303  }
304  s.servo_if->write();
305 
306  s.joint_if->set_position(angle);
307  s.joint_if->set_velocity(vel);
308  s.joint_if->write();
309  }
310  }
311 }
312 
313 
314 /** Process commands. */
315 void
317 {
318  for (auto &sp : servos_) {
319  unsigned int servo_id = sp.first;
320  Servo &s = sp.second;
321 
322  s.servo_if->set_final(is_final(servo_id));
323 
324  while (! s.servo_if->msgq_empty() ) {
325  if (s.servo_if->msgq_first_is<DynamixelServoInterface::GotoMessage>()) {
326  DynamixelServoInterface::GotoMessage *msg = s.servo_if->msgq_first(msg);
327 
328  goto_angle(servo_id, msg->angle());
329  s.servo_if->set_msgid(msg->id());
330  s.servo_if->set_final(false);
331 
332  } else if (s.servo_if->msgq_first_is<DynamixelServoInterface::TimedGotoMessage>()) {
333  DynamixelServoInterface::TimedGotoMessage *msg = s.servo_if->msgq_first(msg);
334 
335  goto_angle_timed(servo_id, msg->angle(), msg->time_sec());
336  s.servo_if->set_msgid(msg->id());
337  s.servo_if->set_final(false);
338 
339  } else if (s.servo_if->msgq_first_is<DynamixelServoInterface::SetEnabledMessage>()) {
340  DynamixelServoInterface::SetEnabledMessage *msg = s.servo_if->msgq_first(msg);
341 
342  set_enabled(servo_id, msg->is_enabled());
343 
344  } else if (s.servo_if->msgq_first_is<DynamixelServoInterface::SetVelocityMessage>()) {
345  DynamixelServoInterface::SetVelocityMessage *msg = s.servo_if->msgq_first(msg);
346 
347  if (msg->velocity() > s.servo_if->max_velocity()) {
348  logger->log_warn(name(), "Desired velocity %f too high, max is %f",
349  msg->velocity(), s.servo_if->max_velocity());
350  } else {
351  set_velocity(servo_id, msg->velocity());
352  }
353 
354  } else if (s.servo_if->msgq_first_is<DynamixelServoInterface::SetMarginMessage>()) {
355  DynamixelServoInterface::SetMarginMessage *msg = s.servo_if->msgq_first(msg);
356 
357  set_margin(servo_id, msg->angle_margin());
358  s.servo_if->set_angle_margin(msg->angle_margin());
359 
360  } else if (s.servo_if->msgq_first_is<DynamixelServoInterface::ResetRawErrorMessage>()) {
361  s.servo_if->set_error(0);
362 
363  } else if (s.servo_if->msgq_first_is<DynamixelServoInterface::SetPreventAlarmShutdownMessage>()) {
364  DynamixelServoInterface::SetPreventAlarmShutdownMessage *msg = s.servo_if->msgq_first(msg);
366 
367  } else if (s.servo_if->msgq_first_is<DynamixelServoInterface::SetModeMessage>()) {
368  DynamixelServoInterface::SetModeMessage *msg = s.servo_if->msgq_first(msg);
369  set_mode(servo_id, msg->mode());
370 
371  } else if (s.servo_if->msgq_first_is<DynamixelServoInterface::SetSpeedMessage>()) {
372  DynamixelServoInterface::SetSpeedMessage *msg = s.servo_if->msgq_first(msg);
373  set_speed(servo_id, msg->speed());
374 
375  } else if (s.servo_if->msgq_first_is<DynamixelServoInterface::SetAutorecoverEnabledMessage>()) {
376  DynamixelServoInterface::SetAutorecoverEnabledMessage *msg = s.servo_if->msgq_first(msg);
378 
379  } else if (s.servo_if->msgq_first_is<DynamixelServoInterface::SetTorqueLimitMessage>()) {
380  DynamixelServoInterface::SetTorqueLimitMessage *msg = s.servo_if->msgq_first(msg);
381  s.recover_pending = true;
382  s.torque_limit = msg->torque_limit();
383 
384  } else if (s.servo_if->msgq_first_is<DynamixelServoInterface::RecoverMessage>()) {
385  s.recover_pending = true;
386 
387  } else {
388  logger->log_warn(name(), "Unknown message received");
389  }
390 
391  s.servo_if->msgq_pop();
392  }
393 
394  s.servo_if->write();
395 
396  bool write_led_if = false;
397  while (! s.led_if->msgq_empty() ) {
398  write_led_if = true;
399  if (s.led_if->msgq_first_is<LedInterface::SetIntensityMessage>()) {
400  LedInterface::SetIntensityMessage *msg = s.led_if->msgq_first(msg);
401  set_led_enabled(servo_id, (msg->intensity() >= 0.5));
402  s.led_if->set_intensity((msg->intensity() >= 0.5) ? LedInterface::ON : LedInterface::OFF);
403  } else if (s.led_if->msgq_first_is<LedInterface::TurnOnMessage>()) {
404  set_led_enabled(servo_id, true);
405  s.led_if->set_intensity(LedInterface::ON);
406  } else if (s.led_if->msgq_first_is<LedInterface::TurnOffMessage>()) {
407  set_led_enabled(servo_id, false);
408  s.led_if->set_intensity(LedInterface::OFF);
409  }
410 
411  s.led_if->msgq_pop();
412  }
413  if (write_led_if) s.led_if->write();
414  }
415 }
416 
417 
418 bool
420  Message *message) throw()
421 {
422  std::map<unsigned int, Servo>::iterator si =
423  std::find_if(servos_.begin(), servos_.end(),
424  [interface](const std::pair<unsigned int, Servo> &sp){
425  return (strcmp(sp.second.servo_if->uid(), interface->uid()) == 0);
426  });
427  if (si != servos_.end()) {
428  if (message->is_of_type<DynamixelServoInterface::StopMessage>()) {
429  stop_motion(si->first);
430  return false; // do not enqueue StopMessage
431  } else if (message->is_of_type<DynamixelServoInterface::FlushMessage>()) {
432  stop_motion(si->first);
433  logger->log_info(name(), "Flushing message queue");
434  si->second.servo_if->msgq_flush();
435  return false;
436  } else {
437  logger->log_info(name(), "Received message of type %s, enqueueing", message->type());
438  return true;
439  }
440  }
441  return true;
442 }
443 
444 
445 /** Enable or disable servo.
446  * @param enabled true to enable servos, false to turn them off
447  */
448 void
449 DynamixelDriverThread::set_enabled(unsigned int servo_id, bool enabled)
450 {
451  if (servos_.find(servo_id) == servos_.end()) {
452  logger->log_warn(name(), "No servo with ID %u in chain %s, cannot set LED",
453  servo_id, cfg_name_.c_str());
454  return;
455  }
456 
457  Servo &s = servos_[servo_id];
458 
459  logger->log_debug(name(), "Lock %zu", __LINE__);
460  ScopedRWLock lock(s.value_rwlock);
461  if (enabled) {
462  s.enable = true;
463  s.disable = false;
464  } else {
465  s.enable = false;
466  s.disable = true;
467  }
468  wakeup();
469  logger->log_debug(name(), "UNLock %zu", __LINE__);
470 }
471 
472 
473 /** Enable or disable LED.
474  * @param enabled true to enable LED, false to turn it off
475  */
476 void
477 DynamixelDriverThread::set_led_enabled(unsigned int servo_id, bool enabled)
478 {
479  if (servos_.find(servo_id) == servos_.end()) {
480  logger->log_warn(name(), "No servo with ID %u in chain %s, cannot set LED",
481  servo_id, cfg_name_.c_str());
482  return;
483  }
484 
485  Servo &s = servos_[servo_id];
486  logger->log_debug(name(), "Lock %zu", __LINE__);
487  ScopedRWLock lock(s.value_rwlock);
488  if (enabled) {
489  s.led_enable = true;
490  s.led_disable = false;
491  } else {
492  s.led_enable = false;
493  s.led_disable = true;
494  }
495  wakeup();
496  logger->log_debug(name(), "UNLock %zu", __LINE__);
497 }
498 
499 
500 /** Stop currently running motion. */
501 void
502 DynamixelDriverThread::stop_motion(unsigned int servo_id)
503 {
504  if (servos_.find(servo_id) == servos_.end()) {
505  logger->log_warn(name(), "No servo with ID %u in chain %s, cannot set LED",
506  servo_id, cfg_name_.c_str());
507  return;
508  }
509 
510  float angle = get_angle(servo_id);
511  goto_angle(servo_id, angle);
512 }
513 
514 
515 /** Goto desired angle value.
516  * @param angle in radians
517  */
518 void
519 DynamixelDriverThread::goto_angle(unsigned int servo_id, float angle)
520 {
521  if (servos_.find(servo_id) == servos_.end()) {
522  logger->log_warn(name(), "No servo with ID %u in chain %s, cannot set LED",
523  servo_id, cfg_name_.c_str());
524  return;
525  }
526 
527  Servo &s = servos_[servo_id];
528 
529  logger->log_debug(name(), "Lock %zu", __LINE__);
530  ScopedRWLock lock(s.value_rwlock);
531  s.target_angle = angle;
532  s.move_pending = true;
533  wakeup();
534  logger->log_debug(name(), "UNLock %zu", __LINE__);
535 }
536 
537 
538 /** Goto desired angle value in a specified time.
539  * @param angle in radians
540  * @param time_sec time when to reach the desired angle value
541  */
542 void
543 DynamixelDriverThread::goto_angle_timed(unsigned int servo_id, float angle, float time_sec)
544 {
545  if (servos_.find(servo_id) == servos_.end()) {
546  logger->log_warn(name(), "No servo with ID %u in chain %s, cannot set LED",
547  servo_id, cfg_name_.c_str());
548  return;
549  }
550  Servo &s = servos_[servo_id];
551 
552  s.target_angle = angle;
553  s.move_pending = true;
554 
555  float cangle = get_angle(servo_id);
556  float angle_diff = fabs(angle - cangle);
557  float req_angle_vel = angle_diff / time_sec;
558 
559  if (req_angle_vel > s.max_speed) {
560  logger->log_warn(name(), "Requested move to %f in %f sec requires a "
561  "angle speed of %f rad/s, which is greater than the maximum "
562  "of %f rad/s, reducing to max", angle, time_sec, req_angle_vel);
563  req_angle_vel = s.max_speed;
564  }
565  set_velocity(servo_id, req_angle_vel);
566 
567  wakeup();
568 }
569 
570 
571 /** Set desired velocity.
572  * @param vel the desired velocity in rad/s
573  */
574 void
575 DynamixelDriverThread::set_velocity(unsigned int servo_id, float vel)
576 {
577  if (servos_.find(servo_id) == servos_.end()) {
578  logger->log_warn(name(), "No servo with ID %u in chain %s, cannot set velocity",
579  servo_id, cfg_name_.c_str());
580  return;
581  }
582  Servo &s = servos_[servo_id];
583 
584  float velo_tmp = roundf((vel / s.max_speed) * DynamixelChain::MAX_SPEED);
585  set_speed(servo_id, (unsigned int) velo_tmp);
586 }
587 
588 
589 
590 /** Set desired speed.
591  * When the servo is set to wheel mode, bit 10 of the speed value is used
592  * to either move cw (1) or ccw (0).
593  * @param speed the speed
594  */
595 void
596 DynamixelDriverThread::set_speed(unsigned int servo_id, unsigned int speed)
597 {
598  if (servos_.find(servo_id) == servos_.end()) {
599  logger->log_warn(name(), "No servo with ID %u in chain %s, cannot set speed",
600  servo_id, cfg_name_.c_str());
601  return;
602  }
603  Servo &s = servos_[servo_id];
604 
605  ScopedRWLock lock(s.value_rwlock);
606  if (speed <= DynamixelChain::MAX_SPEED) {
607  s.vel = speed;
608  s.velo_pending = true;
609  } else {
610  logger->log_warn(name(), "Calculated velocity value out of bounds, "
611  "min: 0 max: %u des: %u",
613  }
614 }
615 
616 
617 /** Set desired mode.
618  * @param mode, either DynamixelServoInterface.JOINT or DynamixelServoInterface.WHEEL
619  */
620 void
621 DynamixelDriverThread::set_mode(unsigned int servo_id, unsigned int mode)
622 {
623  if (servos_.find(servo_id) == servos_.end()) {
624  logger->log_warn(name(), "No servo with ID %u in chain %s, cannot set mode",
625  servo_id, cfg_name_.c_str());
626  return;
627  }
628  Servo &s = servos_[servo_id];
629 
630  ScopedRWLock(s.value_rwlock);
631  s.mode_set_pending = true;
632  s.new_mode = mode;
633  s.servo_if->set_mode(mode == DynamixelServoInterface::JOINT ? "JOINT" : "WHEEL");
634 }
635 
636 
637 /** Get current velocity.
638  */
639 float
640 DynamixelDriverThread::get_velocity(unsigned int servo_id)
641 {
642  if (servos_.find(servo_id) == servos_.end()) {
643  logger->log_warn(name(), "No servo with ID %u in chain %s, cannot set velocity",
644  servo_id, cfg_name_.c_str());
645  return 0.;
646  }
647  Servo &s = servos_[servo_id];
648 
649  unsigned int velticks = chain_->get_speed(servo_id);
650 
651  return
652  (((float)velticks / (float)DynamixelChain::MAX_SPEED) * s.max_speed);
653 }
654 
655 
656 /** Set desired angle margin.
657  * @param angle_margin the desired angle_margin
658  */
659 void
660 DynamixelDriverThread::set_margin(unsigned int servo_id, float angle_margin)
661 {
662  if (servos_.find(servo_id) == servos_.end()) {
663  logger->log_warn(name(), "No servo with ID %u in chain %s, cannot set velocity",
664  servo_id, cfg_name_.c_str());
665  return;
666  }
667  Servo &s = servos_[servo_id];
668  if (angle_margin > 0.0) s.angle_margin = angle_margin;
669 }
670 
671 
672 /** Get angle - the position from -2.62 to + 2.62 (-150 to +150 degrees)
673  */
674 float
675 DynamixelDriverThread::get_angle(unsigned int servo_id)
676 {
677  if (servos_.find(servo_id) == servos_.end()) {
678  logger->log_warn(name(), "No servo with ID %u in chain %s, cannot set velocity",
679  servo_id, cfg_name_.c_str());
680  return 0.;
681  }
682 
683  ScopedRWLock lock(chain_rwlock_, ScopedRWLock::LOCK_READ);
684 
685  int ticks = ((int)chain_->get_position(servo_id) - (int)DynamixelChain::CENTER_POSITION);
686 
687  return ticks * DynamixelChain::RAD_PER_POS_TICK;
688 }
689 
690 
691 /** Get angle value with time.
692  * @param time upon return contains the time the angle value was read
693  */
694 float
695 DynamixelDriverThread::get_angle(unsigned int servo_id,
696  fawkes::Time &time)
697 {
698  if (servos_.find(servo_id) == servos_.end()) {
699  logger->log_warn(name(), "No servo with ID %u in chain %s, cannot set velocity",
700  servo_id, cfg_name_.c_str());
701  return 0.;
702  }
703  Servo &s = servos_[servo_id];
704 
705  time = s.time;
706  return get_angle(servo_id);
707 }
708 
709 
710 /** Check if motion is final.
711  * @return true if motion is final, false otherwise
712  */
713 bool
714 DynamixelDriverThread::is_final(unsigned int servo_id)
715 {
716  if (servos_.find(servo_id) == servos_.end()) {
717  logger->log_warn(name(), "No servo with ID %u in chain %s, cannot set velocity",
718  servo_id, cfg_name_.c_str());
719  return 0.;
720  }
721  Servo &s = servos_[servo_id];
722 
723  float angle = get_angle(servo_id);
724 
725  ScopedRWLock lock(chain_rwlock_, ScopedRWLock::LOCK_READ);
726 
727  return ( (fabs(angle - s.target_angle) <= s.angle_margin) ||
728  (! chain_->is_moving(servo_id)));
729 }
730 
731 
732 /** Check if servo is enabled.
733  * @return true if torque is enabled for both servos, false otherwise
734  */
735 bool
736 DynamixelDriverThread::is_enabled(unsigned int servo_id)
737 {
738  return chain_->is_torque_enabled(servo_id);
739 }
740 
741 
742 /** Check is fresh sensor data is available.
743  * Note that this method will return true at once per sensor update cycle.
744  * @return true if fresh data is available, false otherwise
745  */
746 bool
747 DynamixelDriverThread::has_fresh_data()
748 {
749  MutexLocker lock(fresh_data_mutex_);
750 
751  bool rv = fresh_data_;
752  fresh_data_ = false;
753  return rv;
754 }
755 
756 
757 void
759 {
760  for (auto &sp : servos_) {
761  unsigned int servo_id = sp.first;
762  Servo &s = sp.second;
763  if (s.enable) {
764  s.value_rwlock->lock_for_write();
765  s.enable = false;
766  s.value_rwlock->unlock();
767  ScopedRWLock lock(chain_rwlock_);
768  chain_->set_led_enabled(servo_id, true);
769  chain_->set_torque_enabled(servo_id, true);
770  if (s.led_enable || s.led_disable || s.velo_pending || s.move_pending || s.mode_set_pending || s.recover_pending) usleep(3000);
771  } else if (s.disable) {
772  s.value_rwlock->lock_for_write();
773  s.disable = false;
774  s.value_rwlock->unlock();
775  ScopedRWLock lock(chain_rwlock_);
776  chain_->set_torque_enabled(servo_id, false);
777  if (s.led_enable || s.led_disable || s.velo_pending || s.move_pending || s.mode_set_pending || s.recover_pending) usleep(3000);
778  }
779 
780  if (s.led_enable) {
781  s.value_rwlock->lock_for_write();
782  s.led_enable = false;
783  s.value_rwlock->unlock();
784  ScopedRWLock lock(chain_rwlock_);
785  chain_->set_led_enabled(servo_id, true);
786  if (s.velo_pending || s.move_pending || s.mode_set_pending || s.recover_pending) usleep(3000);
787  } else if (s.led_disable) {
788  s.value_rwlock->lock_for_write();
789  s.led_disable = false;
790  s.value_rwlock->unlock();
791  ScopedRWLock lock(chain_rwlock_);
792  chain_->set_led_enabled(servo_id, false);
793  if (s.velo_pending || s.move_pending || s.mode_set_pending || s.recover_pending) usleep(3000);
794  }
795 
796  if (s.velo_pending) {
797  s.value_rwlock->lock_for_write();
798  s.velo_pending = false;
799  unsigned int vel = s.vel;
800  s.value_rwlock->unlock();
801  ScopedRWLock lock(chain_rwlock_);
802  chain_->set_goal_speed(servo_id, vel);
803  if (s.move_pending || s.mode_set_pending || s.recover_pending) usleep(3000);
804  }
805 
806  if (s.move_pending) {
807  s.value_rwlock->lock_for_write();
808  s.move_pending = false;
809  float target_angle = s.target_angle;
810  s.value_rwlock->unlock();
811  exec_goto_angle(servo_id, target_angle);
812  if (s.mode_set_pending || s.recover_pending) usleep(3000);
813  }
814 
815  if (s.mode_set_pending) {
816  s.value_rwlock->lock_for_write();
817  s.mode_set_pending = false;
818  exec_set_mode(servo_id, s.new_mode);
819  s.value_rwlock->unlock();
820  if (s.recover_pending) usleep(3000);
821  }
822 
823  if (s.recover_pending) {
824  s.value_rwlock->lock_for_write();
825  s.recover_pending = false;
826  chain_->set_torque_limit(servo_id, s.torque_limit);
827  s.value_rwlock->unlock();
828  }
829 
830  try {
831  ScopedRWLock lock(chain_rwlock_, ScopedRWLock::LOCK_READ);
832  chain_->read_table_values(servo_id);
833 
834  MutexLocker lock_fresh_data(fresh_data_mutex_);
835  fresh_data_ = true;
836  s.time.stamp();
837  } catch (Exception &e) {
838  // usually just a timeout, too noisy
839  //logger_->log_warn(name(), "Error while reading table values from servos, exception follows");
840  //logger_->log_warn(name(), e);
841  }
842  }
843 
844  update_waitcond_->wake_all();
845 
846  // Wakeup ourselves for faster updates
847  wakeup();
848 }
849 
850 
851 /** Execute angle motion.
852  * @param angle_rad angle in rad to move to
853  */
854 void
855 DynamixelDriverThread::exec_goto_angle(unsigned int servo_id, float angle_rad)
856 {
857  unsigned int pos_min = 0, pos_max = 0;
858  chain_->get_angle_limits(servo_id, pos_min, pos_max);
859 
860  int pos = (int)roundf(DynamixelChain::POS_TICKS_PER_RAD * angle_rad)
862 
863  if ( (pos < 0) || ((unsigned int)pos < pos_min) || ((unsigned int)pos > pos_max) ) {
864  logger->log_warn(name(), "Position out of bounds, min: %u max: %u des: %i",
865  pos_min, pos_max, pos);
866  return;
867  }
868 
869  ScopedRWLock lock(chain_rwlock_);
870  chain_->goto_position(servo_id, pos);
871 }
872 
873 
874 /** Execute set mode.
875  * @param new_mode - either DynamixelServoInterface::JOINT or DynamixelServoInterface::WHEEL
876  */
877 void
878 DynamixelDriverThread::exec_set_mode(unsigned int servo_id, unsigned int new_mode)
879 {
880  if (new_mode == DynamixelServoInterface::JOINT) {
881  ScopedRWLock lock(chain_rwlock_);
882  chain_->set_angle_limits(servo_id, 0, 1023);
883  }
884  else if (new_mode == DynamixelServoInterface::WHEEL) {
885  ScopedRWLock lock(chain_rwlock_);
886  chain_->set_angle_limits(servo_id, 0, 0);
887  }
888  else {
889  logger->log_error(name(), "Mode %d cannot be set - unknown",
890  new_mode);
891  }
892 
893  return;
894 }
895 
896 
897 /** Wait for fresh data to be received.
898  * Blocks the calling thread.
899  */
900 void
901 DynamixelDriverThread::wait_for_fresh_data()
902 {
903  update_waitcond_->wait();
904 }
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:44
unsigned int id() const
Get message ID.
Definition: message.cpp:197
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:134
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.
bool is_autorecover_enabled() const
Get autorecover_enabled value.
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:134
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:91
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:661
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:218
Thread class encapsulation of pthreads.
Definition: thread.h:42
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:79
static const float RAD_PER_POS_TICK
RAD_PER_POS_TICK.
Definition: servo_chain.h:139
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:44
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:190
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:1000
void set_name(const char *format,...)
Set name of thread.
Definition: thread.cpp:761
Base class for exceptions in Fawkes.
Definition: exception.h:36
static const unsigned char SRL_RESPOND_READ
SRL_RESPOND_READ.
Definition: servo_chain.h:131
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:140
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:326
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:95
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.
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:136
DeviceList discover(unsigned int total_timeout_ms=50, const std::vector< unsigned int > servos=std::vector< unsigned int >())
Discover devices on the bus.
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:37
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:44
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:143
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.