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     FORCE_MODE_DATA = 7
00012     ADDITIONAL_INFO = 8
00013     CALIBRATION_DATA = 9
00014 
00015 class RobotMode(object):
00016     RUNNING = 0
00017     FREEDRIVE = 1
00018     READY = 2
00019     INITIALIZING = 3
00020     SECURITY_STOPPED = 4
00021     EMERGENCY_STOPPED = 5
00022     FATAL_ERROR = 6
00023     NO_POWER = 7
00024     NOT_CONNECTED = 8
00025     SHUTDOWN = 9
00026     SAFEGUARD_STOP = 10
00027 
00028 class JointMode(object):
00029     PART_D_CALIBRATION = 237
00030     BACKDRIVE = 238
00031     POWER_OFF = 239
00032     EMERGENCY_STOPPED = 240
00033     CALVAL_INITIALIZATION = 241
00034     ERROR = 242
00035     FREEDRIVE = 243
00036     SIMULATED = 244
00037     NOT_RESPONDING = 245
00038     MOTOR_INITIALISATION = 246
00039     ADC_CALIBRATION = 247
00040     DEAD_COMMUTATION = 248
00041     BOOTLOADER = 249
00042     CALIBRATION = 250
00043     STOPPED = 251
00044     FAULT = 252
00045     RUNNING = 253
00046     INITIALIZATION = 254
00047     IDLE = 255
00048 
00049 class ToolMode(object):
00050     BOOTLOADER = 249
00051     RUNNING = 253
00052     IDLE = 255
00053 
00054 class MasterSafetyState(object):
00055     UNDEFINED = 0
00056     BOOTLOADER = 1
00057     FAULT = 2
00058     BOOTING = 3
00059     INITIALIZING = 4
00060     ROBOT_EMERGENCY_STOP = 5
00061     EXTERNAL_EMERGENCY_STOP = 6
00062     SAFEGUARD_STOP = 7
00063     OK = 8
00064 
00065 class MasterOnOffState(object):
00066     OFF = 0
00067     TURNING_ON = 1
00068     ON = 2
00069     TURNING_OFF = 3
00070 
00071 #this class handles different protocol versions
00072 class RobotModeData(object):
00073     @staticmethod
00074     def unpack(buf):
00075         rmd = RobotModeData()
00076         (plen, ptype) = struct.unpack_from("!IB", buf)
00077         if plen == 29:
00078             return RobotModeData_V18.unpack(buf)
00079         elif plen == 38:
00080             return RobotModeData_V30.unpack(buf)
00081         else:
00082             print "RobotModeData has wrong length: " + str(plen)
00083             return rmd
00084 
00085 #this parses RobotModeData for versions <= v1.8 (i.e. 1.6, 1.7, 1.8)
00086 class RobotModeData_V18(object):
00087     __slots__ = ['timestamp', 'robot_connected', 'real_robot_enabled',
00088                  'power_on_robot', 'emergency_stopped',
00089                  'security_stopped', 'program_running', 'program_paused',
00090                  'robot_mode', 'speed_fraction']
00091     @staticmethod
00092     def unpack(buf):
00093         rmd = RobotModeData_V18()
00094         (_, _,
00095          rmd.timestamp, rmd.robot_connected, rmd.real_robot_enabled,
00096          rmd.power_on_robot, rmd.emergency_stopped, rmd.security_stopped,
00097          rmd.program_running, rmd.program_paused, rmd.robot_mode,
00098          rmd.speed_fraction) = struct.unpack_from("!IBQ???????Bd", buf)
00099         return rmd
00100 
00101 #this parses RobotModeData for versions >=3.0 (i.e. 3.0)
00102 class RobotModeData_V30(object):
00103     __slots__ = ['timestamp', 'robot_connected', 'real_robot_enabled',
00104                  'power_on_robot', 'emergency_stopped',
00105                  'security_stopped', 'program_running', 'program_paused',
00106                  'robot_mode', 'control_mode', 'target_speed_fraction',
00107                  'speed_scaling']
00108     @staticmethod
00109     def unpack(buf):
00110         rmd = RobotModeData_V30()
00111         (_, _,
00112          rmd.timestamp, rmd.robot_connected, rmd.real_robot_enabled,
00113          rmd.power_on_robot, rmd.emergency_stopped, rmd.security_stopped,
00114          rmd.program_running, rmd.program_paused, rmd.robot_mode, rmd.control_mode,
00115          rmd.target_speed_fraction, rmd.speed_scaling) = struct.unpack_from("!IBQ???????BBdd", buf)
00116         return rmd
00117 
00118 #this parses JointData for all versions (i.e. 1.6, 1.7, 1.8, 3.0)
00119 class JointData(object):
00120     __slots__ = ['q_actual', 'q_target', 'qd_actual',
00121                  'I_actual', 'V_actual', 'T_motor', 'T_micro', 'joint_mode']
00122     @staticmethod
00123     def unpack(buf):
00124         all_joints = []
00125         offset = 5
00126         for i in range(6):
00127             jd = JointData()
00128             (jd.q_actual, jd.q_target, jd.qd_actual, jd.I_actual, jd.V_actual,
00129              jd.T_motor, jd.T_micro,
00130              jd.joint_mode) = struct.unpack_from("!dddffffB", buf, offset)
00131             offset += 41
00132             all_joints.append(jd)
00133         return all_joints
00134 
00135 #this parses JointData for all versions (i.e. 1.6, 1.7, 1.8, 3.0)
00136 class ToolData(object):
00137     __slots__ = ['analog_input_range2', 'analog_input_range3',
00138                  'analog_input2', 'analog_input3',
00139                  'tool_voltage_48V', 'tool_output_voltage', 'tool_current',
00140                  'tool_temperature', 'tool_mode']
00141     @staticmethod
00142     def unpack(buf):
00143         td = ToolData()
00144         (_, _,
00145          td.analog_input_range2, td.analog_input_range3,
00146          td.analog_input2, td.analog_input3,
00147          td.tool_voltage_48V, td.tool_output_voltage, td.tool_current,
00148          td.tool_temperature, td.tool_mode) = struct.unpack_from("!IBbbddfBffB", buf)
00149         return td
00150 
00151 #this class handles different protocol versions
00152 class MasterboardData(object):
00153     @staticmethod
00154     def unpack(buf):
00155         md = MasterboardData()
00156         (plen, ptype) = struct.unpack_from("!IB", buf)
00157         if (plen == 64) or (plen == 76): # Euromap67 interface = 12 bytes
00158             return MasterboardData_V18.unpack(buf)
00159         elif (plen == 72) or (plen == 92): # Euromap67 interface = 20 bytes
00160             return MasterboardData_V30.unpack(buf)
00161         else:
00162             print "MasterboardData has wrong length: " + str(plen)
00163             print "Euromap67Interface is ignored"
00164             return md
00165 
00166 #this parses MasterboardData for versions <= v1.8 (i.e. 1.6, 1.7, 1.8)
00167 class MasterboardData_V18(object):
00168     __slots__ = ['digital_input_bits', 'digital_output_bits',
00169                  'analog_input_range0', 'analog_input_range1',
00170                  'analog_input0', 'analog_input1',
00171                  'analog_output_domain0', 'analog_output_domain1',
00172                  'analog_output0', 'analog_output1',
00173                  'masterboard_temperature',
00174                  'robot_voltage_48V', 'robot_current',
00175                  'master_io_current', 'master_safety_state',
00176                  'master_onoff_state']#subsequent slots related to 'euromap' ignored
00177     @staticmethod
00178     def unpack(buf):
00179         md = MasterboardData_V18()
00180         (_, _,
00181          md.digital_input_bits, md.digital_output_bits,
00182          md.analog_input_range0, md.analog_input_range1,
00183          md.analog_input0, md.analog_input1,
00184          md.analog_output_domain0, md.analog_output_domain1,
00185          md.analog_output0, md.analog_output1,
00186          md.masterboard_temperature,
00187          md.robot_voltage_48V, md.robot_current,
00188          md.master_io_current, md.master_safety_state,
00189          md.master_onoff_state) = struct.unpack_from("!IBhhbbddbbddffffBB", buf)
00190         return md
00191 
00192 #this parses MasterboardData for versions >=3.0 (i.e. 3.0)
00193 class MasterboardData_V30(object):
00194     __slots__ = ['digital_input_bits', 'digital_output_bits',
00195                  'analog_input_range0', 'analog_input_range1',
00196                  'analog_input0', 'analog_input1',
00197                  'analog_output_domain0', 'analog_output_domain1',
00198                  'analog_output0', 'analog_output1',
00199                  'masterboard_temperature',
00200                  'robot_voltage_48V', 'robot_current',
00201                  'master_io_current', 'safety_mode',
00202                  'in_reduced_mode']#subsequent slots related to 'euromap' ignored
00203     @staticmethod
00204     def unpack(buf):
00205         md = MasterboardData_V30()
00206         (_, _, _UNDOCUMENTED_,
00207          md.digital_input_bits, md.digital_output_bits,
00208          md.analog_input_range0, md.analog_input_range1,
00209          md.analog_input0, md.analog_input1,
00210          md.analog_output_domain0, md.analog_output_domain1,
00211          md.analog_output0, md.analog_output1,
00212          md.masterboard_temperature,
00213          md.robot_voltage_48V, md.robot_current,
00214          md.master_io_current, md.safety_mode,
00215          md.in_reduced_mode) = struct.unpack_from("!IBIiibbddbbddffffBB", buf)
00216         return md
00217 
00218 #this parses JointData for all versions (i.e. 1.6, 1.7, 1.8, 3.0)
00219 class CartesianInfo(object):
00220     __slots__ = ['x', 'y', 'z', 'rx', 'ry', 'rz']
00221     @staticmethod
00222     def unpack(buf):
00223         ci = CartesianInfo()
00224         (_, _,
00225          ci.x, ci.y, ci.z, ci.rx, ci.ry, ci.rz) = struct.unpack_from("!IB6d", buf)
00226         return ci
00227 
00228 #this parses KinematicsInfo for versions (i.e. 1.8, 3.0)
00229 #KinematicsInfo is not available in 1.6 and 1.7
00230 class KinematicsInfo(object):
00231     @staticmethod
00232     def unpack(buf):
00233         return KinematicsInfo()
00234 
00235 #helper class for ConfigurationData
00236 class JointLimitData(object):
00237     __slots__ = ['min_limit', 'max_limit', 'max_speed', 'max_acceleration']
00238 
00239 #this parses ConfigurationData for versions (i.e. 1.8, 3.0)
00240 #ConfigurationData is not available in 1.6 and 1.7
00241 class ConfigurationData(object):
00242     __slots__ = ['joint_limit_data',
00243                  'v_joint_default', 'a_joint_default',
00244                  'v_tool_default', 'a_tool_default', 'eq_radius',
00245                  'dh_a', 'dh_d', 'dh_alpha', 'dh_theta',
00246                  'masterboard_version', 'controller_box_type',
00247                  'robot_type', 'robot_subtype']#in v1.8 there is an additional slot 'motor_type' for each joint, which currently is ignored!
00248     @staticmethod
00249     def unpack(buf):
00250         cd = ConfigurationData()
00251         cd.joint_limit_data = []
00252         for i in range(6):
00253             jld = JointLimitData()
00254             (jld.min_limit, jld.max_limit) = struct.unpack_from("!dd", buf, 5+16*i)
00255             (jld.max_speed, jld.max_acceleration) = struct.unpack_from("!dd", buf, 5+16*6+16*i)
00256             cd.joint_limit_data.append(jld)
00257         (cd.v_joint_default, cd.a_joint_default, cd.v_tool_default, cd.a_tool_default,
00258          cd.eq_radius) = struct.unpack_from("!ddddd", buf, 5+32*6)
00259         (cd.masterboard_version, cd.controller_box_type, cd.robot_type,
00260          cd.robot_subtype) = struct.unpack_from("!iiii", buf, 5+32*6+5*8+6*32)
00261         return cd
00262 
00263 #this parses KinematicsInfo for versions (i.e. 1.8, 3.0)
00264 #KinematicsInfo is not available in 1.6 and 1.7
00265 class ForceModeData(object):
00266     __slots__ = ['x', 'y', 'z', 'rx', 'ry', 'rz', 'robot_dexterity']
00267     @staticmethod
00268     def unpack(buf):
00269         fmd = ForceModeData()
00270         (_, _, fmd.x, fmd.y, fmd.z, fmd.rx, fmd.ry, fmd.rz,
00271          fmd.robot_dexterity) = struct.unpack_from("!IBddddddd", buf)
00272         return fmd
00273 
00274 #this class handles different protocol versions
00275 class AdditionalInfo(object):
00276     @staticmethod
00277     def unpack(buf):
00278         ai = AdditionalInfo()
00279         (plen, ptype) = struct.unpack_from("!IB", buf)
00280         if plen == 10:
00281             return AdditionalInfoOld.unpack(buf)
00282         elif plen == 7:
00283             return AdditionalInfoNew.unpack(buf)
00284         else:
00285             print "AdditionalInfo has wrong length: " + str(plen)
00286             return ai
00287 
00288 class AdditionalInfoOld(object):
00289     __slots__ = ['ctrl_bits', 'teach_button']
00290     @staticmethod
00291     def unpack(buf):
00292         ai = AdditionalInfoOld()
00293         (_,_,ai.ctrl_bits, ai.teach_button) = struct.unpack_from("!IBIB", buf)
00294         return ai
00295 
00296 class AdditionalInfoNew(object):
00297     __slots__ = ['teach_button_enabled','teach_button_pressed']
00298     @staticmethod
00299     def unpack(buf):
00300         ai = AdditionalInfoNew()
00301         (_,_,ai.teach_button_enabled, ai.teach_button_pressed) = struct.unpack_from("!IBBB", buf)
00302         return ai
00303 
00304 
00305 class RobotState(object):
00306     __slots__ = ['robot_mode_data', 'joint_data', 'tool_data',
00307                  'masterboard_data', 'cartesian_info',
00308                  'kinematics_info', 'configuration_data',
00309                  'force_mode_data', 'additional_info',
00310                  'unknown_ptypes']
00311 
00312     def __init__(self):
00313         self.unknown_ptypes = []
00314 
00315     @staticmethod
00316     def unpack(buf):
00317         length, mtype = struct.unpack_from("!IB", buf)
00318         if length != len(buf):
00319             raise Exception("Could not unpack packet: length field is incorrect")
00320         if mtype != 16:
00321             if mtype == 20:
00322                 print "Likely a syntax error:"
00323                 print buf[:2048]
00324             raise Exception("Fatal error when unpacking RobotState packet")
00325 
00326         rs = RobotState()
00327         offset = 5
00328         while offset < len(buf):
00329             length, ptype = struct.unpack_from("!IB", buf, offset)
00330             assert offset + length <= len(buf)
00331             package_buf = buffer(buf, offset, length)
00332             offset += length
00333 
00334             if ptype == PackageType.ROBOT_MODE_DATA:
00335                 rs.robot_mode_data = RobotModeData.unpack(package_buf)
00336             elif ptype == PackageType.JOINT_DATA:
00337                 rs.joint_data = JointData.unpack(package_buf)
00338             elif ptype == PackageType.TOOL_DATA:
00339                 rs.tool_data = ToolData.unpack(package_buf)
00340             elif ptype == PackageType.MASTERBOARD_DATA:
00341                 rs.masterboard_data = MasterboardData.unpack(package_buf)
00342             elif ptype == PackageType.CARTESIAN_INFO:
00343                 rs.cartesian_info = CartesianInfo.unpack(package_buf)
00344             elif ptype == PackageType.KINEMATICS_INFO:
00345                 rs.kinematics_info = KinematicsInfo.unpack(package_buf)
00346             elif ptype == PackageType.CONFIGURATION_DATA:
00347                 rs.configuration_data = ConfigurationData.unpack(package_buf)
00348             elif ptype == PackageType.FORCE_MODE_DATA:
00349                 rs.force_mode_data = ForceModeData.unpack(package_buf)
00350             elif ptype == PackageType.ADDITIONAL_INFO:
00351                 rs.additional_info = AdditionalInfo.unpack(package_buf)
00352             elif ptype == PackageType.CALIBRATION_DATA:
00353                 pass # internal data, should be skipped
00354             else:
00355                 rs.unknown_ptypes.append(ptype)
00356         return rs
00357 
00358 def pstate(o, indent=''):
00359     for s in o.__slots__:
00360         child = getattr(o, s, None)
00361         if child is None:
00362             print "%s%s: None" % (indent, s)
00363         elif hasattr(child, '__slots__'):
00364             print "%s%s:" % (indent, s)
00365             pstate(child, indent + '    ')
00366         elif hasattr(child, '__iter__'):
00367             print "%s%s:" % (indent, s)
00368             for i, c in enumerate(child):
00369                 print "%s  [%i]:" % (indent, i)
00370                 pstate(c, indent + '    ')
00371         else:
00372             print "%s%s: %s" % (indent, s, child)


ur_driver
Author(s): Stuart Glaser, Shaun Edwards , Felix Messmer
autogenerated on Fri Aug 28 2015 13:31:30