From 4c12857fc4730cb86439579dd2bb35261fd3a519 Mon Sep 17 00:00:00 2001 From: Erik Andresen Date: Mon, 13 Apr 2020 09:46:55 +0200 Subject: [PATCH] Added ~get_error service --- CMakeLists.txt | 29 ++++++++++++++--------------- include/open_manipulator_i2c.h | 29 +++++++++++++++++++++++++++-- msg/HardwareError.msg | 6 ++++++ package.xml | 2 ++ srv/GetError.srv | 2 ++ 5 files changed, 51 insertions(+), 17 deletions(-) create mode 100644 msg/HardwareError.msg create mode 100644 srv/GetError.srv diff --git a/CMakeLists.txt b/CMakeLists.txt index 0d17567..1eba409 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -11,6 +11,7 @@ find_package(catkin REQUIRED COMPONENTS roscpp joint_trajectory_controller controller_manager + message_generation ) ## System dependencies are found with CMake's conventions @@ -47,18 +48,16 @@ find_package(catkin REQUIRED COMPONENTS ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) ## Generate messages in the 'msg' folder -# add_message_files( -# FILES -# Message1.msg -# Message2.msg -# ) +add_message_files( + FILES + HardwareError.msg +) ## Generate services in the 'srv' folder -# add_service_files( -# FILES -# Service1.srv -# Service2.srv -# ) +add_service_files( + FILES + GetError.srv +) ## Generate actions in the 'action' folder # add_action_files( @@ -68,10 +67,10 @@ find_package(catkin REQUIRED COMPONENTS # ) ## Generate added messages and services with any dependencies listed here -# generate_messages( -# DEPENDENCIES -# std_msgs # Or other packages containing msgs -# ) +generate_messages( + DEPENDENCIES + std_msgs +) ################################################ ## Declare ROS dynamic reconfigure parameters ## @@ -143,7 +142,7 @@ add_executable(${PROJECT_NAME}_node src/wt_open_manipulator_node.cpp) ## Add cmake target dependencies of the executable ## same as for the library above -# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) ## Specify libraries to link a library or executable target against target_link_libraries(${PROJECT_NAME}_node diff --git a/include/open_manipulator_i2c.h b/include/open_manipulator_i2c.h index da6d18e..f46045c 100644 --- a/include/open_manipulator_i2c.h +++ b/include/open_manipulator_i2c.h @@ -10,11 +10,13 @@ #include #include #include "std_srvs/SetBool.h" +#include "wt_open_manipulator/GetError.h" #define I2C_FILE "/dev/i2c-2" #define I2C_ADDR (0x60>>1) #define XM_OPERATING_MODE 11 #define XM_TORQUE_ENABLE 64 +#define XM_HARDWARE_ERROR_STATUS 70 #define XM_GOAL_CURRENT 102 #define XM_PROFILE_ACCELERATION 108 #define XM_PROFILE_VELOCITY 112 @@ -34,6 +36,7 @@ class WtOpenManipulatorI2C : public hardware_interface::RobotHW uint8_t mode=255; this->nh = nh; torque_enable_service = nh.advertiseService("torque_enable", &WtOpenManipulatorI2C::cbTorqueEnable, this); + get_error_service = nh.advertiseService("get_error", &WtOpenManipulatorI2C::cbGetError, this); // connect and register the joint state interface hardware_interface::JointStateHandle state_handle_1("joint1", &pos[0], &vel[0], &eff[0]); @@ -207,7 +210,7 @@ class WtOpenManipulatorI2C : public hardware_interface::RobotHW double pos[5] = {0, 0, 0, 0, 0}; double vel[5] = {0, 0, 0, 0, 0}; double eff[5] = {0, 0, 0, 0, 0}; - ros::ServiceServer torque_enable_service; + ros::ServiceServer torque_enable_service, get_error_service; std::mutex comm_mutex; int dynamixel_write(uint8_t *write_data, int write_num, uint8_t *read_data, int read_num) { @@ -311,9 +314,31 @@ class WtOpenManipulatorI2C : public hardware_interface::RobotHW return 255; } - bool cbTorqueEnable(std_srvs::SetBool::Request &req, std_srvs::SetBool::Response &res) { + bool cbTorqueEnable(std_srvs::SetBool::Request &req, std_srvs::SetBool::Response &res) { ROS_INFO("Setting torque enable=%d", req.data); res.success = torque_enable(req.data); return true; } + + bool cbGetError(wt_open_manipulator::GetError::Request &req, wt_open_manipulator::GetError::Response &res) { + ROS_INFO("Reading Hardware Error Status"); + + for(int id=11, i=0; id<=15; id++, i++) { + wt_open_manipulator::HardwareError hw_error; + uint8_t value; + if (dynamixel_readbyte(id, XM_HARDWARE_ERROR_STATUS, &value) != 1) { + ROS_ERROR("I2C dynamixel id=%d reading hardware error status", id); + return false; + } + hw_error.id = id; + hw_error.inputVoltage = value & (1<<0); + hw_error.overHeating = value & (1<<2); + hw_error.motorEncoder = value & (1<<3); + hw_error.electricalShock = value & (1<<4); + hw_error.overload = value & (1<<5); + res.errors.push_back(hw_error); + } + + return true; + } }; diff --git a/msg/HardwareError.msg b/msg/HardwareError.msg new file mode 100644 index 0000000..1a02a64 --- /dev/null +++ b/msg/HardwareError.msg @@ -0,0 +1,6 @@ +int32 id +bool inputVoltage +bool overHeating +bool motorEncoder +bool electricalShock +bool overload diff --git a/package.xml b/package.xml index bb8c8a4..0c9f3a0 100644 --- a/package.xml +++ b/package.xml @@ -52,8 +52,10 @@ controller_manager joint_trajectory_controller libi2c-dev + message_generation controller_manager open_manipulator_description + message_runtime diff --git a/srv/GetError.srv b/srv/GetError.srv new file mode 100644 index 0000000..97c21d5 --- /dev/null +++ b/srv/GetError.srv @@ -0,0 +1,2 @@ +--- +wt_open_manipulator/HardwareError[] errors -- 2.39.5