diff --git a/README.md b/README.md index 7718f5c..b52eeea 100644 --- a/README.md +++ b/README.md @@ -7,16 +7,15 @@ are welcome too. Make sure the [DynamixelSDK](https://github.com/ROBOTIS-GIT/DynamixelSDK) for ROS2 is installed. 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/ -git checkout ros2 ``` finally build with colcon `colcon build` ## Configure Dynamixel motor parameters via URDF -Add the `serial_port`, `baud_rate`, and `id` parameters to the ros2_control part of your `.urdf` / `.xarco` files. +Add the `serial_port`, `baud_rate`, and `id` parameters to the ros2_control part of your `.urdf` / `.xarco` files. ```xml @@ -27,6 +26,7 @@ Add the `serial_port`, `baud_rate`, and `id` parameters to the ros2_control part 10 + @@ -35,6 +35,8 @@ Add the `serial_port`, `baud_rate`, and `id` parameters to the ros2_control part ... ``` +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 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: diff --git a/include/dynamixel_ros2_control/dynamixel_driver.hpp b/include/dynamixel_ros2_control/dynamixel_driver.hpp index dae3de7..46dfa61 100644 --- a/include/dynamixel_ros2_control/dynamixel_driver.hpp +++ b/include/dynamixel_ros2_control/dynamixel_driver.hpp @@ -41,6 +41,7 @@ class Driver { const model_info* model; bool led; + bool rebooting; double goal_position; @@ -86,7 +87,7 @@ class Driver { 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(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 { model = &model_infos.at(type); } catch (const std::exception &e) { @@ -173,7 +174,9 @@ class Driver { error_callback(hardware_status::Overload_Error); } //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) { if(torque && motor->hw_error) { + if(motor->rebooting) + return; throw hardware_error(motor, "Can't enable torque!"); } field command_field; @@ -606,6 +611,7 @@ class Driver { auto it_motor = motors.find(joint_name); if (it_motor == motors.end()) throw std::invalid_argument("Joint \"" + joint_name + "\" not added to driver!"); + it_motor->second->rebooting = true; reboot(it_motor->second); } }; diff --git a/src/dynamixel_ros2_control.cpp b/src/dynamixel_ros2_control.cpp index 7ebd782..eacfe1c 100644 --- a/src/dynamixel_ros2_control.cpp +++ b/src/dynamixel_ros2_control.cpp @@ -148,7 +148,7 @@ void DynamixelHardwareInterface::overload_error_callback( try { dynamixel_driver_->reboot(joint_name); RCLCPP_INFO(rclcpp::get_logger(DYN_LOGGER_NAME_STR), - ("\"" + joint_name + " rebootet").c_str()); + ("\"" + joint_name + "\" rebooting").c_str()); // succeeded = true; } catch (std::invalid_argument &e) { RCLCPP_WARN(rclcpp::get_logger(DYN_LOGGER_NAME_STR), "%s", e.what());