+
+ 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;
+ }