00001 """autogenerated by genmsg_py from ArticulatedObjectSrvRequest.msg. Do not edit."""
00002 import roslib.message
00003 import struct
00004
00005 import roslib.rostime
00006 import articulation_msgs.msg
00007 import geometry_msgs.msg
00008 import sensor_msgs.msg
00009 import visualization_msgs.msg
00010 import std_msgs.msg
00011
00012 class ArticulatedObjectSrvRequest(roslib.message.Message):
00013 _md5sum = "929d83624d0744de900991740e4bff8e"
00014 _type = "articulation_msgs/ArticulatedObjectSrvRequest"
00015 _has_header = False
00016 _full_text = """
00017
00018
00019
00020
00021
00022
00023
00024
00025 articulation_msgs/ArticulatedObjectMsg object
00026
00027
00028 ================================================================================
00029 MSG: articulation_msgs/ArticulatedObjectMsg
00030 Header header
00031
00032 articulation_msgs/TrackMsg[] parts # observed trajectories for each object part
00033 articulation_msgs/ParamMsg[] params # global parameters
00034 articulation_msgs/ModelMsg[] models # models, describing relationships between parts
00035 visualization_msgs/MarkerArray markers # marker visualization of models/object
00036 ================================================================================
00037 MSG: std_msgs/Header
00038 # Standard metadata for higher-level stamped data types.
00039 # This is generally used to communicate timestamped data
00040 # in a particular coordinate frame.
00041 #
00042 # sequence ID: consecutively increasing ID
00043 uint32 seq
00044 #Two-integer timestamp that is expressed as:
00045 # * stamp.secs: seconds (stamp_secs) since epoch
00046 # * stamp.nsecs: nanoseconds since stamp_secs
00047 # time-handling sugar is provided by the client library
00048 time stamp
00049 #Frame this data is associated with
00050 # 0: no frame
00051 # 1: global frame
00052 string frame_id
00053
00054 ================================================================================
00055 MSG: articulation_msgs/TrackMsg
00056 # Single kinematic trajectory
00057 #
00058 # This message contains a kinematic trajectory. The trajectory is specified
00059 # as a vector of 6D poses. An additional flag, track_type, indicates whether
00060 # the track is valid, and whether it includes orientation. The track id
00061 # can be used for automatic coloring in the RVIZ track plugin, and can be
00062 # freely chosen by the client.
00063 #
00064 # A model is fitting only from the trajectory stored in the pose[]-vector.
00065 # Additional information may be associated to each pose using the channels
00066 # vector, with arbitrary # fields (e.g., to include applied/measured forces).
00067 #
00068 # After model evaluation,
00069 # also the associated configurations of the object are stored in the channels,
00070 # named "q[0]".."q[DOF-1]", where DOF is the number of degrees of freedom.
00071 # Model evaluation also projects the poses in the pose vector onto the model,
00072 # and stores these ideal poses in the vector pose_projected. Further, during model
00073 # evaluation, a new set of (uniform) configurations over the valid configuration
00074 # range is sampled, and the result is stored in pose_resampled.
00075 # The vector pose_flags contains additional display flags for the poses in the
00076 # pose vector, for example, whether a pose is visible and/or
00077 # the end of a trajectory segment. At the moment, this is only used by the
00078 # prior_model_learner.
00079 #
00080
00081 Header header # Timestamp and frame
00082 int32 id # used-specified track id
00083
00084 geometry_msgs/Pose[] pose # sequence of poses, defining the observed trajectory
00085 geometry_msgs/Pose[] pose_projected # sequence of poses, projected to the model
00086 # (after model evaluation)
00087 geometry_msgs/Pose[] pose_resampled # sequence of poses, re-sampled from the model in
00088 # the valid configuration range
00089 uint32[] pose_flags # bit-wise combination of POSE_VISIBLE and POSE_END_OF_SEGMENT
00090
00091 uint32 POSE_VISIBLE=1
00092 uint32 POSE_END_OF_SEGMENT=2
00093
00094 # Each channel should have the same number of elements as pose array,
00095 # and the data in each channel should correspond 1:1 with each pose
00096 # possible channels: "width", "height", "rgb", ...
00097 sensor_msgs/ChannelFloat32[] channels
00098
00099
00100
00101 ================================================================================
00102 MSG: geometry_msgs/Pose
00103 # A representation of pose in free space, composed of postion and orientation.
00104 Point position
00105 Quaternion orientation
00106
00107 ================================================================================
00108 MSG: geometry_msgs/Point
00109 # This contains the position of a point in free space
00110 float64 x
00111 float64 y
00112 float64 z
00113
00114 ================================================================================
00115 MSG: geometry_msgs/Quaternion
00116 # This represents an orientation in free space in quaternion form.
00117
00118 float64 x
00119 float64 y
00120 float64 z
00121 float64 w
00122
00123 ================================================================================
00124 MSG: sensor_msgs/ChannelFloat32
00125 # This message is used by the PointCloud message to hold optional data
00126 # associated with each point in the cloud. The length of the values
00127 # array should be the same as the length of the points array in the
00128 # PointCloud, and each value should be associated with the corresponding
00129 # point.
00130
00131 # Channel names in existing practice include:
00132 # "u", "v" - row and column (respectively) in the left stereo image.
00133 # This is opposite to usual conventions but remains for
00134 # historical reasons. The newer PointCloud2 message has no
00135 # such problem.
00136 # "rgb" - For point clouds produced by color stereo cameras. uint8
00137 # (R,G,B) values packed into the least significant 24 bits,
00138 # in order.
00139 # "intensity" - laser or pixel intensity.
00140 # "distance"
00141
00142 # The channel name should give semantics of the channel (e.g.
00143 # "intensity" instead of "value").
00144 string name
00145
00146 # The values array should be 1-1 with the elements of the associated
00147 # PointCloud.
00148 float32[] values
00149
00150 ================================================================================
00151 MSG: articulation_msgs/ParamMsg
00152 # Single parameter passed to or from model fitting
00153 #
00154 # This mechanism allows to flexibly pass parameters to
00155 # model fitting (and vice versa). Note that these parameters
00156 # are model-specific: A client may supply additional
00157 # parameters to the model estimator, and, similarly, a estimator
00158 # may add the estimated model parameters to the model message.
00159 # When the model is then evaluated, for example to make predictions
00160 # or to compute the likelihood, the model class can then use
00161 # these parameters.
00162 #
00163 # A parameter has a name, a value, and a type. The type globally
00164 # indicates whether it is a prior parameter (prior to model fitting),
00165 # or a model parameter (found during model fitting, using a maximum-
00166 # likelihood estimator), or a cached evaluation (e.g., the likelihood
00167 # or the BIC are a typical "side"-product of model estimation, and
00168 # can therefore already be cached).
00169 #
00170 # For a list of currently used parameters, see the documentation at
00171 # http://www.ros.org/wiki/articulation_models
00172 #
00173
00174 uint8 PRIOR=0 # indicates a prior model parameter
00175 # (e.g., "sigma_position")
00176 uint8 PARAM=1 # indicates a estimated model parameter
00177 # (e.g., "rot_radius", the estimated radius)
00178 uint8 EVAL=2 # indicates a cached evaluation of the model, given
00179 # the current trajectory
00180 # (e.g., "loglikelihood", the log likelihood of the
00181 # data, given the model and its parameters)
00182
00183 string name # name of the parameter
00184 float64 value # value of the parameter
00185 uint8 type # type of the parameter (PRIOR, PARAM, EVAL)
00186
00187
00188 ================================================================================
00189 MSG: articulation_msgs/ModelMsg
00190 # Single kinematic model
00191 #
00192 # A kinematic model is defined by its model class name, and a set of parameters.
00193 # The client may additionally specify a model id, that can be used to colorize the
00194 # model when visualized using the RVIZ model display.
00195 #
00196 # For a list of currently implemented models, see the documetation at
00197 # http://www.ros.org/wiki/articulation_models
00198 #
00199 #
00200
00201 Header header # frame and timestamp
00202
00203 int32 id # user specified model id
00204 string name # name of the model family (e.g. "rotational",
00205 # "prismatic", "pca-gp", "rigid")
00206 articulation_msgs/ParamMsg[] params # model parameters
00207 articulation_msgs/TrackMsg track # trajectory from which the model is estimated, or
00208 # that is evaluated using the model
00209
00210 ================================================================================
00211 MSG: visualization_msgs/MarkerArray
00212 Marker[] markers
00213
00214 ================================================================================
00215 MSG: visualization_msgs/Marker
00216 # See http://www.ros.org/wiki/rviz/DisplayTypes/Marker and http://www.ros.org/wiki/rviz/Tutorials/Markers%3A%20Basic%20Shapes for more information on using this message with rviz
00217
00218 byte ARROW=0
00219 byte CUBE=1
00220 byte SPHERE=2
00221 byte CYLINDER=3
00222 byte LINE_STRIP=4
00223 byte LINE_LIST=5
00224 byte CUBE_LIST=6
00225 byte SPHERE_LIST=7
00226 byte POINTS=8
00227 byte TEXT_VIEW_FACING=9
00228 byte MESH_RESOURCE=10
00229 byte TRIANGLE_LIST=11
00230
00231 byte ADD=0
00232 byte MODIFY=0
00233 byte DELETE=2
00234
00235 Header header # header for time/frame information
00236 string ns # Namespace to place this object in... used in conjunction with id to create a unique name for the object
00237 int32 id # object ID useful in conjunction with the namespace for manipulating and deleting the object later
00238 int32 type # Type of object
00239 int32 action # 0 add/modify an object, 1 (deprecated), 2 deletes an object
00240 geometry_msgs/Pose pose # Pose of the object
00241 geometry_msgs/Vector3 scale # Scale of the object 1,1,1 means default (usually 1 meter square)
00242 std_msgs/ColorRGBA color # Color [0.0-1.0]
00243 duration lifetime # How long the object should last before being automatically deleted. 0 means forever
00244 bool frame_locked # If this marker should be frame-locked, i.e. retransformed into its frame every timestep
00245
00246 #Only used if the type specified has some use for them (eg. POINTS, LINE_STRIP, ...)
00247 geometry_msgs/Point[] points
00248 #Only used if the type specified has some use for them (eg. POINTS, LINE_STRIP, ...)
00249 #number of colors must either be 0 or equal to the number of points
00250 #NOTE: alpha is not yet used
00251 std_msgs/ColorRGBA[] colors
00252
00253 # NOTE: only used for text markers
00254 string text
00255
00256 # NOTE: only used for MESH_RESOURCE markers
00257 string mesh_resource
00258 bool mesh_use_embedded_materials
00259
00260 ================================================================================
00261 MSG: geometry_msgs/Vector3
00262 # This represents a vector in free space.
00263
00264 float64 x
00265 float64 y
00266 float64 z
00267 ================================================================================
00268 MSG: std_msgs/ColorRGBA
00269 float32 r
00270 float32 g
00271 float32 b
00272 float32 a
00273
00274 """
00275 __slots__ = ['object']
00276 _slot_types = ['articulation_msgs/ArticulatedObjectMsg']
00277
00278 def __init__(self, *args, **kwds):
00279 """
00280 Constructor. Any message fields that are implicitly/explicitly
00281 set to None will be assigned a default value. The recommend
00282 use is keyword arguments as this is more robust to future message
00283 changes. You cannot mix in-order arguments and keyword arguments.
00284
00285 The available fields are:
00286 object
00287
00288 @param args: complete set of field values, in .msg order
00289 @param kwds: use keyword arguments corresponding to message field names
00290 to set specific fields.
00291 """
00292 if args or kwds:
00293 super(ArticulatedObjectSrvRequest, self).__init__(*args, **kwds)
00294 #message fields cannot be None, assign default values for those that are
00295 if self.object is None:
00296 self.object = articulation_msgs.msg.ArticulatedObjectMsg()
00297 else:
00298 self.object = articulation_msgs.msg.ArticulatedObjectMsg()
00299
00300 def _get_types(self):
00301 """
00302 internal API method
00303 """
00304 return self._slot_types
00305
00306 def serialize(self, buff):
00307 """
00308 serialize message into buffer
00309 @param buff: buffer
00310 @type buff: StringIO
00311 """
00312 try:
00313 _x = self
00314 buff.write(_struct_3I.pack(_x.object.header.seq, _x.object.header.stamp.secs, _x.object.header.stamp.nsecs))
00315 _x = self.object.header.frame_id
00316 length = len(_x)
00317 buff.write(struct.pack('<I%ss'%length, length, _x))
00318 length = len(self.object.parts)
00319 buff.write(_struct_I.pack(length))
00320 for val1 in self.object.parts:
00321 _v1 = val1.header
00322 buff.write(_struct_I.pack(_v1.seq))
00323 _v2 = _v1.stamp
00324 _x = _v2
00325 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
00326 _x = _v1.frame_id
00327 length = len(_x)
00328 buff.write(struct.pack('<I%ss'%length, length, _x))
00329 buff.write(_struct_i.pack(val1.id))
00330 length = len(val1.pose)
00331 buff.write(_struct_I.pack(length))
00332 for val2 in val1.pose:
00333 _v3 = val2.position
00334 _x = _v3
00335 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00336 _v4 = val2.orientation
00337 _x = _v4
00338 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
00339 length = len(val1.pose_projected)
00340 buff.write(_struct_I.pack(length))
00341 for val2 in val1.pose_projected:
00342 _v5 = val2.position
00343 _x = _v5
00344 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00345 _v6 = val2.orientation
00346 _x = _v6
00347 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
00348 length = len(val1.pose_resampled)
00349 buff.write(_struct_I.pack(length))
00350 for val2 in val1.pose_resampled:
00351 _v7 = val2.position
00352 _x = _v7
00353 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00354 _v8 = val2.orientation
00355 _x = _v8
00356 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
00357 length = len(val1.pose_flags)
00358 buff.write(_struct_I.pack(length))
00359 pattern = '<%sI'%length
00360 buff.write(struct.pack(pattern, *val1.pose_flags))
00361 length = len(val1.channels)
00362 buff.write(_struct_I.pack(length))
00363 for val2 in val1.channels:
00364 _x = val2.name
00365 length = len(_x)
00366 buff.write(struct.pack('<I%ss'%length, length, _x))
00367 length = len(val2.values)
00368 buff.write(_struct_I.pack(length))
00369 pattern = '<%sf'%length
00370 buff.write(struct.pack(pattern, *val2.values))
00371 length = len(self.object.params)
00372 buff.write(_struct_I.pack(length))
00373 for val1 in self.object.params:
00374 _x = val1.name
00375 length = len(_x)
00376 buff.write(struct.pack('<I%ss'%length, length, _x))
00377 _x = val1
00378 buff.write(_struct_dB.pack(_x.value, _x.type))
00379 length = len(self.object.models)
00380 buff.write(_struct_I.pack(length))
00381 for val1 in self.object.models:
00382 _v9 = val1.header
00383 buff.write(_struct_I.pack(_v9.seq))
00384 _v10 = _v9.stamp
00385 _x = _v10
00386 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
00387 _x = _v9.frame_id
00388 length = len(_x)
00389 buff.write(struct.pack('<I%ss'%length, length, _x))
00390 buff.write(_struct_i.pack(val1.id))
00391 _x = val1.name
00392 length = len(_x)
00393 buff.write(struct.pack('<I%ss'%length, length, _x))
00394 length = len(val1.params)
00395 buff.write(_struct_I.pack(length))
00396 for val2 in val1.params:
00397 _x = val2.name
00398 length = len(_x)
00399 buff.write(struct.pack('<I%ss'%length, length, _x))
00400 _x = val2
00401 buff.write(_struct_dB.pack(_x.value, _x.type))
00402 _v11 = val1.track
00403 _v12 = _v11.header
00404 buff.write(_struct_I.pack(_v12.seq))
00405 _v13 = _v12.stamp
00406 _x = _v13
00407 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
00408 _x = _v12.frame_id
00409 length = len(_x)
00410 buff.write(struct.pack('<I%ss'%length, length, _x))
00411 buff.write(_struct_i.pack(_v11.id))
00412 length = len(_v11.pose)
00413 buff.write(_struct_I.pack(length))
00414 for val3 in _v11.pose:
00415 _v14 = val3.position
00416 _x = _v14
00417 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00418 _v15 = val3.orientation
00419 _x = _v15
00420 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
00421 length = len(_v11.pose_projected)
00422 buff.write(_struct_I.pack(length))
00423 for val3 in _v11.pose_projected:
00424 _v16 = val3.position
00425 _x = _v16
00426 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00427 _v17 = val3.orientation
00428 _x = _v17
00429 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
00430 length = len(_v11.pose_resampled)
00431 buff.write(_struct_I.pack(length))
00432 for val3 in _v11.pose_resampled:
00433 _v18 = val3.position
00434 _x = _v18
00435 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00436 _v19 = val3.orientation
00437 _x = _v19
00438 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
00439 length = len(_v11.pose_flags)
00440 buff.write(_struct_I.pack(length))
00441 pattern = '<%sI'%length
00442 buff.write(struct.pack(pattern, *_v11.pose_flags))
00443 length = len(_v11.channels)
00444 buff.write(_struct_I.pack(length))
00445 for val3 in _v11.channels:
00446 _x = val3.name
00447 length = len(_x)
00448 buff.write(struct.pack('<I%ss'%length, length, _x))
00449 length = len(val3.values)
00450 buff.write(_struct_I.pack(length))
00451 pattern = '<%sf'%length
00452 buff.write(struct.pack(pattern, *val3.values))
00453 length = len(self.object.markers.markers)
00454 buff.write(_struct_I.pack(length))
00455 for val1 in self.object.markers.markers:
00456 _v20 = val1.header
00457 buff.write(_struct_I.pack(_v20.seq))
00458 _v21 = _v20.stamp
00459 _x = _v21
00460 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
00461 _x = _v20.frame_id
00462 length = len(_x)
00463 buff.write(struct.pack('<I%ss'%length, length, _x))
00464 _x = val1.ns
00465 length = len(_x)
00466 buff.write(struct.pack('<I%ss'%length, length, _x))
00467 _x = val1
00468 buff.write(_struct_3i.pack(_x.id, _x.type, _x.action))
00469 _v22 = val1.pose
00470 _v23 = _v22.position
00471 _x = _v23
00472 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00473 _v24 = _v22.orientation
00474 _x = _v24
00475 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
00476 _v25 = val1.scale
00477 _x = _v25
00478 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00479 _v26 = val1.color
00480 _x = _v26
00481 buff.write(_struct_4f.pack(_x.r, _x.g, _x.b, _x.a))
00482 _v27 = val1.lifetime
00483 _x = _v27
00484 buff.write(_struct_2i.pack(_x.secs, _x.nsecs))
00485 buff.write(_struct_B.pack(val1.frame_locked))
00486 length = len(val1.points)
00487 buff.write(_struct_I.pack(length))
00488 for val2 in val1.points:
00489 _x = val2
00490 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00491 length = len(val1.colors)
00492 buff.write(_struct_I.pack(length))
00493 for val2 in val1.colors:
00494 _x = val2
00495 buff.write(_struct_4f.pack(_x.r, _x.g, _x.b, _x.a))
00496 _x = val1.text
00497 length = len(_x)
00498 buff.write(struct.pack('<I%ss'%length, length, _x))
00499 _x = val1.mesh_resource
00500 length = len(_x)
00501 buff.write(struct.pack('<I%ss'%length, length, _x))
00502 buff.write(_struct_B.pack(val1.mesh_use_embedded_materials))
00503 except struct.error, se: self._check_types(se)
00504 except TypeError, te: self._check_types(te)
00505
00506 def deserialize(self, str):
00507 """
00508 unpack serialized message in str into this message instance
00509 @param str: byte array of serialized message
00510 @type str: str
00511 """
00512 try:
00513 if self.object is None:
00514 self.object = articulation_msgs.msg.ArticulatedObjectMsg()
00515 end = 0
00516 _x = self
00517 start = end
00518 end += 12
00519 (_x.object.header.seq, _x.object.header.stamp.secs, _x.object.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
00520 start = end
00521 end += 4
00522 (length,) = _struct_I.unpack(str[start:end])
00523 start = end
00524 end += length
00525 self.object.header.frame_id = str[start:end]
00526 start = end
00527 end += 4
00528 (length,) = _struct_I.unpack(str[start:end])
00529 self.object.parts = []
00530 for i in xrange(0, length):
00531 val1 = articulation_msgs.msg.TrackMsg()
00532 _v28 = val1.header
00533 start = end
00534 end += 4
00535 (_v28.seq,) = _struct_I.unpack(str[start:end])
00536 _v29 = _v28.stamp
00537 _x = _v29
00538 start = end
00539 end += 8
00540 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
00541 start = end
00542 end += 4
00543 (length,) = _struct_I.unpack(str[start:end])
00544 start = end
00545 end += length
00546 _v28.frame_id = str[start:end]
00547 start = end
00548 end += 4
00549 (val1.id,) = _struct_i.unpack(str[start:end])
00550 start = end
00551 end += 4
00552 (length,) = _struct_I.unpack(str[start:end])
00553 val1.pose = []
00554 for i in xrange(0, length):
00555 val2 = geometry_msgs.msg.Pose()
00556 _v30 = val2.position
00557 _x = _v30
00558 start = end
00559 end += 24
00560 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
00561 _v31 = val2.orientation
00562 _x = _v31
00563 start = end
00564 end += 32
00565 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
00566 val1.pose.append(val2)
00567 start = end
00568 end += 4
00569 (length,) = _struct_I.unpack(str[start:end])
00570 val1.pose_projected = []
00571 for i in xrange(0, length):
00572 val2 = geometry_msgs.msg.Pose()
00573 _v32 = val2.position
00574 _x = _v32
00575 start = end
00576 end += 24
00577 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
00578 _v33 = val2.orientation
00579 _x = _v33
00580 start = end
00581 end += 32
00582 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
00583 val1.pose_projected.append(val2)
00584 start = end
00585 end += 4
00586 (length,) = _struct_I.unpack(str[start:end])
00587 val1.pose_resampled = []
00588 for i in xrange(0, length):
00589 val2 = geometry_msgs.msg.Pose()
00590 _v34 = val2.position
00591 _x = _v34
00592 start = end
00593 end += 24
00594 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
00595 _v35 = val2.orientation
00596 _x = _v35
00597 start = end
00598 end += 32
00599 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
00600 val1.pose_resampled.append(val2)
00601 start = end
00602 end += 4
00603 (length,) = _struct_I.unpack(str[start:end])
00604 pattern = '<%sI'%length
00605 start = end
00606 end += struct.calcsize(pattern)
00607 val1.pose_flags = struct.unpack(pattern, str[start:end])
00608 start = end
00609 end += 4
00610 (length,) = _struct_I.unpack(str[start:end])
00611 val1.channels = []
00612 for i in xrange(0, length):
00613 val2 = sensor_msgs.msg.ChannelFloat32()
00614 start = end
00615 end += 4
00616 (length,) = _struct_I.unpack(str[start:end])
00617 start = end
00618 end += length
00619 val2.name = str[start:end]
00620 start = end
00621 end += 4
00622 (length,) = _struct_I.unpack(str[start:end])
00623 pattern = '<%sf'%length
00624 start = end
00625 end += struct.calcsize(pattern)
00626 val2.values = struct.unpack(pattern, str[start:end])
00627 val1.channels.append(val2)
00628 self.object.parts.append(val1)
00629 start = end
00630 end += 4
00631 (length,) = _struct_I.unpack(str[start:end])
00632 self.object.params = []
00633 for i in xrange(0, length):
00634 val1 = articulation_msgs.msg.ParamMsg()
00635 start = end
00636 end += 4
00637 (length,) = _struct_I.unpack(str[start:end])
00638 start = end
00639 end += length
00640 val1.name = str[start:end]
00641 _x = val1
00642 start = end
00643 end += 9
00644 (_x.value, _x.type,) = _struct_dB.unpack(str[start:end])
00645 self.object.params.append(val1)
00646 start = end
00647 end += 4
00648 (length,) = _struct_I.unpack(str[start:end])
00649 self.object.models = []
00650 for i in xrange(0, length):
00651 val1 = articulation_msgs.msg.ModelMsg()
00652 _v36 = val1.header
00653 start = end
00654 end += 4
00655 (_v36.seq,) = _struct_I.unpack(str[start:end])
00656 _v37 = _v36.stamp
00657 _x = _v37
00658 start = end
00659 end += 8
00660 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
00661 start = end
00662 end += 4
00663 (length,) = _struct_I.unpack(str[start:end])
00664 start = end
00665 end += length
00666 _v36.frame_id = str[start:end]
00667 start = end
00668 end += 4
00669 (val1.id,) = _struct_i.unpack(str[start:end])
00670 start = end
00671 end += 4
00672 (length,) = _struct_I.unpack(str[start:end])
00673 start = end
00674 end += length
00675 val1.name = str[start:end]
00676 start = end
00677 end += 4
00678 (length,) = _struct_I.unpack(str[start:end])
00679 val1.params = []
00680 for i in xrange(0, length):
00681 val2 = articulation_msgs.msg.ParamMsg()
00682 start = end
00683 end += 4
00684 (length,) = _struct_I.unpack(str[start:end])
00685 start = end
00686 end += length
00687 val2.name = str[start:end]
00688 _x = val2
00689 start = end
00690 end += 9
00691 (_x.value, _x.type,) = _struct_dB.unpack(str[start:end])
00692 val1.params.append(val2)
00693 _v38 = val1.track
00694 _v39 = _v38.header
00695 start = end
00696 end += 4
00697 (_v39.seq,) = _struct_I.unpack(str[start:end])
00698 _v40 = _v39.stamp
00699 _x = _v40
00700 start = end
00701 end += 8
00702 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
00703 start = end
00704 end += 4
00705 (length,) = _struct_I.unpack(str[start:end])
00706 start = end
00707 end += length
00708 _v39.frame_id = str[start:end]
00709 start = end
00710 end += 4
00711 (_v38.id,) = _struct_i.unpack(str[start:end])
00712 start = end
00713 end += 4
00714 (length,) = _struct_I.unpack(str[start:end])
00715 _v38.pose = []
00716 for i in xrange(0, length):
00717 val3 = geometry_msgs.msg.Pose()
00718 _v41 = val3.position
00719 _x = _v41
00720 start = end
00721 end += 24
00722 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
00723 _v42 = val3.orientation
00724 _x = _v42
00725 start = end
00726 end += 32
00727 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
00728 _v38.pose.append(val3)
00729 start = end
00730 end += 4
00731 (length,) = _struct_I.unpack(str[start:end])
00732 _v38.pose_projected = []
00733 for i in xrange(0, length):
00734 val3 = geometry_msgs.msg.Pose()
00735 _v43 = val3.position
00736 _x = _v43
00737 start = end
00738 end += 24
00739 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
00740 _v44 = val3.orientation
00741 _x = _v44
00742 start = end
00743 end += 32
00744 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
00745 _v38.pose_projected.append(val3)
00746 start = end
00747 end += 4
00748 (length,) = _struct_I.unpack(str[start:end])
00749 _v38.pose_resampled = []
00750 for i in xrange(0, length):
00751 val3 = geometry_msgs.msg.Pose()
00752 _v45 = val3.position
00753 _x = _v45
00754 start = end
00755 end += 24
00756 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
00757 _v46 = val3.orientation
00758 _x = _v46
00759 start = end
00760 end += 32
00761 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
00762 _v38.pose_resampled.append(val3)
00763 start = end
00764 end += 4
00765 (length,) = _struct_I.unpack(str[start:end])
00766 pattern = '<%sI'%length
00767 start = end
00768 end += struct.calcsize(pattern)
00769 _v38.pose_flags = struct.unpack(pattern, str[start:end])
00770 start = end
00771 end += 4
00772 (length,) = _struct_I.unpack(str[start:end])
00773 _v38.channels = []
00774 for i in xrange(0, length):
00775 val3 = sensor_msgs.msg.ChannelFloat32()
00776 start = end
00777 end += 4
00778 (length,) = _struct_I.unpack(str[start:end])
00779 start = end
00780 end += length
00781 val3.name = str[start:end]
00782 start = end
00783 end += 4
00784 (length,) = _struct_I.unpack(str[start:end])
00785 pattern = '<%sf'%length
00786 start = end
00787 end += struct.calcsize(pattern)
00788 val3.values = struct.unpack(pattern, str[start:end])
00789 _v38.channels.append(val3)
00790 self.object.models.append(val1)
00791 start = end
00792 end += 4
00793 (length,) = _struct_I.unpack(str[start:end])
00794 self.object.markers.markers = []
00795 for i in xrange(0, length):
00796 val1 = visualization_msgs.msg.Marker()
00797 _v47 = val1.header
00798 start = end
00799 end += 4
00800 (_v47.seq,) = _struct_I.unpack(str[start:end])
00801 _v48 = _v47.stamp
00802 _x = _v48
00803 start = end
00804 end += 8
00805 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
00806 start = end
00807 end += 4
00808 (length,) = _struct_I.unpack(str[start:end])
00809 start = end
00810 end += length
00811 _v47.frame_id = str[start:end]
00812 start = end
00813 end += 4
00814 (length,) = _struct_I.unpack(str[start:end])
00815 start = end
00816 end += length
00817 val1.ns = str[start:end]
00818 _x = val1
00819 start = end
00820 end += 12
00821 (_x.id, _x.type, _x.action,) = _struct_3i.unpack(str[start:end])
00822 _v49 = val1.pose
00823 _v50 = _v49.position
00824 _x = _v50
00825 start = end
00826 end += 24
00827 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
00828 _v51 = _v49.orientation
00829 _x = _v51
00830 start = end
00831 end += 32
00832 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
00833 _v52 = val1.scale
00834 _x = _v52
00835 start = end
00836 end += 24
00837 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
00838 _v53 = val1.color
00839 _x = _v53
00840 start = end
00841 end += 16
00842 (_x.r, _x.g, _x.b, _x.a,) = _struct_4f.unpack(str[start:end])
00843 _v54 = val1.lifetime
00844 _x = _v54
00845 start = end
00846 end += 8
00847 (_x.secs, _x.nsecs,) = _struct_2i.unpack(str[start:end])
00848 start = end
00849 end += 1
00850 (val1.frame_locked,) = _struct_B.unpack(str[start:end])
00851 val1.frame_locked = bool(val1.frame_locked)
00852 start = end
00853 end += 4
00854 (length,) = _struct_I.unpack(str[start:end])
00855 val1.points = []
00856 for i in xrange(0, length):
00857 val2 = geometry_msgs.msg.Point()
00858 _x = val2
00859 start = end
00860 end += 24
00861 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
00862 val1.points.append(val2)
00863 start = end
00864 end += 4
00865 (length,) = _struct_I.unpack(str[start:end])
00866 val1.colors = []
00867 for i in xrange(0, length):
00868 val2 = std_msgs.msg.ColorRGBA()
00869 _x = val2
00870 start = end
00871 end += 16
00872 (_x.r, _x.g, _x.b, _x.a,) = _struct_4f.unpack(str[start:end])
00873 val1.colors.append(val2)
00874 start = end
00875 end += 4
00876 (length,) = _struct_I.unpack(str[start:end])
00877 start = end
00878 end += length
00879 val1.text = str[start:end]
00880 start = end
00881 end += 4
00882 (length,) = _struct_I.unpack(str[start:end])
00883 start = end
00884 end += length
00885 val1.mesh_resource = str[start:end]
00886 start = end
00887 end += 1
00888 (val1.mesh_use_embedded_materials,) = _struct_B.unpack(str[start:end])
00889 val1.mesh_use_embedded_materials = bool(val1.mesh_use_embedded_materials)
00890 self.object.markers.markers.append(val1)
00891 return self
00892 except struct.error, e:
00893 raise roslib.message.DeserializationError(e) #most likely buffer underfill
00894
00895
00896 def serialize_numpy(self, buff, numpy):
00897 """
00898 serialize message with numpy array types into buffer
00899 @param buff: buffer
00900 @type buff: StringIO
00901 @param numpy: numpy python module
00902 @type numpy module
00903 """
00904 try:
00905 _x = self
00906 buff.write(_struct_3I.pack(_x.object.header.seq, _x.object.header.stamp.secs, _x.object.header.stamp.nsecs))
00907 _x = self.object.header.frame_id
00908 length = len(_x)
00909 buff.write(struct.pack('<I%ss'%length, length, _x))
00910 length = len(self.object.parts)
00911 buff.write(_struct_I.pack(length))
00912 for val1 in self.object.parts:
00913 _v55 = val1.header
00914 buff.write(_struct_I.pack(_v55.seq))
00915 _v56 = _v55.stamp
00916 _x = _v56
00917 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
00918 _x = _v55.frame_id
00919 length = len(_x)
00920 buff.write(struct.pack('<I%ss'%length, length, _x))
00921 buff.write(_struct_i.pack(val1.id))
00922 length = len(val1.pose)
00923 buff.write(_struct_I.pack(length))
00924 for val2 in val1.pose:
00925 _v57 = val2.position
00926 _x = _v57
00927 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00928 _v58 = val2.orientation
00929 _x = _v58
00930 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
00931 length = len(val1.pose_projected)
00932 buff.write(_struct_I.pack(length))
00933 for val2 in val1.pose_projected:
00934 _v59 = val2.position
00935 _x = _v59
00936 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00937 _v60 = val2.orientation
00938 _x = _v60
00939 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
00940 length = len(val1.pose_resampled)
00941 buff.write(_struct_I.pack(length))
00942 for val2 in val1.pose_resampled:
00943 _v61 = val2.position
00944 _x = _v61
00945 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00946 _v62 = val2.orientation
00947 _x = _v62
00948 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
00949 length = len(val1.pose_flags)
00950 buff.write(_struct_I.pack(length))
00951 pattern = '<%sI'%length
00952 buff.write(val1.pose_flags.tostring())
00953 length = len(val1.channels)
00954 buff.write(_struct_I.pack(length))
00955 for val2 in val1.channels:
00956 _x = val2.name
00957 length = len(_x)
00958 buff.write(struct.pack('<I%ss'%length, length, _x))
00959 length = len(val2.values)
00960 buff.write(_struct_I.pack(length))
00961 pattern = '<%sf'%length
00962 buff.write(val2.values.tostring())
00963 length = len(self.object.params)
00964 buff.write(_struct_I.pack(length))
00965 for val1 in self.object.params:
00966 _x = val1.name
00967 length = len(_x)
00968 buff.write(struct.pack('<I%ss'%length, length, _x))
00969 _x = val1
00970 buff.write(_struct_dB.pack(_x.value, _x.type))
00971 length = len(self.object.models)
00972 buff.write(_struct_I.pack(length))
00973 for val1 in self.object.models:
00974 _v63 = val1.header
00975 buff.write(_struct_I.pack(_v63.seq))
00976 _v64 = _v63.stamp
00977 _x = _v64
00978 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
00979 _x = _v63.frame_id
00980 length = len(_x)
00981 buff.write(struct.pack('<I%ss'%length, length, _x))
00982 buff.write(_struct_i.pack(val1.id))
00983 _x = val1.name
00984 length = len(_x)
00985 buff.write(struct.pack('<I%ss'%length, length, _x))
00986 length = len(val1.params)
00987 buff.write(_struct_I.pack(length))
00988 for val2 in val1.params:
00989 _x = val2.name
00990 length = len(_x)
00991 buff.write(struct.pack('<I%ss'%length, length, _x))
00992 _x = val2
00993 buff.write(_struct_dB.pack(_x.value, _x.type))
00994 _v65 = val1.track
00995 _v66 = _v65.header
00996 buff.write(_struct_I.pack(_v66.seq))
00997 _v67 = _v66.stamp
00998 _x = _v67
00999 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
01000 _x = _v66.frame_id
01001 length = len(_x)
01002 buff.write(struct.pack('<I%ss'%length, length, _x))
01003 buff.write(_struct_i.pack(_v65.id))
01004 length = len(_v65.pose)
01005 buff.write(_struct_I.pack(length))
01006 for val3 in _v65.pose:
01007 _v68 = val3.position
01008 _x = _v68
01009 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
01010 _v69 = val3.orientation
01011 _x = _v69
01012 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
01013 length = len(_v65.pose_projected)
01014 buff.write(_struct_I.pack(length))
01015 for val3 in _v65.pose_projected:
01016 _v70 = val3.position
01017 _x = _v70
01018 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
01019 _v71 = val3.orientation
01020 _x = _v71
01021 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
01022 length = len(_v65.pose_resampled)
01023 buff.write(_struct_I.pack(length))
01024 for val3 in _v65.pose_resampled:
01025 _v72 = val3.position
01026 _x = _v72
01027 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
01028 _v73 = val3.orientation
01029 _x = _v73
01030 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
01031 length = len(_v65.pose_flags)
01032 buff.write(_struct_I.pack(length))
01033 pattern = '<%sI'%length
01034 buff.write(_v65.pose_flags.tostring())
01035 length = len(_v65.channels)
01036 buff.write(_struct_I.pack(length))
01037 for val3 in _v65.channels:
01038 _x = val3.name
01039 length = len(_x)
01040 buff.write(struct.pack('<I%ss'%length, length, _x))
01041 length = len(val3.values)
01042 buff.write(_struct_I.pack(length))
01043 pattern = '<%sf'%length
01044 buff.write(val3.values.tostring())
01045 length = len(self.object.markers.markers)
01046 buff.write(_struct_I.pack(length))
01047 for val1 in self.object.markers.markers:
01048 _v74 = val1.header
01049 buff.write(_struct_I.pack(_v74.seq))
01050 _v75 = _v74.stamp
01051 _x = _v75
01052 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
01053 _x = _v74.frame_id
01054 length = len(_x)
01055 buff.write(struct.pack('<I%ss'%length, length, _x))
01056 _x = val1.ns
01057 length = len(_x)
01058 buff.write(struct.pack('<I%ss'%length, length, _x))
01059 _x = val1
01060 buff.write(_struct_3i.pack(_x.id, _x.type, _x.action))
01061 _v76 = val1.pose
01062 _v77 = _v76.position
01063 _x = _v77
01064 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
01065 _v78 = _v76.orientation
01066 _x = _v78
01067 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
01068 _v79 = val1.scale
01069 _x = _v79
01070 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
01071 _v80 = val1.color
01072 _x = _v80
01073 buff.write(_struct_4f.pack(_x.r, _x.g, _x.b, _x.a))
01074 _v81 = val1.lifetime
01075 _x = _v81
01076 buff.write(_struct_2i.pack(_x.secs, _x.nsecs))
01077 buff.write(_struct_B.pack(val1.frame_locked))
01078 length = len(val1.points)
01079 buff.write(_struct_I.pack(length))
01080 for val2 in val1.points:
01081 _x = val2
01082 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
01083 length = len(val1.colors)
01084 buff.write(_struct_I.pack(length))
01085 for val2 in val1.colors:
01086 _x = val2
01087 buff.write(_struct_4f.pack(_x.r, _x.g, _x.b, _x.a))
01088 _x = val1.text
01089 length = len(_x)
01090 buff.write(struct.pack('<I%ss'%length, length, _x))
01091 _x = val1.mesh_resource
01092 length = len(_x)
01093 buff.write(struct.pack('<I%ss'%length, length, _x))
01094 buff.write(_struct_B.pack(val1.mesh_use_embedded_materials))
01095 except struct.error, se: self._check_types(se)
01096 except TypeError, te: self._check_types(te)
01097
01098 def deserialize_numpy(self, str, numpy):
01099 """
01100 unpack serialized message in str into this message instance using numpy for array types
01101 @param str: byte array of serialized message
01102 @type str: str
01103 @param numpy: numpy python module
01104 @type numpy: module
01105 """
01106 try:
01107 if self.object is None:
01108 self.object = articulation_msgs.msg.ArticulatedObjectMsg()
01109 end = 0
01110 _x = self
01111 start = end
01112 end += 12
01113 (_x.object.header.seq, _x.object.header.stamp.secs, _x.object.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
01114 start = end
01115 end += 4
01116 (length,) = _struct_I.unpack(str[start:end])
01117 start = end
01118 end += length
01119 self.object.header.frame_id = str[start:end]
01120 start = end
01121 end += 4
01122 (length,) = _struct_I.unpack(str[start:end])
01123 self.object.parts = []
01124 for i in xrange(0, length):
01125 val1 = articulation_msgs.msg.TrackMsg()
01126 _v82 = val1.header
01127 start = end
01128 end += 4
01129 (_v82.seq,) = _struct_I.unpack(str[start:end])
01130 _v83 = _v82.stamp
01131 _x = _v83
01132 start = end
01133 end += 8
01134 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
01135 start = end
01136 end += 4
01137 (length,) = _struct_I.unpack(str[start:end])
01138 start = end
01139 end += length
01140 _v82.frame_id = str[start:end]
01141 start = end
01142 end += 4
01143 (val1.id,) = _struct_i.unpack(str[start:end])
01144 start = end
01145 end += 4
01146 (length,) = _struct_I.unpack(str[start:end])
01147 val1.pose = []
01148 for i in xrange(0, length):
01149 val2 = geometry_msgs.msg.Pose()
01150 _v84 = val2.position
01151 _x = _v84
01152 start = end
01153 end += 24
01154 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
01155 _v85 = val2.orientation
01156 _x = _v85
01157 start = end
01158 end += 32
01159 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
01160 val1.pose.append(val2)
01161 start = end
01162 end += 4
01163 (length,) = _struct_I.unpack(str[start:end])
01164 val1.pose_projected = []
01165 for i in xrange(0, length):
01166 val2 = geometry_msgs.msg.Pose()
01167 _v86 = val2.position
01168 _x = _v86
01169 start = end
01170 end += 24
01171 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
01172 _v87 = val2.orientation
01173 _x = _v87
01174 start = end
01175 end += 32
01176 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
01177 val1.pose_projected.append(val2)
01178 start = end
01179 end += 4
01180 (length,) = _struct_I.unpack(str[start:end])
01181 val1.pose_resampled = []
01182 for i in xrange(0, length):
01183 val2 = geometry_msgs.msg.Pose()
01184 _v88 = val2.position
01185 _x = _v88
01186 start = end
01187 end += 24
01188 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
01189 _v89 = val2.orientation
01190 _x = _v89
01191 start = end
01192 end += 32
01193 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
01194 val1.pose_resampled.append(val2)
01195 start = end
01196 end += 4
01197 (length,) = _struct_I.unpack(str[start:end])
01198 pattern = '<%sI'%length
01199 start = end
01200 end += struct.calcsize(pattern)
01201 val1.pose_flags = numpy.frombuffer(str[start:end], dtype=numpy.uint32, count=length)
01202 start = end
01203 end += 4
01204 (length,) = _struct_I.unpack(str[start:end])
01205 val1.channels = []
01206 for i in xrange(0, length):
01207 val2 = sensor_msgs.msg.ChannelFloat32()
01208 start = end
01209 end += 4
01210 (length,) = _struct_I.unpack(str[start:end])
01211 start = end
01212 end += length
01213 val2.name = str[start:end]
01214 start = end
01215 end += 4
01216 (length,) = _struct_I.unpack(str[start:end])
01217 pattern = '<%sf'%length
01218 start = end
01219 end += struct.calcsize(pattern)
01220 val2.values = numpy.frombuffer(str[start:end], dtype=numpy.float32, count=length)
01221 val1.channels.append(val2)
01222 self.object.parts.append(val1)
01223 start = end
01224 end += 4
01225 (length,) = _struct_I.unpack(str[start:end])
01226 self.object.params = []
01227 for i in xrange(0, length):
01228 val1 = articulation_msgs.msg.ParamMsg()
01229 start = end
01230 end += 4
01231 (length,) = _struct_I.unpack(str[start:end])
01232 start = end
01233 end += length
01234 val1.name = str[start:end]
01235 _x = val1
01236 start = end
01237 end += 9
01238 (_x.value, _x.type,) = _struct_dB.unpack(str[start:end])
01239 self.object.params.append(val1)
01240 start = end
01241 end += 4
01242 (length,) = _struct_I.unpack(str[start:end])
01243 self.object.models = []
01244 for i in xrange(0, length):
01245 val1 = articulation_msgs.msg.ModelMsg()
01246 _v90 = val1.header
01247 start = end
01248 end += 4
01249 (_v90.seq,) = _struct_I.unpack(str[start:end])
01250 _v91 = _v90.stamp
01251 _x = _v91
01252 start = end
01253 end += 8
01254 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
01255 start = end
01256 end += 4
01257 (length,) = _struct_I.unpack(str[start:end])
01258 start = end
01259 end += length
01260 _v90.frame_id = str[start:end]
01261 start = end
01262 end += 4
01263 (val1.id,) = _struct_i.unpack(str[start:end])
01264 start = end
01265 end += 4
01266 (length,) = _struct_I.unpack(str[start:end])
01267 start = end
01268 end += length
01269 val1.name = str[start:end]
01270 start = end
01271 end += 4
01272 (length,) = _struct_I.unpack(str[start:end])
01273 val1.params = []
01274 for i in xrange(0, length):
01275 val2 = articulation_msgs.msg.ParamMsg()
01276 start = end
01277 end += 4
01278 (length,) = _struct_I.unpack(str[start:end])
01279 start = end
01280 end += length
01281 val2.name = str[start:end]
01282 _x = val2
01283 start = end
01284 end += 9
01285 (_x.value, _x.type,) = _struct_dB.unpack(str[start:end])
01286 val1.params.append(val2)
01287 _v92 = val1.track
01288 _v93 = _v92.header
01289 start = end
01290 end += 4
01291 (_v93.seq,) = _struct_I.unpack(str[start:end])
01292 _v94 = _v93.stamp
01293 _x = _v94
01294 start = end
01295 end += 8
01296 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
01297 start = end
01298 end += 4
01299 (length,) = _struct_I.unpack(str[start:end])
01300 start = end
01301 end += length
01302 _v93.frame_id = str[start:end]
01303 start = end
01304 end += 4
01305 (_v92.id,) = _struct_i.unpack(str[start:end])
01306 start = end
01307 end += 4
01308 (length,) = _struct_I.unpack(str[start:end])
01309 _v92.pose = []
01310 for i in xrange(0, length):
01311 val3 = geometry_msgs.msg.Pose()
01312 _v95 = val3.position
01313 _x = _v95
01314 start = end
01315 end += 24
01316 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
01317 _v96 = val3.orientation
01318 _x = _v96
01319 start = end
01320 end += 32
01321 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
01322 _v92.pose.append(val3)
01323 start = end
01324 end += 4
01325 (length,) = _struct_I.unpack(str[start:end])
01326 _v92.pose_projected = []
01327 for i in xrange(0, length):
01328 val3 = geometry_msgs.msg.Pose()
01329 _v97 = val3.position
01330 _x = _v97
01331 start = end
01332 end += 24
01333 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
01334 _v98 = val3.orientation
01335 _x = _v98
01336 start = end
01337 end += 32
01338 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
01339 _v92.pose_projected.append(val3)
01340 start = end
01341 end += 4
01342 (length,) = _struct_I.unpack(str[start:end])
01343 _v92.pose_resampled = []
01344 for i in xrange(0, length):
01345 val3 = geometry_msgs.msg.Pose()
01346 _v99 = val3.position
01347 _x = _v99
01348 start = end
01349 end += 24
01350 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
01351 _v100 = val3.orientation
01352 _x = _v100
01353 start = end
01354 end += 32
01355 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
01356 _v92.pose_resampled.append(val3)
01357 start = end
01358 end += 4
01359 (length,) = _struct_I.unpack(str[start:end])
01360 pattern = '<%sI'%length
01361 start = end
01362 end += struct.calcsize(pattern)
01363 _v92.pose_flags = numpy.frombuffer(str[start:end], dtype=numpy.uint32, count=length)
01364 start = end
01365 end += 4
01366 (length,) = _struct_I.unpack(str[start:end])
01367 _v92.channels = []
01368 for i in xrange(0, length):
01369 val3 = sensor_msgs.msg.ChannelFloat32()
01370 start = end
01371 end += 4
01372 (length,) = _struct_I.unpack(str[start:end])
01373 start = end
01374 end += length
01375 val3.name = str[start:end]
01376 start = end
01377 end += 4
01378 (length,) = _struct_I.unpack(str[start:end])
01379 pattern = '<%sf'%length
01380 start = end
01381 end += struct.calcsize(pattern)
01382 val3.values = numpy.frombuffer(str[start:end], dtype=numpy.float32, count=length)
01383 _v92.channels.append(val3)
01384 self.object.models.append(val1)
01385 start = end
01386 end += 4
01387 (length,) = _struct_I.unpack(str[start:end])
01388 self.object.markers.markers = []
01389 for i in xrange(0, length):
01390 val1 = visualization_msgs.msg.Marker()
01391 _v101 = val1.header
01392 start = end
01393 end += 4
01394 (_v101.seq,) = _struct_I.unpack(str[start:end])
01395 _v102 = _v101.stamp
01396 _x = _v102
01397 start = end
01398 end += 8
01399 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
01400 start = end
01401 end += 4
01402 (length,) = _struct_I.unpack(str[start:end])
01403 start = end
01404 end += length
01405 _v101.frame_id = str[start:end]
01406 start = end
01407 end += 4
01408 (length,) = _struct_I.unpack(str[start:end])
01409 start = end
01410 end += length
01411 val1.ns = str[start:end]
01412 _x = val1
01413 start = end
01414 end += 12
01415 (_x.id, _x.type, _x.action,) = _struct_3i.unpack(str[start:end])
01416 _v103 = val1.pose
01417 _v104 = _v103.position
01418 _x = _v104
01419 start = end
01420 end += 24
01421 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
01422 _v105 = _v103.orientation
01423 _x = _v105
01424 start = end
01425 end += 32
01426 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
01427 _v106 = val1.scale
01428 _x = _v106
01429 start = end
01430 end += 24
01431 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
01432 _v107 = val1.color
01433 _x = _v107
01434 start = end
01435 end += 16
01436 (_x.r, _x.g, _x.b, _x.a,) = _struct_4f.unpack(str[start:end])
01437 _v108 = val1.lifetime
01438 _x = _v108
01439 start = end
01440 end += 8
01441 (_x.secs, _x.nsecs,) = _struct_2i.unpack(str[start:end])
01442 start = end
01443 end += 1
01444 (val1.frame_locked,) = _struct_B.unpack(str[start:end])
01445 val1.frame_locked = bool(val1.frame_locked)
01446 start = end
01447 end += 4
01448 (length,) = _struct_I.unpack(str[start:end])
01449 val1.points = []
01450 for i in xrange(0, length):
01451 val2 = geometry_msgs.msg.Point()
01452 _x = val2
01453 start = end
01454 end += 24
01455 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
01456 val1.points.append(val2)
01457 start = end
01458 end += 4
01459 (length,) = _struct_I.unpack(str[start:end])
01460 val1.colors = []
01461 for i in xrange(0, length):
01462 val2 = std_msgs.msg.ColorRGBA()
01463 _x = val2
01464 start = end
01465 end += 16
01466 (_x.r, _x.g, _x.b, _x.a,) = _struct_4f.unpack(str[start:end])
01467 val1.colors.append(val2)
01468 start = end
01469 end += 4
01470 (length,) = _struct_I.unpack(str[start:end])
01471 start = end
01472 end += length
01473 val1.text = str[start:end]
01474 start = end
01475 end += 4
01476 (length,) = _struct_I.unpack(str[start:end])
01477 start = end
01478 end += length
01479 val1.mesh_resource = str[start:end]
01480 start = end
01481 end += 1
01482 (val1.mesh_use_embedded_materials,) = _struct_B.unpack(str[start:end])
01483 val1.mesh_use_embedded_materials = bool(val1.mesh_use_embedded_materials)
01484 self.object.markers.markers.append(val1)
01485 return self
01486 except struct.error, e:
01487 raise roslib.message.DeserializationError(e) #most likely buffer underfill
01488
01489 _struct_I = roslib.message.struct_I
01490 _struct_B = struct.Struct("<B")
01491 _struct_dB = struct.Struct("<dB")
01492 _struct_3i = struct.Struct("<3i")
01493 _struct_2i = struct.Struct("<2i")
01494 _struct_i = struct.Struct("<i")
01495 _struct_3I = struct.Struct("<3I")
01496 _struct_4f = struct.Struct("<4f")
01497 _struct_4d = struct.Struct("<4d")
01498 _struct_2I = struct.Struct("<2I")
01499 _struct_3d = struct.Struct("<3d")
01500 """autogenerated by genmsg_py from ArticulatedObjectSrvResponse.msg. Do not edit."""
01501 import roslib.message
01502 import struct
01503
01504 import roslib.rostime
01505 import articulation_msgs.msg
01506 import geometry_msgs.msg
01507 import sensor_msgs.msg
01508 import visualization_msgs.msg
01509 import std_msgs.msg
01510
01511 class ArticulatedObjectSrvResponse(roslib.message.Message):
01512 _md5sum = "929d83624d0744de900991740e4bff8e"
01513 _type = "articulation_msgs/ArticulatedObjectSrvResponse"
01514 _has_header = False #flag to mark the presence of a Header object
01515 _full_text = """articulation_msgs/ArticulatedObjectMsg object
01516
01517
01518
01519
01520
01521
01522 ================================================================================
01523 MSG: articulation_msgs/ArticulatedObjectMsg
01524 Header header
01525
01526 articulation_msgs/TrackMsg[] parts # observed trajectories for each object part
01527 articulation_msgs/ParamMsg[] params # global parameters
01528 articulation_msgs/ModelMsg[] models # models, describing relationships between parts
01529 visualization_msgs/MarkerArray markers # marker visualization of models/object
01530 ================================================================================
01531 MSG: std_msgs/Header
01532 # Standard metadata for higher-level stamped data types.
01533 # This is generally used to communicate timestamped data
01534 # in a particular coordinate frame.
01535 #
01536 # sequence ID: consecutively increasing ID
01537 uint32 seq
01538 #Two-integer timestamp that is expressed as:
01539 # * stamp.secs: seconds (stamp_secs) since epoch
01540 # * stamp.nsecs: nanoseconds since stamp_secs
01541 # time-handling sugar is provided by the client library
01542 time stamp
01543 #Frame this data is associated with
01544 # 0: no frame
01545 # 1: global frame
01546 string frame_id
01547
01548 ================================================================================
01549 MSG: articulation_msgs/TrackMsg
01550 # Single kinematic trajectory
01551 #
01552 # This message contains a kinematic trajectory. The trajectory is specified
01553 # as a vector of 6D poses. An additional flag, track_type, indicates whether
01554 # the track is valid, and whether it includes orientation. The track id
01555 # can be used for automatic coloring in the RVIZ track plugin, and can be
01556 # freely chosen by the client.
01557 #
01558 # A model is fitting only from the trajectory stored in the pose[]-vector.
01559 # Additional information may be associated to each pose using the channels
01560 # vector, with arbitrary # fields (e.g., to include applied/measured forces).
01561 #
01562 # After model evaluation,
01563 # also the associated configurations of the object are stored in the channels,
01564 # named "q[0]".."q[DOF-1]", where DOF is the number of degrees of freedom.
01565 # Model evaluation also projects the poses in the pose vector onto the model,
01566 # and stores these ideal poses in the vector pose_projected. Further, during model
01567 # evaluation, a new set of (uniform) configurations over the valid configuration
01568 # range is sampled, and the result is stored in pose_resampled.
01569 # The vector pose_flags contains additional display flags for the poses in the
01570 # pose vector, for example, whether a pose is visible and/or
01571 # the end of a trajectory segment. At the moment, this is only used by the
01572 # prior_model_learner.
01573 #
01574
01575 Header header # Timestamp and frame
01576 int32 id # used-specified track id
01577
01578 geometry_msgs/Pose[] pose # sequence of poses, defining the observed trajectory
01579 geometry_msgs/Pose[] pose_projected # sequence of poses, projected to the model
01580 # (after model evaluation)
01581 geometry_msgs/Pose[] pose_resampled # sequence of poses, re-sampled from the model in
01582 # the valid configuration range
01583 uint32[] pose_flags # bit-wise combination of POSE_VISIBLE and POSE_END_OF_SEGMENT
01584
01585 uint32 POSE_VISIBLE=1
01586 uint32 POSE_END_OF_SEGMENT=2
01587
01588 # Each channel should have the same number of elements as pose array,
01589 # and the data in each channel should correspond 1:1 with each pose
01590 # possible channels: "width", "height", "rgb", ...
01591 sensor_msgs/ChannelFloat32[] channels
01592
01593
01594
01595 ================================================================================
01596 MSG: geometry_msgs/Pose
01597 # A representation of pose in free space, composed of postion and orientation.
01598 Point position
01599 Quaternion orientation
01600
01601 ================================================================================
01602 MSG: geometry_msgs/Point
01603 # This contains the position of a point in free space
01604 float64 x
01605 float64 y
01606 float64 z
01607
01608 ================================================================================
01609 MSG: geometry_msgs/Quaternion
01610 # This represents an orientation in free space in quaternion form.
01611
01612 float64 x
01613 float64 y
01614 float64 z
01615 float64 w
01616
01617 ================================================================================
01618 MSG: sensor_msgs/ChannelFloat32
01619 # This message is used by the PointCloud message to hold optional data
01620 # associated with each point in the cloud. The length of the values
01621 # array should be the same as the length of the points array in the
01622 # PointCloud, and each value should be associated with the corresponding
01623 # point.
01624
01625 # Channel names in existing practice include:
01626 # "u", "v" - row and column (respectively) in the left stereo image.
01627 # This is opposite to usual conventions but remains for
01628 # historical reasons. The newer PointCloud2 message has no
01629 # such problem.
01630 # "rgb" - For point clouds produced by color stereo cameras. uint8
01631 # (R,G,B) values packed into the least significant 24 bits,
01632 # in order.
01633 # "intensity" - laser or pixel intensity.
01634 # "distance"
01635
01636 # The channel name should give semantics of the channel (e.g.
01637 # "intensity" instead of "value").
01638 string name
01639
01640 # The values array should be 1-1 with the elements of the associated
01641 # PointCloud.
01642 float32[] values
01643
01644 ================================================================================
01645 MSG: articulation_msgs/ParamMsg
01646 # Single parameter passed to or from model fitting
01647 #
01648 # This mechanism allows to flexibly pass parameters to
01649 # model fitting (and vice versa). Note that these parameters
01650 # are model-specific: A client may supply additional
01651 # parameters to the model estimator, and, similarly, a estimator
01652 # may add the estimated model parameters to the model message.
01653 # When the model is then evaluated, for example to make predictions
01654 # or to compute the likelihood, the model class can then use
01655 # these parameters.
01656 #
01657 # A parameter has a name, a value, and a type. The type globally
01658 # indicates whether it is a prior parameter (prior to model fitting),
01659 # or a model parameter (found during model fitting, using a maximum-
01660 # likelihood estimator), or a cached evaluation (e.g., the likelihood
01661 # or the BIC are a typical "side"-product of model estimation, and
01662 # can therefore already be cached).
01663 #
01664 # For a list of currently used parameters, see the documentation at
01665 # http://www.ros.org/wiki/articulation_models
01666 #
01667
01668 uint8 PRIOR=0 # indicates a prior model parameter
01669 # (e.g., "sigma_position")
01670 uint8 PARAM=1 # indicates a estimated model parameter
01671 # (e.g., "rot_radius", the estimated radius)
01672 uint8 EVAL=2 # indicates a cached evaluation of the model, given
01673 # the current trajectory
01674 # (e.g., "loglikelihood", the log likelihood of the
01675 # data, given the model and its parameters)
01676
01677 string name # name of the parameter
01678 float64 value # value of the parameter
01679 uint8 type # type of the parameter (PRIOR, PARAM, EVAL)
01680
01681
01682 ================================================================================
01683 MSG: articulation_msgs/ModelMsg
01684 # Single kinematic model
01685 #
01686 # A kinematic model is defined by its model class name, and a set of parameters.
01687 # The client may additionally specify a model id, that can be used to colorize the
01688 # model when visualized using the RVIZ model display.
01689 #
01690 # For a list of currently implemented models, see the documetation at
01691 # http://www.ros.org/wiki/articulation_models
01692 #
01693 #
01694
01695 Header header # frame and timestamp
01696
01697 int32 id # user specified model id
01698 string name # name of the model family (e.g. "rotational",
01699 # "prismatic", "pca-gp", "rigid")
01700 articulation_msgs/ParamMsg[] params # model parameters
01701 articulation_msgs/TrackMsg track # trajectory from which the model is estimated, or
01702 # that is evaluated using the model
01703
01704 ================================================================================
01705 MSG: visualization_msgs/MarkerArray
01706 Marker[] markers
01707
01708 ================================================================================
01709 MSG: visualization_msgs/Marker
01710 # See http://www.ros.org/wiki/rviz/DisplayTypes/Marker and http://www.ros.org/wiki/rviz/Tutorials/Markers%3A%20Basic%20Shapes for more information on using this message with rviz
01711
01712 byte ARROW=0
01713 byte CUBE=1
01714 byte SPHERE=2
01715 byte CYLINDER=3
01716 byte LINE_STRIP=4
01717 byte LINE_LIST=5
01718 byte CUBE_LIST=6
01719 byte SPHERE_LIST=7
01720 byte POINTS=8
01721 byte TEXT_VIEW_FACING=9
01722 byte MESH_RESOURCE=10
01723 byte TRIANGLE_LIST=11
01724
01725 byte ADD=0
01726 byte MODIFY=0
01727 byte DELETE=2
01728
01729 Header header # header for time/frame information
01730 string ns # Namespace to place this object in... used in conjunction with id to create a unique name for the object
01731 int32 id # object ID useful in conjunction with the namespace for manipulating and deleting the object later
01732 int32 type # Type of object
01733 int32 action # 0 add/modify an object, 1 (deprecated), 2 deletes an object
01734 geometry_msgs/Pose pose # Pose of the object
01735 geometry_msgs/Vector3 scale # Scale of the object 1,1,1 means default (usually 1 meter square)
01736 std_msgs/ColorRGBA color # Color [0.0-1.0]
01737 duration lifetime # How long the object should last before being automatically deleted. 0 means forever
01738 bool frame_locked # If this marker should be frame-locked, i.e. retransformed into its frame every timestep
01739
01740 #Only used if the type specified has some use for them (eg. POINTS, LINE_STRIP, ...)
01741 geometry_msgs/Point[] points
01742 #Only used if the type specified has some use for them (eg. POINTS, LINE_STRIP, ...)
01743 #number of colors must either be 0 or equal to the number of points
01744 #NOTE: alpha is not yet used
01745 std_msgs/ColorRGBA[] colors
01746
01747 # NOTE: only used for text markers
01748 string text
01749
01750 # NOTE: only used for MESH_RESOURCE markers
01751 string mesh_resource
01752 bool mesh_use_embedded_materials
01753
01754 ================================================================================
01755 MSG: geometry_msgs/Vector3
01756 # This represents a vector in free space.
01757
01758 float64 x
01759 float64 y
01760 float64 z
01761 ================================================================================
01762 MSG: std_msgs/ColorRGBA
01763 float32 r
01764 float32 g
01765 float32 b
01766 float32 a
01767
01768 """
01769 __slots__ = ['object']
01770 _slot_types = ['articulation_msgs/ArticulatedObjectMsg']
01771
01772 def __init__(self, *args, **kwds):
01773 """
01774 Constructor. Any message fields that are implicitly/explicitly
01775 set to None will be assigned a default value. The recommend
01776 use is keyword arguments as this is more robust to future message
01777 changes. You cannot mix in-order arguments and keyword arguments.
01778
01779 The available fields are:
01780 object
01781
01782 @param args: complete set of field values, in .msg order
01783 @param kwds: use keyword arguments corresponding to message field names
01784 to set specific fields.
01785 """
01786 if args or kwds:
01787 super(ArticulatedObjectSrvResponse, self).__init__(*args, **kwds)
01788 #message fields cannot be None, assign default values for those that are
01789 if self.object is None:
01790 self.object = articulation_msgs.msg.ArticulatedObjectMsg()
01791 else:
01792 self.object = articulation_msgs.msg.ArticulatedObjectMsg()
01793
01794 def _get_types(self):
01795 """
01796 internal API method
01797 """
01798 return self._slot_types
01799
01800 def serialize(self, buff):
01801 """
01802 serialize message into buffer
01803 @param buff: buffer
01804 @type buff: StringIO
01805 """
01806 try:
01807 _x = self
01808 buff.write(_struct_3I.pack(_x.object.header.seq, _x.object.header.stamp.secs, _x.object.header.stamp.nsecs))
01809 _x = self.object.header.frame_id
01810 length = len(_x)
01811 buff.write(struct.pack('<I%ss'%length, length, _x))
01812 length = len(self.object.parts)
01813 buff.write(_struct_I.pack(length))
01814 for val1 in self.object.parts:
01815 _v109 = val1.header
01816 buff.write(_struct_I.pack(_v109.seq))
01817 _v110 = _v109.stamp
01818 _x = _v110
01819 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
01820 _x = _v109.frame_id
01821 length = len(_x)
01822 buff.write(struct.pack('<I%ss'%length, length, _x))
01823 buff.write(_struct_i.pack(val1.id))
01824 length = len(val1.pose)
01825 buff.write(_struct_I.pack(length))
01826 for val2 in val1.pose:
01827 _v111 = val2.position
01828 _x = _v111
01829 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
01830 _v112 = val2.orientation
01831 _x = _v112
01832 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
01833 length = len(val1.pose_projected)
01834 buff.write(_struct_I.pack(length))
01835 for val2 in val1.pose_projected:
01836 _v113 = val2.position
01837 _x = _v113
01838 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
01839 _v114 = val2.orientation
01840 _x = _v114
01841 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
01842 length = len(val1.pose_resampled)
01843 buff.write(_struct_I.pack(length))
01844 for val2 in val1.pose_resampled:
01845 _v115 = val2.position
01846 _x = _v115
01847 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
01848 _v116 = val2.orientation
01849 _x = _v116
01850 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
01851 length = len(val1.pose_flags)
01852 buff.write(_struct_I.pack(length))
01853 pattern = '<%sI'%length
01854 buff.write(struct.pack(pattern, *val1.pose_flags))
01855 length = len(val1.channels)
01856 buff.write(_struct_I.pack(length))
01857 for val2 in val1.channels:
01858 _x = val2.name
01859 length = len(_x)
01860 buff.write(struct.pack('<I%ss'%length, length, _x))
01861 length = len(val2.values)
01862 buff.write(_struct_I.pack(length))
01863 pattern = '<%sf'%length
01864 buff.write(struct.pack(pattern, *val2.values))
01865 length = len(self.object.params)
01866 buff.write(_struct_I.pack(length))
01867 for val1 in self.object.params:
01868 _x = val1.name
01869 length = len(_x)
01870 buff.write(struct.pack('<I%ss'%length, length, _x))
01871 _x = val1
01872 buff.write(_struct_dB.pack(_x.value, _x.type))
01873 length = len(self.object.models)
01874 buff.write(_struct_I.pack(length))
01875 for val1 in self.object.models:
01876 _v117 = val1.header
01877 buff.write(_struct_I.pack(_v117.seq))
01878 _v118 = _v117.stamp
01879 _x = _v118
01880 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
01881 _x = _v117.frame_id
01882 length = len(_x)
01883 buff.write(struct.pack('<I%ss'%length, length, _x))
01884 buff.write(_struct_i.pack(val1.id))
01885 _x = val1.name
01886 length = len(_x)
01887 buff.write(struct.pack('<I%ss'%length, length, _x))
01888 length = len(val1.params)
01889 buff.write(_struct_I.pack(length))
01890 for val2 in val1.params:
01891 _x = val2.name
01892 length = len(_x)
01893 buff.write(struct.pack('<I%ss'%length, length, _x))
01894 _x = val2
01895 buff.write(_struct_dB.pack(_x.value, _x.type))
01896 _v119 = val1.track
01897 _v120 = _v119.header
01898 buff.write(_struct_I.pack(_v120.seq))
01899 _v121 = _v120.stamp
01900 _x = _v121
01901 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
01902 _x = _v120.frame_id
01903 length = len(_x)
01904 buff.write(struct.pack('<I%ss'%length, length, _x))
01905 buff.write(_struct_i.pack(_v119.id))
01906 length = len(_v119.pose)
01907 buff.write(_struct_I.pack(length))
01908 for val3 in _v119.pose:
01909 _v122 = val3.position
01910 _x = _v122
01911 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
01912 _v123 = val3.orientation
01913 _x = _v123
01914 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
01915 length = len(_v119.pose_projected)
01916 buff.write(_struct_I.pack(length))
01917 for val3 in _v119.pose_projected:
01918 _v124 = val3.position
01919 _x = _v124
01920 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
01921 _v125 = val3.orientation
01922 _x = _v125
01923 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
01924 length = len(_v119.pose_resampled)
01925 buff.write(_struct_I.pack(length))
01926 for val3 in _v119.pose_resampled:
01927 _v126 = val3.position
01928 _x = _v126
01929 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
01930 _v127 = val3.orientation
01931 _x = _v127
01932 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
01933 length = len(_v119.pose_flags)
01934 buff.write(_struct_I.pack(length))
01935 pattern = '<%sI'%length
01936 buff.write(struct.pack(pattern, *_v119.pose_flags))
01937 length = len(_v119.channels)
01938 buff.write(_struct_I.pack(length))
01939 for val3 in _v119.channels:
01940 _x = val3.name
01941 length = len(_x)
01942 buff.write(struct.pack('<I%ss'%length, length, _x))
01943 length = len(val3.values)
01944 buff.write(_struct_I.pack(length))
01945 pattern = '<%sf'%length
01946 buff.write(struct.pack(pattern, *val3.values))
01947 length = len(self.object.markers.markers)
01948 buff.write(_struct_I.pack(length))
01949 for val1 in self.object.markers.markers:
01950 _v128 = val1.header
01951 buff.write(_struct_I.pack(_v128.seq))
01952 _v129 = _v128.stamp
01953 _x = _v129
01954 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
01955 _x = _v128.frame_id
01956 length = len(_x)
01957 buff.write(struct.pack('<I%ss'%length, length, _x))
01958 _x = val1.ns
01959 length = len(_x)
01960 buff.write(struct.pack('<I%ss'%length, length, _x))
01961 _x = val1
01962 buff.write(_struct_3i.pack(_x.id, _x.type, _x.action))
01963 _v130 = val1.pose
01964 _v131 = _v130.position
01965 _x = _v131
01966 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
01967 _v132 = _v130.orientation
01968 _x = _v132
01969 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
01970 _v133 = val1.scale
01971 _x = _v133
01972 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
01973 _v134 = val1.color
01974 _x = _v134
01975 buff.write(_struct_4f.pack(_x.r, _x.g, _x.b, _x.a))
01976 _v135 = val1.lifetime
01977 _x = _v135
01978 buff.write(_struct_2i.pack(_x.secs, _x.nsecs))
01979 buff.write(_struct_B.pack(val1.frame_locked))
01980 length = len(val1.points)
01981 buff.write(_struct_I.pack(length))
01982 for val2 in val1.points:
01983 _x = val2
01984 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
01985 length = len(val1.colors)
01986 buff.write(_struct_I.pack(length))
01987 for val2 in val1.colors:
01988 _x = val2
01989 buff.write(_struct_4f.pack(_x.r, _x.g, _x.b, _x.a))
01990 _x = val1.text
01991 length = len(_x)
01992 buff.write(struct.pack('<I%ss'%length, length, _x))
01993 _x = val1.mesh_resource
01994 length = len(_x)
01995 buff.write(struct.pack('<I%ss'%length, length, _x))
01996 buff.write(_struct_B.pack(val1.mesh_use_embedded_materials))
01997 except struct.error, se: self._check_types(se)
01998 except TypeError, te: self._check_types(te)
01999
02000 def deserialize(self, str):
02001 """
02002 unpack serialized message in str into this message instance
02003 @param str: byte array of serialized message
02004 @type str: str
02005 """
02006 try:
02007 if self.object is None:
02008 self.object = articulation_msgs.msg.ArticulatedObjectMsg()
02009 end = 0
02010 _x = self
02011 start = end
02012 end += 12
02013 (_x.object.header.seq, _x.object.header.stamp.secs, _x.object.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
02014 start = end
02015 end += 4
02016 (length,) = _struct_I.unpack(str[start:end])
02017 start = end
02018 end += length
02019 self.object.header.frame_id = str[start:end]
02020 start = end
02021 end += 4
02022 (length,) = _struct_I.unpack(str[start:end])
02023 self.object.parts = []
02024 for i in xrange(0, length):
02025 val1 = articulation_msgs.msg.TrackMsg()
02026 _v136 = val1.header
02027 start = end
02028 end += 4
02029 (_v136.seq,) = _struct_I.unpack(str[start:end])
02030 _v137 = _v136.stamp
02031 _x = _v137
02032 start = end
02033 end += 8
02034 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
02035 start = end
02036 end += 4
02037 (length,) = _struct_I.unpack(str[start:end])
02038 start = end
02039 end += length
02040 _v136.frame_id = str[start:end]
02041 start = end
02042 end += 4
02043 (val1.id,) = _struct_i.unpack(str[start:end])
02044 start = end
02045 end += 4
02046 (length,) = _struct_I.unpack(str[start:end])
02047 val1.pose = []
02048 for i in xrange(0, length):
02049 val2 = geometry_msgs.msg.Pose()
02050 _v138 = val2.position
02051 _x = _v138
02052 start = end
02053 end += 24
02054 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
02055 _v139 = val2.orientation
02056 _x = _v139
02057 start = end
02058 end += 32
02059 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
02060 val1.pose.append(val2)
02061 start = end
02062 end += 4
02063 (length,) = _struct_I.unpack(str[start:end])
02064 val1.pose_projected = []
02065 for i in xrange(0, length):
02066 val2 = geometry_msgs.msg.Pose()
02067 _v140 = val2.position
02068 _x = _v140
02069 start = end
02070 end += 24
02071 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
02072 _v141 = val2.orientation
02073 _x = _v141
02074 start = end
02075 end += 32
02076 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
02077 val1.pose_projected.append(val2)
02078 start = end
02079 end += 4
02080 (length,) = _struct_I.unpack(str[start:end])
02081 val1.pose_resampled = []
02082 for i in xrange(0, length):
02083 val2 = geometry_msgs.msg.Pose()
02084 _v142 = val2.position
02085 _x = _v142
02086 start = end
02087 end += 24
02088 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
02089 _v143 = val2.orientation
02090 _x = _v143
02091 start = end
02092 end += 32
02093 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
02094 val1.pose_resampled.append(val2)
02095 start = end
02096 end += 4
02097 (length,) = _struct_I.unpack(str[start:end])
02098 pattern = '<%sI'%length
02099 start = end
02100 end += struct.calcsize(pattern)
02101 val1.pose_flags = struct.unpack(pattern, str[start:end])
02102 start = end
02103 end += 4
02104 (length,) = _struct_I.unpack(str[start:end])
02105 val1.channels = []
02106 for i in xrange(0, length):
02107 val2 = sensor_msgs.msg.ChannelFloat32()
02108 start = end
02109 end += 4
02110 (length,) = _struct_I.unpack(str[start:end])
02111 start = end
02112 end += length
02113 val2.name = str[start:end]
02114 start = end
02115 end += 4
02116 (length,) = _struct_I.unpack(str[start:end])
02117 pattern = '<%sf'%length
02118 start = end
02119 end += struct.calcsize(pattern)
02120 val2.values = struct.unpack(pattern, str[start:end])
02121 val1.channels.append(val2)
02122 self.object.parts.append(val1)
02123 start = end
02124 end += 4
02125 (length,) = _struct_I.unpack(str[start:end])
02126 self.object.params = []
02127 for i in xrange(0, length):
02128 val1 = articulation_msgs.msg.ParamMsg()
02129 start = end
02130 end += 4
02131 (length,) = _struct_I.unpack(str[start:end])
02132 start = end
02133 end += length
02134 val1.name = str[start:end]
02135 _x = val1
02136 start = end
02137 end += 9
02138 (_x.value, _x.type,) = _struct_dB.unpack(str[start:end])
02139 self.object.params.append(val1)
02140 start = end
02141 end += 4
02142 (length,) = _struct_I.unpack(str[start:end])
02143 self.object.models = []
02144 for i in xrange(0, length):
02145 val1 = articulation_msgs.msg.ModelMsg()
02146 _v144 = val1.header
02147 start = end
02148 end += 4
02149 (_v144.seq,) = _struct_I.unpack(str[start:end])
02150 _v145 = _v144.stamp
02151 _x = _v145
02152 start = end
02153 end += 8
02154 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
02155 start = end
02156 end += 4
02157 (length,) = _struct_I.unpack(str[start:end])
02158 start = end
02159 end += length
02160 _v144.frame_id = str[start:end]
02161 start = end
02162 end += 4
02163 (val1.id,) = _struct_i.unpack(str[start:end])
02164 start = end
02165 end += 4
02166 (length,) = _struct_I.unpack(str[start:end])
02167 start = end
02168 end += length
02169 val1.name = str[start:end]
02170 start = end
02171 end += 4
02172 (length,) = _struct_I.unpack(str[start:end])
02173 val1.params = []
02174 for i in xrange(0, length):
02175 val2 = articulation_msgs.msg.ParamMsg()
02176 start = end
02177 end += 4
02178 (length,) = _struct_I.unpack(str[start:end])
02179 start = end
02180 end += length
02181 val2.name = str[start:end]
02182 _x = val2
02183 start = end
02184 end += 9
02185 (_x.value, _x.type,) = _struct_dB.unpack(str[start:end])
02186 val1.params.append(val2)
02187 _v146 = val1.track
02188 _v147 = _v146.header
02189 start = end
02190 end += 4
02191 (_v147.seq,) = _struct_I.unpack(str[start:end])
02192 _v148 = _v147.stamp
02193 _x = _v148
02194 start = end
02195 end += 8
02196 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
02197 start = end
02198 end += 4
02199 (length,) = _struct_I.unpack(str[start:end])
02200 start = end
02201 end += length
02202 _v147.frame_id = str[start:end]
02203 start = end
02204 end += 4
02205 (_v146.id,) = _struct_i.unpack(str[start:end])
02206 start = end
02207 end += 4
02208 (length,) = _struct_I.unpack(str[start:end])
02209 _v146.pose = []
02210 for i in xrange(0, length):
02211 val3 = geometry_msgs.msg.Pose()
02212 _v149 = val3.position
02213 _x = _v149
02214 start = end
02215 end += 24
02216 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
02217 _v150 = val3.orientation
02218 _x = _v150
02219 start = end
02220 end += 32
02221 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
02222 _v146.pose.append(val3)
02223 start = end
02224 end += 4
02225 (length,) = _struct_I.unpack(str[start:end])
02226 _v146.pose_projected = []
02227 for i in xrange(0, length):
02228 val3 = geometry_msgs.msg.Pose()
02229 _v151 = val3.position
02230 _x = _v151
02231 start = end
02232 end += 24
02233 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
02234 _v152 = val3.orientation
02235 _x = _v152
02236 start = end
02237 end += 32
02238 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
02239 _v146.pose_projected.append(val3)
02240 start = end
02241 end += 4
02242 (length,) = _struct_I.unpack(str[start:end])
02243 _v146.pose_resampled = []
02244 for i in xrange(0, length):
02245 val3 = geometry_msgs.msg.Pose()
02246 _v153 = val3.position
02247 _x = _v153
02248 start = end
02249 end += 24
02250 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
02251 _v154 = val3.orientation
02252 _x = _v154
02253 start = end
02254 end += 32
02255 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
02256 _v146.pose_resampled.append(val3)
02257 start = end
02258 end += 4
02259 (length,) = _struct_I.unpack(str[start:end])
02260 pattern = '<%sI'%length
02261 start = end
02262 end += struct.calcsize(pattern)
02263 _v146.pose_flags = struct.unpack(pattern, str[start:end])
02264 start = end
02265 end += 4
02266 (length,) = _struct_I.unpack(str[start:end])
02267 _v146.channels = []
02268 for i in xrange(0, length):
02269 val3 = sensor_msgs.msg.ChannelFloat32()
02270 start = end
02271 end += 4
02272 (length,) = _struct_I.unpack(str[start:end])
02273 start = end
02274 end += length
02275 val3.name = str[start:end]
02276 start = end
02277 end += 4
02278 (length,) = _struct_I.unpack(str[start:end])
02279 pattern = '<%sf'%length
02280 start = end
02281 end += struct.calcsize(pattern)
02282 val3.values = struct.unpack(pattern, str[start:end])
02283 _v146.channels.append(val3)
02284 self.object.models.append(val1)
02285 start = end
02286 end += 4
02287 (length,) = _struct_I.unpack(str[start:end])
02288 self.object.markers.markers = []
02289 for i in xrange(0, length):
02290 val1 = visualization_msgs.msg.Marker()
02291 _v155 = val1.header
02292 start = end
02293 end += 4
02294 (_v155.seq,) = _struct_I.unpack(str[start:end])
02295 _v156 = _v155.stamp
02296 _x = _v156
02297 start = end
02298 end += 8
02299 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
02300 start = end
02301 end += 4
02302 (length,) = _struct_I.unpack(str[start:end])
02303 start = end
02304 end += length
02305 _v155.frame_id = str[start:end]
02306 start = end
02307 end += 4
02308 (length,) = _struct_I.unpack(str[start:end])
02309 start = end
02310 end += length
02311 val1.ns = str[start:end]
02312 _x = val1
02313 start = end
02314 end += 12
02315 (_x.id, _x.type, _x.action,) = _struct_3i.unpack(str[start:end])
02316 _v157 = val1.pose
02317 _v158 = _v157.position
02318 _x = _v158
02319 start = end
02320 end += 24
02321 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
02322 _v159 = _v157.orientation
02323 _x = _v159
02324 start = end
02325 end += 32
02326 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
02327 _v160 = val1.scale
02328 _x = _v160
02329 start = end
02330 end += 24
02331 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
02332 _v161 = val1.color
02333 _x = _v161
02334 start = end
02335 end += 16
02336 (_x.r, _x.g, _x.b, _x.a,) = _struct_4f.unpack(str[start:end])
02337 _v162 = val1.lifetime
02338 _x = _v162
02339 start = end
02340 end += 8
02341 (_x.secs, _x.nsecs,) = _struct_2i.unpack(str[start:end])
02342 start = end
02343 end += 1
02344 (val1.frame_locked,) = _struct_B.unpack(str[start:end])
02345 val1.frame_locked = bool(val1.frame_locked)
02346 start = end
02347 end += 4
02348 (length,) = _struct_I.unpack(str[start:end])
02349 val1.points = []
02350 for i in xrange(0, length):
02351 val2 = geometry_msgs.msg.Point()
02352 _x = val2
02353 start = end
02354 end += 24
02355 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
02356 val1.points.append(val2)
02357 start = end
02358 end += 4
02359 (length,) = _struct_I.unpack(str[start:end])
02360 val1.colors = []
02361 for i in xrange(0, length):
02362 val2 = std_msgs.msg.ColorRGBA()
02363 _x = val2
02364 start = end
02365 end += 16
02366 (_x.r, _x.g, _x.b, _x.a,) = _struct_4f.unpack(str[start:end])
02367 val1.colors.append(val2)
02368 start = end
02369 end += 4
02370 (length,) = _struct_I.unpack(str[start:end])
02371 start = end
02372 end += length
02373 val1.text = str[start:end]
02374 start = end
02375 end += 4
02376 (length,) = _struct_I.unpack(str[start:end])
02377 start = end
02378 end += length
02379 val1.mesh_resource = str[start:end]
02380 start = end
02381 end += 1
02382 (val1.mesh_use_embedded_materials,) = _struct_B.unpack(str[start:end])
02383 val1.mesh_use_embedded_materials = bool(val1.mesh_use_embedded_materials)
02384 self.object.markers.markers.append(val1)
02385 return self
02386 except struct.error, e:
02387 raise roslib.message.DeserializationError(e) #most likely buffer underfill
02388
02389
02390 def serialize_numpy(self, buff, numpy):
02391 """
02392 serialize message with numpy array types into buffer
02393 @param buff: buffer
02394 @type buff: StringIO
02395 @param numpy: numpy python module
02396 @type numpy module
02397 """
02398 try:
02399 _x = self
02400 buff.write(_struct_3I.pack(_x.object.header.seq, _x.object.header.stamp.secs, _x.object.header.stamp.nsecs))
02401 _x = self.object.header.frame_id
02402 length = len(_x)
02403 buff.write(struct.pack('<I%ss'%length, length, _x))
02404 length = len(self.object.parts)
02405 buff.write(_struct_I.pack(length))
02406 for val1 in self.object.parts:
02407 _v163 = val1.header
02408 buff.write(_struct_I.pack(_v163.seq))
02409 _v164 = _v163.stamp
02410 _x = _v164
02411 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
02412 _x = _v163.frame_id
02413 length = len(_x)
02414 buff.write(struct.pack('<I%ss'%length, length, _x))
02415 buff.write(_struct_i.pack(val1.id))
02416 length = len(val1.pose)
02417 buff.write(_struct_I.pack(length))
02418 for val2 in val1.pose:
02419 _v165 = val2.position
02420 _x = _v165
02421 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
02422 _v166 = val2.orientation
02423 _x = _v166
02424 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
02425 length = len(val1.pose_projected)
02426 buff.write(_struct_I.pack(length))
02427 for val2 in val1.pose_projected:
02428 _v167 = val2.position
02429 _x = _v167
02430 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
02431 _v168 = val2.orientation
02432 _x = _v168
02433 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
02434 length = len(val1.pose_resampled)
02435 buff.write(_struct_I.pack(length))
02436 for val2 in val1.pose_resampled:
02437 _v169 = val2.position
02438 _x = _v169
02439 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
02440 _v170 = val2.orientation
02441 _x = _v170
02442 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
02443 length = len(val1.pose_flags)
02444 buff.write(_struct_I.pack(length))
02445 pattern = '<%sI'%length
02446 buff.write(val1.pose_flags.tostring())
02447 length = len(val1.channels)
02448 buff.write(_struct_I.pack(length))
02449 for val2 in val1.channels:
02450 _x = val2.name
02451 length = len(_x)
02452 buff.write(struct.pack('<I%ss'%length, length, _x))
02453 length = len(val2.values)
02454 buff.write(_struct_I.pack(length))
02455 pattern = '<%sf'%length
02456 buff.write(val2.values.tostring())
02457 length = len(self.object.params)
02458 buff.write(_struct_I.pack(length))
02459 for val1 in self.object.params:
02460 _x = val1.name
02461 length = len(_x)
02462 buff.write(struct.pack('<I%ss'%length, length, _x))
02463 _x = val1
02464 buff.write(_struct_dB.pack(_x.value, _x.type))
02465 length = len(self.object.models)
02466 buff.write(_struct_I.pack(length))
02467 for val1 in self.object.models:
02468 _v171 = val1.header
02469 buff.write(_struct_I.pack(_v171.seq))
02470 _v172 = _v171.stamp
02471 _x = _v172
02472 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
02473 _x = _v171.frame_id
02474 length = len(_x)
02475 buff.write(struct.pack('<I%ss'%length, length, _x))
02476 buff.write(_struct_i.pack(val1.id))
02477 _x = val1.name
02478 length = len(_x)
02479 buff.write(struct.pack('<I%ss'%length, length, _x))
02480 length = len(val1.params)
02481 buff.write(_struct_I.pack(length))
02482 for val2 in val1.params:
02483 _x = val2.name
02484 length = len(_x)
02485 buff.write(struct.pack('<I%ss'%length, length, _x))
02486 _x = val2
02487 buff.write(_struct_dB.pack(_x.value, _x.type))
02488 _v173 = val1.track
02489 _v174 = _v173.header
02490 buff.write(_struct_I.pack(_v174.seq))
02491 _v175 = _v174.stamp
02492 _x = _v175
02493 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
02494 _x = _v174.frame_id
02495 length = len(_x)
02496 buff.write(struct.pack('<I%ss'%length, length, _x))
02497 buff.write(_struct_i.pack(_v173.id))
02498 length = len(_v173.pose)
02499 buff.write(_struct_I.pack(length))
02500 for val3 in _v173.pose:
02501 _v176 = val3.position
02502 _x = _v176
02503 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
02504 _v177 = val3.orientation
02505 _x = _v177
02506 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
02507 length = len(_v173.pose_projected)
02508 buff.write(_struct_I.pack(length))
02509 for val3 in _v173.pose_projected:
02510 _v178 = val3.position
02511 _x = _v178
02512 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
02513 _v179 = val3.orientation
02514 _x = _v179
02515 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
02516 length = len(_v173.pose_resampled)
02517 buff.write(_struct_I.pack(length))
02518 for val3 in _v173.pose_resampled:
02519 _v180 = val3.position
02520 _x = _v180
02521 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
02522 _v181 = val3.orientation
02523 _x = _v181
02524 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
02525 length = len(_v173.pose_flags)
02526 buff.write(_struct_I.pack(length))
02527 pattern = '<%sI'%length
02528 buff.write(_v173.pose_flags.tostring())
02529 length = len(_v173.channels)
02530 buff.write(_struct_I.pack(length))
02531 for val3 in _v173.channels:
02532 _x = val3.name
02533 length = len(_x)
02534 buff.write(struct.pack('<I%ss'%length, length, _x))
02535 length = len(val3.values)
02536 buff.write(_struct_I.pack(length))
02537 pattern = '<%sf'%length
02538 buff.write(val3.values.tostring())
02539 length = len(self.object.markers.markers)
02540 buff.write(_struct_I.pack(length))
02541 for val1 in self.object.markers.markers:
02542 _v182 = val1.header
02543 buff.write(_struct_I.pack(_v182.seq))
02544 _v183 = _v182.stamp
02545 _x = _v183
02546 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
02547 _x = _v182.frame_id
02548 length = len(_x)
02549 buff.write(struct.pack('<I%ss'%length, length, _x))
02550 _x = val1.ns
02551 length = len(_x)
02552 buff.write(struct.pack('<I%ss'%length, length, _x))
02553 _x = val1
02554 buff.write(_struct_3i.pack(_x.id, _x.type, _x.action))
02555 _v184 = val1.pose
02556 _v185 = _v184.position
02557 _x = _v185
02558 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
02559 _v186 = _v184.orientation
02560 _x = _v186
02561 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
02562 _v187 = val1.scale
02563 _x = _v187
02564 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
02565 _v188 = val1.color
02566 _x = _v188
02567 buff.write(_struct_4f.pack(_x.r, _x.g, _x.b, _x.a))
02568 _v189 = val1.lifetime
02569 _x = _v189
02570 buff.write(_struct_2i.pack(_x.secs, _x.nsecs))
02571 buff.write(_struct_B.pack(val1.frame_locked))
02572 length = len(val1.points)
02573 buff.write(_struct_I.pack(length))
02574 for val2 in val1.points:
02575 _x = val2
02576 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
02577 length = len(val1.colors)
02578 buff.write(_struct_I.pack(length))
02579 for val2 in val1.colors:
02580 _x = val2
02581 buff.write(_struct_4f.pack(_x.r, _x.g, _x.b, _x.a))
02582 _x = val1.text
02583 length = len(_x)
02584 buff.write(struct.pack('<I%ss'%length, length, _x))
02585 _x = val1.mesh_resource
02586 length = len(_x)
02587 buff.write(struct.pack('<I%ss'%length, length, _x))
02588 buff.write(_struct_B.pack(val1.mesh_use_embedded_materials))
02589 except struct.error, se: self._check_types(se)
02590 except TypeError, te: self._check_types(te)
02591
02592 def deserialize_numpy(self, str, numpy):
02593 """
02594 unpack serialized message in str into this message instance using numpy for array types
02595 @param str: byte array of serialized message
02596 @type str: str
02597 @param numpy: numpy python module
02598 @type numpy: module
02599 """
02600 try:
02601 if self.object is None:
02602 self.object = articulation_msgs.msg.ArticulatedObjectMsg()
02603 end = 0
02604 _x = self
02605 start = end
02606 end += 12
02607 (_x.object.header.seq, _x.object.header.stamp.secs, _x.object.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
02608 start = end
02609 end += 4
02610 (length,) = _struct_I.unpack(str[start:end])
02611 start = end
02612 end += length
02613 self.object.header.frame_id = str[start:end]
02614 start = end
02615 end += 4
02616 (length,) = _struct_I.unpack(str[start:end])
02617 self.object.parts = []
02618 for i in xrange(0, length):
02619 val1 = articulation_msgs.msg.TrackMsg()
02620 _v190 = val1.header
02621 start = end
02622 end += 4
02623 (_v190.seq,) = _struct_I.unpack(str[start:end])
02624 _v191 = _v190.stamp
02625 _x = _v191
02626 start = end
02627 end += 8
02628 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
02629 start = end
02630 end += 4
02631 (length,) = _struct_I.unpack(str[start:end])
02632 start = end
02633 end += length
02634 _v190.frame_id = str[start:end]
02635 start = end
02636 end += 4
02637 (val1.id,) = _struct_i.unpack(str[start:end])
02638 start = end
02639 end += 4
02640 (length,) = _struct_I.unpack(str[start:end])
02641 val1.pose = []
02642 for i in xrange(0, length):
02643 val2 = geometry_msgs.msg.Pose()
02644 _v192 = val2.position
02645 _x = _v192
02646 start = end
02647 end += 24
02648 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
02649 _v193 = val2.orientation
02650 _x = _v193
02651 start = end
02652 end += 32
02653 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
02654 val1.pose.append(val2)
02655 start = end
02656 end += 4
02657 (length,) = _struct_I.unpack(str[start:end])
02658 val1.pose_projected = []
02659 for i in xrange(0, length):
02660 val2 = geometry_msgs.msg.Pose()
02661 _v194 = val2.position
02662 _x = _v194
02663 start = end
02664 end += 24
02665 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
02666 _v195 = val2.orientation
02667 _x = _v195
02668 start = end
02669 end += 32
02670 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
02671 val1.pose_projected.append(val2)
02672 start = end
02673 end += 4
02674 (length,) = _struct_I.unpack(str[start:end])
02675 val1.pose_resampled = []
02676 for i in xrange(0, length):
02677 val2 = geometry_msgs.msg.Pose()
02678 _v196 = val2.position
02679 _x = _v196
02680 start = end
02681 end += 24
02682 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
02683 _v197 = val2.orientation
02684 _x = _v197
02685 start = end
02686 end += 32
02687 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
02688 val1.pose_resampled.append(val2)
02689 start = end
02690 end += 4
02691 (length,) = _struct_I.unpack(str[start:end])
02692 pattern = '<%sI'%length
02693 start = end
02694 end += struct.calcsize(pattern)
02695 val1.pose_flags = numpy.frombuffer(str[start:end], dtype=numpy.uint32, count=length)
02696 start = end
02697 end += 4
02698 (length,) = _struct_I.unpack(str[start:end])
02699 val1.channels = []
02700 for i in xrange(0, length):
02701 val2 = sensor_msgs.msg.ChannelFloat32()
02702 start = end
02703 end += 4
02704 (length,) = _struct_I.unpack(str[start:end])
02705 start = end
02706 end += length
02707 val2.name = str[start:end]
02708 start = end
02709 end += 4
02710 (length,) = _struct_I.unpack(str[start:end])
02711 pattern = '<%sf'%length
02712 start = end
02713 end += struct.calcsize(pattern)
02714 val2.values = numpy.frombuffer(str[start:end], dtype=numpy.float32, count=length)
02715 val1.channels.append(val2)
02716 self.object.parts.append(val1)
02717 start = end
02718 end += 4
02719 (length,) = _struct_I.unpack(str[start:end])
02720 self.object.params = []
02721 for i in xrange(0, length):
02722 val1 = articulation_msgs.msg.ParamMsg()
02723 start = end
02724 end += 4
02725 (length,) = _struct_I.unpack(str[start:end])
02726 start = end
02727 end += length
02728 val1.name = str[start:end]
02729 _x = val1
02730 start = end
02731 end += 9
02732 (_x.value, _x.type,) = _struct_dB.unpack(str[start:end])
02733 self.object.params.append(val1)
02734 start = end
02735 end += 4
02736 (length,) = _struct_I.unpack(str[start:end])
02737 self.object.models = []
02738 for i in xrange(0, length):
02739 val1 = articulation_msgs.msg.ModelMsg()
02740 _v198 = val1.header
02741 start = end
02742 end += 4
02743 (_v198.seq,) = _struct_I.unpack(str[start:end])
02744 _v199 = _v198.stamp
02745 _x = _v199
02746 start = end
02747 end += 8
02748 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
02749 start = end
02750 end += 4
02751 (length,) = _struct_I.unpack(str[start:end])
02752 start = end
02753 end += length
02754 _v198.frame_id = str[start:end]
02755 start = end
02756 end += 4
02757 (val1.id,) = _struct_i.unpack(str[start:end])
02758 start = end
02759 end += 4
02760 (length,) = _struct_I.unpack(str[start:end])
02761 start = end
02762 end += length
02763 val1.name = str[start:end]
02764 start = end
02765 end += 4
02766 (length,) = _struct_I.unpack(str[start:end])
02767 val1.params = []
02768 for i in xrange(0, length):
02769 val2 = articulation_msgs.msg.ParamMsg()
02770 start = end
02771 end += 4
02772 (length,) = _struct_I.unpack(str[start:end])
02773 start = end
02774 end += length
02775 val2.name = str[start:end]
02776 _x = val2
02777 start = end
02778 end += 9
02779 (_x.value, _x.type,) = _struct_dB.unpack(str[start:end])
02780 val1.params.append(val2)
02781 _v200 = val1.track
02782 _v201 = _v200.header
02783 start = end
02784 end += 4
02785 (_v201.seq,) = _struct_I.unpack(str[start:end])
02786 _v202 = _v201.stamp
02787 _x = _v202
02788 start = end
02789 end += 8
02790 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
02791 start = end
02792 end += 4
02793 (length,) = _struct_I.unpack(str[start:end])
02794 start = end
02795 end += length
02796 _v201.frame_id = str[start:end]
02797 start = end
02798 end += 4
02799 (_v200.id,) = _struct_i.unpack(str[start:end])
02800 start = end
02801 end += 4
02802 (length,) = _struct_I.unpack(str[start:end])
02803 _v200.pose = []
02804 for i in xrange(0, length):
02805 val3 = geometry_msgs.msg.Pose()
02806 _v203 = val3.position
02807 _x = _v203
02808 start = end
02809 end += 24
02810 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
02811 _v204 = val3.orientation
02812 _x = _v204
02813 start = end
02814 end += 32
02815 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
02816 _v200.pose.append(val3)
02817 start = end
02818 end += 4
02819 (length,) = _struct_I.unpack(str[start:end])
02820 _v200.pose_projected = []
02821 for i in xrange(0, length):
02822 val3 = geometry_msgs.msg.Pose()
02823 _v205 = val3.position
02824 _x = _v205
02825 start = end
02826 end += 24
02827 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
02828 _v206 = val3.orientation
02829 _x = _v206
02830 start = end
02831 end += 32
02832 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
02833 _v200.pose_projected.append(val3)
02834 start = end
02835 end += 4
02836 (length,) = _struct_I.unpack(str[start:end])
02837 _v200.pose_resampled = []
02838 for i in xrange(0, length):
02839 val3 = geometry_msgs.msg.Pose()
02840 _v207 = val3.position
02841 _x = _v207
02842 start = end
02843 end += 24
02844 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
02845 _v208 = val3.orientation
02846 _x = _v208
02847 start = end
02848 end += 32
02849 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
02850 _v200.pose_resampled.append(val3)
02851 start = end
02852 end += 4
02853 (length,) = _struct_I.unpack(str[start:end])
02854 pattern = '<%sI'%length
02855 start = end
02856 end += struct.calcsize(pattern)
02857 _v200.pose_flags = numpy.frombuffer(str[start:end], dtype=numpy.uint32, count=length)
02858 start = end
02859 end += 4
02860 (length,) = _struct_I.unpack(str[start:end])
02861 _v200.channels = []
02862 for i in xrange(0, length):
02863 val3 = sensor_msgs.msg.ChannelFloat32()
02864 start = end
02865 end += 4
02866 (length,) = _struct_I.unpack(str[start:end])
02867 start = end
02868 end += length
02869 val3.name = str[start:end]
02870 start = end
02871 end += 4
02872 (length,) = _struct_I.unpack(str[start:end])
02873 pattern = '<%sf'%length
02874 start = end
02875 end += struct.calcsize(pattern)
02876 val3.values = numpy.frombuffer(str[start:end], dtype=numpy.float32, count=length)
02877 _v200.channels.append(val3)
02878 self.object.models.append(val1)
02879 start = end
02880 end += 4
02881 (length,) = _struct_I.unpack(str[start:end])
02882 self.object.markers.markers = []
02883 for i in xrange(0, length):
02884 val1 = visualization_msgs.msg.Marker()
02885 _v209 = val1.header
02886 start = end
02887 end += 4
02888 (_v209.seq,) = _struct_I.unpack(str[start:end])
02889 _v210 = _v209.stamp
02890 _x = _v210
02891 start = end
02892 end += 8
02893 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
02894 start = end
02895 end += 4
02896 (length,) = _struct_I.unpack(str[start:end])
02897 start = end
02898 end += length
02899 _v209.frame_id = str[start:end]
02900 start = end
02901 end += 4
02902 (length,) = _struct_I.unpack(str[start:end])
02903 start = end
02904 end += length
02905 val1.ns = str[start:end]
02906 _x = val1
02907 start = end
02908 end += 12
02909 (_x.id, _x.type, _x.action,) = _struct_3i.unpack(str[start:end])
02910 _v211 = val1.pose
02911 _v212 = _v211.position
02912 _x = _v212
02913 start = end
02914 end += 24
02915 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
02916 _v213 = _v211.orientation
02917 _x = _v213
02918 start = end
02919 end += 32
02920 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
02921 _v214 = val1.scale
02922 _x = _v214
02923 start = end
02924 end += 24
02925 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
02926 _v215 = val1.color
02927 _x = _v215
02928 start = end
02929 end += 16
02930 (_x.r, _x.g, _x.b, _x.a,) = _struct_4f.unpack(str[start:end])
02931 _v216 = val1.lifetime
02932 _x = _v216
02933 start = end
02934 end += 8
02935 (_x.secs, _x.nsecs,) = _struct_2i.unpack(str[start:end])
02936 start = end
02937 end += 1
02938 (val1.frame_locked,) = _struct_B.unpack(str[start:end])
02939 val1.frame_locked = bool(val1.frame_locked)
02940 start = end
02941 end += 4
02942 (length,) = _struct_I.unpack(str[start:end])
02943 val1.points = []
02944 for i in xrange(0, length):
02945 val2 = geometry_msgs.msg.Point()
02946 _x = val2
02947 start = end
02948 end += 24
02949 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
02950 val1.points.append(val2)
02951 start = end
02952 end += 4
02953 (length,) = _struct_I.unpack(str[start:end])
02954 val1.colors = []
02955 for i in xrange(0, length):
02956 val2 = std_msgs.msg.ColorRGBA()
02957 _x = val2
02958 start = end
02959 end += 16
02960 (_x.r, _x.g, _x.b, _x.a,) = _struct_4f.unpack(str[start:end])
02961 val1.colors.append(val2)
02962 start = end
02963 end += 4
02964 (length,) = _struct_I.unpack(str[start:end])
02965 start = end
02966 end += length
02967 val1.text = str[start:end]
02968 start = end
02969 end += 4
02970 (length,) = _struct_I.unpack(str[start:end])
02971 start = end
02972 end += length
02973 val1.mesh_resource = str[start:end]
02974 start = end
02975 end += 1
02976 (val1.mesh_use_embedded_materials,) = _struct_B.unpack(str[start:end])
02977 val1.mesh_use_embedded_materials = bool(val1.mesh_use_embedded_materials)
02978 self.object.markers.markers.append(val1)
02979 return self
02980 except struct.error, e:
02981 raise roslib.message.DeserializationError(e) #most likely buffer underfill
02982
02983 _struct_I = roslib.message.struct_I
02984 _struct_B = struct.Struct("<B")
02985 _struct_dB = struct.Struct("<dB")
02986 _struct_3i = struct.Struct("<3i")
02987 _struct_2i = struct.Struct("<2i")
02988 _struct_i = struct.Struct("<i")
02989 _struct_3I = struct.Struct("<3I")
02990 _struct_4f = struct.Struct("<4f")
02991 _struct_4d = struct.Struct("<4d")
02992 _struct_2I = struct.Struct("<2I")
02993 _struct_3d = struct.Struct("<3d")
02994 class ArticulatedObjectSrv(roslib.message.ServiceDefinition):
02995 _type = 'articulation_msgs/ArticulatedObjectSrv'
02996 _md5sum = 'd13520b58d3ca45a23cc7f92f8661f22'
02997 _request_class = ArticulatedObjectSrvRequest
02998 _response_class = ArticulatedObjectSrvResponse