deserialize.py
Go to the documentation of this file.
00001 import struct
00002 
00003 class PackageType(object):
00004     ROBOT_MODE_DATA = 0
00005     JOINT_DATA = 1
00006     TOOL_DATA = 2
00007     MASTERBOARD_DATA = 3
00008     CARTESIAN_INFO = 4
00009     KINEMATICS_INFO = 5
00010     CONFIGURATION_DATA = 6
00011 
00012 class RobotMode(object):
00013     RUNNING = 0
00014     FREEDRIVE = 1
00015     READY = 2
00016     INITIALIZING = 3
00017     SECURITY_STOPPED = 4
00018     EMERGENCY_STOPPED = 5
00019     FATAL_ERROR = 6
00020     NO_POWER = 7
00021     NOT_CONNECTED = 8
00022     SHUTDOWN = 9
00023     SAFEGUARD_STOP = 10
00024 
00025 class JointMode(object):
00026     PART_D_CALIBRATION = 237
00027     BACKDRIVE = 238
00028     POWER_OFF = 239
00029     EMERGENCY_STOPPED = 240
00030     CALVAL_INITIALIZATION = 241
00031     ERROR = 242
00032     FREEDRIVE = 243
00033     SIMULATED = 244
00034     NOT_RESPONDING = 245
00035     MOTOR_INITIALISATION = 246
00036     ADC_CALIBRATION = 247
00037     DEAD_COMMUTATION = 248
00038     BOOTLOADER = 249
00039     CALIBRATION = 250
00040     STOPPED = 251
00041     FAULT = 252
00042     RUNNING = 253
00043     INITIALIZATION = 254
00044     IDLE = 255
00045 
00046 class ToolMode(object):
00047     BOOTLOADER = 249
00048     RUNNING = 253
00049     IDLE = 255
00050     
00051 class MasterSafetyState(object):
00052     UNDEFINED = 0
00053     BOOTLOADER = 1
00054     FAULT = 2
00055     BOOTING = 3
00056     INITIALIZING = 4
00057     ROBOT_EMERGENCY_STOP = 5
00058     EXTERNAL_EMERGENCY_STOP = 6
00059     SAFEGUARD_STOP = 7
00060     OK = 8
00061 
00062 class MasterOnOffState(object):
00063     OFF = 0
00064     TURNING_ON = 1
00065     ON = 2
00066     TURNING_OFF = 3
00067 
00068 class RobotModeData(object):
00069     __slots__ = ['timestamp', 'robot_connected', 'real_robot_enabled',
00070                  'power_on_robot', 'emergency_stopped',
00071                  'security_stopped', 'program_running', 'program_paused',
00072                  'robot_mode', 'speed_fraction']
00073     @staticmethod
00074     def unpack(buf):
00075         rmd = RobotModeData()
00076         (_, _,
00077          rmd.timestamp, rmd.robot_connected, rmd.real_robot_enabled,
00078          rmd.power_on_robot, rmd.emergency_stopped, rmd.security_stopped,
00079          rmd.program_running, rmd.program_paused, rmd.robot_mode,
00080          rmd.speed_fraction) = struct.unpack_from("!IBQ???????Bd", buf)
00081         return rmd
00082     
00083 class JointData(object):
00084     __slots__ = ['q_actual', 'q_target', 'qd_actual',
00085                  'I_actual', 'V_actual', 'T_motor', 'T_micro', 'joint_mode']
00086     @staticmethod
00087     def unpack(buf):
00088         all_joints = []
00089         offset = 5
00090         for i in range(6):
00091             jd = JointData()
00092             (jd.q_actual, jd.q_target, jd.qd_actual, jd.I_actual, jd.V_actual,
00093              jd.T_motor, jd.T_micro, 
00094              jd.joint_mode) = struct.unpack_from("!dddffffB", buf, offset)
00095             offset += 41
00096             all_joints.append(jd)
00097         return all_joints
00098 
00099 class ToolData(object):
00100     __slots__ = ['analog_input_range2', 'analog_input_range3',
00101                  'analog_input2', 'analog_input3',
00102                  'tool_voltage_48V', 'tool_output_voltage', 'tool_current',
00103                  'tool_temperature', 'tool_mode']
00104     @staticmethod
00105     def unpack(buf):
00106         td = ToolData()
00107         (_, _,
00108          td.analog_input_range2, td.analog_input_range3,
00109          td.analog_input2, td.analog_input3,
00110          td.tool_voltage_48V, td.tool_output_voltage, td.tool_current,
00111          td.tool_temperature, td.tool_mode) = struct.unpack_from("!IBbbddfBffB", buf)
00112         return td
00113 
00114 class MasterboardData(object):
00115     __slots__ = ['digital_input_bits', 'digital_output_bits',
00116                  'analog_input_range0', 'analog_input_range1',
00117                  'analog_input0', 'analog_input1',
00118                  'analog_output_domain0', 'analog_output_domain1',
00119                  'analog_output0', 'analog_output1',
00120                  'masterboard_temperature',
00121                  'robot_voltage_48V', 'robot_current',
00122                  'master_io_current', 'master_safety_state',
00123                  'master_onoff_state']
00124     @staticmethod
00125     def unpack(buf):
00126         md = MasterboardData()
00127         (_, _,
00128          md.digital_input_bits, md.digital_output_bits,
00129          md.analog_input_range0, md.analog_input_range1,
00130          md.analog_input0, md.analog_input1,
00131          md.analog_output_domain0, md.analog_output_domain1,
00132          md.analog_output0, md.analog_output1,
00133          md.masterboard_temperature,
00134          md.robot_voltage_48V, md.robot_current,
00135          md.master_io_current, md.master_safety_state,
00136          md.master_onoff_state) = struct.unpack_from("!IBhhbbddbbddffffBB", buf)
00137         return md
00138 
00139 class CartesianInfo(object):
00140     __slots__ = ['x', 'y', 'z', 'rx', 'ry', 'rz']
00141     @staticmethod
00142     def unpack(buf):
00143         ci = CartesianInfo()
00144         (_, _,
00145          ci.x, ci.y, ci.z, ci.rx, ci.ry, ci.rz) = struct.unpack_from("!IB6d", buf)
00146         return ci
00147 
00148 class KinematicsInfo(object):
00149     @staticmethod
00150     def unpack(buf):
00151         return KinematicsInfo()
00152 
00153 class JointLimitData(object):
00154     __slots__ = ['min_limit', 'max_limit', 'max_speed', 'max_acceleration']
00155 
00156 class ConfigurationData(object):
00157     __slots__ = ['joint_limit_data',
00158                  'v_joint_default', 'a_joint_default', 
00159                  'v_tool_default', 'a_tool_default', 'eq_radius',
00160                  'dh_a', 'dh_d', 'dh_alpha', 'dh_theta',
00161                  'masterboard_version', 'controller_box_type',
00162                  'robot_type', 'robot_subtype']
00163     @staticmethod
00164     def unpack(buf):
00165         cd = ConfigurationData()
00166         cd.joint_limit_data = []
00167         for i in range(6):
00168             jld = JointLimitData()
00169             (jld.min_limit, jld.max_limit) = struct.unpack_from("!dd", buf, 5+16*i)
00170             (jld.max_speed, jld.max_acceleration) = struct.unpack_from("!dd", buf, 5+16*6+16*i)
00171             cd.joint_limit_data.append(jld)
00172         (cd.v_joint_default, cd.a_joint_default, cd.v_tool_default, cd.a_tool_default,
00173          cd.eq_radius) = struct.unpack_from("!ddddd", buf, 5+32*6)
00174         (cd.masterboard_version, cd.controller_box_type, cd.robot_type,
00175          cd.robot_subtype) = struct.unpack_from("!iiii", buf, 5+32*6+5*8+6*32)
00176         return cd
00177 
00178 class RobotState(object):
00179     __slots__ = ['robot_mode_data', 'joint_data', 'tool_data',
00180                  'masterboard_data', 'cartesian_info',
00181                  'kinematics_info', 'configuration_data']
00182 
00183     def __init__(self):
00184         pass
00185 
00186     @staticmethod
00187     def unpack(buf):
00188         length, mtype = struct.unpack_from("!IB", buf)
00189         if length != len(buf):
00190             raise Exception("Could not unpack packet: length field is incorrect")
00191         if mtype != 16:
00192             if mtype == 20:
00193                 print "Likely a syntax error:"
00194                 print buf[:2048]
00195             raise Exception("Fatal error when unpacking RobotState packet")
00196 
00197         rs = RobotState()
00198         offset = 5
00199         while offset < len(buf):
00200             length, ptype = struct.unpack_from("!IB", buf, offset)
00201             assert offset + length <= len(buf)
00202             package_buf = buffer(buf, offset, length)
00203             offset += length
00204 
00205             if ptype == PackageType.ROBOT_MODE_DATA:
00206                 rs.robot_mode_data = RobotModeData.unpack(package_buf)
00207             elif ptype == PackageType.JOINT_DATA:
00208                 rs.joint_data = JointData.unpack(package_buf)
00209             elif ptype == PackageType.TOOL_DATA:
00210                 rs.tool_data = ToolData.unpack(package_buf)
00211             elif ptype == PackageType.MASTERBOARD_DATA:
00212                 rs.masterboard_data = MasterboardData.unpack(package_buf)
00213             elif ptype == PackageType.CARTESIAN_INFO:
00214                 rs.cartesian_info = CartesianInfo.unpack(package_buf)
00215             elif ptype == PackageType.KINEMATICS_INFO:
00216                 rs.kinematics_info = KinematicsInfo.unpack(package_buf)
00217             elif ptype == PackageType.CONFIGURATION_DATA:
00218                 rs.configuration_data = ConfigurationData.unpack(package_buf)
00219             else:
00220                 raise Exception("Unknown package type: %i" % ptype)
00221         return rs
00222 
00223 def pstate(o, indent=''):
00224     for s in o.__slots__:
00225         child = getattr(o, s, None)
00226         if child is None:
00227             print "%s%s: None" % (indent, s)
00228         elif hasattr(child, '__slots__'):
00229             print "%s%s:" % (indent, s)
00230             pstate(child, indent + '    ')
00231         elif hasattr(child, '__iter__'):
00232             print "%s%s:" % (indent, s)
00233             for i, c in enumerate(child):
00234                 print "%s  [%i]:" % (indent, i)
00235                 pstate(c, indent + '    ')
00236         else:
00237             print "%s%s: %s" % (indent, s, child)
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Friends


ur5_driver
Author(s): Stuart Glaser
autogenerated on Mon Aug 19 2013 12:33:19