Added code for deserializing the ur5 binary format

ur5_driver/deserialize.py

+import struct
+
+class PackageType(object):
+    ROBOT_MODE_DATA = 0
+    JOINT_DATA = 1
+    TOOL_DATA = 2
+    MASTERBOARD_DATA = 3
+    CARTESIAN_INFO = 4
+    KINEMATICS_INFO = 5
+    CONFIGURATION_DATA = 6
+
+class RobotMode(object):
+    RUNNING = 0
+    FREEDRIVE = 1
+    READY = 2
+    INITIALIZING = 3
+    SECURITY_STOPPED = 4
+    EMERGENCY_STOPPED = 5
+    FATAL_ERROR = 6
+    NO_POWER = 7
+    NOT_CONNECTED = 8
+    SHUTDOWN = 9
+    SAFEGUARD_STOP = 10
+
+class JointMode(object):
+    PART_D_CALIBRATION = 237
+    BACKDRIVE = 238
+    POWER_OFF = 239
+    EMERGENCY_STOPPED = 240
+    CALVAL_INITIALIZATION = 241
+    ERROR = 242
+    FREEDRIVE = 243
+    SIMULATED = 244
+    NOT_RESPONDING = 245
+    MOTOR_INITIALISATION = 246
+    ADC_CALIBRATION = 247
+    DEAD_COMMUTATION = 248
+    BOOTLOADER = 249
+    CALIBRATION = 250
+    STOPPED = 251
+    FAULT = 252
+    RUNNING = 253
+    INITIALIZATION = 254
+    IDLE = 255
+
+class ToolMode(object):
+    BOOTLOADER = 249
+    RUNNING = 253
+    IDLE = 255
+    
+class MasterSafetyState(object):
+    UNDEFINED = 0
+    BOOTLOADER = 1
+    FAULT = 2
+    BOOTING = 3
+    INITIALIZING = 4
+    ROBOT_EMERGENCY_STOP = 5
+    EXTERNAL_EMERGENCY_STOP = 6
+    SAFEGUARD_STOP = 7
+    OK = 8
+
+class MasterOnOffState(object):
+    OFF = 0
+    TURNING_ON = 1
+    ON = 2
+    TURNING_OFF = 3
+
+class RobotModeData(object):
+    __slots__ = ['timestamp', 'robot_connected', 'real_robot_enabled',
+                 'power_on_robot', 'emergency_stopped',
+                 'security_stopped', 'program_running', 'program_paused',
+                 'robot_mode', 'speed_fraction']
+    @staticmethod
+    def unpack(buf):
+        rmd = RobotModeData()
+        (_, _,
+         rmd.timestamp, rmd.robot_connected, rmd.real_robot_enabled,
+         rmd.power_on_robot, rmd.emergency_stopped, rmd.security_stopped,
+         rmd.program_running, rmd.program_paused, rmd.robot_mode,
+         rmd.speed_fraction) = struct.unpack_from("!IBQ???????Bd", buf)
+        return rmd
+    
+class JointData(object):
+    __slots__ = ['q_actual', 'q_target', 'qd_actual',
+                 'I_actual', 'V_actual', 'T_motor', 'T_micro', 'joint_mode']
+    @staticmethod
+    def unpack(buf):
+        all_joints = []
+        offset = 5
+        for i in range(6):
+            jd = JointData()
+            (jd.q_actual, jd.q_target, jd.qd_actual, jd.I_actual, jd.V_actual,
+             jd.T_motor, jd.T_micro, 
+             jd.joint_mode) = struct.unpack_from("!dddffffB", buf, offset)
+            offset += 41
+            all_joints.append(jd)
+        return all_joints
+
+class ToolData(object):
+    __slots__ = ['analog_input_range2', 'analog_input_range3',
+                 'analog_input2', 'analog_input3',
+                 'tool_voltage_48V', 'tool_output_voltage', 'tool_current',
+                 'tool_temperature', 'tool_mode']
+    @staticmethod
+    def unpack(buf):
+        td = ToolData()
+        (_, _,
+         td.analog_input_range2, td.analog_input_range3,
+         td.analog_input2, td.analog_input3,
+         td.tool_voltage_48V, td.tool_output_voltage, td.tool_current,
+         td.tool_temperature, td.tool_mode) = struct.unpack_from("!IBbbddfBffB", buf)
+        return td
+
+class MasterboardData(object):
+    __slots__ = ['digital_input_bits', 'digital_output_bits',
+                 'analog_input_range0', 'analog_input_range1',
+                 'analog_input0', 'analog_input1',
+                 'analog_output_domain0', 'analog_output_domain1',
+                 'analog_output0', 'analog_output1',
+                 'masterboard_temperature',
+                 'robot_voltage_48V', 'robot_current',
+                 'master_io_current', 'master_safety_state',
+                 'master_onoff_state']
+    @staticmethod
+    def unpack(buf):
+        md = MasterboardData()
+        (_, _,
+         md.digital_input_bits, md.digital_output_bits,
+         md.analog_input_range0, md.analog_input_range1,
+         md.analog_input0, md.analog_input1,
+         md.analog_output_domain0, md.analog_output_domain1,
+         md.analog_output0, md.analog_output1,
+         md.masterboard_temperature,
+         md.robot_voltage_48V, md.robot_current,
+         md.master_io_current, md.master_safety_state,
+         md.master_onoff_state) = struct.unpack_from("!IBhhbbddbbddffffBB", buf)
+        return md
+
+class CartesianInfo(object):
+    __slots__ = ['x', 'y', 'z', 'rx', 'ry', 'rz']
+    @staticmethod
+    def unpack(buf):
+        ci = CartesianInfo()
+        (_, _,
+         ci.x, ci.y, ci.z, ci.rx, ci.ry, ci.rz) = struct.unpack_from("!IB6d", buf)
+        return ci
+
+class KinematicsInfo(object):
+    @staticmethod
+    def unpack(buf):
+        return KinematicsInfo()
+
+class JointLimitData(object):
+    __slots__ = ['min_limit', 'max_limit', 'max_speed', 'max_acceleration']
+
+class ConfigurationData(object):
+    __slots__ = ['joint_limit_data',
+                 'v_joint_default', 'a_joint_default', 
+                 'v_tool_default', 'a_tool_default', 'eq_radius',
+                 'dh_a', 'dh_d', 'dh_alpha', 'dh_theta',
+                 'masterboard_version', 'controller_box_type',
+                 'robot_type', 'robot_subtype']
+    @staticmethod
+    def unpack(buf):
+        cd = ConfigurationData()
+        cd.joint_limit_data = []
+        for i in range(6):
+            jld = JointLimitData()
+            (jld.min_limit, jld.max_limit) = struct.unpack_from("!dd", buf, 5+16*i)
+            (jld.max_speed, jld.max_acceleration) = struct.unpack_from("!dd", buf, 5+16*6+16*i)
+            cd.joint_limit_data.append(jld)
+        (cd.v_joint_default, cd.a_joint_default, cd.v_tool_default, cd.a_tool_default,
+         cd.eq_radius) = struct.unpack_from("!ddddd", buf, 5+32*6)
+        (cd.masterboard_version, cd.controller_box_type, cd.robot_type,
+         cd.robot_subtype) = struct.unpack_from("!iiii", buf, 5+32*6+5*8+6*32)
+        return cd
+
+class RobotState(object):
+    __slots__ = ['robot_mode_data', 'joint_data', 'tool_data',
+                 'masterboard_data', 'cartesian_info',
+                 'kinematics_info', 'configuration_data']
+
+    def __init__(self):
+        pass
+
+    @staticmethod
+    def unpack(buf):
+        length, mtype = struct.unpack_from("!IB", buf)
+        assert length == len(buf)
+        assert mtype == 16
+
+        rs = RobotState()
+        offset = 5
+        while offset < len(buf):
+            length, ptype = struct.unpack_from("!IB", buf, offset)
+            assert offset + length <= len(buf)
+            package_buf = buffer(buf, offset, length)
+            offset += length
+
+            if ptype == PackageType.ROBOT_MODE_DATA:
+                rs.robot_mode_data = RobotModeData.unpack(package_buf)
+            elif ptype == PackageType.JOINT_DATA:
+                rs.joint_data = JointData.unpack(package_buf)
+            elif ptype == PackageType.TOOL_DATA:
+                rs.tool_data = ToolData.unpack(package_buf)
+            elif ptype == PackageType.MASTERBOARD_DATA:
+                rs.masterboard_data = MasterboardData.unpack(package_buf)
+            elif ptype == PackageType.CARTESIAN_INFO:
+                rs.cartesian_info = CartesianInfo.unpack(package_buf)
+            elif ptype == PackageType.KINEMATICS_INFO:
+                rs.kinematics_info = KinematicsInfo.unpack(package_buf)
+            elif ptype == PackageType.CONFIGURATION_DATA:
+                rs.configuration_data = ConfigurationData.unpack(package_buf)
+            else:
+                raise Exception("Unknown package type: %i" % ptype)
+        return rs
+
+def pstate(o, indent=''):
+    for s in o.__slots__:
+        child = getattr(o, s, None)
+        if child is None:
+            print "%s%s: None" % (indent, s)
+        elif hasattr(child, '__slots__'):
+            print "%s%s:" % (indent, s)
+            pstate(child, indent + '    ')
+        elif hasattr(child, '__iter__'):
+            print "%s%s:" % (indent, s)
+            for i, c in enumerate(child):
+                print "%s  [%i]:" % (indent, i)
+                pstate(c, indent + '    ')
+        else:
+            print "%s%s: %s" % (indent, s, child)