nils/dynamixel_ros2_control
0
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

try restart motor on error

Browse Source
This commit is contained in:
Nils Schulte 2022-09-13 12:15:54 +02:00
parent b8e3f915cf
commit 8d3ff7b5cd
3 changed files with 33 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

31
include/dynamixel_ros2_control/dynamixel_driver.hpp
View File

@ -54,6 +54,7 @@ class Driver {
//double goal_velocity;
double velocity;
double effort;
bool torque_target;
bool torque;
uint8_t hw_error;
@ -101,6 +102,7 @@ class Driver {
rebooted(false),
goal_position(std::numeric_limits<double>::quiet_NaN()),
position(std::numeric_limits<double>::quiet_NaN()),
torque_target(false),
p_gain(std::numeric_limits<double>::quiet_NaN()),
i_gain(std::numeric_limits<double>::quiet_NaN()),
d_gain(std::numeric_limits<double>::quiet_NaN())
@ -127,9 +129,9 @@ class Driver {
command::Torque_Enable,
[this](uint32_t data) {
this->torque = bool(data);
if (this->torque == false) {
goal_position = std::numeric_limits<double>::quiet_NaN();
}
//if (this->torque == false) {
//goal_position = std::numeric_limits<double>::quiet_NaN();
//}
});
//Present Position
@ -321,9 +323,11 @@ class Driver {
* to prepare it for the writeing over the bus.
*/
void set_torque(const dynamixel::Driver::Motor::Ptr& motor, bool torque) {
motor->torque_target = torque;
if(torque && motor->hw_error) {
if(motor->rebooting)
if(motor->rebooting) {
return;
}
throw hardware_error(motor, "Can't enable torque!");
}
field command_field;
@ -331,13 +335,19 @@ class Driver {
uint32_t p = torque;
add_syncwrite_data(motor->id, command_field, reinterpret_cast<uint8_t*>(&p));
}
void reboot(dynamixel::Driver::Motor::Ptr& motor) {
motor->after_reboot_functions.clear();
//bool old_torque_target = motor->torque_target;
motor->after_reboot_functions.push_back([this, motor](){
motor->torque = true;
set_torque(motor, false);
this->pid_gains(motor->name, motor->p_gain_target, motor->i_gain_target, motor->d_gain_target);
});
packetHandler->reboot(portHandler.get(),motor->id);
set_torque(motor, false);
motor->torque = false;
write();
}
hw_error_callback overload_error_callback;
@ -450,6 +460,9 @@ class Driver {
*/
void write(bool enable_torque = false) {
for (const auto& [joint_name, motor]: motors) {
if(motor->torque_target != motor->torque) {
set_torque(motor, motor->torque_target);
}
if(motor->torque || enable_torque) {
if(std::isnan(motor->goal_position))
motor->goal_position = motor->position;
@ -502,6 +515,9 @@ class Driver {
after_reboot_f();
}
}
if(motor->rebooted) {
motor->after_reboot_functions.clear();
}
}
}
}
@ -670,9 +686,10 @@ class Driver {
auto motor = it_motor->second; //motors[joint_name];
if (!motor->torque) {
if (enable_torque)
if (enable_torque) {
set_torque(motor, true);
else
return;
} else
throw torque_not_enabled(motor);
}

3
include/dynamixel_ros2_control/dynamixel_ros2_controll.hpp
View File

@ -18,6 +18,9 @@ class DynamixelHardwareInterface : public hardware_interface::SystemInterface {
public:
RCLCPP_SHARED_PTR_DEFINITIONS(DynamixelHardwareInterface)
DYNAMIXEL_HARDWARE_INTERFACE_PUBLIC
DynamixelHardwareInterface();
DYNAMIXEL_HARDWARE_INTERFACE_PUBLIC
CallbackReturn on_init(const hardware_interface::HardwareInfo &info) override;

6
src/dynamixel_ros2_control.cpp
View File

@ -47,6 +47,12 @@
#define DYN_DEFAULT_PORT_STR "/dev/ttyUSB0"
namespace dynamixel_ros2_control {
DynamixelHardwareInterface::DynamixelHardwareInterface() {
rclcpp::on_shutdown([&](){
dynamixel_driver_->set_torque_all(false);
dynamixel_driver_->write();
});
}
CallbackReturn DynamixelHardwareInterface::on_init(
const hardware_interface::HardwareInfo &info) {