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