00001 """autogenerated by genmsg_py from CollisionObject.msg. Do not edit."""
00002 import roslib.message
00003 import struct
00004
00005 import mapping_msgs.msg
00006 import geometry_msgs.msg
00007 import geometric_shapes_msgs.msg
00008 import std_msgs.msg
00009
00010 class CollisionObject(roslib.message.Message):
00011 _md5sum = "c25d22faff81b340d88e28e270ae03f5"
00012 _type = "mapping_msgs/CollisionObject"
00013 _has_header = True
00014 _full_text = """# a header, used for interpreting the poses
00015 Header header
00016
00017 # the id of the object
00018 string id
00019
00020 #This contains what is to be done with the object
00021 CollisionObjectOperation operation
00022
00023 #the shapes associated with the object
00024 geometric_shapes_msgs/Shape[] shapes
00025
00026 #the poses associated with the shapes - will be transformed using the header
00027 geometry_msgs/Pose[] poses
00028
00029 ================================================================================
00030 MSG: std_msgs/Header
00031 # Standard metadata for higher-level stamped data types.
00032 # This is generally used to communicate timestamped data
00033 # in a particular coordinate frame.
00034 #
00035 # sequence ID: consecutively increasing ID
00036 uint32 seq
00037 #Two-integer timestamp that is expressed as:
00038 # * stamp.secs: seconds (stamp_secs) since epoch
00039 # * stamp.nsecs: nanoseconds since stamp_secs
00040 # time-handling sugar is provided by the client library
00041 time stamp
00042 #Frame this data is associated with
00043 # 0: no frame
00044 # 1: global frame
00045 string frame_id
00046
00047 ================================================================================
00048 MSG: mapping_msgs/CollisionObjectOperation
00049 #Puts the object into the environment
00050 #or updates the object if already added
00051 byte ADD=0
00052
00053 #Removes the object from the environment entirely
00054 byte REMOVE=1
00055
00056 #Only valid within the context of a CollisionAttachedObject message
00057 #Will be ignored if sent with an CollisionObject message
00058 #Takes an attached object, detaches from the attached link
00059 #But adds back in as regular object
00060 byte DETACH_AND_ADD_AS_OBJECT=2
00061
00062 #Only valid within the context of a CollisionAttachedObject message
00063 #Will be ignored if sent with an CollisionObject message
00064 #Takes current object in the environment and removes it as
00065 #a regular object
00066 byte ATTACH_AND_REMOVE_AS_OBJECT=3
00067
00068 # Byte code for operation
00069 byte operation
00070
00071 ================================================================================
00072 MSG: geometric_shapes_msgs/Shape
00073 byte SPHERE=0
00074 byte BOX=1
00075 byte CYLINDER=2
00076 byte MESH=3
00077
00078 byte type
00079
00080
00081 #### define sphere, box, cylinder ####
00082 # the origin of each shape is considered at the shape's center
00083
00084 # for sphere
00085 # radius := dimensions[0]
00086
00087 # for cylinder
00088 # radius := dimensions[0]
00089 # length := dimensions[1]
00090 # the length is along the Z axis
00091
00092 # for box
00093 # size_x := dimensions[0]
00094 # size_y := dimensions[1]
00095 # size_z := dimensions[2]
00096 float64[] dimensions
00097
00098
00099 #### define mesh ####
00100
00101 # list of triangles; triangle k is defined by tre vertices located
00102 # at indices triangles[3k], triangles[3k+1], triangles[3k+2]
00103 int32[] triangles
00104 geometry_msgs/Point[] vertices
00105
00106 ================================================================================
00107 MSG: geometry_msgs/Point
00108 # This contains the position of a point in free space
00109 float64 x
00110 float64 y
00111 float64 z
00112
00113 ================================================================================
00114 MSG: geometry_msgs/Pose
00115 # A representation of pose in free space, composed of postion and orientation.
00116 Point position
00117 Quaternion orientation
00118
00119 ================================================================================
00120 MSG: geometry_msgs/Quaternion
00121 # This represents an orientation in free space in quaternion form.
00122
00123 float64 x
00124 float64 y
00125 float64 z
00126 float64 w
00127
00128 """
00129 __slots__ = ['header','id','operation','shapes','poses']
00130 _slot_types = ['Header','string','mapping_msgs/CollisionObjectOperation','geometric_shapes_msgs/Shape[]','geometry_msgs/Pose[]']
00131
00132 def __init__(self, *args, **kwds):
00133 """
00134 Constructor. Any message fields that are implicitly/explicitly
00135 set to None will be assigned a default value. The recommend
00136 use is keyword arguments as this is more robust to future message
00137 changes. You cannot mix in-order arguments and keyword arguments.
00138
00139 The available fields are:
00140 header,id,operation,shapes,poses
00141
00142 @param args: complete set of field values, in .msg order
00143 @param kwds: use keyword arguments corresponding to message field names
00144 to set specific fields.
00145 """
00146 if args or kwds:
00147 super(CollisionObject, self).__init__(*args, **kwds)
00148
00149 if self.header is None:
00150 self.header = std_msgs.msg._Header.Header()
00151 if self.id is None:
00152 self.id = ''
00153 if self.operation is None:
00154 self.operation = mapping_msgs.msg.CollisionObjectOperation()
00155 if self.shapes is None:
00156 self.shapes = []
00157 if self.poses is None:
00158 self.poses = []
00159 else:
00160 self.header = std_msgs.msg._Header.Header()
00161 self.id = ''
00162 self.operation = mapping_msgs.msg.CollisionObjectOperation()
00163 self.shapes = []
00164 self.poses = []
00165
00166 def _get_types(self):
00167 """
00168 internal API method
00169 """
00170 return self._slot_types
00171
00172 def serialize(self, buff):
00173 """
00174 serialize message into buffer
00175 @param buff: buffer
00176 @type buff: StringIO
00177 """
00178 try:
00179 _x = self
00180 buff.write(_struct_3I.pack(_x.header.seq, _x.header.stamp.secs, _x.header.stamp.nsecs))
00181 _x = self.header.frame_id
00182 length = len(_x)
00183 buff.write(struct.pack('<I%ss'%length, length, _x))
00184 _x = self.id
00185 length = len(_x)
00186 buff.write(struct.pack('<I%ss'%length, length, _x))
00187 buff.write(_struct_b.pack(self.operation.operation))
00188 length = len(self.shapes)
00189 buff.write(_struct_I.pack(length))
00190 for val1 in self.shapes:
00191 buff.write(_struct_b.pack(val1.type))
00192 length = len(val1.dimensions)
00193 buff.write(_struct_I.pack(length))
00194 pattern = '<%sd'%length
00195 buff.write(struct.pack(pattern, *val1.dimensions))
00196 length = len(val1.triangles)
00197 buff.write(_struct_I.pack(length))
00198 pattern = '<%si'%length
00199 buff.write(struct.pack(pattern, *val1.triangles))
00200 length = len(val1.vertices)
00201 buff.write(_struct_I.pack(length))
00202 for val2 in val1.vertices:
00203 _x = val2
00204 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00205 length = len(self.poses)
00206 buff.write(_struct_I.pack(length))
00207 for val1 in self.poses:
00208 _v1 = val1.position
00209 _x = _v1
00210 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00211 _v2 = val1.orientation
00212 _x = _v2
00213 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
00214 except struct.error, se: self._check_types(se)
00215 except TypeError, te: self._check_types(te)
00216
00217 def deserialize(self, str):
00218 """
00219 unpack serialized message in str into this message instance
00220 @param str: byte array of serialized message
00221 @type str: str
00222 """
00223 try:
00224 if self.header is None:
00225 self.header = std_msgs.msg._Header.Header()
00226 if self.operation is None:
00227 self.operation = mapping_msgs.msg.CollisionObjectOperation()
00228 end = 0
00229 _x = self
00230 start = end
00231 end += 12
00232 (_x.header.seq, _x.header.stamp.secs, _x.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
00233 start = end
00234 end += 4
00235 (length,) = _struct_I.unpack(str[start:end])
00236 start = end
00237 end += length
00238 self.header.frame_id = str[start:end]
00239 start = end
00240 end += 4
00241 (length,) = _struct_I.unpack(str[start:end])
00242 start = end
00243 end += length
00244 self.id = str[start:end]
00245 start = end
00246 end += 1
00247 (self.operation.operation,) = _struct_b.unpack(str[start:end])
00248 start = end
00249 end += 4
00250 (length,) = _struct_I.unpack(str[start:end])
00251 self.shapes = []
00252 for i in xrange(0, length):
00253 val1 = geometric_shapes_msgs.msg.Shape()
00254 start = end
00255 end += 1
00256 (val1.type,) = _struct_b.unpack(str[start:end])
00257 start = end
00258 end += 4
00259 (length,) = _struct_I.unpack(str[start:end])
00260 pattern = '<%sd'%length
00261 start = end
00262 end += struct.calcsize(pattern)
00263 val1.dimensions = struct.unpack(pattern, str[start:end])
00264 start = end
00265 end += 4
00266 (length,) = _struct_I.unpack(str[start:end])
00267 pattern = '<%si'%length
00268 start = end
00269 end += struct.calcsize(pattern)
00270 val1.triangles = struct.unpack(pattern, str[start:end])
00271 start = end
00272 end += 4
00273 (length,) = _struct_I.unpack(str[start:end])
00274 val1.vertices = []
00275 for i in xrange(0, length):
00276 val2 = geometry_msgs.msg.Point()
00277 _x = val2
00278 start = end
00279 end += 24
00280 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
00281 val1.vertices.append(val2)
00282 self.shapes.append(val1)
00283 start = end
00284 end += 4
00285 (length,) = _struct_I.unpack(str[start:end])
00286 self.poses = []
00287 for i in xrange(0, length):
00288 val1 = geometry_msgs.msg.Pose()
00289 _v3 = val1.position
00290 _x = _v3
00291 start = end
00292 end += 24
00293 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
00294 _v4 = val1.orientation
00295 _x = _v4
00296 start = end
00297 end += 32
00298 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
00299 self.poses.append(val1)
00300 return self
00301 except struct.error, e:
00302 raise roslib.message.DeserializationError(e)
00303
00304
00305 def serialize_numpy(self, buff, numpy):
00306 """
00307 serialize message with numpy array types into buffer
00308 @param buff: buffer
00309 @type buff: StringIO
00310 @param numpy: numpy python module
00311 @type numpy module
00312 """
00313 try:
00314 _x = self
00315 buff.write(_struct_3I.pack(_x.header.seq, _x.header.stamp.secs, _x.header.stamp.nsecs))
00316 _x = self.header.frame_id
00317 length = len(_x)
00318 buff.write(struct.pack('<I%ss'%length, length, _x))
00319 _x = self.id
00320 length = len(_x)
00321 buff.write(struct.pack('<I%ss'%length, length, _x))
00322 buff.write(_struct_b.pack(self.operation.operation))
00323 length = len(self.shapes)
00324 buff.write(_struct_I.pack(length))
00325 for val1 in self.shapes:
00326 buff.write(_struct_b.pack(val1.type))
00327 length = len(val1.dimensions)
00328 buff.write(_struct_I.pack(length))
00329 pattern = '<%sd'%length
00330 buff.write(val1.dimensions.tostring())
00331 length = len(val1.triangles)
00332 buff.write(_struct_I.pack(length))
00333 pattern = '<%si'%length
00334 buff.write(val1.triangles.tostring())
00335 length = len(val1.vertices)
00336 buff.write(_struct_I.pack(length))
00337 for val2 in val1.vertices:
00338 _x = val2
00339 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00340 length = len(self.poses)
00341 buff.write(_struct_I.pack(length))
00342 for val1 in self.poses:
00343 _v5 = val1.position
00344 _x = _v5
00345 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00346 _v6 = val1.orientation
00347 _x = _v6
00348 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
00349 except struct.error, se: self._check_types(se)
00350 except TypeError, te: self._check_types(te)
00351
00352 def deserialize_numpy(self, str, numpy):
00353 """
00354 unpack serialized message in str into this message instance using numpy for array types
00355 @param str: byte array of serialized message
00356 @type str: str
00357 @param numpy: numpy python module
00358 @type numpy: module
00359 """
00360 try:
00361 if self.header is None:
00362 self.header = std_msgs.msg._Header.Header()
00363 if self.operation is None:
00364 self.operation = mapping_msgs.msg.CollisionObjectOperation()
00365 end = 0
00366 _x = self
00367 start = end
00368 end += 12
00369 (_x.header.seq, _x.header.stamp.secs, _x.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
00370 start = end
00371 end += 4
00372 (length,) = _struct_I.unpack(str[start:end])
00373 start = end
00374 end += length
00375 self.header.frame_id = str[start:end]
00376 start = end
00377 end += 4
00378 (length,) = _struct_I.unpack(str[start:end])
00379 start = end
00380 end += length
00381 self.id = str[start:end]
00382 start = end
00383 end += 1
00384 (self.operation.operation,) = _struct_b.unpack(str[start:end])
00385 start = end
00386 end += 4
00387 (length,) = _struct_I.unpack(str[start:end])
00388 self.shapes = []
00389 for i in xrange(0, length):
00390 val1 = geometric_shapes_msgs.msg.Shape()
00391 start = end
00392 end += 1
00393 (val1.type,) = _struct_b.unpack(str[start:end])
00394 start = end
00395 end += 4
00396 (length,) = _struct_I.unpack(str[start:end])
00397 pattern = '<%sd'%length
00398 start = end
00399 end += struct.calcsize(pattern)
00400 val1.dimensions = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
00401 start = end
00402 end += 4
00403 (length,) = _struct_I.unpack(str[start:end])
00404 pattern = '<%si'%length
00405 start = end
00406 end += struct.calcsize(pattern)
00407 val1.triangles = numpy.frombuffer(str[start:end], dtype=numpy.int32, count=length)
00408 start = end
00409 end += 4
00410 (length,) = _struct_I.unpack(str[start:end])
00411 val1.vertices = []
00412 for i in xrange(0, length):
00413 val2 = geometry_msgs.msg.Point()
00414 _x = val2
00415 start = end
00416 end += 24
00417 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
00418 val1.vertices.append(val2)
00419 self.shapes.append(val1)
00420 start = end
00421 end += 4
00422 (length,) = _struct_I.unpack(str[start:end])
00423 self.poses = []
00424 for i in xrange(0, length):
00425 val1 = geometry_msgs.msg.Pose()
00426 _v7 = val1.position
00427 _x = _v7
00428 start = end
00429 end += 24
00430 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
00431 _v8 = val1.orientation
00432 _x = _v8
00433 start = end
00434 end += 32
00435 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
00436 self.poses.append(val1)
00437 return self
00438 except struct.error, e:
00439 raise roslib.message.DeserializationError(e)
00440
00441 _struct_I = roslib.message.struct_I
00442 _struct_3I = struct.Struct("<3I")
00443 _struct_b = struct.Struct("<b")
00444 _struct_4d = struct.Struct("<4d")
00445 _struct_3d = struct.Struct("<3d")