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

auto reset on hardware error

Browse Source
This commit is contained in:
Nils Schulte 2022-02-13 05:50:13 +01:00
parent c03076fc86
commit f38fcac529
3 changed files with 14 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
README.md
View File

@ -7,9 +7,8 @@ are welcome too.
Make sure the [DynamixelSDK](https://github.com/ROBOTIS-GIT/DynamixelSDK) for ROS2 is installed. Make sure the [DynamixelSDK](https://github.com/ROBOTIS-GIT/DynamixelSDK) for ROS2 is installed.
Clone it in your workspace and checkout the branch ros2: Clone it in your workspace and checkout the branch ros2:
``` ```
git clone https://github.com/ROBOTIS-GIT/DynamixelSDK.git git clone https://github.com/ROBOTIS-GIT/DynamixelSDK.git -b ros2
cd DynamixelSDK/ cd DynamixelSDK/
git checkout ros2
``` ```
finally build with colcon finally build with colcon
`colcon build` `colcon build`
@ -27,6 +26,7 @@ Add the `serial_port`, `baud_rate`, and `id` parameters to the ros2_control part
</hardware> </hardware>
<joint name="joint1"> <joint name="joint1">
<param name="id">10</param> <param name="id">10</param>
<!--param name="reboot_on_error">true</param-->
<command_interface name="position"/> <command_interface name="position"/>
<!--command_interface name="velocity"/--> <!--command_interface name="velocity"/-->
<state_interface name="position"/> <state_interface name="position"/>
@ -35,6 +35,8 @@ Add the `serial_port`, `baud_rate`, and `id` parameters to the ros2_control part
</joint> </joint>
... ...
``` ```
When `reboot_on_error` is "true" the motor automaticaly reboots on mechanical overload error (it still loses torque for a brief moment, but that should be enogh to let the motor recover the fault).
## Run a test ## Run a test
You need the urdf of the robot with the ros2_control xml added. You need the urdf of the robot with the ros2_control xml added.
After starting the control nodes and activating the Joint Trajectory Controller you can use ros2cli commands to test the actuators: After starting the control nodes and activating the Joint Trajectory Controller you can use ros2cli commands to test the actuators:

8
include/dynamixel_ros2_control/dynamixel_driver.hpp
View File

@ -41,6 +41,7 @@ class Driver {
const model_info* model; const model_info* model;
bool led; bool led;
bool rebooting;
double goal_position; double goal_position;
@ -86,7 +87,7 @@ class Driver {
void set_velocity_from_raw(uint32_t raw) { velocity = velocity_from_raw(raw); } void set_velocity_from_raw(uint32_t raw) { velocity = velocity_from_raw(raw); }
void set_effort_from_raw(uint32_t raw) { effort = double(static_cast<int16_t>(raw))*0.1; } void set_effort_from_raw(uint32_t raw) { effort = double(static_cast<int16_t>(raw))*0.1; }
Motor(const motor_id p_id, std::string p_name, const model_t type) : id(p_id), name(p_name), goal_position(std::nan("")), position(std::nan("")){ Motor(const motor_id p_id, std::string p_name, const model_t type) : id(p_id), name(p_name), rebooting(false), goal_position(std::nan("")), position(std::nan("")) {
try { try {
model = &model_infos.at(type); model = &model_infos.at(type);
} catch (const std::exception &e) { } catch (const std::exception &e) {
@ -173,6 +174,8 @@ class Driver {
error_callback(hardware_status::Overload_Error); error_callback(hardware_status::Overload_Error);
} }
//https://emanual.robotis.com/docs/en/dxl/x/xl430-w250/#hardware-error-status //https://emanual.robotis.com/docs/en/dxl/x/xl430-w250/#hardware-error-status
} else {
this->rebooting = false;
} }
}); });
} }
@ -272,6 +275,8 @@ class Driver {
*/ */
void set_torque(const dynamixel::Driver::Motor::Ptr& motor, bool torque) { void set_torque(const dynamixel::Driver::Motor::Ptr& motor, bool torque) {
if(torque && motor->hw_error) { if(torque && motor->hw_error) {
if(motor->rebooting)
return;
throw hardware_error(motor, "Can't enable torque!"); throw hardware_error(motor, "Can't enable torque!");
} }
field command_field; field command_field;
@ -606,6 +611,7 @@ class Driver {
auto it_motor = motors.find(joint_name); auto it_motor = motors.find(joint_name);
if (it_motor == motors.end()) if (it_motor == motors.end())
throw std::invalid_argument("Joint \"" + joint_name + "\" not added to driver!"); throw std::invalid_argument("Joint \"" + joint_name + "\" not added to driver!");
it_motor->second->rebooting = true;
reboot(it_motor->second); reboot(it_motor->second);
} }
}; };

2
src/dynamixel_ros2_control.cpp
View File

@ -148,7 +148,7 @@ void DynamixelHardwareInterface::overload_error_callback(
try { try {
dynamixel_driver_->reboot(joint_name); dynamixel_driver_->reboot(joint_name);
RCLCPP_INFO(rclcpp::get_logger(DYN_LOGGER_NAME_STR), RCLCPP_INFO(rclcpp::get_logger(DYN_LOGGER_NAME_STR),
("\"" + joint_name + " rebootet").c_str()); ("\"" + joint_name + "\" rebooting").c_str());
// succeeded = true; // succeeded = true;
} catch (std::invalid_argument &e) { } catch (std::invalid_argument &e) {
RCLCPP_WARN(rclcpp::get_logger(DYN_LOGGER_NAME_STR), "%s", e.what()); RCLCPP_WARN(rclcpp::get_logger(DYN_LOGGER_NAME_STR), "%s", e.what());