Added ~get_error service

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 0d175677f2857afe19a3e1b1ec99a3bbf6ff2c2f..1eba4097881eb81c29768a3ca218319f3eae0eac 100644 (file)
--- 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 da6d18ee911494cf8751589df2a94c32c9a8a440..f46045c7af1cc68c67f8dd91d4be7ff6cdf5f49f 100644 (file)
--- a/include/open_manipulator_i2c.h
+++ b/include/open_manipulator_i2c.h
@@ -10,11 +10,13 @@
 #include <hardware_interface/joint_state_interface.h>
 #include <hardware_interface/robot_hw.h>
 #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 (file)
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 bb8c8a40b0ff80bbde0c467b162fe78d446ff43e..0c9f3a085dc675e54c202629277fa2a2a6bb4052 100644 (file)
--- a/package.xml
+++ b/package.xml
@@ -52,8 +52,10 @@
   <build_depend>controller_manager</build_depend>
   <build_depend>joint_trajectory_controller</build_depend>
   <build_depend>libi2c-dev</build_depend>
+  <build_depend>message_generation</build_depend>
   <exec_depend>controller_manager</exec_depend>
   <exec_depend>open_manipulator_description</exec_depend>
+  <exec_depend>message_runtime</exec_depend>
 
   <!-- The export tag contains other, unspecified, tags -->
   <export>

diff --git a/srv/GetError.srv b/srv/GetError.srv
new file mode 100644 (file)
index 0000000..97c21d5
--- /dev/null
+++ b/srv/GetError.srv
@@ -0,0 +1,2 @@
+---
+wt_open_manipulator/HardwareError[] errors