From 6d15c3b1f4859786ed8eba1ba9e987dfbfb9979d Mon Sep 17 00:00:00 2001 From: nnarayann Date: Tue, 14 Oct 2014 12:52:09 +0200 Subject: [PATCH 1/5] Added motor error warning mechanism --- .../src/dynamixel_driver/dynamixel_serial_proxy.py | 7 +++++++ dynamixel_msgs/CMakeLists.txt | 1 + dynamixel_msgs/msg/MotorError.msg | 3 +++ 3 files changed, 11 insertions(+) create mode 100644 dynamixel_msgs/msg/MotorError.msg diff --git a/dynamixel_driver/src/dynamixel_driver/dynamixel_serial_proxy.py b/dynamixel_driver/src/dynamixel_driver/dynamixel_serial_proxy.py index ddc7abc..711672a 100755 --- a/dynamixel_driver/src/dynamixel_driver/dynamixel_serial_proxy.py +++ b/dynamixel_driver/src/dynamixel_driver/dynamixel_serial_proxy.py @@ -60,6 +60,7 @@ from diagnostic_msgs.msg import DiagnosticStatus from diagnostic_msgs.msg import KeyValue +from dynamixel_msgs.msg import MotorError from dynamixel_msgs.msg import MotorState from dynamixel_msgs.msg import MotorStateList @@ -227,6 +228,12 @@ def __update_motor_states(self): if dynamixel_io.exception: raise dynamixel_io.exception except dynamixel_io.FatalErrorCodeError, fece: rospy.logerr(fece) + me = MotorError() + me.error_type = "FatalErrorCodeError" + me.error_message = fece.message + me.extra_info = fece.message.split(' ')[3][1:] + pub = rospy.Publisher('dynamixel_motor_errors', MotorError, queue_size=None) + pub.publish(me) except dynamixel_io.NonfatalErrorCodeError, nfece: self.error_counts['non_fatal'] += 1 rospy.logdebug(nfece) diff --git a/dynamixel_msgs/CMakeLists.txt b/dynamixel_msgs/CMakeLists.txt index 6f68606..289b7ac 100644 --- a/dynamixel_msgs/CMakeLists.txt +++ b/dynamixel_msgs/CMakeLists.txt @@ -7,6 +7,7 @@ find_package(catkin REQUIRED add_message_files( FILES JointState.msg + MotorError.msg MotorState.msg MotorStateList.msg ) diff --git a/dynamixel_msgs/msg/MotorError.msg b/dynamixel_msgs/msg/MotorError.msg new file mode 100644 index 0000000..98b6b18 --- /dev/null +++ b/dynamixel_msgs/msg/MotorError.msg @@ -0,0 +1,3 @@ +string error_type # error type +string error_message # error message +string extra_info # extra info such a joint number, port... \ No newline at end of file From 731443bc2bdb6af85d823d3c6f0224dbba3c81bd Mon Sep 17 00:00:00 2001 From: Victor Gonzalez Pacheco Date: Tue, 14 Oct 2014 22:45:11 +0200 Subject: [PATCH 2/5] PEP 8 --- .../src/dynamixel_driver/dynamixel_io.py | 325 +++++++++++------- .../dynamixel_serial_proxy.py | 255 ++++++++------ 2 files changed, 367 insertions(+), 213 deletions(-) diff --git a/dynamixel_driver/src/dynamixel_driver/dynamixel_io.py b/dynamixel_driver/src/dynamixel_driver/dynamixel_io.py index a3a5a55..f371661 100755 --- a/dynamixel_driver/src/dynamixel_driver/dynamixel_io.py +++ b/dynamixel_driver/src/dynamixel_driver/dynamixel_io.py @@ -52,11 +52,13 @@ exception = None + class DynamixelIO(object): - """ Provides low level IO with the Dynamixel servos through pyserial. Has the - ability to write instruction packets, request and read register value - packets, send and receive a response to a ping packet, and send a SYNC WRITE - multi-servo instruction packet. + + """ Provides low level IO with the Dynamixel servos through pyserial. + Has the ability to write instruction packets, request + and read register value packets, send and receive a response to a ping + packet, and send a SYNC WRITE multi-servo instruction packet. """ def __init__(self, port, baudrate, readback_echo=False): @@ -70,7 +72,7 @@ def __init__(self, port, baudrate, readback_echo=False): self.port_name = port self.readback_echo = readback_echo except SerialOpenError: - raise SerialOpenError(port, baudrate) + raise SerialOpenError(port, baudrate) def __del__(self): """ Destructor calls DynamixelIO.close """ @@ -97,22 +99,27 @@ def __read_response(self, servo_id): try: data.extend(self.ser.read(4)) - if not data[0:2] == ['\xff', '\xff']: raise Exception('Wrong packet prefix %s' % data[0:2]) + if not data[0:2] == ['\xff', '\xff']: + raise Exception('Wrong packet prefix %s' % data[0:2]) data.extend(self.ser.read(ord(data[3]))) - data = array('B', ''.join(data)).tolist() # [int(b2a_hex(byte), 16) for byte in data] + # [int(b2a_hex(byte), 16) for byte in data] + data = array('B', ''.join(data)).tolist() except Exception, e: - raise DroppedPacketError('Invalid response received from motor %d. %s' % (servo_id, e)) + raise DroppedPacketError( + 'Invalid response received from motor %d. %s' % (servo_id, e)) # verify checksum checksum = 255 - sum(data[2:-1]) % 256 - if not checksum == data[-1]: raise ChecksumError(servo_id, data, checksum) + if not checksum == data[-1]: + raise ChecksumError(servo_id, data, checksum) return data def read(self, servo_id, address, size): """ Read "size" bytes of data from servo with "servo_id" starting at the - register with "address". "address" is an integer between 0 and 57. It is - recommended to use the constants in module dynamixel_const for readability. + register with "address". "address" is an integer between 0 and 57. + It is recommended to use the constants in module dynamixel_const + for readability. To read the position from servo with id 1, the method should be called like: @@ -124,18 +131,21 @@ def read(self, servo_id, address, size): # directly from AX-12 manual: # Check Sum = ~ (ID + LENGTH + INSTRUCTION + PARAM_1 + ... + PARAM_N) # If the calculated value is > 255, the lower byte is the check sum. - checksum = 255 - ( (servo_id + length + DXL_READ_DATA + address + size) % 256 ) + checksum = 255 - \ + ((servo_id + length + DXL_READ_DATA + address + size) % 256) # packet: FF FF ID LENGTH INSTRUCTION PARAM_1 ... CHECKSUM - packet = [0xFF, 0xFF, servo_id, length, DXL_READ_DATA, address, size, checksum] - packetStr = array('B', packet).tostring() # same as: packetStr = ''.join([chr(byte) for byte in packet]) + packet = [0xFF, 0xFF, servo_id, length, + DXL_READ_DATA, address, size, checksum] + # same as: packetStr = ''.join([chr(byte) for byte in packet]) + packetStr = array('B', packet).tostring() with self.serial_mutex: self.__write_serial(packetStr) # wait for response packet from the motor timestamp = time.time() - time.sleep(0.0013)#0.00235) + time.sleep(0.0013) # 0.00235) # read response data = self.__read_response(servo_id) @@ -147,8 +157,9 @@ def write(self, servo_id, address, data): """ Write the values from the "data" list to the servo with "servo_id" starting with data[0] at "address", continuing through data[n-1] at "address" + (n-1), where n = len(data). "address" is an integer between - 0 and 49. It is recommended to use the constants in module dynamixel_const - for readability. "data" is a list/tuple of integers. + 0 and 49. It is recommended to use the constants in + module dynamixel_constfor readability. + "data" is a list/tuple of integers. To set servo with id 1 to position 276, the method should be called like: @@ -160,14 +171,16 @@ def write(self, servo_id, address, data): # directly from AX-12 manual: # Check Sum = ~ (ID + LENGTH + INSTRUCTION + PARAM_1 + ... + PARAM_N) # If the calculated value is > 255, the lower byte is the check sum. - checksum = 255 - ((servo_id + length + DXL_WRITE_DATA + address + sum(data)) % 256) + checksum = 255 - \ + ((servo_id + length + DXL_WRITE_DATA + address + sum(data)) % 256) # packet: FF FF ID LENGTH INSTRUCTION PARAM_1 ... CHECKSUM packet = [0xFF, 0xFF, servo_id, length, DXL_WRITE_DATA, address] packet.extend(data) packet.append(checksum) - packetStr = array('B', packet).tostring() # packetStr = ''.join([chr(byte) for byte in packet]) + # packetStr = ''.join([chr(byte) for byte in packet]) + packetStr = array('B', packet).tostring() with self.serial_mutex: self.__write_serial(packetStr) @@ -186,8 +199,9 @@ def sync_write(self, address, data): """ Use Broadcast message to send multiple servos instructions at the same time. No "status packet" will be returned from any servos. "address" is an integer between 0 and 49. It is recommended to use the - constants in module dynamixel_const for readability. "data" is a tuple of - tuples. Each tuple in "data" must contain the servo id followed by the + constants in module dynamixel_const for readability. + "data" is a tuple of tuples. + Each tuple in "data" must contain the servo id followed by the data that should be written from the starting address. The amount of data can be as long as needed. @@ -198,19 +212,22 @@ def sync_write(self, address, data): # Calculate length and sum of all data flattened = [value for servo in data for value in servo] - # Number of bytes following standard header (0xFF, 0xFF, id, length) plus data + # Number of bytes following standard header (0xFF, 0xFF, id, length) + # plus data length = 4 + len(flattened) - checksum = 255 - ((DXL_BROADCAST + length + \ - DXL_SYNC_WRITE + address + len(data[0][1:]) + \ - sum(flattened)) % 256) + checksum = 255 - ((DXL_BROADCAST + length + + DXL_SYNC_WRITE + address + len(data[0][1:]) + + sum(flattened)) % 256) # packet: FF FF ID LENGTH INSTRUCTION PARAM_1 ... CHECKSUM - packet = [0xFF, 0xFF, DXL_BROADCAST, length, DXL_SYNC_WRITE, address, len(data[0][1:])] + packet = [0xFF, 0xFF, DXL_BROADCAST, length, + DXL_SYNC_WRITE, address, len(data[0][1:])] packet.extend(flattened) packet.append(checksum) - packetStr = array('B', packet).tostring() # packetStr = ''.join([chr(byte) for byte in packet]) + # packetStr = ''.join([chr(byte) for byte in packet]) + packetStr = array('B', packet).tostring() with self.serial_mutex: self.__write_serial(packetStr) @@ -243,7 +260,7 @@ def ping(self, servo_id): try: response = self.__read_response(servo_id) response.append(timestamp) - except Exception, e: + except Exception as e: response = [] if response: @@ -254,7 +271,6 @@ def test_bit(self, number, offset): mask = 1 << offset return (number & mask) - ###################################################################### # These function modify EEPROM data which persists after power cycle # ###################################################################### @@ -266,7 +282,8 @@ def set_id(self, old_id, new_id): """ response = self.write(old_id, DXL_ID, [new_id]) if response: - self.exception_on_error(response[4], old_id, 'setting id to %d' % new_id) + self.exception_on_error( + response[4], old_id, 'setting id to %d' % new_id) return response def set_baud_rate(self, servo_id, baud_rate): @@ -275,7 +292,8 @@ def set_baud_rate(self, servo_id, baud_rate): """ response = self.write(servo_id, DXL_BAUD_RATE, [baud_rate]) if response: - self.exception_on_error(response[4], servo_id, 'setting baud rate to %d' % baud_rate) + self.exception_on_error( + response[4], servo_id, 'setting baud rate to %d' % baud_rate) return response def set_return_delay_time(self, servo_id, delay): @@ -286,7 +304,8 @@ def set_return_delay_time(self, servo_id, delay): """ response = self.write(servo_id, DXL_RETURN_DELAY_TIME, [delay]) if response: - self.exception_on_error(response[4], servo_id, 'setting return delay time to %d' % delay) + self.exception_on_error(response[4], servo_id, + 'setting return delay time to %d' % delay) return response def set_angle_limit_cw(self, servo_id, angle_cw): @@ -298,7 +317,8 @@ def set_angle_limit_cw(self, servo_id, angle_cw): response = self.write(servo_id, DXL_CW_ANGLE_LIMIT_L, (loVal, hiVal)) if response: - self.exception_on_error(response[4], servo_id, 'setting CW angle limits to %d' % angle_cw) + self.exception_on_error(response[4], servo_id, + 'setting CW angle limits to %d' % angle_cw) return response def set_angle_limit_ccw(self, servo_id, angle_ccw): @@ -310,7 +330,8 @@ def set_angle_limit_ccw(self, servo_id, angle_ccw): response = self.write(servo_id, DXL_CCW_ANGLE_LIMIT_L, (loVal, hiVal)) if response: - self.exception_on_error(response[4], servo_id, 'setting CCW angle limits to %d' % angle_ccw) + self.exception_on_error(response[4], servo_id, + 'setting CCW angle limits to %d' % angle_ccw) return response def set_angle_limits(self, servo_id, min_angle, max_angle): @@ -323,9 +344,11 @@ def set_angle_limits(self, servo_id, min_angle, max_angle): hiMaxVal = int(max_angle >> 8) # set 4 register values with low and high bytes for min and max angles - response = self.write(servo_id, DXL_CW_ANGLE_LIMIT_L, (loMinVal, hiMinVal, loMaxVal, hiMaxVal)) + response = self.write( + servo_id, DXL_CW_ANGLE_LIMIT_L, (loMinVal, hiMinVal, loMaxVal, hiMaxVal)) if response: - self.exception_on_error(response[4], servo_id, 'setting CW and CCW angle limits to %d and %d' %(min_angle, max_angle)) + self.exception_on_error(response[ + 4], servo_id, 'setting CW and CCW angle limits to %d and %d' % (min_angle, max_angle)) return response def set_drive_mode(self, servo_id, is_slave=False, is_reverse=False): @@ -336,7 +359,8 @@ def set_drive_mode(self, servo_id, is_slave=False, is_reverse=False): response = self.write(servo_id, DXL_DRIVE_MODE, [drive_mode]) if response: - self.exception_on_error(response[4], servo_id, 'setting drive mode to %d' % drive_mode) + self.exception_on_error( + response[4], servo_id, 'setting drive mode to %d' % drive_mode) return response def set_voltage_limit_min(self, servo_id, min_voltage): @@ -345,12 +369,14 @@ def set_voltage_limit_min(self, servo_id, min_voltage): NOTE: the absolute min is 5v """ - if min_voltage < 5: min_voltage = 5 + if min_voltage < 5: + min_voltage = 5 minVal = int(min_voltage * 10) response = self.write(servo_id, DXL_DOWN_LIMIT_VOLTAGE, [minVal]) if response: - self.exception_on_error(response[4], servo_id, 'setting minimum voltage level to %d' % min_voltage) + self.exception_on_error( + response[4], servo_id, 'setting minimum voltage level to %d' % min_voltage) return response def set_voltage_limit_max(self, servo_id, max_voltage): @@ -359,12 +385,14 @@ def set_voltage_limit_max(self, servo_id, max_voltage): NOTE: the absolute min is 25v """ - if max_voltage > 25: max_voltage = 25 + if max_voltage > 25: + max_voltage = 25 maxVal = int(max_voltage * 10) response = self.write(servo_id, DXL_UP_LIMIT_VOLTAGE, [maxVal]) if response: - self.exception_on_error(response[4], servo_id, 'setting maximum voltage level to %d' % max_voltage) + self.exception_on_error( + response[4], servo_id, 'setting maximum voltage level to %d' % max_voltage) return response def set_voltage_limits(self, servo_id, min_voltage, max_voltage): @@ -373,18 +401,21 @@ def set_voltage_limits(self, servo_id, min_voltage, max_voltage): NOTE: the absolute min is 5v and the absolute max is 25v """ - if min_voltage < 5: min_voltage = 5 - if max_voltage > 25: max_voltage = 25 + if min_voltage < 5: + min_voltage = 5 + if max_voltage > 25: + max_voltage = 25 minVal = int(min_voltage * 10) maxVal = int(max_voltage * 10) - response = self.write(servo_id, DXL_DOWN_LIMIT_VOLTAGE, (minVal, maxVal)) + response = self.write( + servo_id, DXL_DOWN_LIMIT_VOLTAGE, (minVal, maxVal)) if response: - self.exception_on_error(response[4], servo_id, 'setting min and max voltage levels to %d and %d' %(min_voltage, max_voltage)) + self.exception_on_error(response[4], servo_id, + 'setting min and max voltage levels to %d and %d' % (min_voltage, max_voltage)) return response - ############################################################### # These functions can send a single command to a single servo # ############################################################### @@ -397,7 +428,8 @@ def set_torque_enabled(self, servo_id, enabled): """ response = self.write(servo_id, DXL_TORQUE_ENABLE, [enabled]) if response: - self.exception_on_error(response[4], servo_id, '%sabling torque' % 'en' if enabled else 'dis') + self.exception_on_error( + response[4], servo_id, '%sabling torque' % 'en' if enabled else 'dis') return response def set_compliance_margin_cw(self, servo_id, margin): @@ -408,7 +440,8 @@ def set_compliance_margin_cw(self, servo_id, margin): """ response = self.write(servo_id, DXL_CW_COMPLIANCE_MARGIN, [margin]) if response: - self.exception_on_error(response[4], servo_id, 'setting CW compliance margin to %d' % margin) + self.exception_on_error( + response[4], servo_id, 'setting CW compliance margin to %d' % margin) return response def set_compliance_margin_ccw(self, servo_id, margin): @@ -419,7 +452,8 @@ def set_compliance_margin_ccw(self, servo_id, margin): """ response = self.write(servo_id, DXL_CCW_COMPLIANCE_MARGIN, [margin]) if response: - self.exception_on_error(response[4], servo_id, 'setting CCW compliance margin to %d' % margin) + self.exception_on_error( + response[4], servo_id, 'setting CCW compliance margin to %d' % margin) return response def set_compliance_margins(self, servo_id, margin_cw, margin_ccw): @@ -428,9 +462,11 @@ def set_compliance_margins(self, servo_id, margin_cw, margin_ccw): The range of the value is 0 to 255, and the unit is the same as Goal Position. The greater the value, the more difference occurs. """ - response = self.write(servo_id, DXL_CW_COMPLIANCE_MARGIN, (margin_cw, margin_ccw)) + response = self.write( + servo_id, DXL_CW_COMPLIANCE_MARGIN, (margin_cw, margin_ccw)) if response: - self.exception_on_error(response[4], servo_id, 'setting CW and CCW compliance margins to values %d and %d' %(margin_cw, margin_ccw)) + self.exception_on_error(response[ + 4], servo_id, 'setting CW and CCW compliance margins to values %d and %d' % (margin_cw, margin_ccw)) return response def set_compliance_slope_cw(self, servo_id, slope): @@ -440,7 +476,8 @@ def set_compliance_slope_cw(self, servo_id, slope): """ response = self.write(servo_id, DXL_CW_COMPLIANCE_SLOPE, [slope]) if response: - self.exception_on_error(response[4], servo_id, 'setting CW compliance slope to %d' % slope) + self.exception_on_error( + response[4], servo_id, 'setting CW compliance slope to %d' % slope) return response def set_compliance_slope_ccw(self, servo_id, slope): @@ -450,7 +487,8 @@ def set_compliance_slope_ccw(self, servo_id, slope): """ response = self.write(servo_id, DXL_CCW_COMPLIANCE_SLOPE, [slope]) if response: - self.exception_on_error(response[4], servo_id, 'setting CCW compliance slope to %d' % slope) + self.exception_on_error( + response[4], servo_id, 'setting CCW compliance slope to %d' % slope) return response def set_compliance_slopes(self, servo_id, slope_cw, slope_ccw): @@ -458,9 +496,11 @@ def set_compliance_slopes(self, servo_id, slope_cw, slope_ccw): Sets the level of Torque near the goal position in CW/CCW direction. Compliance Slope is set in 7 steps, the higher the value, the more flexibility is obtained. """ - response = self.write(servo_id, DXL_CW_COMPLIANCE_SLOPE, (slope_cw, slope_ccw)) + response = self.write( + servo_id, DXL_CW_COMPLIANCE_SLOPE, (slope_cw, slope_ccw)) if response: - self.exception_on_error(response[4], servo_id, 'setting CW and CCW compliance slopes to %d and %d' %(slope_cw, slope_ccw)) + self.exception_on_error(response[ + 4], servo_id, 'setting CW and CCW compliance slopes to %d and %d' % (slope_cw, slope_ccw)) return response def set_d_gain(self, servo_id, d_gain): @@ -470,7 +510,8 @@ def set_d_gain(self, servo_id, d_gain): """ response = self.write(servo_id, DXL_D_GAIN, [d_gain]) if response: - self.exception_on_error(response[4], servo_id, 'setting D gain value of PID controller to %d' % slope) + self.exception_on_error( + response[4], servo_id, 'setting D gain value of PID controller to %d' % slope) return response def set_i_gain(self, servo_id, i_gain): @@ -480,7 +521,8 @@ def set_i_gain(self, servo_id, i_gain): """ response = self.write(servo_id, DXL_I_GAIN, [i_gain]) if response: - self.exception_on_error(response[4], servo_id, 'setting I gain value of PID controller to %d' % slope) + self.exception_on_error( + response[4], servo_id, 'setting I gain value of PID controller to %d' % slope) return response def set_p_gain(self, servo_id, p_gain): @@ -490,7 +532,8 @@ def set_p_gain(self, servo_id, p_gain): """ response = self.write(servo_id, DXL_P_GAIN, [p_gain]) if response: - self.exception_on_error(response[4], servo_id, 'setting P gain value of PID controller to %d' % slope) + self.exception_on_error( + response[4], servo_id, 'setting P gain value of PID controller to %d' % slope) return response def set_punch(self, servo_id, punch): @@ -504,7 +547,8 @@ def set_punch(self, servo_id, punch): hiVal = int(punch >> 8) response = self.write(servo_id, DXL_PUNCH_L, (loVal, hiVal)) if response: - self.exception_on_error(response[4], servo_id, 'setting punch to %d' % punch) + self.exception_on_error( + response[4], servo_id, 'setting punch to %d' % punch) return response def set_acceleration(self, servo_id, acceleration): @@ -518,9 +562,11 @@ def set_acceleration(self, servo_id, acceleration): raise UnsupportedFeatureError(model, DXL_GOAL_ACCELERATION) if DXL_GOAL_ACCELERATION in DXL_MODEL_TO_PARAMS[model]['features']: - response = self.write(servo_id, DXL_GOAL_ACCELERATION, (acceleration, )) + response = self.write( + servo_id, DXL_GOAL_ACCELERATION, (acceleration, )) if response: - self.exception_on_error(response[4], servo_id, 'setting acceleration to %d' % acceleration) + self.exception_on_error( + response[4], servo_id, 'setting acceleration to %d' % acceleration) return response else: raise UnsupportedFeatureError(model, DXL_GOAL_ACCELERATION) @@ -535,7 +581,8 @@ def set_position(self, servo_id, position): response = self.write(servo_id, DXL_GOAL_POSITION_L, (loVal, hiVal)) if response: - self.exception_on_error(response[4], servo_id, 'setting goal position to %d' % position) + self.exception_on_error( + response[4], servo_id, 'setting goal position to %d' % position) return response def set_speed(self, servo_id, speed): @@ -554,7 +601,8 @@ def set_speed(self, servo_id, speed): # set two register values with low and high byte for the speed response = self.write(servo_id, DXL_GOAL_SPEED_L, (loVal, hiVal)) if response: - self.exception_on_error(response[4], servo_id, 'setting moving speed to %d' % speed) + self.exception_on_error( + response[4], servo_id, 'setting moving speed to %d' % speed) return response def set_torque_limit(self, servo_id, torque): @@ -569,7 +617,8 @@ def set_torque_limit(self, servo_id, torque): response = self.write(servo_id, DXL_TORQUE_LIMIT_L, (loVal, hiVal)) if response: - self.exception_on_error(response[4], servo_id, 'setting torque limit to %d' % torque) + self.exception_on_error( + response[4], servo_id, 'setting torque limit to %d' % torque) return response def set_goal_torque(self, servo_id, torque): @@ -583,19 +632,25 @@ def set_goal_torque(self, servo_id, torque): if not model in DXL_MODEL_TO_PARAMS: raise UnsupportedFeatureError(model, DXL_TORQUE_CONTROL_MODE) - valid_torque = torque is not None and torque >= -1023 and torque <= 1023 + valid_torque = torque is not None and torque >= - \ + 1023 and torque <= 1023 if torque is not None and torque < 0: torque = 1024 - torque if DXL_TORQUE_CONTROL_MODE in DXL_MODEL_TO_PARAMS[model]['features']: if valid_torque: - loVal = int(torque % 256); hiVal = int(torque >> 8); - response = self.write(servo_id, DXL_GOAL_TORQUE_L, (loVal, hiVal)) + loVal = int(torque % 256) + hiVal = int(torque >> 8) + response = self.write( + servo_id, DXL_GOAL_TORQUE_L, (loVal, hiVal)) if response: - self.exception_on_error(response[4], servo_id, 'setting goal torque to %d' % torque) - response = self.write(servo_id, DXL_TORQUE_CONTROL_MODE, (int(valid_torque), )) + self.exception_on_error( + response[4], servo_id, 'setting goal torque to %d' % torque) + response = self.write( + servo_id, DXL_TORQUE_CONTROL_MODE, (int(valid_torque), )) if response: - self.exception_on_error(response[4], servo_id, 'enabling torque mode') + self.exception_on_error( + response[4], servo_id, 'enabling torque mode') return response else: raise UnsupportedFeatureError(model, DXL_TORQUE_CONTROL_MODE) @@ -617,12 +672,13 @@ def set_position_and_speed(self, servo_id, position, speed): loPositionVal = int(position % 256) hiPositionVal = int(position >> 8) - response = self.write(servo_id, DXL_GOAL_POSITION_L, (loPositionVal, hiPositionVal, loSpeedVal, hiSpeedVal)) + response = self.write( + servo_id, DXL_GOAL_POSITION_L, (loPositionVal, hiPositionVal, loSpeedVal, hiSpeedVal)) if response: - self.exception_on_error(response[4], servo_id, 'setting goal position to %d and moving speed to %d' %(position, speed)) + self.exception_on_error(response[ + 4], servo_id, 'setting goal position to %d and moving speed to %d' % (position, speed)) return response - ################################################################# # These functions can send multiple commands to multiple servos # # These commands are used in ROS wrapper as they don't send a # @@ -696,11 +752,11 @@ def set_multi_punch(self, valueTuples): # prepare value tuples for call to syncwrite writeableVals = [] - for sid,punch in valueTuples: + for sid, punch in valueTuples: # split punch into 2 bytes loVal = int(punch % 256) hiVal = int(punch >> 8) - writeableVals.append( (sid, loVal, hiVal) ) + writeableVals.append((sid, loVal, hiVal)) # use sync write to broadcast multi servo message self.sync_write(DXL_PUNCH_L, writeableVals) @@ -720,7 +776,7 @@ def set_multi_position(self, valueTuples): # split position into 2 bytes loVal = int(position % 256) hiVal = int(position >> 8) - writeableVals.append( (sid, loVal, hiVal) ) + writeableVals.append((sid, loVal, hiVal)) # use sync write to broadcast multi servo message self.sync_write(DXL_GOAL_POSITION_L, writeableVals) @@ -746,7 +802,7 @@ def set_multi_speed(self, valueTuples): loVal = int((1023 - speed) % 256) hiVal = int((1023 - speed) >> 8) - writeableVals.append( (sid, loVal, hiVal) ) + writeableVals.append((sid, loVal, hiVal)) # use sync write to broadcast multi servo message self.sync_write(DXL_GOAL_SPEED_L, writeableVals) @@ -760,11 +816,11 @@ def set_multi_torque_limit(self, valueTuples): # prepare value tuples for call to syncwrite writeableVals = [] - for sid,torque in valueTuples: + for sid, torque in valueTuples: # split torque into 2 bytes loVal = int(torque % 256) hiVal = int(torque >> 8) - writeableVals.append( (sid, loVal, hiVal) ) + writeableVals.append((sid, loVal, hiVal)) # use sync write to broadcast multi servo message self.sync_write(DXL_TORQUE_LIMIT_L, writeableVals) @@ -794,12 +850,12 @@ def set_multi_position_and_speed(self, valueTuples): # split position into 2 bytes loPositionVal = int(position % 256) hiPositionVal = int(position >> 8) - writeableVals.append( (sid, loPositionVal, hiPositionVal, loSpeedVal, hiSpeedVal) ) + writeableVals.append( + (sid, loPositionVal, hiPositionVal, loSpeedVal, hiSpeedVal)) # use sync write to broadcast multi servo message self.sync_write(DXL_GOAL_POSITION_L, tuple(writeableVals)) - ################################# # Servo status access functions # ################################# @@ -808,43 +864,49 @@ def get_model_number(self, servo_id): """ Reads the servo's model number (e.g. 12 for AX-12+). """ response = self.read(servo_id, DXL_MODEL_NUMBER_L, 2) if response: - self.exception_on_error(response[4], servo_id, 'fetching model number') + self.exception_on_error( + response[4], servo_id, 'fetching model number') return response[5] + (response[6] << 8) def get_firmware_version(self, servo_id): """ Reads the servo's firmware version. """ response = self.read(servo_id, DXL_VERSION, 1) if response: - self.exception_on_error(response[4], servo_id, 'fetching firmware version') + self.exception_on_error( + response[4], servo_id, 'fetching firmware version') return response[5] def get_return_delay_time(self, servo_id): """ Reads the servo's return delay time. """ response = self.read(servo_id, DXL_RETURN_DELAY_TIME, 1) if response: - self.exception_on_error(response[4], servo_id, 'fetching return delay time') + self.exception_on_error( + response[4], servo_id, 'fetching return delay time') return response[5] def get_angle_limits(self, servo_id): """ Returns the min and max angle limits from the specified servo. """ - # read in 4 consecutive bytes starting with low value of clockwise angle limit + # read in 4 consecutive bytes starting with low value of clockwise + # angle limit response = self.read(servo_id, DXL_CW_ANGLE_LIMIT_L, 4) if response: - self.exception_on_error(response[4], servo_id, 'fetching CW/CCW angle limits') + self.exception_on_error( + response[4], servo_id, 'fetching CW/CCW angle limits') # extract data valus from the raw data cwLimit = response[5] + (response[6] << 8) ccwLimit = response[7] + (response[8] << 8) # return the data in a dictionary - return {'min':cwLimit, 'max':ccwLimit} + return {'min': cwLimit, 'max': ccwLimit} def get_drive_mode(self, servo_id): """ Reads the servo's drive mode. """ response = self.read(servo_id, DXL_DRIVE_MODE, 1) if response: - self.exception_on_error(response[4], servo_id, 'fetching drive mode') + self.exception_on_error( + response[4], servo_id, 'fetching drive mode') return response[5] def get_voltage_limits(self, servo_id): @@ -853,19 +915,21 @@ def get_voltage_limits(self, servo_id): """ response = self.read(servo_id, DXL_DOWN_LIMIT_VOLTAGE, 2) if response: - self.exception_on_error(response[4], servo_id, 'fetching voltage limits') + self.exception_on_error( + response[4], servo_id, 'fetching voltage limits') # extract data valus from the raw data min_voltage = response[5] / 10.0 max_voltage = response[6] / 10.0 # return the data in a dictionary - return {'min':min_voltage, 'max':max_voltage} + return {'min': min_voltage, 'max': max_voltage} def get_position(self, servo_id): """ Reads the servo's position value from its registers. """ response = self.read(servo_id, DXL_PRESENT_POSITION_L, 2) if response: - self.exception_on_error(response[4], servo_id, 'fetching present position') + self.exception_on_error( + response[4], servo_id, 'fetching present position') position = response[5] + (response[6] << 8) return position @@ -873,7 +937,8 @@ def get_speed(self, servo_id): """ Reads the servo's speed value from its registers. """ response = self.read(servo_id, DXL_PRESENT_SPEED_L, 2) if response: - self.exception_on_error(response[4], servo_id, 'fetching present speed') + self.exception_on_error( + response[4], servo_id, 'fetching present speed') speed = response[5] + (response[6] << 8) if speed > 1023: return 1023 - speed @@ -883,7 +948,8 @@ def get_voltage(self, servo_id): """ Reads the servo's voltage. """ response = self.read(servo_id, DXL_PRESENT_VOLTAGE, 1) if response: - self.exception_on_error(response[4], servo_id, 'fetching supplied voltage') + self.exception_on_error( + response[4], servo_id, 'fetching supplied voltage') return response[5] / 10.0 def get_current(self, servo_id): @@ -895,63 +961,69 @@ def get_current(self, servo_id): if DXL_CURRENT_L in DXL_MODEL_TO_PARAMS[model]['features']: response = self.read(servo_id, DXL_CURRENT_L, 2) if response: - self.exception_on_error(response[4], servo_id, 'fetching sensed current') + self.exception_on_error( + response[4], servo_id, 'fetching sensed current') current = response[5] + (response[6] << 8) return 0.0045 * (current - 2048) if DXL_SENSED_CURRENT_L in DXL_MODEL_TO_PARAMS[model]['features']: response = self.read(servo_id, DXL_SENSED_CURRENT_L, 2) if response: - self.exception_on_error(response[4], servo_id, 'fetching sensed current') + self.exception_on_error( + response[4], servo_id, 'fetching sensed current') current = response[5] + (response[6] << 8) return 0.01 * (current - 512) else: raise UnsupportedFeatureError(model, DXL_CURRENT_L) - def get_feedback(self, servo_id): """ Returns the id, goal, position, error, speed, load, voltage, temperature and moving values from the specified servo. """ - # read in 17 consecutive bytes starting with low value for goal position + # read in 17 consecutive bytes starting with low value for goal + # position response = self.read(servo_id, DXL_GOAL_POSITION_L, 17) if response: - self.exception_on_error(response[4], servo_id, 'fetching full servo status') + self.exception_on_error( + response[4], servo_id, 'fetching full servo status') if len(response) == 24: # extract data values from the raw data goal = response[5] + (response[6] << 8) position = response[11] + (response[12] << 8) error = position - goal - speed = response[13] + ( response[14] << 8) - if speed > 1023: speed = 1023 - speed + speed = response[13] + (response[14] << 8) + if speed > 1023: + speed = 1023 - speed load_raw = response[15] + (response[16] << 8) load_direction = 1 if self.test_bit(load_raw, 10) else 0 load = (load_raw & int('1111111111', 2)) / 1024.0 - if load_direction == 1: load = -load + if load_direction == 1: + load = -load voltage = response[17] / 10.0 temperature = response[18] moving = response[21] timestamp = response[-1] # return the data in a dictionary - return { 'timestamp': timestamp, - 'id': servo_id, - 'goal': goal, - 'position': position, - 'error': error, - 'speed': speed, - 'load': load, - 'voltage': voltage, - 'temperature': temperature, - 'moving': bool(moving) } + return {'timestamp': timestamp, + 'id': servo_id, + 'goal': goal, + 'position': position, + 'error': error, + 'speed': speed, + 'load': load, + 'voltage': voltage, + 'temperature': temperature, + 'moving': bool(moving)} def exception_on_error(self, error_code, servo_id, command_failed): global exception exception = None - ex_message = '[servo #%d on %s@%sbps]: %s failed' % (servo_id, self.ser.port, self.ser.baudrate, command_failed) + ex_message = '[servo #%d on %s@%sbps]: %s failed' % ( + servo_id, self.ser.port, self.ser.baudrate, command_failed) if not error_code & DXL_OVERHEATING_ERROR == 0: msg = 'Overheating Error ' + ex_message @@ -975,64 +1047,85 @@ def exception_on_error(self, error_code, servo_id, command_failed): msg = 'Instruction Error ' + ex_message exception = NonfatalErrorCodeError(msg, error_code) + class SerialOpenError(Exception): + def __init__(self, port, baud): Exception.__init__(self) - self.message = "Cannot open port '%s' at %d bps" %(port, baud) + self.message = "Cannot open port '%s' at %d bps" % (port, baud) self.port = port self.baud = baud + def __str__(self): return self.message + class ChecksumError(Exception): + def __init__(self, servo_id, response, checksum): Exception.__init__(self) self.message = 'Checksum received from motor %d does not match the expected one (%d != %d)' \ - %(servo_id, response[-1], checksum) + % (servo_id, response[-1], checksum) self.response_data = response self.expected_checksum = checksum + def __str__(self): return self.message + class FatalErrorCodeError(Exception): + def __init__(self, message, ec_const): Exception.__init__(self) self.message = message self.error_code = ec_const + def __str__(self): return self.message + class NonfatalErrorCodeError(Exception): + def __init__(self, message, ec_const): Exception.__init__(self) self.message = message self.error_code = ec_const + def __str__(self): return self.message + class ErrorCodeError(Exception): + def __init__(self, message, ec_const): Exception.__init__(self) self.message = message self.error_code = ec_const + def __str__(self): return self.message + class DroppedPacketError(Exception): + def __init__(self, message): Exception.__init__(self) self.message = message + def __str__(self): return self.message + class UnsupportedFeatureError(Exception): + def __init__(self, model_id, feature_id): Exception.__init__(self) if model_id in DXL_MODEL_TO_PARAMS: model = DXL_MODEL_TO_PARAMS[model_id]['name'] else: model = 'Unknown' - self.message = "Feature %d not supported by model %d (%s)" %(feature_id, model_id, model) + self.message = "Feature %d not supported by model %d (%s)" % ( + feature_id, model_id, model) + def __str__(self): return self.message - diff --git a/dynamixel_driver/src/dynamixel_driver/dynamixel_serial_proxy.py b/dynamixel_driver/src/dynamixel_driver/dynamixel_serial_proxy.py index 711672a..8af0946 100755 --- a/dynamixel_driver/src/dynamixel_driver/dynamixel_serial_proxy.py +++ b/dynamixel_driver/src/dynamixel_driver/dynamixel_serial_proxy.py @@ -64,7 +64,9 @@ from dynamixel_msgs.msg import MotorState from dynamixel_msgs.msg import MotorStateList + class SerialProxy(): + def __init__(self, port_name='/dev/ttyUSB0', port_namespace='ttyUSB0', @@ -86,26 +88,33 @@ def __init__(self, self.error_level_temp = error_level_temp self.warn_level_temp = warn_level_temp self.readback_echo = readback_echo - + self.actual_rate = update_rate self.error_counts = {'non_fatal': 0, 'checksum': 0, 'dropped': 0} self.current_state = MotorStateList() self.num_ping_retries = 5 - - self.motor_states_pub = rospy.Publisher('motor_states/%s' % self.port_namespace, MotorStateList, queue_size=None) - self.diagnostics_pub = rospy.Publisher('/diagnostics', DiagnosticArray, queue_size=None) + + self.motor_states_pub = \ + rospy.Publisher('motor_states/{}'.format(self.port_namespace), + MotorStateList, + queue_size=None) + self.diagnostics_pub = rospy.Publisher( + '/diagnostics', DiagnosticArray, queue_size=None) def connect(self): try: - self.dxl_io = dynamixel_io.DynamixelIO(self.port_name, self.baud_rate, self.readback_echo) + self.dxl_io = dynamixel_io.DynamixelIO( + self.port_name, self.baud_rate, self.readback_echo) self.__find_motors() except dynamixel_io.SerialOpenError, e: rospy.logfatal(e.message) sys.exit(1) - + self.running = True - if self.update_rate > 0: Thread(target=self.__update_motor_states).start() - if self.diagnostics_rate > 0: Thread(target=self.__publish_diagnostic_information).start() + if self.update_rate > 0: + Thread(target=self.__update_motor_states).start() + if self.diagnostics_rate > 0: + Thread(target=self.__publish_diagnostic_information).start() def disconnect(self): self.running = False @@ -118,66 +127,90 @@ def __fill_motor_parameters(self, motor_id, model_number): angles = self.dxl_io.get_angle_limits(motor_id) voltage = self.dxl_io.get_voltage(motor_id) voltages = self.dxl_io.get_voltage_limits(motor_id) - - rospy.set_param('dynamixel/%s/%d/model_number' %(self.port_namespace, motor_id), model_number) - rospy.set_param('dynamixel/%s/%d/model_name' %(self.port_namespace, motor_id), DXL_MODEL_TO_PARAMS[model_number]['name']) - rospy.set_param('dynamixel/%s/%d/min_angle' %(self.port_namespace, motor_id), angles['min']) - rospy.set_param('dynamixel/%s/%d/max_angle' %(self.port_namespace, motor_id), angles['max']) - + + rospy.set_param('dynamixel/%s/%d/model_number' % + (self.port_namespace, motor_id), model_number) + rospy.set_param('dynamixel/{}/{}/model_name'.format( + self.port_namespace, motor_id), + DXL_MODEL_TO_PARAMS[model_number]['name']) + rospy.set_param('dynamixel/%s/%d/min_angle' % + (self.port_namespace, motor_id), angles['min']) + rospy.set_param('dynamixel/%s/%d/max_angle' % + (self.port_namespace, motor_id), angles['max']) + torque_per_volt = DXL_MODEL_TO_PARAMS[model_number]['torque_per_volt'] - rospy.set_param('dynamixel/%s/%d/torque_per_volt' %(self.port_namespace, motor_id), torque_per_volt) - rospy.set_param('dynamixel/%s/%d/max_torque' %(self.port_namespace, motor_id), torque_per_volt * voltage) - - velocity_per_volt = DXL_MODEL_TO_PARAMS[model_number]['velocity_per_volt'] + rospy.set_param('dynamixel/%s/%d/torque_per_volt' % + (self.port_namespace, motor_id), torque_per_volt) + rospy.set_param('dynamixel/%s/%d/max_torque' % + (self.port_namespace, motor_id), torque_per_volt * voltage) + + velocity_per_volt = DXL_MODEL_TO_PARAMS[ + model_number]['velocity_per_volt'] rpm_per_tick = DXL_MODEL_TO_PARAMS[model_number]['rpm_per_tick'] - rospy.set_param('dynamixel/%s/%d/velocity_per_volt' %(self.port_namespace, motor_id), velocity_per_volt) - rospy.set_param('dynamixel/%s/%d/max_velocity' %(self.port_namespace, motor_id), velocity_per_volt * voltage) - rospy.set_param('dynamixel/%s/%d/radians_second_per_encoder_tick' %(self.port_namespace, motor_id), rpm_per_tick * RPM_TO_RADSEC) - - encoder_resolution = DXL_MODEL_TO_PARAMS[model_number]['encoder_resolution'] + rospy.set_param('dynamixel/%s/%d/velocity_per_volt' % + (self.port_namespace, motor_id), velocity_per_volt) + rospy.set_param('dynamixel/%s/%d/max_velocity' % + (self.port_namespace, motor_id), velocity_per_volt * voltage) + rospy.set_param('dynamixel/%s/%d/radians_second_per_encoder_tick' % + (self.port_namespace, motor_id), rpm_per_tick * RPM_TO_RADSEC) + + encoder_resolution = DXL_MODEL_TO_PARAMS[ + model_number]['encoder_resolution'] range_degrees = DXL_MODEL_TO_PARAMS[model_number]['range_degrees'] range_radians = math.radians(range_degrees) - rospy.set_param('dynamixel/%s/%d/encoder_resolution' %(self.port_namespace, motor_id), encoder_resolution) - rospy.set_param('dynamixel/%s/%d/range_degrees' %(self.port_namespace, motor_id), range_degrees) - rospy.set_param('dynamixel/%s/%d/range_radians' %(self.port_namespace, motor_id), range_radians) - rospy.set_param('dynamixel/%s/%d/encoder_ticks_per_degree' %(self.port_namespace, motor_id), encoder_resolution / range_degrees) - rospy.set_param('dynamixel/%s/%d/encoder_ticks_per_radian' %(self.port_namespace, motor_id), encoder_resolution / range_radians) - rospy.set_param('dynamixel/%s/%d/degrees_per_encoder_tick' %(self.port_namespace, motor_id), range_degrees / encoder_resolution) - rospy.set_param('dynamixel/%s/%d/radians_per_encoder_tick' %(self.port_namespace, motor_id), range_radians / encoder_resolution) - + rospy.set_param('dynamixel/%s/%d/encoder_resolution' % + (self.port_namespace, motor_id), encoder_resolution) + rospy.set_param('dynamixel/%s/%d/range_degrees' % (self.port_namespace, motor_id), + ) + rospy.set_param('dynamixel/%s/%d/range_radians' % + (self.port_namespace, motor_id), range_radians) + rospy.set_param('dynamixel/%s/%d/encoder_ticks_per_degree' % + (self.port_namespace, motor_id), encoder_resolution / range_degrees) + rospy.set_param('dynamixel/%s/%d/encoder_ticks_per_radian' % + (self.port_namespace, motor_id), encoder_resolution / range_radians) + rospy.set_param('dynamixel/%s/%d/degrees_per_encoder_tick' % + (self.port_namespace, motor_id), range_degrees / encoder_resolution) + rospy.set_param('dynamixel/%s/%d/radians_per_encoder_tick' % + (self.port_namespace, motor_id), range_radians / encoder_resolution) + # keep some parameters around for diagnostics self.motor_static_info[motor_id] = {} - self.motor_static_info[motor_id]['model'] = DXL_MODEL_TO_PARAMS[model_number]['name'] - self.motor_static_info[motor_id]['firmware'] = self.dxl_io.get_firmware_version(motor_id) - self.motor_static_info[motor_id]['delay'] = self.dxl_io.get_return_delay_time(motor_id) + self.motor_static_info[motor_id][ + 'model'] = DXL_MODEL_TO_PARAMS[model_number]['name'] + self.motor_static_info[motor_id][ + 'firmware'] = self.dxl_io.get_firmware_version(motor_id) + self.motor_static_info[motor_id][ + 'delay'] = self.dxl_io.get_return_delay_time(motor_id) self.motor_static_info[motor_id]['min_angle'] = angles['min'] self.motor_static_info[motor_id]['max_angle'] = angles['max'] self.motor_static_info[motor_id]['min_voltage'] = voltages['min'] self.motor_static_info[motor_id]['max_voltage'] = voltages['max'] def __find_motors(self): - rospy.loginfo('%s: Pinging motor IDs %d through %d...' % (self.port_namespace, self.min_motor_id, self.max_motor_id)) + rospy.loginfo('%s: Pinging motor IDs %d through %d...' % + (self.port_namespace, self.min_motor_id, self.max_motor_id)) self.motors = [] self.motor_static_info = {} - + for motor_id in range(self.min_motor_id, self.max_motor_id + 1): for trial in range(self.num_ping_retries): try: result = self.dxl_io.ping(motor_id) except Exception as ex: - rospy.logerr('Exception thrown while pinging motor %d - %s' % (motor_id, ex)) + rospy.logerr( + 'Exception thrown while pinging motor %d - %s' % (motor_id, ex)) continue - + if result: self.motors.append(motor_id) break - + if not self.motors: rospy.logfatal('%s: No motors found.' % self.port_namespace) sys.exit(1) - + counts = defaultdict(int) - + to_delete_if_error = [] for motor_id in self.motors: for trial in range(self.num_ping_retries): @@ -185,37 +218,42 @@ def __find_motors(self): model_number = self.dxl_io.get_model_number(motor_id) self.__fill_motor_parameters(motor_id, model_number) except Exception as ex: - rospy.logerr('Exception thrown while getting attributes for motor %d - %s' % (motor_id, ex)) - if trial == self.num_ping_retries - 1: to_delete_if_error.append(motor_id) + rospy.logerr( + 'Exception thrown while getting attributes for motor %d - %s' % (motor_id, ex)) + if trial == self.num_ping_retries - 1: + to_delete_if_error.append(motor_id) continue - + counts[model_number] += 1 break - + for motor_id in to_delete_if_error: self.motors.remove(motor_id) - - rospy.set_param('dynamixel/%s/connected_ids' % self.port_namespace, self.motors) - - status_str = '%s: Found %d motors - ' % (self.port_namespace, len(self.motors)) - for model_number,count in counts.items(): + + rospy.set_param('dynamixel/%s/connected_ids' % + self.port_namespace, self.motors) + + status_str = '%s: Found %d motors - ' % ( + self.port_namespace, len(self.motors)) + for model_number, count in counts.items(): if count: model_name = DXL_MODEL_TO_PARAMS[model_number]['name'] status_str += '%d %s [' % (count, model_name) - + for motor_id in self.motors: if self.motor_static_info[motor_id]['model'] == model_name: status_str += '%d, ' % motor_id - + status_str = status_str[:-2] + '], ' - + rospy.loginfo('%s, initialization complete.' % status_str[:-2]) def __update_motor_states(self): num_events = 50 - rates = deque([float(self.update_rate)]*num_events, maxlen=num_events) + rates = deque( + [float(self.update_rate)] * num_events, maxlen=num_events) last_time = rospy.Time.now() - + rate = rospy.Rate(self.update_rate) while not rospy.is_shutdown() and self.running: # get current state of all motors and publish to motor_states topic @@ -225,14 +263,16 @@ def __update_motor_states(self): state = self.dxl_io.get_feedback(motor_id) if state: motor_states.append(MotorState(**state)) - if dynamixel_io.exception: raise dynamixel_io.exception + if dynamixel_io.exception: + raise dynamixel_io.exception except dynamixel_io.FatalErrorCodeError, fece: rospy.logerr(fece) me = MotorError() me.error_type = "FatalErrorCodeError" me.error_message = fece.message me.extra_info = fece.message.split(' ')[3][1:] - pub = rospy.Publisher('dynamixel_motor_errors', MotorError, queue_size=None) + pub = rospy.Publisher( + 'dynamixel_motor_errors', MotorError, queue_size=None) pub.publish(me) except dynamixel_io.NonfatalErrorCodeError, nfece: self.error_counts['non_fatal'] += 1 @@ -247,75 +287,96 @@ def __update_motor_states(self): if ose.errno != errno.EAGAIN: rospy.logfatal(errno.errorcode[ose.errno]) rospy.signal_shutdown(errno.errorcode[ose.errno]) - + if motor_states: msl = MotorStateList() msl.motor_states = motor_states self.motor_states_pub.publish(msl) - + self.current_state = msl - + # calculate actual update rate current_time = rospy.Time.now() rates.append(1.0 / (current_time - last_time).to_sec()) - self.actual_rate = round(sum(rates)/num_events, 2) + self.actual_rate = round(sum(rates) / num_events, 2) last_time = current_time - + rate.sleep() def __publish_diagnostic_information(self): diag_msg = DiagnosticArray() - + rate = rospy.Rate(self.diagnostics_rate) while not rospy.is_shutdown() and self.running: diag_msg.status = [] diag_msg.header.stamp = rospy.Time.now() - + status = DiagnosticStatus() - + status.name = 'Dynamixel Serial Bus (%s)' % self.port_namespace status.hardware_id = 'Dynamixel Serial Bus on port %s' % self.port_name status.values.append(KeyValue('Baud Rate', str(self.baud_rate))) - status.values.append(KeyValue('Min Motor ID', str(self.min_motor_id))) - status.values.append(KeyValue('Max Motor ID', str(self.max_motor_id))) - status.values.append(KeyValue('Desired Update Rate', str(self.update_rate))) - status.values.append(KeyValue('Actual Update Rate', str(self.actual_rate))) - status.values.append(KeyValue('# Non Fatal Errors', str(self.error_counts['non_fatal']))) - status.values.append(KeyValue('# Checksum Errors', str(self.error_counts['checksum']))) - status.values.append(KeyValue('# Dropped Packet Errors', str(self.error_counts['dropped']))) + status.values.append( + KeyValue('Min Motor ID', str(self.min_motor_id))) + status.values.append( + KeyValue('Max Motor ID', str(self.max_motor_id))) + status.values.append( + KeyValue('Desired Update Rate', str(self.update_rate))) + status.values.append( + KeyValue('Actual Update Rate', str(self.actual_rate))) + status.values.append( + KeyValue('# Non Fatal Errors', str(self.error_counts['non_fatal']))) + status.values.append( + KeyValue('# Checksum Errors', str(self.error_counts['checksum']))) + status.values.append( + KeyValue('# Dropped Packet Errors', str(self.error_counts['dropped']))) status.level = DiagnosticStatus.OK status.message = 'OK' - + if self.actual_rate - self.update_rate < -5: status.level = DiagnosticStatus.WARN status.message = 'Actual update rate is lower than desired' - + diag_msg.status.append(status) - + for motor_state in self.current_state.motor_states: mid = motor_state.id - + status = DiagnosticStatus() - - status.name = 'Robotis Dynamixel Motor %d on port %s' % (mid, self.port_namespace) - status.hardware_id = 'DXL-%d@%s' % (motor_state.id, self.port_namespace) - status.values.append(KeyValue('Model Name', str(self.motor_static_info[mid]['model']))) - status.values.append(KeyValue('Firmware Version', str(self.motor_static_info[mid]['firmware']))) - status.values.append(KeyValue('Return Delay Time', str(self.motor_static_info[mid]['delay']))) - status.values.append(KeyValue('Minimum Voltage', str(self.motor_static_info[mid]['min_voltage']))) - status.values.append(KeyValue('Maximum Voltage', str(self.motor_static_info[mid]['max_voltage']))) - status.values.append(KeyValue('Minimum Position (CW)', str(self.motor_static_info[mid]['min_angle']))) - status.values.append(KeyValue('Maximum Position (CCW)', str(self.motor_static_info[mid]['max_angle']))) - + + status.name = 'Robotis Dynamixel Motor %d on port %s' % ( + mid, self.port_namespace) + status.hardware_id = 'DXL-%d@%s' % ( + motor_state.id, self.port_namespace) + status.values.append( + KeyValue('Model Name', str(self.motor_static_info[mid]['model']))) + status.values.append( + KeyValue('Firmware Version', str(self.motor_static_info[mid]['firmware']))) + status.values.append( + KeyValue('Return Delay Time', str(self.motor_static_info[mid]['delay']))) + status.values.append( + KeyValue('Minimum Voltage', str(self.motor_static_info[mid]['min_voltage']))) + status.values.append( + KeyValue('Maximum Voltage', str(self.motor_static_info[mid]['max_voltage']))) + status.values.append( + KeyValue('Minimum Position (CW)', str(self.motor_static_info[mid]['min_angle']))) + status.values.append(KeyValue( + 'Maximum Position (CCW)', str(self.motor_static_info[mid]['max_angle']))) + status.values.append(KeyValue('Goal', str(motor_state.goal))) - status.values.append(KeyValue('Position', str(motor_state.position))) + status.values.append( + KeyValue('Position', str(motor_state.position))) status.values.append(KeyValue('Error', str(motor_state.error))) - status.values.append(KeyValue('Velocity', str(motor_state.speed))) + status.values.append( + KeyValue('Velocity', str(motor_state.speed))) status.values.append(KeyValue('Load', str(motor_state.load))) - status.values.append(KeyValue('Voltage', str(motor_state.voltage))) - status.values.append(KeyValue('Temperature', str(motor_state.temperature))) - status.values.append(KeyValue('Moving', str(motor_state.moving))) - + status.values.append( + KeyValue('Voltage', str(motor_state.voltage))) + status.values.append( + KeyValue('Temperature', str(motor_state.temperature))) + status.values.append( + KeyValue('Moving', str(motor_state.moving))) + if motor_state.temperature >= self.error_level_temp: status.level = DiagnosticStatus.ERROR status.message = 'OVERHEATING' @@ -325,9 +386,9 @@ def __publish_diagnostic_information(self): else: status.level = DiagnosticStatus.OK status.message = 'OK' - + diag_msg.status.append(status) - + self.diagnostics_pub.publish(diag_msg) rate.sleep() @@ -337,5 +398,5 @@ def __publish_diagnostic_information(self): serial_proxy.connect() rospy.spin() serial_proxy.disconnect() - except rospy.ROSInterruptException: pass - + except rospy.ROSInterruptException: + pass From df2a580ce5091c07f965866b11a9b8c01c9d6172 Mon Sep 17 00:00:00 2001 From: Victor Gonzalez Pacheco Date: Tue, 14 Oct 2014 22:49:38 +0200 Subject: [PATCH 3/5] exceptions use the 'as' clause instead a comma --- .../dynamixel_driver/dynamixel_serial_proxy.py | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/dynamixel_driver/src/dynamixel_driver/dynamixel_serial_proxy.py b/dynamixel_driver/src/dynamixel_driver/dynamixel_serial_proxy.py index 8af0946..4539e5c 100755 --- a/dynamixel_driver/src/dynamixel_driver/dynamixel_serial_proxy.py +++ b/dynamixel_driver/src/dynamixel_driver/dynamixel_serial_proxy.py @@ -106,7 +106,7 @@ def connect(self): self.dxl_io = dynamixel_io.DynamixelIO( self.port_name, self.baud_rate, self.readback_echo) self.__find_motors() - except dynamixel_io.SerialOpenError, e: + except dynamixel_io.SerialOpenError as e: rospy.logfatal(e.message) sys.exit(1) @@ -130,9 +130,9 @@ def __fill_motor_parameters(self, motor_id, model_number): rospy.set_param('dynamixel/%s/%d/model_number' % (self.port_namespace, motor_id), model_number) - rospy.set_param('dynamixel/{}/{}/model_name'.format( - self.port_namespace, motor_id), - DXL_MODEL_TO_PARAMS[model_number]['name']) + rospy.set_param('dynamixel/{}/{}/model_name' + .format(self.port_namespace, motor_id), + DXL_MODEL_TO_PARAMS[model_number]['name']) rospy.set_param('dynamixel/%s/%d/min_angle' % (self.port_namespace, motor_id), angles['min']) rospy.set_param('dynamixel/%s/%d/max_angle' % @@ -265,7 +265,7 @@ def __update_motor_states(self): motor_states.append(MotorState(**state)) if dynamixel_io.exception: raise dynamixel_io.exception - except dynamixel_io.FatalErrorCodeError, fece: + except dynamixel_io.FatalErrorCodeError as fece: rospy.logerr(fece) me = MotorError() me.error_type = "FatalErrorCodeError" @@ -274,16 +274,16 @@ def __update_motor_states(self): pub = rospy.Publisher( 'dynamixel_motor_errors', MotorError, queue_size=None) pub.publish(me) - except dynamixel_io.NonfatalErrorCodeError, nfece: + except dynamixel_io.NonfatalErrorCodeError as nfece: self.error_counts['non_fatal'] += 1 rospy.logdebug(nfece) - except dynamixel_io.ChecksumError, cse: + except dynamixel_io.ChecksumError as cse: self.error_counts['checksum'] += 1 rospy.logdebug(cse) - except dynamixel_io.DroppedPacketError, dpe: + except dynamixel_io.DroppedPacketError as dpe: self.error_counts['dropped'] += 1 rospy.logdebug(dpe.message) - except OSError, ose: + except OSError as ose: if ose.errno != errno.EAGAIN: rospy.logfatal(errno.errorcode[ose.errno]) rospy.signal_shutdown(errno.errorcode[ose.errno]) From ac246212d12c5fa4185b47689699a30b7994959d Mon Sep 17 00:00:00 2001 From: Victor Gonzalez Pacheco Date: Wed, 15 Oct 2014 10:47:43 +0200 Subject: [PATCH 4/5] Removing duplicate code in parameter setting function --- .../dynamixel_serial_proxy.py | 68 ++++++++----------- 1 file changed, 30 insertions(+), 38 deletions(-) diff --git a/dynamixel_driver/src/dynamixel_driver/dynamixel_serial_proxy.py b/dynamixel_driver/src/dynamixel_driver/dynamixel_serial_proxy.py index 4539e5c..842bc35 100755 --- a/dynamixel_driver/src/dynamixel_driver/dynamixel_serial_proxy.py +++ b/dynamixel_driver/src/dynamixel_driver/dynamixel_serial_proxy.py @@ -128,50 +128,41 @@ def __fill_motor_parameters(self, motor_id, model_number): voltage = self.dxl_io.get_voltage(motor_id) voltages = self.dxl_io.get_voltage_limits(motor_id) - rospy.set_param('dynamixel/%s/%d/model_number' % - (self.port_namespace, motor_id), model_number) - rospy.set_param('dynamixel/{}/{}/model_name' - .format(self.port_namespace, motor_id), + ns = 'dynamixel/{}/{}/'.format(self.port_namespace, motor_id) + + rospy.set_param(ns + 'model_number', model_number) + rospy.set_param(ns + 'model_name', DXL_MODEL_TO_PARAMS[model_number]['name']) - rospy.set_param('dynamixel/%s/%d/min_angle' % - (self.port_namespace, motor_id), angles['min']) - rospy.set_param('dynamixel/%s/%d/max_angle' % - (self.port_namespace, motor_id), angles['max']) + rospy.set_param(ns + 'min_angle', angles['min']) + rospy.set_param(ns + 'max_angle', angles['max']) torque_per_volt = DXL_MODEL_TO_PARAMS[model_number]['torque_per_volt'] - rospy.set_param('dynamixel/%s/%d/torque_per_volt' % - (self.port_namespace, motor_id), torque_per_volt) - rospy.set_param('dynamixel/%s/%d/max_torque' % - (self.port_namespace, motor_id), torque_per_volt * voltage) + rospy.set_param(ns + 'torque_per_volt', torque_per_volt) + rospy.set_param(ns + 'max_torque', torque_per_volt * voltage) - velocity_per_volt = DXL_MODEL_TO_PARAMS[ - model_number]['velocity_per_volt'] + velocity_per_volt = \ + DXL_MODEL_TO_PARAMS[model_number]['velocity_per_volt'] rpm_per_tick = DXL_MODEL_TO_PARAMS[model_number]['rpm_per_tick'] - rospy.set_param('dynamixel/%s/%d/velocity_per_volt' % - (self.port_namespace, motor_id), velocity_per_volt) - rospy.set_param('dynamixel/%s/%d/max_velocity' % - (self.port_namespace, motor_id), velocity_per_volt * voltage) - rospy.set_param('dynamixel/%s/%d/radians_second_per_encoder_tick' % - (self.port_namespace, motor_id), rpm_per_tick * RPM_TO_RADSEC) - - encoder_resolution = DXL_MODEL_TO_PARAMS[ - model_number]['encoder_resolution'] + rospy.set_param(ns + 'velocity_per_volt', velocity_per_volt) + rospy.set_param(ns + 'max_velocity', velocity_per_volt * voltage) + rospy.set_param(ns + 'radians_second_per_encoder_tick', + rpm_per_tick * RPM_TO_RADSEC) + + encoder_resolution = \ + DXL_MODEL_TO_PARAMS[model_number]['encoder_resolution'] range_degrees = DXL_MODEL_TO_PARAMS[model_number]['range_degrees'] range_radians = math.radians(range_degrees) - rospy.set_param('dynamixel/%s/%d/encoder_resolution' % - (self.port_namespace, motor_id), encoder_resolution) - rospy.set_param('dynamixel/%s/%d/range_degrees' % (self.port_namespace, motor_id), - ) - rospy.set_param('dynamixel/%s/%d/range_radians' % - (self.port_namespace, motor_id), range_radians) - rospy.set_param('dynamixel/%s/%d/encoder_ticks_per_degree' % - (self.port_namespace, motor_id), encoder_resolution / range_degrees) - rospy.set_param('dynamixel/%s/%d/encoder_ticks_per_radian' % - (self.port_namespace, motor_id), encoder_resolution / range_radians) - rospy.set_param('dynamixel/%s/%d/degrees_per_encoder_tick' % - (self.port_namespace, motor_id), range_degrees / encoder_resolution) - rospy.set_param('dynamixel/%s/%d/radians_per_encoder_tick' % - (self.port_namespace, motor_id), range_radians / encoder_resolution) + rospy.set_param(ns + 'encoder_resolution', encoder_resolution) + rospy.set_param(ns + 'range_degrees', range_degrees) + rospy.set_param(ns + 'range_radians', range_radians) + rospy.set_param( + ns + 'encoder_ticks_per_degree', encoder_resolution / range_degrees) + rospy.set_param( + ns + 'encoder_ticks_per_radian', encoder_resolution / range_radians) + rospy.set_param( + ns + 'degrees_per_encoder_tick', range_degrees / encoder_resolution) + rospy.set_param( + ns + 'radians_per_encoder_tick', range_radians / encoder_resolution) # keep some parameters around for diagnostics self.motor_static_info[motor_id] = {} @@ -188,7 +179,8 @@ def __fill_motor_parameters(self, motor_id, model_number): def __find_motors(self): rospy.loginfo('%s: Pinging motor IDs %d through %d...' % - (self.port_namespace, self.min_motor_id, self.max_motor_id)) + (self.port_namespace, self.min_motor_id, + self.max_motor_id)) self.motors = [] self.motor_static_info = {} From 0039b7738bb3b422c13a1f44bae8a8a5b6da71f4 Mon Sep 17 00:00:00 2001 From: nnarayann Date: Wed, 15 Oct 2014 12:13:29 +0200 Subject: [PATCH 5/5] Moving publisher to the __init__ method. Removing Indigo warnings --- .../src/dynamixel_driver/dynamixel_serial_proxy.py | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/dynamixel_driver/src/dynamixel_driver/dynamixel_serial_proxy.py b/dynamixel_driver/src/dynamixel_driver/dynamixel_serial_proxy.py index 842bc35..fad45d4 100755 --- a/dynamixel_driver/src/dynamixel_driver/dynamixel_serial_proxy.py +++ b/dynamixel_driver/src/dynamixel_driver/dynamixel_serial_proxy.py @@ -97,9 +97,12 @@ def __init__(self, self.motor_states_pub = \ rospy.Publisher('motor_states/{}'.format(self.port_namespace), MotorStateList, - queue_size=None) + queue_size=3) self.diagnostics_pub = rospy.Publisher( - '/diagnostics', DiagnosticArray, queue_size=None) + '/diagnostics', DiagnosticArray, queue_size=3) + + self.error_pub = rospy.Publisher( + 'dynamixel_motor_errors', MotorError, queue_size=3) def connect(self): try: @@ -263,9 +266,7 @@ def __update_motor_states(self): me.error_type = "FatalErrorCodeError" me.error_message = fece.message me.extra_info = fece.message.split(' ')[3][1:] - pub = rospy.Publisher( - 'dynamixel_motor_errors', MotorError, queue_size=None) - pub.publish(me) + self.error_pub.publish(me) except dynamixel_io.NonfatalErrorCodeError as nfece: self.error_counts['non_fatal'] += 1 rospy.logdebug(nfece)