_TrackModelSrv.py
Go to the documentation of this file.
00001 """autogenerated by genpy from articulation_msgs/TrackModelSrvRequest.msg. Do not edit."""
00002 import sys
00003 python3 = True if sys.hexversion > 0x03000000 else False
00004 import genpy
00005 import struct
00006 
00007 import sensor_msgs.msg
00008 import geometry_msgs.msg
00009 import articulation_msgs.msg
00010 import std_msgs.msg
00011 
00012 class TrackModelSrvRequest(genpy.Message):
00013   _md5sum = "492b025b4d335f83e3fab65eb3211a27"
00014   _type = "articulation_msgs/TrackModelSrvRequest"
00015   _has_header = False #flag to mark the presence of a Header object
00016   _full_text = """
00017 
00018 
00019 
00020 
00021 
00022 
00023 
00024 
00025 
00026 
00027 articulation_msgs/ModelMsg model
00028 
00029 
00030 ================================================================================
00031 MSG: articulation_msgs/ModelMsg
00032 # Single kinematic model
00033 #
00034 # A kinematic model is defined by its model class name, and a set of parameters. 
00035 # The client may additionally specify a model id, that can be used to colorize the
00036 # model when visualized using the RVIZ model display.
00037 # 
00038 # For a list of currently implemented models, see the documetation at
00039 # http://www.ros.org/wiki/articulation_models
00040 #
00041 #
00042 
00043 std_msgs/Header header                        # frame and timestamp
00044 
00045 int32 id                             # user specified model id
00046 string name                          # name of the model family (e.g. "rotational",
00047                                      # "prismatic", "pca-gp", "rigid")
00048 articulation_msgs/ParamMsg[] params  # model parameters
00049 articulation_msgs/TrackMsg track     # trajectory from which the model is estimated, or
00050                                      # that is evaluated using the model
00051 
00052 ================================================================================
00053 MSG: std_msgs/Header
00054 # Standard metadata for higher-level stamped data types.
00055 # This is generally used to communicate timestamped data 
00056 # in a particular coordinate frame.
00057 # 
00058 # sequence ID: consecutively increasing ID 
00059 uint32 seq
00060 #Two-integer timestamp that is expressed as:
00061 # * stamp.secs: seconds (stamp_secs) since epoch
00062 # * stamp.nsecs: nanoseconds since stamp_secs
00063 # time-handling sugar is provided by the client library
00064 time stamp
00065 #Frame this data is associated with
00066 # 0: no frame
00067 # 1: global frame
00068 string frame_id
00069 
00070 ================================================================================
00071 MSG: articulation_msgs/ParamMsg
00072 # Single parameter passed to or from model fitting
00073 #
00074 # This mechanism allows to flexibly pass parameters to 
00075 # model fitting (and vice versa). Note that these parameters 
00076 # are model-specific: A client may supply additional
00077 # parameters to the model estimator, and, similarly, a estimator
00078 # may add the estimated model parameters to the model message.
00079 # When the model is then evaluated, for example to make predictions
00080 # or to compute the likelihood, the model class can then use
00081 # these parameters.
00082 #
00083 # A parameter has a name, a value, and a type. The type globally
00084 # indicates whether it is a prior parameter (prior to model fitting),
00085 # or a model parameter (found during model fitting, using a maximum-
00086 # likelihood estimator), or a cached evaluation (e.g., the likelihood
00087 # or the BIC are a typical "side"-product of model estimation, and
00088 # can therefore already be cached).
00089 #
00090 # For a list of currently used parameters, see the documentation at
00091 # http://www.ros.org/wiki/articulation_models
00092 #
00093 
00094 uint8 PRIOR=0   # indicates a prior model parameter 
00095                 # (e.g., "sigma_position")
00096 uint8 PARAM=1   # indicates a estimated model parameter 
00097                 # (e.g., "rot_radius", the estimated radius)
00098 uint8 EVAL=2    # indicates a cached evaluation of the model, given 
00099                 # the current trajectory
00100                 # (e.g., "loglikelihood", the log likelihood of the
00101                 # data, given the model and its parameters)
00102 
00103 string name     # name of the parameter
00104 float64 value   # value of the parameter
00105 uint8 type      # type of the parameter (PRIOR, PARAM, EVAL)
00106 
00107 
00108 ================================================================================
00109 MSG: articulation_msgs/TrackMsg
00110 # Single kinematic trajectory
00111 #
00112 # This message contains a kinematic trajectory. The trajectory is specified
00113 # as a vector of 6D poses. An additional flag, track_type, indicates whether
00114 # the track is valid, and whether it includes orientation. The track id
00115 # can be used for automatic coloring in the RVIZ track plugin, and can be 
00116 # freely chosen by the client. 
00117 #
00118 # A model is fitting only from the trajectory stored in the pose[]-vector. 
00119 # Additional information may be associated to each pose using the channels
00120 # vector, with arbitrary # fields (e.g., to include applied/measured forces). 
00121 #
00122 # After model evaluation,
00123 # also the associated configurations of the object are stored in the channels,
00124 # named "q[0]".."q[DOF-1]", where DOF is the number of degrees of freedom.
00125 # Model evaluation also projects the poses in the pose vector onto the model,
00126 # and stores these ideal poses in the vector pose_projected. Further, during model
00127 # evaluation, a new set of (uniform) configurations over the valid configuration
00128 # range is sampled, and the result is stored in pose_resampled.
00129 # The vector pose_flags contains additional display flags for the poses in the
00130 # pose vector, for example, whether a pose is visible and/or
00131 # the end of a trajectory segment. At the moment, this is only used by the
00132 # prior_model_learner.
00133 #
00134 
00135 std_msgs/Header header                        # frame and timestamp
00136 int32 id                                # used-specified track id
00137 
00138 geometry_msgs/Pose[] pose               # sequence of poses, defining the observed trajectory
00139 std_msgs/Header[] pose_headers                   # Timestamp and frame for each pose (and pose_projected)
00140 geometry_msgs/Pose[] pose_projected     # sequence of poses, projected to the model 
00141                                         # (after model evaluation)
00142 geometry_msgs/Pose[] pose_resampled     # sequence of poses, re-sampled from the model in
00143                                         # the valid configuration range
00144 uint32[] pose_flags                     # bit-wise combination of POSE_VISIBLE and POSE_END_OF_SEGMENT
00145 
00146 uint32 POSE_VISIBLE=1
00147 uint32 POSE_END_OF_SEGMENT=2
00148 
00149 # Each channel should have the same number of elements as pose array, 
00150 # and the data in each channel should correspond 1:1 with each pose
00151 # possible channels: "width", "height", "rgb", ...
00152 sensor_msgs/ChannelFloat32[] channels       
00153 
00154 
00155 
00156 ================================================================================
00157 MSG: geometry_msgs/Pose
00158 # A representation of pose in free space, composed of postion and orientation. 
00159 Point position
00160 Quaternion orientation
00161 
00162 ================================================================================
00163 MSG: geometry_msgs/Point
00164 # This contains the position of a point in free space
00165 float64 x
00166 float64 y
00167 float64 z
00168 
00169 ================================================================================
00170 MSG: geometry_msgs/Quaternion
00171 # This represents an orientation in free space in quaternion form.
00172 
00173 float64 x
00174 float64 y
00175 float64 z
00176 float64 w
00177 
00178 ================================================================================
00179 MSG: sensor_msgs/ChannelFloat32
00180 # This message is used by the PointCloud message to hold optional data
00181 # associated with each point in the cloud. The length of the values
00182 # array should be the same as the length of the points array in the
00183 # PointCloud, and each value should be associated with the corresponding
00184 # point.
00185 
00186 # Channel names in existing practice include:
00187 #   "u", "v" - row and column (respectively) in the left stereo image.
00188 #              This is opposite to usual conventions but remains for
00189 #              historical reasons. The newer PointCloud2 message has no
00190 #              such problem.
00191 #   "rgb" - For point clouds produced by color stereo cameras. uint8
00192 #           (R,G,B) values packed into the least significant 24 bits,
00193 #           in order.
00194 #   "intensity" - laser or pixel intensity.
00195 #   "distance"
00196 
00197 # The channel name should give semantics of the channel (e.g.
00198 # "intensity" instead of "value").
00199 string name
00200 
00201 # The values array should be 1-1 with the elements of the associated
00202 # PointCloud.
00203 float32[] values
00204 
00205 """
00206   __slots__ = ['model']
00207   _slot_types = ['articulation_msgs/ModelMsg']
00208 
00209   def __init__(self, *args, **kwds):
00210     """
00211     Constructor. Any message fields that are implicitly/explicitly
00212     set to None will be assigned a default value. The recommend
00213     use is keyword arguments as this is more robust to future message
00214     changes.  You cannot mix in-order arguments and keyword arguments.
00215 
00216     The available fields are:
00217        model
00218 
00219     :param args: complete set of field values, in .msg order
00220     :param kwds: use keyword arguments corresponding to message field names
00221     to set specific fields.
00222     """
00223     if args or kwds:
00224       super(TrackModelSrvRequest, self).__init__(*args, **kwds)
00225       #message fields cannot be None, assign default values for those that are
00226       if self.model is None:
00227         self.model = articulation_msgs.msg.ModelMsg()
00228     else:
00229       self.model = articulation_msgs.msg.ModelMsg()
00230 
00231   def _get_types(self):
00232     """
00233     internal API method
00234     """
00235     return self._slot_types
00236 
00237   def serialize(self, buff):
00238     """
00239     serialize message into buffer
00240     :param buff: buffer, ``StringIO``
00241     """
00242     try:
00243       _x = self
00244       buff.write(_struct_3I.pack(_x.model.header.seq, _x.model.header.stamp.secs, _x.model.header.stamp.nsecs))
00245       _x = self.model.header.frame_id
00246       length = len(_x)
00247       if python3 or type(_x) == unicode:
00248         _x = _x.encode('utf-8')
00249         length = len(_x)
00250       buff.write(struct.pack('<I%ss'%length, length, _x))
00251       buff.write(_struct_i.pack(self.model.id))
00252       _x = self.model.name
00253       length = len(_x)
00254       if python3 or type(_x) == unicode:
00255         _x = _x.encode('utf-8')
00256         length = len(_x)
00257       buff.write(struct.pack('<I%ss'%length, length, _x))
00258       length = len(self.model.params)
00259       buff.write(_struct_I.pack(length))
00260       for val1 in self.model.params:
00261         _x = val1.name
00262         length = len(_x)
00263         if python3 or type(_x) == unicode:
00264           _x = _x.encode('utf-8')
00265           length = len(_x)
00266         buff.write(struct.pack('<I%ss'%length, length, _x))
00267         _x = val1
00268         buff.write(_struct_dB.pack(_x.value, _x.type))
00269       _x = self
00270       buff.write(_struct_3I.pack(_x.model.track.header.seq, _x.model.track.header.stamp.secs, _x.model.track.header.stamp.nsecs))
00271       _x = self.model.track.header.frame_id
00272       length = len(_x)
00273       if python3 or type(_x) == unicode:
00274         _x = _x.encode('utf-8')
00275         length = len(_x)
00276       buff.write(struct.pack('<I%ss'%length, length, _x))
00277       buff.write(_struct_i.pack(self.model.track.id))
00278       length = len(self.model.track.pose)
00279       buff.write(_struct_I.pack(length))
00280       for val1 in self.model.track.pose:
00281         _v1 = val1.position
00282         _x = _v1
00283         buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00284         _v2 = val1.orientation
00285         _x = _v2
00286         buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
00287       length = len(self.model.track.pose_headers)
00288       buff.write(_struct_I.pack(length))
00289       for val1 in self.model.track.pose_headers:
00290         buff.write(_struct_I.pack(val1.seq))
00291         _v3 = val1.stamp
00292         _x = _v3
00293         buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
00294         _x = val1.frame_id
00295         length = len(_x)
00296         if python3 or type(_x) == unicode:
00297           _x = _x.encode('utf-8')
00298           length = len(_x)
00299         buff.write(struct.pack('<I%ss'%length, length, _x))
00300       length = len(self.model.track.pose_projected)
00301       buff.write(_struct_I.pack(length))
00302       for val1 in self.model.track.pose_projected:
00303         _v4 = val1.position
00304         _x = _v4
00305         buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00306         _v5 = val1.orientation
00307         _x = _v5
00308         buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
00309       length = len(self.model.track.pose_resampled)
00310       buff.write(_struct_I.pack(length))
00311       for val1 in self.model.track.pose_resampled:
00312         _v6 = val1.position
00313         _x = _v6
00314         buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00315         _v7 = val1.orientation
00316         _x = _v7
00317         buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
00318       length = len(self.model.track.pose_flags)
00319       buff.write(_struct_I.pack(length))
00320       pattern = '<%sI'%length
00321       buff.write(struct.pack(pattern, *self.model.track.pose_flags))
00322       length = len(self.model.track.channels)
00323       buff.write(_struct_I.pack(length))
00324       for val1 in self.model.track.channels:
00325         _x = val1.name
00326         length = len(_x)
00327         if python3 or type(_x) == unicode:
00328           _x = _x.encode('utf-8')
00329           length = len(_x)
00330         buff.write(struct.pack('<I%ss'%length, length, _x))
00331         length = len(val1.values)
00332         buff.write(_struct_I.pack(length))
00333         pattern = '<%sf'%length
00334         buff.write(struct.pack(pattern, *val1.values))
00335     except struct.error as se: self._check_types(se)
00336     except TypeError as te: self._check_types(te)
00337 
00338   def deserialize(self, str):
00339     """
00340     unpack serialized message in str into this message instance
00341     :param str: byte array of serialized message, ``str``
00342     """
00343     try:
00344       if self.model is None:
00345         self.model = articulation_msgs.msg.ModelMsg()
00346       end = 0
00347       _x = self
00348       start = end
00349       end += 12
00350       (_x.model.header.seq, _x.model.header.stamp.secs, _x.model.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
00351       start = end
00352       end += 4
00353       (length,) = _struct_I.unpack(str[start:end])
00354       start = end
00355       end += length
00356       if python3:
00357         self.model.header.frame_id = str[start:end].decode('utf-8')
00358       else:
00359         self.model.header.frame_id = str[start:end]
00360       start = end
00361       end += 4
00362       (self.model.id,) = _struct_i.unpack(str[start:end])
00363       start = end
00364       end += 4
00365       (length,) = _struct_I.unpack(str[start:end])
00366       start = end
00367       end += length
00368       if python3:
00369         self.model.name = str[start:end].decode('utf-8')
00370       else:
00371         self.model.name = str[start:end]
00372       start = end
00373       end += 4
00374       (length,) = _struct_I.unpack(str[start:end])
00375       self.model.params = []
00376       for i in range(0, length):
00377         val1 = articulation_msgs.msg.ParamMsg()
00378         start = end
00379         end += 4
00380         (length,) = _struct_I.unpack(str[start:end])
00381         start = end
00382         end += length
00383         if python3:
00384           val1.name = str[start:end].decode('utf-8')
00385         else:
00386           val1.name = str[start:end]
00387         _x = val1
00388         start = end
00389         end += 9
00390         (_x.value, _x.type,) = _struct_dB.unpack(str[start:end])
00391         self.model.params.append(val1)
00392       _x = self
00393       start = end
00394       end += 12
00395       (_x.model.track.header.seq, _x.model.track.header.stamp.secs, _x.model.track.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
00396       start = end
00397       end += 4
00398       (length,) = _struct_I.unpack(str[start:end])
00399       start = end
00400       end += length
00401       if python3:
00402         self.model.track.header.frame_id = str[start:end].decode('utf-8')
00403       else:
00404         self.model.track.header.frame_id = str[start:end]
00405       start = end
00406       end += 4
00407       (self.model.track.id,) = _struct_i.unpack(str[start:end])
00408       start = end
00409       end += 4
00410       (length,) = _struct_I.unpack(str[start:end])
00411       self.model.track.pose = []
00412       for i in range(0, length):
00413         val1 = geometry_msgs.msg.Pose()
00414         _v8 = val1.position
00415         _x = _v8
00416         start = end
00417         end += 24
00418         (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
00419         _v9 = val1.orientation
00420         _x = _v9
00421         start = end
00422         end += 32
00423         (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
00424         self.model.track.pose.append(val1)
00425       start = end
00426       end += 4
00427       (length,) = _struct_I.unpack(str[start:end])
00428       self.model.track.pose_headers = []
00429       for i in range(0, length):
00430         val1 = std_msgs.msg.Header()
00431         start = end
00432         end += 4
00433         (val1.seq,) = _struct_I.unpack(str[start:end])
00434         _v10 = val1.stamp
00435         _x = _v10
00436         start = end
00437         end += 8
00438         (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
00439         start = end
00440         end += 4
00441         (length,) = _struct_I.unpack(str[start:end])
00442         start = end
00443         end += length
00444         if python3:
00445           val1.frame_id = str[start:end].decode('utf-8')
00446         else:
00447           val1.frame_id = str[start:end]
00448         self.model.track.pose_headers.append(val1)
00449       start = end
00450       end += 4
00451       (length,) = _struct_I.unpack(str[start:end])
00452       self.model.track.pose_projected = []
00453       for i in range(0, length):
00454         val1 = geometry_msgs.msg.Pose()
00455         _v11 = val1.position
00456         _x = _v11
00457         start = end
00458         end += 24
00459         (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
00460         _v12 = val1.orientation
00461         _x = _v12
00462         start = end
00463         end += 32
00464         (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
00465         self.model.track.pose_projected.append(val1)
00466       start = end
00467       end += 4
00468       (length,) = _struct_I.unpack(str[start:end])
00469       self.model.track.pose_resampled = []
00470       for i in range(0, length):
00471         val1 = geometry_msgs.msg.Pose()
00472         _v13 = val1.position
00473         _x = _v13
00474         start = end
00475         end += 24
00476         (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
00477         _v14 = val1.orientation
00478         _x = _v14
00479         start = end
00480         end += 32
00481         (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
00482         self.model.track.pose_resampled.append(val1)
00483       start = end
00484       end += 4
00485       (length,) = _struct_I.unpack(str[start:end])
00486       pattern = '<%sI'%length
00487       start = end
00488       end += struct.calcsize(pattern)
00489       self.model.track.pose_flags = struct.unpack(pattern, str[start:end])
00490       start = end
00491       end += 4
00492       (length,) = _struct_I.unpack(str[start:end])
00493       self.model.track.channels = []
00494       for i in range(0, length):
00495         val1 = sensor_msgs.msg.ChannelFloat32()
00496         start = end
00497         end += 4
00498         (length,) = _struct_I.unpack(str[start:end])
00499         start = end
00500         end += length
00501         if python3:
00502           val1.name = str[start:end].decode('utf-8')
00503         else:
00504           val1.name = str[start:end]
00505         start = end
00506         end += 4
00507         (length,) = _struct_I.unpack(str[start:end])
00508         pattern = '<%sf'%length
00509         start = end
00510         end += struct.calcsize(pattern)
00511         val1.values = struct.unpack(pattern, str[start:end])
00512         self.model.track.channels.append(val1)
00513       return self
00514     except struct.error as e:
00515       raise genpy.DeserializationError(e) #most likely buffer underfill
00516 
00517 
00518   def serialize_numpy(self, buff, numpy):
00519     """
00520     serialize message with numpy array types into buffer
00521     :param buff: buffer, ``StringIO``
00522     :param numpy: numpy python module
00523     """
00524     try:
00525       _x = self
00526       buff.write(_struct_3I.pack(_x.model.header.seq, _x.model.header.stamp.secs, _x.model.header.stamp.nsecs))
00527       _x = self.model.header.frame_id
00528       length = len(_x)
00529       if python3 or type(_x) == unicode:
00530         _x = _x.encode('utf-8')
00531         length = len(_x)
00532       buff.write(struct.pack('<I%ss'%length, length, _x))
00533       buff.write(_struct_i.pack(self.model.id))
00534       _x = self.model.name
00535       length = len(_x)
00536       if python3 or type(_x) == unicode:
00537         _x = _x.encode('utf-8')
00538         length = len(_x)
00539       buff.write(struct.pack('<I%ss'%length, length, _x))
00540       length = len(self.model.params)
00541       buff.write(_struct_I.pack(length))
00542       for val1 in self.model.params:
00543         _x = val1.name
00544         length = len(_x)
00545         if python3 or type(_x) == unicode:
00546           _x = _x.encode('utf-8')
00547           length = len(_x)
00548         buff.write(struct.pack('<I%ss'%length, length, _x))
00549         _x = val1
00550         buff.write(_struct_dB.pack(_x.value, _x.type))
00551       _x = self
00552       buff.write(_struct_3I.pack(_x.model.track.header.seq, _x.model.track.header.stamp.secs, _x.model.track.header.stamp.nsecs))
00553       _x = self.model.track.header.frame_id
00554       length = len(_x)
00555       if python3 or type(_x) == unicode:
00556         _x = _x.encode('utf-8')
00557         length = len(_x)
00558       buff.write(struct.pack('<I%ss'%length, length, _x))
00559       buff.write(_struct_i.pack(self.model.track.id))
00560       length = len(self.model.track.pose)
00561       buff.write(_struct_I.pack(length))
00562       for val1 in self.model.track.pose:
00563         _v15 = val1.position
00564         _x = _v15
00565         buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00566         _v16 = val1.orientation
00567         _x = _v16
00568         buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
00569       length = len(self.model.track.pose_headers)
00570       buff.write(_struct_I.pack(length))
00571       for val1 in self.model.track.pose_headers:
00572         buff.write(_struct_I.pack(val1.seq))
00573         _v17 = val1.stamp
00574         _x = _v17
00575         buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
00576         _x = val1.frame_id
00577         length = len(_x)
00578         if python3 or type(_x) == unicode:
00579           _x = _x.encode('utf-8')
00580           length = len(_x)
00581         buff.write(struct.pack('<I%ss'%length, length, _x))
00582       length = len(self.model.track.pose_projected)
00583       buff.write(_struct_I.pack(length))
00584       for val1 in self.model.track.pose_projected:
00585         _v18 = val1.position
00586         _x = _v18
00587         buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00588         _v19 = val1.orientation
00589         _x = _v19
00590         buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
00591       length = len(self.model.track.pose_resampled)
00592       buff.write(_struct_I.pack(length))
00593       for val1 in self.model.track.pose_resampled:
00594         _v20 = val1.position
00595         _x = _v20
00596         buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00597         _v21 = val1.orientation
00598         _x = _v21
00599         buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
00600       length = len(self.model.track.pose_flags)
00601       buff.write(_struct_I.pack(length))
00602       pattern = '<%sI'%length
00603       buff.write(self.model.track.pose_flags.tostring())
00604       length = len(self.model.track.channels)
00605       buff.write(_struct_I.pack(length))
00606       for val1 in self.model.track.channels:
00607         _x = val1.name
00608         length = len(_x)
00609         if python3 or type(_x) == unicode:
00610           _x = _x.encode('utf-8')
00611           length = len(_x)
00612         buff.write(struct.pack('<I%ss'%length, length, _x))
00613         length = len(val1.values)
00614         buff.write(_struct_I.pack(length))
00615         pattern = '<%sf'%length
00616         buff.write(val1.values.tostring())
00617     except struct.error as se: self._check_types(se)
00618     except TypeError as te: self._check_types(te)
00619 
00620   def deserialize_numpy(self, str, numpy):
00621     """
00622     unpack serialized message in str into this message instance using numpy for array types
00623     :param str: byte array of serialized message, ``str``
00624     :param numpy: numpy python module
00625     """
00626     try:
00627       if self.model is None:
00628         self.model = articulation_msgs.msg.ModelMsg()
00629       end = 0
00630       _x = self
00631       start = end
00632       end += 12
00633       (_x.model.header.seq, _x.model.header.stamp.secs, _x.model.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
00634       start = end
00635       end += 4
00636       (length,) = _struct_I.unpack(str[start:end])
00637       start = end
00638       end += length
00639       if python3:
00640         self.model.header.frame_id = str[start:end].decode('utf-8')
00641       else:
00642         self.model.header.frame_id = str[start:end]
00643       start = end
00644       end += 4
00645       (self.model.id,) = _struct_i.unpack(str[start:end])
00646       start = end
00647       end += 4
00648       (length,) = _struct_I.unpack(str[start:end])
00649       start = end
00650       end += length
00651       if python3:
00652         self.model.name = str[start:end].decode('utf-8')
00653       else:
00654         self.model.name = str[start:end]
00655       start = end
00656       end += 4
00657       (length,) = _struct_I.unpack(str[start:end])
00658       self.model.params = []
00659       for i in range(0, length):
00660         val1 = articulation_msgs.msg.ParamMsg()
00661         start = end
00662         end += 4
00663         (length,) = _struct_I.unpack(str[start:end])
00664         start = end
00665         end += length
00666         if python3:
00667           val1.name = str[start:end].decode('utf-8')
00668         else:
00669           val1.name = str[start:end]
00670         _x = val1
00671         start = end
00672         end += 9
00673         (_x.value, _x.type,) = _struct_dB.unpack(str[start:end])
00674         self.model.params.append(val1)
00675       _x = self
00676       start = end
00677       end += 12
00678       (_x.model.track.header.seq, _x.model.track.header.stamp.secs, _x.model.track.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
00679       start = end
00680       end += 4
00681       (length,) = _struct_I.unpack(str[start:end])
00682       start = end
00683       end += length
00684       if python3:
00685         self.model.track.header.frame_id = str[start:end].decode('utf-8')
00686       else:
00687         self.model.track.header.frame_id = str[start:end]
00688       start = end
00689       end += 4
00690       (self.model.track.id,) = _struct_i.unpack(str[start:end])
00691       start = end
00692       end += 4
00693       (length,) = _struct_I.unpack(str[start:end])
00694       self.model.track.pose = []
00695       for i in range(0, length):
00696         val1 = geometry_msgs.msg.Pose()
00697         _v22 = val1.position
00698         _x = _v22
00699         start = end
00700         end += 24
00701         (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
00702         _v23 = val1.orientation
00703         _x = _v23
00704         start = end
00705         end += 32
00706         (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
00707         self.model.track.pose.append(val1)
00708       start = end
00709       end += 4
00710       (length,) = _struct_I.unpack(str[start:end])
00711       self.model.track.pose_headers = []
00712       for i in range(0, length):
00713         val1 = std_msgs.msg.Header()
00714         start = end
00715         end += 4
00716         (val1.seq,) = _struct_I.unpack(str[start:end])
00717         _v24 = val1.stamp
00718         _x = _v24
00719         start = end
00720         end += 8
00721         (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
00722         start = end
00723         end += 4
00724         (length,) = _struct_I.unpack(str[start:end])
00725         start = end
00726         end += length
00727         if python3:
00728           val1.frame_id = str[start:end].decode('utf-8')
00729         else:
00730           val1.frame_id = str[start:end]
00731         self.model.track.pose_headers.append(val1)
00732       start = end
00733       end += 4
00734       (length,) = _struct_I.unpack(str[start:end])
00735       self.model.track.pose_projected = []
00736       for i in range(0, length):
00737         val1 = geometry_msgs.msg.Pose()
00738         _v25 = val1.position
00739         _x = _v25
00740         start = end
00741         end += 24
00742         (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
00743         _v26 = val1.orientation
00744         _x = _v26
00745         start = end
00746         end += 32
00747         (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
00748         self.model.track.pose_projected.append(val1)
00749       start = end
00750       end += 4
00751       (length,) = _struct_I.unpack(str[start:end])
00752       self.model.track.pose_resampled = []
00753       for i in range(0, length):
00754         val1 = geometry_msgs.msg.Pose()
00755         _v27 = val1.position
00756         _x = _v27
00757         start = end
00758         end += 24
00759         (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
00760         _v28 = val1.orientation
00761         _x = _v28
00762         start = end
00763         end += 32
00764         (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
00765         self.model.track.pose_resampled.append(val1)
00766       start = end
00767       end += 4
00768       (length,) = _struct_I.unpack(str[start:end])
00769       pattern = '<%sI'%length
00770       start = end
00771       end += struct.calcsize(pattern)
00772       self.model.track.pose_flags = numpy.frombuffer(str[start:end], dtype=numpy.uint32, count=length)
00773       start = end
00774       end += 4
00775       (length,) = _struct_I.unpack(str[start:end])
00776       self.model.track.channels = []
00777       for i in range(0, length):
00778         val1 = sensor_msgs.msg.ChannelFloat32()
00779         start = end
00780         end += 4
00781         (length,) = _struct_I.unpack(str[start:end])
00782         start = end
00783         end += length
00784         if python3:
00785           val1.name = str[start:end].decode('utf-8')
00786         else:
00787           val1.name = str[start:end]
00788         start = end
00789         end += 4
00790         (length,) = _struct_I.unpack(str[start:end])
00791         pattern = '<%sf'%length
00792         start = end
00793         end += struct.calcsize(pattern)
00794         val1.values = numpy.frombuffer(str[start:end], dtype=numpy.float32, count=length)
00795         self.model.track.channels.append(val1)
00796       return self
00797     except struct.error as e:
00798       raise genpy.DeserializationError(e) #most likely buffer underfill
00799 
00800 _struct_I = genpy.struct_I
00801 _struct_i = struct.Struct("<i")
00802 _struct_dB = struct.Struct("<dB")
00803 _struct_3I = struct.Struct("<3I")
00804 _struct_4d = struct.Struct("<4d")
00805 _struct_2I = struct.Struct("<2I")
00806 _struct_3d = struct.Struct("<3d")
00807 """autogenerated by genpy from articulation_msgs/TrackModelSrvResponse.msg. Do not edit."""
00808 import sys
00809 python3 = True if sys.hexversion > 0x03000000 else False
00810 import genpy
00811 import struct
00812 
00813 import sensor_msgs.msg
00814 import geometry_msgs.msg
00815 import articulation_msgs.msg
00816 import std_msgs.msg
00817 
00818 class TrackModelSrvResponse(genpy.Message):
00819   _md5sum = "492b025b4d335f83e3fab65eb3211a27"
00820   _type = "articulation_msgs/TrackModelSrvResponse"
00821   _has_header = False #flag to mark the presence of a Header object
00822   _full_text = """articulation_msgs/ModelMsg model
00823 
00824 
00825 
00826 
00827 
00828 
00829 ================================================================================
00830 MSG: articulation_msgs/ModelMsg
00831 # Single kinematic model
00832 #
00833 # A kinematic model is defined by its model class name, and a set of parameters. 
00834 # The client may additionally specify a model id, that can be used to colorize the
00835 # model when visualized using the RVIZ model display.
00836 # 
00837 # For a list of currently implemented models, see the documetation at
00838 # http://www.ros.org/wiki/articulation_models
00839 #
00840 #
00841 
00842 std_msgs/Header header                        # frame and timestamp
00843 
00844 int32 id                             # user specified model id
00845 string name                          # name of the model family (e.g. "rotational",
00846                                      # "prismatic", "pca-gp", "rigid")
00847 articulation_msgs/ParamMsg[] params  # model parameters
00848 articulation_msgs/TrackMsg track     # trajectory from which the model is estimated, or
00849                                      # that is evaluated using the model
00850 
00851 ================================================================================
00852 MSG: std_msgs/Header
00853 # Standard metadata for higher-level stamped data types.
00854 # This is generally used to communicate timestamped data 
00855 # in a particular coordinate frame.
00856 # 
00857 # sequence ID: consecutively increasing ID 
00858 uint32 seq
00859 #Two-integer timestamp that is expressed as:
00860 # * stamp.secs: seconds (stamp_secs) since epoch
00861 # * stamp.nsecs: nanoseconds since stamp_secs
00862 # time-handling sugar is provided by the client library
00863 time stamp
00864 #Frame this data is associated with
00865 # 0: no frame
00866 # 1: global frame
00867 string frame_id
00868 
00869 ================================================================================
00870 MSG: articulation_msgs/ParamMsg
00871 # Single parameter passed to or from model fitting
00872 #
00873 # This mechanism allows to flexibly pass parameters to 
00874 # model fitting (and vice versa). Note that these parameters 
00875 # are model-specific: A client may supply additional
00876 # parameters to the model estimator, and, similarly, a estimator
00877 # may add the estimated model parameters to the model message.
00878 # When the model is then evaluated, for example to make predictions
00879 # or to compute the likelihood, the model class can then use
00880 # these parameters.
00881 #
00882 # A parameter has a name, a value, and a type. The type globally
00883 # indicates whether it is a prior parameter (prior to model fitting),
00884 # or a model parameter (found during model fitting, using a maximum-
00885 # likelihood estimator), or a cached evaluation (e.g., the likelihood
00886 # or the BIC are a typical "side"-product of model estimation, and
00887 # can therefore already be cached).
00888 #
00889 # For a list of currently used parameters, see the documentation at
00890 # http://www.ros.org/wiki/articulation_models
00891 #
00892 
00893 uint8 PRIOR=0   # indicates a prior model parameter 
00894                 # (e.g., "sigma_position")
00895 uint8 PARAM=1   # indicates a estimated model parameter 
00896                 # (e.g., "rot_radius", the estimated radius)
00897 uint8 EVAL=2    # indicates a cached evaluation of the model, given 
00898                 # the current trajectory
00899                 # (e.g., "loglikelihood", the log likelihood of the
00900                 # data, given the model and its parameters)
00901 
00902 string name     # name of the parameter
00903 float64 value   # value of the parameter
00904 uint8 type      # type of the parameter (PRIOR, PARAM, EVAL)
00905 
00906 
00907 ================================================================================
00908 MSG: articulation_msgs/TrackMsg
00909 # Single kinematic trajectory
00910 #
00911 # This message contains a kinematic trajectory. The trajectory is specified
00912 # as a vector of 6D poses. An additional flag, track_type, indicates whether
00913 # the track is valid, and whether it includes orientation. The track id
00914 # can be used for automatic coloring in the RVIZ track plugin, and can be 
00915 # freely chosen by the client. 
00916 #
00917 # A model is fitting only from the trajectory stored in the pose[]-vector. 
00918 # Additional information may be associated to each pose using the channels
00919 # vector, with arbitrary # fields (e.g., to include applied/measured forces). 
00920 #
00921 # After model evaluation,
00922 # also the associated configurations of the object are stored in the channels,
00923 # named "q[0]".."q[DOF-1]", where DOF is the number of degrees of freedom.
00924 # Model evaluation also projects the poses in the pose vector onto the model,
00925 # and stores these ideal poses in the vector pose_projected. Further, during model
00926 # evaluation, a new set of (uniform) configurations over the valid configuration
00927 # range is sampled, and the result is stored in pose_resampled.
00928 # The vector pose_flags contains additional display flags for the poses in the
00929 # pose vector, for example, whether a pose is visible and/or
00930 # the end of a trajectory segment. At the moment, this is only used by the
00931 # prior_model_learner.
00932 #
00933 
00934 std_msgs/Header header                        # frame and timestamp
00935 int32 id                                # used-specified track id
00936 
00937 geometry_msgs/Pose[] pose               # sequence of poses, defining the observed trajectory
00938 std_msgs/Header[] pose_headers                   # Timestamp and frame for each pose (and pose_projected)
00939 geometry_msgs/Pose[] pose_projected     # sequence of poses, projected to the model 
00940                                         # (after model evaluation)
00941 geometry_msgs/Pose[] pose_resampled     # sequence of poses, re-sampled from the model in
00942                                         # the valid configuration range
00943 uint32[] pose_flags                     # bit-wise combination of POSE_VISIBLE and POSE_END_OF_SEGMENT
00944 
00945 uint32 POSE_VISIBLE=1
00946 uint32 POSE_END_OF_SEGMENT=2
00947 
00948 # Each channel should have the same number of elements as pose array, 
00949 # and the data in each channel should correspond 1:1 with each pose
00950 # possible channels: "width", "height", "rgb", ...
00951 sensor_msgs/ChannelFloat32[] channels       
00952 
00953 
00954 
00955 ================================================================================
00956 MSG: geometry_msgs/Pose
00957 # A representation of pose in free space, composed of postion and orientation. 
00958 Point position
00959 Quaternion orientation
00960 
00961 ================================================================================
00962 MSG: geometry_msgs/Point
00963 # This contains the position of a point in free space
00964 float64 x
00965 float64 y
00966 float64 z
00967 
00968 ================================================================================
00969 MSG: geometry_msgs/Quaternion
00970 # This represents an orientation in free space in quaternion form.
00971 
00972 float64 x
00973 float64 y
00974 float64 z
00975 float64 w
00976 
00977 ================================================================================
00978 MSG: sensor_msgs/ChannelFloat32
00979 # This message is used by the PointCloud message to hold optional data
00980 # associated with each point in the cloud. The length of the values
00981 # array should be the same as the length of the points array in the
00982 # PointCloud, and each value should be associated with the corresponding
00983 # point.
00984 
00985 # Channel names in existing practice include:
00986 #   "u", "v" - row and column (respectively) in the left stereo image.
00987 #              This is opposite to usual conventions but remains for
00988 #              historical reasons. The newer PointCloud2 message has no
00989 #              such problem.
00990 #   "rgb" - For point clouds produced by color stereo cameras. uint8
00991 #           (R,G,B) values packed into the least significant 24 bits,
00992 #           in order.
00993 #   "intensity" - laser or pixel intensity.
00994 #   "distance"
00995 
00996 # The channel name should give semantics of the channel (e.g.
00997 # "intensity" instead of "value").
00998 string name
00999 
01000 # The values array should be 1-1 with the elements of the associated
01001 # PointCloud.
01002 float32[] values
01003 
01004 """
01005   __slots__ = ['model']
01006   _slot_types = ['articulation_msgs/ModelMsg']
01007 
01008   def __init__(self, *args, **kwds):
01009     """
01010     Constructor. Any message fields that are implicitly/explicitly
01011     set to None will be assigned a default value. The recommend
01012     use is keyword arguments as this is more robust to future message
01013     changes.  You cannot mix in-order arguments and keyword arguments.
01014 
01015     The available fields are:
01016        model
01017 
01018     :param args: complete set of field values, in .msg order
01019     :param kwds: use keyword arguments corresponding to message field names
01020     to set specific fields.
01021     """
01022     if args or kwds:
01023       super(TrackModelSrvResponse, self).__init__(*args, **kwds)
01024       #message fields cannot be None, assign default values for those that are
01025       if self.model is None:
01026         self.model = articulation_msgs.msg.ModelMsg()
01027     else:
01028       self.model = articulation_msgs.msg.ModelMsg()
01029 
01030   def _get_types(self):
01031     """
01032     internal API method
01033     """
01034     return self._slot_types
01035 
01036   def serialize(self, buff):
01037     """
01038     serialize message into buffer
01039     :param buff: buffer, ``StringIO``
01040     """
01041     try:
01042       _x = self
01043       buff.write(_struct_3I.pack(_x.model.header.seq, _x.model.header.stamp.secs, _x.model.header.stamp.nsecs))
01044       _x = self.model.header.frame_id
01045       length = len(_x)
01046       if python3 or type(_x) == unicode:
01047         _x = _x.encode('utf-8')
01048         length = len(_x)
01049       buff.write(struct.pack('<I%ss'%length, length, _x))
01050       buff.write(_struct_i.pack(self.model.id))
01051       _x = self.model.name
01052       length = len(_x)
01053       if python3 or type(_x) == unicode:
01054         _x = _x.encode('utf-8')
01055         length = len(_x)
01056       buff.write(struct.pack('<I%ss'%length, length, _x))
01057       length = len(self.model.params)
01058       buff.write(_struct_I.pack(length))
01059       for val1 in self.model.params:
01060         _x = val1.name
01061         length = len(_x)
01062         if python3 or type(_x) == unicode:
01063           _x = _x.encode('utf-8')
01064           length = len(_x)
01065         buff.write(struct.pack('<I%ss'%length, length, _x))
01066         _x = val1
01067         buff.write(_struct_dB.pack(_x.value, _x.type))
01068       _x = self
01069       buff.write(_struct_3I.pack(_x.model.track.header.seq, _x.model.track.header.stamp.secs, _x.model.track.header.stamp.nsecs))
01070       _x = self.model.track.header.frame_id
01071       length = len(_x)
01072       if python3 or type(_x) == unicode:
01073         _x = _x.encode('utf-8')
01074         length = len(_x)
01075       buff.write(struct.pack('<I%ss'%length, length, _x))
01076       buff.write(_struct_i.pack(self.model.track.id))
01077       length = len(self.model.track.pose)
01078       buff.write(_struct_I.pack(length))
01079       for val1 in self.model.track.pose:
01080         _v29 = val1.position
01081         _x = _v29
01082         buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
01083         _v30 = val1.orientation
01084         _x = _v30
01085         buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
01086       length = len(self.model.track.pose_headers)
01087       buff.write(_struct_I.pack(length))
01088       for val1 in self.model.track.pose_headers:
01089         buff.write(_struct_I.pack(val1.seq))
01090         _v31 = val1.stamp
01091         _x = _v31
01092         buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
01093         _x = val1.frame_id
01094         length = len(_x)
01095         if python3 or type(_x) == unicode:
01096           _x = _x.encode('utf-8')
01097           length = len(_x)
01098         buff.write(struct.pack('<I%ss'%length, length, _x))
01099       length = len(self.model.track.pose_projected)
01100       buff.write(_struct_I.pack(length))
01101       for val1 in self.model.track.pose_projected:
01102         _v32 = val1.position
01103         _x = _v32
01104         buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
01105         _v33 = val1.orientation
01106         _x = _v33
01107         buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
01108       length = len(self.model.track.pose_resampled)
01109       buff.write(_struct_I.pack(length))
01110       for val1 in self.model.track.pose_resampled:
01111         _v34 = val1.position
01112         _x = _v34
01113         buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
01114         _v35 = val1.orientation
01115         _x = _v35
01116         buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
01117       length = len(self.model.track.pose_flags)
01118       buff.write(_struct_I.pack(length))
01119       pattern = '<%sI'%length
01120       buff.write(struct.pack(pattern, *self.model.track.pose_flags))
01121       length = len(self.model.track.channels)
01122       buff.write(_struct_I.pack(length))
01123       for val1 in self.model.track.channels:
01124         _x = val1.name
01125         length = len(_x)
01126         if python3 or type(_x) == unicode:
01127           _x = _x.encode('utf-8')
01128           length = len(_x)
01129         buff.write(struct.pack('<I%ss'%length, length, _x))
01130         length = len(val1.values)
01131         buff.write(_struct_I.pack(length))
01132         pattern = '<%sf'%length
01133         buff.write(struct.pack(pattern, *val1.values))
01134     except struct.error as se: self._check_types(se)
01135     except TypeError as te: self._check_types(te)
01136 
01137   def deserialize(self, str):
01138     """
01139     unpack serialized message in str into this message instance
01140     :param str: byte array of serialized message, ``str``
01141     """
01142     try:
01143       if self.model is None:
01144         self.model = articulation_msgs.msg.ModelMsg()
01145       end = 0
01146       _x = self
01147       start = end
01148       end += 12
01149       (_x.model.header.seq, _x.model.header.stamp.secs, _x.model.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
01150       start = end
01151       end += 4
01152       (length,) = _struct_I.unpack(str[start:end])
01153       start = end
01154       end += length
01155       if python3:
01156         self.model.header.frame_id = str[start:end].decode('utf-8')
01157       else:
01158         self.model.header.frame_id = str[start:end]
01159       start = end
01160       end += 4
01161       (self.model.id,) = _struct_i.unpack(str[start:end])
01162       start = end
01163       end += 4
01164       (length,) = _struct_I.unpack(str[start:end])
01165       start = end
01166       end += length
01167       if python3:
01168         self.model.name = str[start:end].decode('utf-8')
01169       else:
01170         self.model.name = str[start:end]
01171       start = end
01172       end += 4
01173       (length,) = _struct_I.unpack(str[start:end])
01174       self.model.params = []
01175       for i in range(0, length):
01176         val1 = articulation_msgs.msg.ParamMsg()
01177         start = end
01178         end += 4
01179         (length,) = _struct_I.unpack(str[start:end])
01180         start = end
01181         end += length
01182         if python3:
01183           val1.name = str[start:end].decode('utf-8')
01184         else:
01185           val1.name = str[start:end]
01186         _x = val1
01187         start = end
01188         end += 9
01189         (_x.value, _x.type,) = _struct_dB.unpack(str[start:end])
01190         self.model.params.append(val1)
01191       _x = self
01192       start = end
01193       end += 12
01194       (_x.model.track.header.seq, _x.model.track.header.stamp.secs, _x.model.track.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
01195       start = end
01196       end += 4
01197       (length,) = _struct_I.unpack(str[start:end])
01198       start = end
01199       end += length
01200       if python3:
01201         self.model.track.header.frame_id = str[start:end].decode('utf-8')
01202       else:
01203         self.model.track.header.frame_id = str[start:end]
01204       start = end
01205       end += 4
01206       (self.model.track.id,) = _struct_i.unpack(str[start:end])
01207       start = end
01208       end += 4
01209       (length,) = _struct_I.unpack(str[start:end])
01210       self.model.track.pose = []
01211       for i in range(0, length):
01212         val1 = geometry_msgs.msg.Pose()
01213         _v36 = val1.position
01214         _x = _v36
01215         start = end
01216         end += 24
01217         (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
01218         _v37 = val1.orientation
01219         _x = _v37
01220         start = end
01221         end += 32
01222         (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
01223         self.model.track.pose.append(val1)
01224       start = end
01225       end += 4
01226       (length,) = _struct_I.unpack(str[start:end])
01227       self.model.track.pose_headers = []
01228       for i in range(0, length):
01229         val1 = std_msgs.msg.Header()
01230         start = end
01231         end += 4
01232         (val1.seq,) = _struct_I.unpack(str[start:end])
01233         _v38 = val1.stamp
01234         _x = _v38
01235         start = end
01236         end += 8
01237         (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
01238         start = end
01239         end += 4
01240         (length,) = _struct_I.unpack(str[start:end])
01241         start = end
01242         end += length
01243         if python3:
01244           val1.frame_id = str[start:end].decode('utf-8')
01245         else:
01246           val1.frame_id = str[start:end]
01247         self.model.track.pose_headers.append(val1)
01248       start = end
01249       end += 4
01250       (length,) = _struct_I.unpack(str[start:end])
01251       self.model.track.pose_projected = []
01252       for i in range(0, length):
01253         val1 = geometry_msgs.msg.Pose()
01254         _v39 = val1.position
01255         _x = _v39
01256         start = end
01257         end += 24
01258         (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
01259         _v40 = val1.orientation
01260         _x = _v40
01261         start = end
01262         end += 32
01263         (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
01264         self.model.track.pose_projected.append(val1)
01265       start = end
01266       end += 4
01267       (length,) = _struct_I.unpack(str[start:end])
01268       self.model.track.pose_resampled = []
01269       for i in range(0, length):
01270         val1 = geometry_msgs.msg.Pose()
01271         _v41 = val1.position
01272         _x = _v41
01273         start = end
01274         end += 24
01275         (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
01276         _v42 = val1.orientation
01277         _x = _v42
01278         start = end
01279         end += 32
01280         (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
01281         self.model.track.pose_resampled.append(val1)
01282       start = end
01283       end += 4
01284       (length,) = _struct_I.unpack(str[start:end])
01285       pattern = '<%sI'%length
01286       start = end
01287       end += struct.calcsize(pattern)
01288       self.model.track.pose_flags = struct.unpack(pattern, str[start:end])
01289       start = end
01290       end += 4
01291       (length,) = _struct_I.unpack(str[start:end])
01292       self.model.track.channels = []
01293       for i in range(0, length):
01294         val1 = sensor_msgs.msg.ChannelFloat32()
01295         start = end
01296         end += 4
01297         (length,) = _struct_I.unpack(str[start:end])
01298         start = end
01299         end += length
01300         if python3:
01301           val1.name = str[start:end].decode('utf-8')
01302         else:
01303           val1.name = str[start:end]
01304         start = end
01305         end += 4
01306         (length,) = _struct_I.unpack(str[start:end])
01307         pattern = '<%sf'%length
01308         start = end
01309         end += struct.calcsize(pattern)
01310         val1.values = struct.unpack(pattern, str[start:end])
01311         self.model.track.channels.append(val1)
01312       return self
01313     except struct.error as e:
01314       raise genpy.DeserializationError(e) #most likely buffer underfill
01315 
01316 
01317   def serialize_numpy(self, buff, numpy):
01318     """
01319     serialize message with numpy array types into buffer
01320     :param buff: buffer, ``StringIO``
01321     :param numpy: numpy python module
01322     """
01323     try:
01324       _x = self
01325       buff.write(_struct_3I.pack(_x.model.header.seq, _x.model.header.stamp.secs, _x.model.header.stamp.nsecs))
01326       _x = self.model.header.frame_id
01327       length = len(_x)
01328       if python3 or type(_x) == unicode:
01329         _x = _x.encode('utf-8')
01330         length = len(_x)
01331       buff.write(struct.pack('<I%ss'%length, length, _x))
01332       buff.write(_struct_i.pack(self.model.id))
01333       _x = self.model.name
01334       length = len(_x)
01335       if python3 or type(_x) == unicode:
01336         _x = _x.encode('utf-8')
01337         length = len(_x)
01338       buff.write(struct.pack('<I%ss'%length, length, _x))
01339       length = len(self.model.params)
01340       buff.write(_struct_I.pack(length))
01341       for val1 in self.model.params:
01342         _x = val1.name
01343         length = len(_x)
01344         if python3 or type(_x) == unicode:
01345           _x = _x.encode('utf-8')
01346           length = len(_x)
01347         buff.write(struct.pack('<I%ss'%length, length, _x))
01348         _x = val1
01349         buff.write(_struct_dB.pack(_x.value, _x.type))
01350       _x = self
01351       buff.write(_struct_3I.pack(_x.model.track.header.seq, _x.model.track.header.stamp.secs, _x.model.track.header.stamp.nsecs))
01352       _x = self.model.track.header.frame_id
01353       length = len(_x)
01354       if python3 or type(_x) == unicode:
01355         _x = _x.encode('utf-8')
01356         length = len(_x)
01357       buff.write(struct.pack('<I%ss'%length, length, _x))
01358       buff.write(_struct_i.pack(self.model.track.id))
01359       length = len(self.model.track.pose)
01360       buff.write(_struct_I.pack(length))
01361       for val1 in self.model.track.pose:
01362         _v43 = val1.position
01363         _x = _v43
01364         buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
01365         _v44 = val1.orientation
01366         _x = _v44
01367         buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
01368       length = len(self.model.track.pose_headers)
01369       buff.write(_struct_I.pack(length))
01370       for val1 in self.model.track.pose_headers:
01371         buff.write(_struct_I.pack(val1.seq))
01372         _v45 = val1.stamp
01373         _x = _v45
01374         buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
01375         _x = val1.frame_id
01376         length = len(_x)
01377         if python3 or type(_x) == unicode:
01378           _x = _x.encode('utf-8')
01379           length = len(_x)
01380         buff.write(struct.pack('<I%ss'%length, length, _x))
01381       length = len(self.model.track.pose_projected)
01382       buff.write(_struct_I.pack(length))
01383       for val1 in self.model.track.pose_projected:
01384         _v46 = val1.position
01385         _x = _v46
01386         buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
01387         _v47 = val1.orientation
01388         _x = _v47
01389         buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
01390       length = len(self.model.track.pose_resampled)
01391       buff.write(_struct_I.pack(length))
01392       for val1 in self.model.track.pose_resampled:
01393         _v48 = val1.position
01394         _x = _v48
01395         buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
01396         _v49 = val1.orientation
01397         _x = _v49
01398         buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
01399       length = len(self.model.track.pose_flags)
01400       buff.write(_struct_I.pack(length))
01401       pattern = '<%sI'%length
01402       buff.write(self.model.track.pose_flags.tostring())
01403       length = len(self.model.track.channels)
01404       buff.write(_struct_I.pack(length))
01405       for val1 in self.model.track.channels:
01406         _x = val1.name
01407         length = len(_x)
01408         if python3 or type(_x) == unicode:
01409           _x = _x.encode('utf-8')
01410           length = len(_x)
01411         buff.write(struct.pack('<I%ss'%length, length, _x))
01412         length = len(val1.values)
01413         buff.write(_struct_I.pack(length))
01414         pattern = '<%sf'%length
01415         buff.write(val1.values.tostring())
01416     except struct.error as se: self._check_types(se)
01417     except TypeError as te: self._check_types(te)
01418 
01419   def deserialize_numpy(self, str, numpy):
01420     """
01421     unpack serialized message in str into this message instance using numpy for array types
01422     :param str: byte array of serialized message, ``str``
01423     :param numpy: numpy python module
01424     """
01425     try:
01426       if self.model is None:
01427         self.model = articulation_msgs.msg.ModelMsg()
01428       end = 0
01429       _x = self
01430       start = end
01431       end += 12
01432       (_x.model.header.seq, _x.model.header.stamp.secs, _x.model.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
01433       start = end
01434       end += 4
01435       (length,) = _struct_I.unpack(str[start:end])
01436       start = end
01437       end += length
01438       if python3:
01439         self.model.header.frame_id = str[start:end].decode('utf-8')
01440       else:
01441         self.model.header.frame_id = str[start:end]
01442       start = end
01443       end += 4
01444       (self.model.id,) = _struct_i.unpack(str[start:end])
01445       start = end
01446       end += 4
01447       (length,) = _struct_I.unpack(str[start:end])
01448       start = end
01449       end += length
01450       if python3:
01451         self.model.name = str[start:end].decode('utf-8')
01452       else:
01453         self.model.name = str[start:end]
01454       start = end
01455       end += 4
01456       (length,) = _struct_I.unpack(str[start:end])
01457       self.model.params = []
01458       for i in range(0, length):
01459         val1 = articulation_msgs.msg.ParamMsg()
01460         start = end
01461         end += 4
01462         (length,) = _struct_I.unpack(str[start:end])
01463         start = end
01464         end += length
01465         if python3:
01466           val1.name = str[start:end].decode('utf-8')
01467         else:
01468           val1.name = str[start:end]
01469         _x = val1
01470         start = end
01471         end += 9
01472         (_x.value, _x.type,) = _struct_dB.unpack(str[start:end])
01473         self.model.params.append(val1)
01474       _x = self
01475       start = end
01476       end += 12
01477       (_x.model.track.header.seq, _x.model.track.header.stamp.secs, _x.model.track.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
01478       start = end
01479       end += 4
01480       (length,) = _struct_I.unpack(str[start:end])
01481       start = end
01482       end += length
01483       if python3:
01484         self.model.track.header.frame_id = str[start:end].decode('utf-8')
01485       else:
01486         self.model.track.header.frame_id = str[start:end]
01487       start = end
01488       end += 4
01489       (self.model.track.id,) = _struct_i.unpack(str[start:end])
01490       start = end
01491       end += 4
01492       (length,) = _struct_I.unpack(str[start:end])
01493       self.model.track.pose = []
01494       for i in range(0, length):
01495         val1 = geometry_msgs.msg.Pose()
01496         _v50 = val1.position
01497         _x = _v50
01498         start = end
01499         end += 24
01500         (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
01501         _v51 = val1.orientation
01502         _x = _v51
01503         start = end
01504         end += 32
01505         (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
01506         self.model.track.pose.append(val1)
01507       start = end
01508       end += 4
01509       (length,) = _struct_I.unpack(str[start:end])
01510       self.model.track.pose_headers = []
01511       for i in range(0, length):
01512         val1 = std_msgs.msg.Header()
01513         start = end
01514         end += 4
01515         (val1.seq,) = _struct_I.unpack(str[start:end])
01516         _v52 = val1.stamp
01517         _x = _v52
01518         start = end
01519         end += 8
01520         (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
01521         start = end
01522         end += 4
01523         (length,) = _struct_I.unpack(str[start:end])
01524         start = end
01525         end += length
01526         if python3:
01527           val1.frame_id = str[start:end].decode('utf-8')
01528         else:
01529           val1.frame_id = str[start:end]
01530         self.model.track.pose_headers.append(val1)
01531       start = end
01532       end += 4
01533       (length,) = _struct_I.unpack(str[start:end])
01534       self.model.track.pose_projected = []
01535       for i in range(0, length):
01536         val1 = geometry_msgs.msg.Pose()
01537         _v53 = val1.position
01538         _x = _v53
01539         start = end
01540         end += 24
01541         (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
01542         _v54 = val1.orientation
01543         _x = _v54
01544         start = end
01545         end += 32
01546         (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
01547         self.model.track.pose_projected.append(val1)
01548       start = end
01549       end += 4
01550       (length,) = _struct_I.unpack(str[start:end])
01551       self.model.track.pose_resampled = []
01552       for i in range(0, length):
01553         val1 = geometry_msgs.msg.Pose()
01554         _v55 = val1.position
01555         _x = _v55
01556         start = end
01557         end += 24
01558         (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
01559         _v56 = val1.orientation
01560         _x = _v56
01561         start = end
01562         end += 32
01563         (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
01564         self.model.track.pose_resampled.append(val1)
01565       start = end
01566       end += 4
01567       (length,) = _struct_I.unpack(str[start:end])
01568       pattern = '<%sI'%length
01569       start = end
01570       end += struct.calcsize(pattern)
01571       self.model.track.pose_flags = numpy.frombuffer(str[start:end], dtype=numpy.uint32, count=length)
01572       start = end
01573       end += 4
01574       (length,) = _struct_I.unpack(str[start:end])
01575       self.model.track.channels = []
01576       for i in range(0, length):
01577         val1 = sensor_msgs.msg.ChannelFloat32()
01578         start = end
01579         end += 4
01580         (length,) = _struct_I.unpack(str[start:end])
01581         start = end
01582         end += length
01583         if python3:
01584           val1.name = str[start:end].decode('utf-8')
01585         else:
01586           val1.name = str[start:end]
01587         start = end
01588         end += 4
01589         (length,) = _struct_I.unpack(str[start:end])
01590         pattern = '<%sf'%length
01591         start = end
01592         end += struct.calcsize(pattern)
01593         val1.values = numpy.frombuffer(str[start:end], dtype=numpy.float32, count=length)
01594         self.model.track.channels.append(val1)
01595       return self
01596     except struct.error as e:
01597       raise genpy.DeserializationError(e) #most likely buffer underfill
01598 
01599 _struct_I = genpy.struct_I
01600 _struct_i = struct.Struct("<i")
01601 _struct_dB = struct.Struct("<dB")
01602 _struct_3I = struct.Struct("<3I")
01603 _struct_4d = struct.Struct("<4d")
01604 _struct_2I = struct.Struct("<2I")
01605 _struct_3d = struct.Struct("<3d")
01606 class TrackModelSrv(object):
01607   _type          = 'articulation_msgs/TrackModelSrv'
01608   _md5sum = 'c9416af83362c7c0e98c9f877c3e7b6b'
01609   _request_class  = TrackModelSrvRequest
01610   _response_class = TrackModelSrvResponse
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Properties Friends


articulation_msgs
Author(s): Juergen Sturm
autogenerated on Wed Dec 26 2012 15:30:49