00001 """autogenerated by genpy from tabletop_object_detector/TabletopDetectionResult.msg. Do not edit."""
00002 import sys
00003 python3 = True if sys.hexversion > 0x03000000 else False
00004 import genpy
00005 import struct
00006
00007 import arm_navigation_msgs.msg
00008 import tabletop_object_detector.msg
00009 import geometry_msgs.msg
00010 import sensor_msgs.msg
00011 import std_msgs.msg
00012 import household_objects_database_msgs.msg
00013
00014 class TabletopDetectionResult(genpy.Message):
00015 _md5sum = "026c0e5e13258b4a96f7d3999ea3cd64"
00016 _type = "tabletop_object_detector/TabletopDetectionResult"
00017 _has_header = False
00018 _full_text = """# Contains all the information from one run of the tabletop detection node
00019
00020 # The information for the plane that has been detected
00021 Table table
00022
00023 # The raw clusters detected in the scan
00024 sensor_msgs/PointCloud[] clusters
00025
00026 # The list of potential models that have been detected for each cluster
00027 # An empty list will be returned for a cluster that has no recognition results at all
00028 household_objects_database_msgs/DatabaseModelPoseList[] models
00029
00030 # For each cluster, the index of the list of models that was fit to that cluster
00031 # keep in mind that multiple raw clusters can correspond to a single fit
00032 int32[] cluster_model_indices
00033
00034 # Whether the detection has succeeded or failed
00035 int32 NO_CLOUD_RECEIVED = 1
00036 int32 NO_TABLE = 2
00037 int32 OTHER_ERROR = 3
00038 int32 SUCCESS = 4
00039 int32 result
00040
00041 ================================================================================
00042 MSG: tabletop_object_detector/Table
00043 # Informs that a planar table has been detected at a given location
00044
00045 # The pose gives you the transform that take you to the coordinate system
00046 # of the table, with the origin somewhere in the table plane and the
00047 # z axis normal to the plane
00048 geometry_msgs/PoseStamped pose
00049
00050 # These values give you the observed extents of the table, along x and y,
00051 # in the table's own coordinate system (above)
00052 # there is no guarantee that the origin of the table coordinate system is
00053 # inside the boundary defined by these values.
00054 float32 x_min
00055 float32 x_max
00056 float32 y_min
00057 float32 y_max
00058
00059 # There is no guarantee that the table does NOT extend further than these
00060 # values; this is just as far as we've observed it.
00061
00062
00063 # Newer table definition as triangle mesh of convex hull (relative to pose)
00064 arm_navigation_msgs/Shape convex_hull
00065
00066 ================================================================================
00067 MSG: geometry_msgs/PoseStamped
00068 # A Pose with reference coordinate frame and timestamp
00069 Header header
00070 Pose pose
00071
00072 ================================================================================
00073 MSG: std_msgs/Header
00074 # Standard metadata for higher-level stamped data types.
00075 # This is generally used to communicate timestamped data
00076 # in a particular coordinate frame.
00077 #
00078 # sequence ID: consecutively increasing ID
00079 uint32 seq
00080 #Two-integer timestamp that is expressed as:
00081 # * stamp.secs: seconds (stamp_secs) since epoch
00082 # * stamp.nsecs: nanoseconds since stamp_secs
00083 # time-handling sugar is provided by the client library
00084 time stamp
00085 #Frame this data is associated with
00086 # 0: no frame
00087 # 1: global frame
00088 string frame_id
00089
00090 ================================================================================
00091 MSG: geometry_msgs/Pose
00092 # A representation of pose in free space, composed of postion and orientation.
00093 Point position
00094 Quaternion orientation
00095
00096 ================================================================================
00097 MSG: geometry_msgs/Point
00098 # This contains the position of a point in free space
00099 float64 x
00100 float64 y
00101 float64 z
00102
00103 ================================================================================
00104 MSG: geometry_msgs/Quaternion
00105 # This represents an orientation in free space in quaternion form.
00106
00107 float64 x
00108 float64 y
00109 float64 z
00110 float64 w
00111
00112 ================================================================================
00113 MSG: arm_navigation_msgs/Shape
00114 byte SPHERE=0
00115 byte BOX=1
00116 byte CYLINDER=2
00117 byte MESH=3
00118
00119 byte type
00120
00121
00122 #### define sphere, box, cylinder ####
00123 # the origin of each shape is considered at the shape's center
00124
00125 # for sphere
00126 # radius := dimensions[0]
00127
00128 # for cylinder
00129 # radius := dimensions[0]
00130 # length := dimensions[1]
00131 # the length is along the Z axis
00132
00133 # for box
00134 # size_x := dimensions[0]
00135 # size_y := dimensions[1]
00136 # size_z := dimensions[2]
00137 float64[] dimensions
00138
00139
00140 #### define mesh ####
00141
00142 # list of triangles; triangle k is defined by tre vertices located
00143 # at indices triangles[3k], triangles[3k+1], triangles[3k+2]
00144 int32[] triangles
00145 geometry_msgs/Point[] vertices
00146
00147 ================================================================================
00148 MSG: sensor_msgs/PointCloud
00149 # This message holds a collection of 3d points, plus optional additional
00150 # information about each point.
00151
00152 # Time of sensor data acquisition, coordinate frame ID.
00153 Header header
00154
00155 # Array of 3d points. Each Point32 should be interpreted as a 3d point
00156 # in the frame given in the header.
00157 geometry_msgs/Point32[] points
00158
00159 # Each channel should have the same number of elements as points array,
00160 # and the data in each channel should correspond 1:1 with each point.
00161 # Channel names in common practice are listed in ChannelFloat32.msg.
00162 ChannelFloat32[] channels
00163
00164 ================================================================================
00165 MSG: geometry_msgs/Point32
00166 # This contains the position of a point in free space(with 32 bits of precision).
00167 # It is recommeded to use Point wherever possible instead of Point32.
00168 #
00169 # This recommendation is to promote interoperability.
00170 #
00171 # This message is designed to take up less space when sending
00172 # lots of points at once, as in the case of a PointCloud.
00173
00174 float32 x
00175 float32 y
00176 float32 z
00177 ================================================================================
00178 MSG: sensor_msgs/ChannelFloat32
00179 # This message is used by the PointCloud message to hold optional data
00180 # associated with each point in the cloud. The length of the values
00181 # array should be the same as the length of the points array in the
00182 # PointCloud, and each value should be associated with the corresponding
00183 # point.
00184
00185 # Channel names in existing practice include:
00186 # "u", "v" - row and column (respectively) in the left stereo image.
00187 # This is opposite to usual conventions but remains for
00188 # historical reasons. The newer PointCloud2 message has no
00189 # such problem.
00190 # "rgb" - For point clouds produced by color stereo cameras. uint8
00191 # (R,G,B) values packed into the least significant 24 bits,
00192 # in order.
00193 # "intensity" - laser or pixel intensity.
00194 # "distance"
00195
00196 # The channel name should give semantics of the channel (e.g.
00197 # "intensity" instead of "value").
00198 string name
00199
00200 # The values array should be 1-1 with the elements of the associated
00201 # PointCloud.
00202 float32[] values
00203
00204 ================================================================================
00205 MSG: household_objects_database_msgs/DatabaseModelPoseList
00206 # stores a list of possible database models recognition results
00207 DatabaseModelPose[] model_list
00208 ================================================================================
00209 MSG: household_objects_database_msgs/DatabaseModelPose
00210 # Informs that a specific model from the Model Database has been
00211 # identified at a certain location
00212
00213 # the database id of the model
00214 int32 model_id
00215
00216 # the pose that it can be found in
00217 geometry_msgs/PoseStamped pose
00218
00219 # a measure of the confidence level in this detection result
00220 float32 confidence
00221
00222 # the name of the object detector that generated this detection result
00223 string detector_name
00224
00225 """
00226 # Pseudo-constants
00227 NO_CLOUD_RECEIVED = 1
00228 NO_TABLE = 2
00229 OTHER_ERROR = 3
00230 SUCCESS = 4
00231
00232 __slots__ = ['table','clusters','models','cluster_model_indices','result']
00233 _slot_types = ['tabletop_object_detector/Table','sensor_msgs/PointCloud[]','household_objects_database_msgs/DatabaseModelPoseList[]','int32[]','int32']
00234
00235 def __init__(self, *args, **kwds):
00236 """
00237 Constructor. Any message fields that are implicitly/explicitly
00238 set to None will be assigned a default value. The recommend
00239 use is keyword arguments as this is more robust to future message
00240 changes. You cannot mix in-order arguments and keyword arguments.
00241
00242 The available fields are:
00243 table,clusters,models,cluster_model_indices,result
00244
00245 :param args: complete set of field values, in .msg order
00246 :param kwds: use keyword arguments corresponding to message field names
00247 to set specific fields.
00248 """
00249 if args or kwds:
00250 super(TabletopDetectionResult, self).__init__(*args, **kwds)
00251 #message fields cannot be None, assign default values for those that are
00252 if self.table is None:
00253 self.table = tabletop_object_detector.msg.Table()
00254 if self.clusters is None:
00255 self.clusters = []
00256 if self.models is None:
00257 self.models = []
00258 if self.cluster_model_indices is None:
00259 self.cluster_model_indices = []
00260 if self.result is None:
00261 self.result = 0
00262 else:
00263 self.table = tabletop_object_detector.msg.Table()
00264 self.clusters = []
00265 self.models = []
00266 self.cluster_model_indices = []
00267 self.result = 0
00268
00269 def _get_types(self):
00270 """
00271 internal API method
00272 """
00273 return self._slot_types
00274
00275 def serialize(self, buff):
00276 """
00277 serialize message into buffer
00278 :param buff: buffer, ``StringIO``
00279 """
00280 try:
00281 _x = self
00282 buff.write(_struct_3I.pack(_x.table.pose.header.seq, _x.table.pose.header.stamp.secs, _x.table.pose.header.stamp.nsecs))
00283 _x = self.table.pose.header.frame_id
00284 length = len(_x)
00285 if python3 or type(_x) == unicode:
00286 _x = _x.encode('utf-8')
00287 length = len(_x)
00288 buff.write(struct.pack('<I%ss'%length, length, _x))
00289 _x = self
00290 buff.write(_struct_7d4fb.pack(_x.table.pose.pose.position.x, _x.table.pose.pose.position.y, _x.table.pose.pose.position.z, _x.table.pose.pose.orientation.x, _x.table.pose.pose.orientation.y, _x.table.pose.pose.orientation.z, _x.table.pose.pose.orientation.w, _x.table.x_min, _x.table.x_max, _x.table.y_min, _x.table.y_max, _x.table.convex_hull.type))
00291 length = len(self.table.convex_hull.dimensions)
00292 buff.write(_struct_I.pack(length))
00293 pattern = '<%sd'%length
00294 buff.write(struct.pack(pattern, *self.table.convex_hull.dimensions))
00295 length = len(self.table.convex_hull.triangles)
00296 buff.write(_struct_I.pack(length))
00297 pattern = '<%si'%length
00298 buff.write(struct.pack(pattern, *self.table.convex_hull.triangles))
00299 length = len(self.table.convex_hull.vertices)
00300 buff.write(_struct_I.pack(length))
00301 for val1 in self.table.convex_hull.vertices:
00302 _x = val1
00303 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00304 length = len(self.clusters)
00305 buff.write(_struct_I.pack(length))
00306 for val1 in self.clusters:
00307 _v1 = val1.header
00308 buff.write(_struct_I.pack(_v1.seq))
00309 _v2 = _v1.stamp
00310 _x = _v2
00311 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
00312 _x = _v1.frame_id
00313 length = len(_x)
00314 if python3 or type(_x) == unicode:
00315 _x = _x.encode('utf-8')
00316 length = len(_x)
00317 buff.write(struct.pack('<I%ss'%length, length, _x))
00318 length = len(val1.points)
00319 buff.write(_struct_I.pack(length))
00320 for val2 in val1.points:
00321 _x = val2
00322 buff.write(_struct_3f.pack(_x.x, _x.y, _x.z))
00323 length = len(val1.channels)
00324 buff.write(_struct_I.pack(length))
00325 for val2 in val1.channels:
00326 _x = val2.name
00327 length = len(_x)
00328 if python3 or type(_x) == unicode:
00329 _x = _x.encode('utf-8')
00330 length = len(_x)
00331 buff.write(struct.pack('<I%ss'%length, length, _x))
00332 length = len(val2.values)
00333 buff.write(_struct_I.pack(length))
00334 pattern = '<%sf'%length
00335 buff.write(struct.pack(pattern, *val2.values))
00336 length = len(self.models)
00337 buff.write(_struct_I.pack(length))
00338 for val1 in self.models:
00339 length = len(val1.model_list)
00340 buff.write(_struct_I.pack(length))
00341 for val2 in val1.model_list:
00342 buff.write(_struct_i.pack(val2.model_id))
00343 _v3 = val2.pose
00344 _v4 = _v3.header
00345 buff.write(_struct_I.pack(_v4.seq))
00346 _v5 = _v4.stamp
00347 _x = _v5
00348 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
00349 _x = _v4.frame_id
00350 length = len(_x)
00351 if python3 or type(_x) == unicode:
00352 _x = _x.encode('utf-8')
00353 length = len(_x)
00354 buff.write(struct.pack('<I%ss'%length, length, _x))
00355 _v6 = _v3.pose
00356 _v7 = _v6.position
00357 _x = _v7
00358 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00359 _v8 = _v6.orientation
00360 _x = _v8
00361 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
00362 buff.write(_struct_f.pack(val2.confidence))
00363 _x = val2.detector_name
00364 length = len(_x)
00365 if python3 or type(_x) == unicode:
00366 _x = _x.encode('utf-8')
00367 length = len(_x)
00368 buff.write(struct.pack('<I%ss'%length, length, _x))
00369 length = len(self.cluster_model_indices)
00370 buff.write(_struct_I.pack(length))
00371 pattern = '<%si'%length
00372 buff.write(struct.pack(pattern, *self.cluster_model_indices))
00373 buff.write(_struct_i.pack(self.result))
00374 except struct.error as se: self._check_types(se)
00375 except TypeError as te: self._check_types(te)
00376
00377 def deserialize(self, str):
00378 """
00379 unpack serialized message in str into this message instance
00380 :param str: byte array of serialized message, ``str``
00381 """
00382 try:
00383 if self.table is None:
00384 self.table = tabletop_object_detector.msg.Table()
00385 if self.clusters is None:
00386 self.clusters = None
00387 if self.models is None:
00388 self.models = None
00389 end = 0
00390 _x = self
00391 start = end
00392 end += 12
00393 (_x.table.pose.header.seq, _x.table.pose.header.stamp.secs, _x.table.pose.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
00394 start = end
00395 end += 4
00396 (length,) = _struct_I.unpack(str[start:end])
00397 start = end
00398 end += length
00399 if python3:
00400 self.table.pose.header.frame_id = str[start:end].decode('utf-8')
00401 else:
00402 self.table.pose.header.frame_id = str[start:end]
00403 _x = self
00404 start = end
00405 end += 73
00406 (_x.table.pose.pose.position.x, _x.table.pose.pose.position.y, _x.table.pose.pose.position.z, _x.table.pose.pose.orientation.x, _x.table.pose.pose.orientation.y, _x.table.pose.pose.orientation.z, _x.table.pose.pose.orientation.w, _x.table.x_min, _x.table.x_max, _x.table.y_min, _x.table.y_max, _x.table.convex_hull.type,) = _struct_7d4fb.unpack(str[start:end])
00407 start = end
00408 end += 4
00409 (length,) = _struct_I.unpack(str[start:end])
00410 pattern = '<%sd'%length
00411 start = end
00412 end += struct.calcsize(pattern)
00413 self.table.convex_hull.dimensions = struct.unpack(pattern, str[start:end])
00414 start = end
00415 end += 4
00416 (length,) = _struct_I.unpack(str[start:end])
00417 pattern = '<%si'%length
00418 start = end
00419 end += struct.calcsize(pattern)
00420 self.table.convex_hull.triangles = struct.unpack(pattern, str[start:end])
00421 start = end
00422 end += 4
00423 (length,) = _struct_I.unpack(str[start:end])
00424 self.table.convex_hull.vertices = []
00425 for i in range(0, length):
00426 val1 = geometry_msgs.msg.Point()
00427 _x = val1
00428 start = end
00429 end += 24
00430 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
00431 self.table.convex_hull.vertices.append(val1)
00432 start = end
00433 end += 4
00434 (length,) = _struct_I.unpack(str[start:end])
00435 self.clusters = []
00436 for i in range(0, length):
00437 val1 = sensor_msgs.msg.PointCloud()
00438 _v9 = val1.header
00439 start = end
00440 end += 4
00441 (_v9.seq,) = _struct_I.unpack(str[start:end])
00442 _v10 = _v9.stamp
00443 _x = _v10
00444 start = end
00445 end += 8
00446 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
00447 start = end
00448 end += 4
00449 (length,) = _struct_I.unpack(str[start:end])
00450 start = end
00451 end += length
00452 if python3:
00453 _v9.frame_id = str[start:end].decode('utf-8')
00454 else:
00455 _v9.frame_id = str[start:end]
00456 start = end
00457 end += 4
00458 (length,) = _struct_I.unpack(str[start:end])
00459 val1.points = []
00460 for i in range(0, length):
00461 val2 = geometry_msgs.msg.Point32()
00462 _x = val2
00463 start = end
00464 end += 12
00465 (_x.x, _x.y, _x.z,) = _struct_3f.unpack(str[start:end])
00466 val1.points.append(val2)
00467 start = end
00468 end += 4
00469 (length,) = _struct_I.unpack(str[start:end])
00470 val1.channels = []
00471 for i in range(0, length):
00472 val2 = sensor_msgs.msg.ChannelFloat32()
00473 start = end
00474 end += 4
00475 (length,) = _struct_I.unpack(str[start:end])
00476 start = end
00477 end += length
00478 if python3:
00479 val2.name = str[start:end].decode('utf-8')
00480 else:
00481 val2.name = str[start:end]
00482 start = end
00483 end += 4
00484 (length,) = _struct_I.unpack(str[start:end])
00485 pattern = '<%sf'%length
00486 start = end
00487 end += struct.calcsize(pattern)
00488 val2.values = struct.unpack(pattern, str[start:end])
00489 val1.channels.append(val2)
00490 self.clusters.append(val1)
00491 start = end
00492 end += 4
00493 (length,) = _struct_I.unpack(str[start:end])
00494 self.models = []
00495 for i in range(0, length):
00496 val1 = household_objects_database_msgs.msg.DatabaseModelPoseList()
00497 start = end
00498 end += 4
00499 (length,) = _struct_I.unpack(str[start:end])
00500 val1.model_list = []
00501 for i in range(0, length):
00502 val2 = household_objects_database_msgs.msg.DatabaseModelPose()
00503 start = end
00504 end += 4
00505 (val2.model_id,) = _struct_i.unpack(str[start:end])
00506 _v11 = val2.pose
00507 _v12 = _v11.header
00508 start = end
00509 end += 4
00510 (_v12.seq,) = _struct_I.unpack(str[start:end])
00511 _v13 = _v12.stamp
00512 _x = _v13
00513 start = end
00514 end += 8
00515 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
00516 start = end
00517 end += 4
00518 (length,) = _struct_I.unpack(str[start:end])
00519 start = end
00520 end += length
00521 if python3:
00522 _v12.frame_id = str[start:end].decode('utf-8')
00523 else:
00524 _v12.frame_id = str[start:end]
00525 _v14 = _v11.pose
00526 _v15 = _v14.position
00527 _x = _v15
00528 start = end
00529 end += 24
00530 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
00531 _v16 = _v14.orientation
00532 _x = _v16
00533 start = end
00534 end += 32
00535 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
00536 start = end
00537 end += 4
00538 (val2.confidence,) = _struct_f.unpack(str[start:end])
00539 start = end
00540 end += 4
00541 (length,) = _struct_I.unpack(str[start:end])
00542 start = end
00543 end += length
00544 if python3:
00545 val2.detector_name = str[start:end].decode('utf-8')
00546 else:
00547 val2.detector_name = str[start:end]
00548 val1.model_list.append(val2)
00549 self.models.append(val1)
00550 start = end
00551 end += 4
00552 (length,) = _struct_I.unpack(str[start:end])
00553 pattern = '<%si'%length
00554 start = end
00555 end += struct.calcsize(pattern)
00556 self.cluster_model_indices = struct.unpack(pattern, str[start:end])
00557 start = end
00558 end += 4
00559 (self.result,) = _struct_i.unpack(str[start:end])
00560 return self
00561 except struct.error as e:
00562 raise genpy.DeserializationError(e) #most likely buffer underfill
00563
00564
00565 def serialize_numpy(self, buff, numpy):
00566 """
00567 serialize message with numpy array types into buffer
00568 :param buff: buffer, ``StringIO``
00569 :param numpy: numpy python module
00570 """
00571 try:
00572 _x = self
00573 buff.write(_struct_3I.pack(_x.table.pose.header.seq, _x.table.pose.header.stamp.secs, _x.table.pose.header.stamp.nsecs))
00574 _x = self.table.pose.header.frame_id
00575 length = len(_x)
00576 if python3 or type(_x) == unicode:
00577 _x = _x.encode('utf-8')
00578 length = len(_x)
00579 buff.write(struct.pack('<I%ss'%length, length, _x))
00580 _x = self
00581 buff.write(_struct_7d4fb.pack(_x.table.pose.pose.position.x, _x.table.pose.pose.position.y, _x.table.pose.pose.position.z, _x.table.pose.pose.orientation.x, _x.table.pose.pose.orientation.y, _x.table.pose.pose.orientation.z, _x.table.pose.pose.orientation.w, _x.table.x_min, _x.table.x_max, _x.table.y_min, _x.table.y_max, _x.table.convex_hull.type))
00582 length = len(self.table.convex_hull.dimensions)
00583 buff.write(_struct_I.pack(length))
00584 pattern = '<%sd'%length
00585 buff.write(self.table.convex_hull.dimensions.tostring())
00586 length = len(self.table.convex_hull.triangles)
00587 buff.write(_struct_I.pack(length))
00588 pattern = '<%si'%length
00589 buff.write(self.table.convex_hull.triangles.tostring())
00590 length = len(self.table.convex_hull.vertices)
00591 buff.write(_struct_I.pack(length))
00592 for val1 in self.table.convex_hull.vertices:
00593 _x = val1
00594 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00595 length = len(self.clusters)
00596 buff.write(_struct_I.pack(length))
00597 for val1 in self.clusters:
00598 _v17 = val1.header
00599 buff.write(_struct_I.pack(_v17.seq))
00600 _v18 = _v17.stamp
00601 _x = _v18
00602 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
00603 _x = _v17.frame_id
00604 length = len(_x)
00605 if python3 or type(_x) == unicode:
00606 _x = _x.encode('utf-8')
00607 length = len(_x)
00608 buff.write(struct.pack('<I%ss'%length, length, _x))
00609 length = len(val1.points)
00610 buff.write(_struct_I.pack(length))
00611 for val2 in val1.points:
00612 _x = val2
00613 buff.write(_struct_3f.pack(_x.x, _x.y, _x.z))
00614 length = len(val1.channels)
00615 buff.write(_struct_I.pack(length))
00616 for val2 in val1.channels:
00617 _x = val2.name
00618 length = len(_x)
00619 if python3 or type(_x) == unicode:
00620 _x = _x.encode('utf-8')
00621 length = len(_x)
00622 buff.write(struct.pack('<I%ss'%length, length, _x))
00623 length = len(val2.values)
00624 buff.write(_struct_I.pack(length))
00625 pattern = '<%sf'%length
00626 buff.write(val2.values.tostring())
00627 length = len(self.models)
00628 buff.write(_struct_I.pack(length))
00629 for val1 in self.models:
00630 length = len(val1.model_list)
00631 buff.write(_struct_I.pack(length))
00632 for val2 in val1.model_list:
00633 buff.write(_struct_i.pack(val2.model_id))
00634 _v19 = val2.pose
00635 _v20 = _v19.header
00636 buff.write(_struct_I.pack(_v20.seq))
00637 _v21 = _v20.stamp
00638 _x = _v21
00639 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
00640 _x = _v20.frame_id
00641 length = len(_x)
00642 if python3 or type(_x) == unicode:
00643 _x = _x.encode('utf-8')
00644 length = len(_x)
00645 buff.write(struct.pack('<I%ss'%length, length, _x))
00646 _v22 = _v19.pose
00647 _v23 = _v22.position
00648 _x = _v23
00649 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00650 _v24 = _v22.orientation
00651 _x = _v24
00652 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
00653 buff.write(_struct_f.pack(val2.confidence))
00654 _x = val2.detector_name
00655 length = len(_x)
00656 if python3 or type(_x) == unicode:
00657 _x = _x.encode('utf-8')
00658 length = len(_x)
00659 buff.write(struct.pack('<I%ss'%length, length, _x))
00660 length = len(self.cluster_model_indices)
00661 buff.write(_struct_I.pack(length))
00662 pattern = '<%si'%length
00663 buff.write(self.cluster_model_indices.tostring())
00664 buff.write(_struct_i.pack(self.result))
00665 except struct.error as se: self._check_types(se)
00666 except TypeError as te: self._check_types(te)
00667
00668 def deserialize_numpy(self, str, numpy):
00669 """
00670 unpack serialized message in str into this message instance using numpy for array types
00671 :param str: byte array of serialized message, ``str``
00672 :param numpy: numpy python module
00673 """
00674 try:
00675 if self.table is None:
00676 self.table = tabletop_object_detector.msg.Table()
00677 if self.clusters is None:
00678 self.clusters = None
00679 if self.models is None:
00680 self.models = None
00681 end = 0
00682 _x = self
00683 start = end
00684 end += 12
00685 (_x.table.pose.header.seq, _x.table.pose.header.stamp.secs, _x.table.pose.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
00686 start = end
00687 end += 4
00688 (length,) = _struct_I.unpack(str[start:end])
00689 start = end
00690 end += length
00691 if python3:
00692 self.table.pose.header.frame_id = str[start:end].decode('utf-8')
00693 else:
00694 self.table.pose.header.frame_id = str[start:end]
00695 _x = self
00696 start = end
00697 end += 73
00698 (_x.table.pose.pose.position.x, _x.table.pose.pose.position.y, _x.table.pose.pose.position.z, _x.table.pose.pose.orientation.x, _x.table.pose.pose.orientation.y, _x.table.pose.pose.orientation.z, _x.table.pose.pose.orientation.w, _x.table.x_min, _x.table.x_max, _x.table.y_min, _x.table.y_max, _x.table.convex_hull.type,) = _struct_7d4fb.unpack(str[start:end])
00699 start = end
00700 end += 4
00701 (length,) = _struct_I.unpack(str[start:end])
00702 pattern = '<%sd'%length
00703 start = end
00704 end += struct.calcsize(pattern)
00705 self.table.convex_hull.dimensions = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
00706 start = end
00707 end += 4
00708 (length,) = _struct_I.unpack(str[start:end])
00709 pattern = '<%si'%length
00710 start = end
00711 end += struct.calcsize(pattern)
00712 self.table.convex_hull.triangles = numpy.frombuffer(str[start:end], dtype=numpy.int32, count=length)
00713 start = end
00714 end += 4
00715 (length,) = _struct_I.unpack(str[start:end])
00716 self.table.convex_hull.vertices = []
00717 for i in range(0, length):
00718 val1 = geometry_msgs.msg.Point()
00719 _x = val1
00720 start = end
00721 end += 24
00722 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
00723 self.table.convex_hull.vertices.append(val1)
00724 start = end
00725 end += 4
00726 (length,) = _struct_I.unpack(str[start:end])
00727 self.clusters = []
00728 for i in range(0, length):
00729 val1 = sensor_msgs.msg.PointCloud()
00730 _v25 = val1.header
00731 start = end
00732 end += 4
00733 (_v25.seq,) = _struct_I.unpack(str[start:end])
00734 _v26 = _v25.stamp
00735 _x = _v26
00736 start = end
00737 end += 8
00738 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
00739 start = end
00740 end += 4
00741 (length,) = _struct_I.unpack(str[start:end])
00742 start = end
00743 end += length
00744 if python3:
00745 _v25.frame_id = str[start:end].decode('utf-8')
00746 else:
00747 _v25.frame_id = str[start:end]
00748 start = end
00749 end += 4
00750 (length,) = _struct_I.unpack(str[start:end])
00751 val1.points = []
00752 for i in range(0, length):
00753 val2 = geometry_msgs.msg.Point32()
00754 _x = val2
00755 start = end
00756 end += 12
00757 (_x.x, _x.y, _x.z,) = _struct_3f.unpack(str[start:end])
00758 val1.points.append(val2)
00759 start = end
00760 end += 4
00761 (length,) = _struct_I.unpack(str[start:end])
00762 val1.channels = []
00763 for i in range(0, length):
00764 val2 = sensor_msgs.msg.ChannelFloat32()
00765 start = end
00766 end += 4
00767 (length,) = _struct_I.unpack(str[start:end])
00768 start = end
00769 end += length
00770 if python3:
00771 val2.name = str[start:end].decode('utf-8')
00772 else:
00773 val2.name = str[start:end]
00774 start = end
00775 end += 4
00776 (length,) = _struct_I.unpack(str[start:end])
00777 pattern = '<%sf'%length
00778 start = end
00779 end += struct.calcsize(pattern)
00780 val2.values = numpy.frombuffer(str[start:end], dtype=numpy.float32, count=length)
00781 val1.channels.append(val2)
00782 self.clusters.append(val1)
00783 start = end
00784 end += 4
00785 (length,) = _struct_I.unpack(str[start:end])
00786 self.models = []
00787 for i in range(0, length):
00788 val1 = household_objects_database_msgs.msg.DatabaseModelPoseList()
00789 start = end
00790 end += 4
00791 (length,) = _struct_I.unpack(str[start:end])
00792 val1.model_list = []
00793 for i in range(0, length):
00794 val2 = household_objects_database_msgs.msg.DatabaseModelPose()
00795 start = end
00796 end += 4
00797 (val2.model_id,) = _struct_i.unpack(str[start:end])
00798 _v27 = val2.pose
00799 _v28 = _v27.header
00800 start = end
00801 end += 4
00802 (_v28.seq,) = _struct_I.unpack(str[start:end])
00803 _v29 = _v28.stamp
00804 _x = _v29
00805 start = end
00806 end += 8
00807 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
00808 start = end
00809 end += 4
00810 (length,) = _struct_I.unpack(str[start:end])
00811 start = end
00812 end += length
00813 if python3:
00814 _v28.frame_id = str[start:end].decode('utf-8')
00815 else:
00816 _v28.frame_id = str[start:end]
00817 _v30 = _v27.pose
00818 _v31 = _v30.position
00819 _x = _v31
00820 start = end
00821 end += 24
00822 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
00823 _v32 = _v30.orientation
00824 _x = _v32
00825 start = end
00826 end += 32
00827 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
00828 start = end
00829 end += 4
00830 (val2.confidence,) = _struct_f.unpack(str[start:end])
00831 start = end
00832 end += 4
00833 (length,) = _struct_I.unpack(str[start:end])
00834 start = end
00835 end += length
00836 if python3:
00837 val2.detector_name = str[start:end].decode('utf-8')
00838 else:
00839 val2.detector_name = str[start:end]
00840 val1.model_list.append(val2)
00841 self.models.append(val1)
00842 start = end
00843 end += 4
00844 (length,) = _struct_I.unpack(str[start:end])
00845 pattern = '<%si'%length
00846 start = end
00847 end += struct.calcsize(pattern)
00848 self.cluster_model_indices = numpy.frombuffer(str[start:end], dtype=numpy.int32, count=length)
00849 start = end
00850 end += 4
00851 (self.result,) = _struct_i.unpack(str[start:end])
00852 return self
00853 except struct.error as e:
00854 raise genpy.DeserializationError(e) #most likely buffer underfill
00855
00856 _struct_I = genpy.struct_I
00857 _struct_f = struct.Struct("<f")
00858 _struct_7d4fb = struct.Struct("<7d4fb")
00859 _struct_2I = struct.Struct("<2I")
00860 _struct_i = struct.Struct("<i")
00861 _struct_3I = struct.Struct("<3I")
00862 _struct_4d = struct.Struct("<4d")
00863 _struct_3f = struct.Struct("<3f")
00864 _struct_3d = struct.Struct("<3d")