00001 """autogenerated by genmsg_py from PlaceResult.msg. Do not edit."""
00002 import roslib.message
00003 import struct
00004
00005 import geometry_msgs.msg
00006 import object_manipulation_msgs.msg
00007 import std_msgs.msg
00008
00009 class PlaceResult(roslib.message.Message):
00010 _md5sum = "531ddb04b8422b6734fb74421d431059"
00011 _type = "object_manipulation_msgs/PlaceResult"
00012 _has_header = False
00013 _full_text = """# ====== DO NOT MODIFY! AUTOGENERATED FROM AN ACTION DEFINITION ======
00014
00015 # The result of the pickup attempt
00016 ManipulationResult manipulation_result
00017
00018 # The successful place location, if any
00019 geometry_msgs/PoseStamped place_location
00020
00021 # the list of attempted locations, in the order in which they were attempted
00022 # the successful one should be the last one in this list
00023 geometry_msgs/PoseStamped[] attempted_locations
00024
00025 # the outcomes of the attempted locations, in the same order as attempted_locations
00026 PlaceLocationResult[] attempted_location_results
00027
00028
00029 ================================================================================
00030 MSG: object_manipulation_msgs/ManipulationResult
00031 # Result codes for manipulation tasks
00032
00033 # task completed as expected
00034 # generally means you can proceed as planned
00035 int32 SUCCESS = 1
00036
00037 # task not possible (e.g. out of reach or obstacles in the way)
00038 # generally means that the world was not disturbed, so you can try another task
00039 int32 UNFEASIBLE = -1
00040
00041 # task was thought possible, but failed due to unexpected events during execution
00042 # it is likely that the world was disturbed, so you are encouraged to refresh
00043 # your sensed world model before proceeding to another task
00044 int32 FAILED = -2
00045
00046 # a lower level error prevented task completion (e.g. joint controller not responding)
00047 # generally requires human attention
00048 int32 ERROR = -3
00049
00050 # means that at some point during execution we ended up in a state that the collision-aware
00051 # arm navigation module will not move out of. The world was likely not disturbed, but you
00052 # probably need a new collision map to move the arm out of the stuck position
00053 int32 ARM_MOVEMENT_PREVENTED = -4
00054
00055 # specific to grasp actions
00056 # the object was grasped successfully, but the lift attempt could not achieve the minimum lift distance requested
00057 # it is likely that the collision environment will see collisions between the hand/object and the support surface
00058 int32 LIFT_FAILED = -5
00059
00060 # specific to place actions
00061 # the object was placed successfully, but the retreat attempt could not achieve the minimum retreat distance requested
00062 # it is likely that the collision environment will see collisions between the hand and the object
00063 int32 RETREAT_FAILED = -6
00064
00065 # indicates that somewhere along the line a human said "wait, stop, this is bad, go back and do something else"
00066 int32 CANCELLED = -7
00067
00068 # the actual value of this error code
00069 int32 value
00070
00071 ================================================================================
00072 MSG: geometry_msgs/PoseStamped
00073 # A Pose with reference coordinate frame and timestamp
00074 Header header
00075 Pose pose
00076
00077 ================================================================================
00078 MSG: std_msgs/Header
00079 # Standard metadata for higher-level stamped data types.
00080 # This is generally used to communicate timestamped data
00081 # in a particular coordinate frame.
00082 #
00083 # sequence ID: consecutively increasing ID
00084 uint32 seq
00085 #Two-integer timestamp that is expressed as:
00086 # * stamp.secs: seconds (stamp_secs) since epoch
00087 # * stamp.nsecs: nanoseconds since stamp_secs
00088 # time-handling sugar is provided by the client library
00089 time stamp
00090 #Frame this data is associated with
00091 # 0: no frame
00092 # 1: global frame
00093 string frame_id
00094
00095 ================================================================================
00096 MSG: geometry_msgs/Pose
00097 # A representation of pose in free space, composed of postion and orientation.
00098 Point position
00099 Quaternion orientation
00100
00101 ================================================================================
00102 MSG: geometry_msgs/Point
00103 # This contains the position of a point in free space
00104 float64 x
00105 float64 y
00106 float64 z
00107
00108 ================================================================================
00109 MSG: geometry_msgs/Quaternion
00110 # This represents an orientation in free space in quaternion form.
00111
00112 float64 x
00113 float64 y
00114 float64 z
00115 float64 w
00116
00117 ================================================================================
00118 MSG: object_manipulation_msgs/PlaceLocationResult
00119 int32 SUCCESS = 1
00120 int32 PLACE_OUT_OF_REACH = 2
00121 int32 PLACE_IN_COLLISION = 3
00122 int32 PLACE_UNFEASIBLE = 4
00123 int32 PREPLACE_OUT_OF_REACH = 5
00124 int32 PREPLACE_IN_COLLISION = 6
00125 int32 PREPLACE_UNFEASIBLE = 7
00126 int32 RETREAT_OUT_OF_REACH = 8
00127 int32 RETREAT_IN_COLLISION = 9
00128 int32 RETREAT_UNFEASIBLE = 10
00129 int32 MOVE_ARM_FAILED = 11
00130 int32 PLACE_FAILED = 12
00131 int32 RETREAT_FAILED = 13
00132 int32 result_code
00133
00134 # whether the state of the world was disturbed by this attempt. generally, this flag
00135 # shows if another task can be attempted, or a new sensed world model is recommeded
00136 # before proceeding
00137 bool continuation_possible
00138
00139 """
00140 __slots__ = ['manipulation_result','place_location','attempted_locations','attempted_location_results']
00141 _slot_types = ['object_manipulation_msgs/ManipulationResult','geometry_msgs/PoseStamped','geometry_msgs/PoseStamped[]','object_manipulation_msgs/PlaceLocationResult[]']
00142
00143 def __init__(self, *args, **kwds):
00144 """
00145 Constructor. Any message fields that are implicitly/explicitly
00146 set to None will be assigned a default value. The recommend
00147 use is keyword arguments as this is more robust to future message
00148 changes. You cannot mix in-order arguments and keyword arguments.
00149
00150 The available fields are:
00151 manipulation_result,place_location,attempted_locations,attempted_location_results
00152
00153 @param args: complete set of field values, in .msg order
00154 @param kwds: use keyword arguments corresponding to message field names
00155 to set specific fields.
00156 """
00157 if args or kwds:
00158 super(PlaceResult, self).__init__(*args, **kwds)
00159
00160 if self.manipulation_result is None:
00161 self.manipulation_result = object_manipulation_msgs.msg.ManipulationResult()
00162 if self.place_location is None:
00163 self.place_location = geometry_msgs.msg.PoseStamped()
00164 if self.attempted_locations is None:
00165 self.attempted_locations = []
00166 if self.attempted_location_results is None:
00167 self.attempted_location_results = []
00168 else:
00169 self.manipulation_result = object_manipulation_msgs.msg.ManipulationResult()
00170 self.place_location = geometry_msgs.msg.PoseStamped()
00171 self.attempted_locations = []
00172 self.attempted_location_results = []
00173
00174 def _get_types(self):
00175 """
00176 internal API method
00177 """
00178 return self._slot_types
00179
00180 def serialize(self, buff):
00181 """
00182 serialize message into buffer
00183 @param buff: buffer
00184 @type buff: StringIO
00185 """
00186 try:
00187 _x = self
00188 buff.write(_struct_i3I.pack(_x.manipulation_result.value, _x.place_location.header.seq, _x.place_location.header.stamp.secs, _x.place_location.header.stamp.nsecs))
00189 _x = self.place_location.header.frame_id
00190 length = len(_x)
00191 buff.write(struct.pack('<I%ss'%length, length, _x))
00192 _x = self
00193 buff.write(_struct_7d.pack(_x.place_location.pose.position.x, _x.place_location.pose.position.y, _x.place_location.pose.position.z, _x.place_location.pose.orientation.x, _x.place_location.pose.orientation.y, _x.place_location.pose.orientation.z, _x.place_location.pose.orientation.w))
00194 length = len(self.attempted_locations)
00195 buff.write(_struct_I.pack(length))
00196 for val1 in self.attempted_locations:
00197 _v1 = val1.header
00198 buff.write(_struct_I.pack(_v1.seq))
00199 _v2 = _v1.stamp
00200 _x = _v2
00201 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
00202 _x = _v1.frame_id
00203 length = len(_x)
00204 buff.write(struct.pack('<I%ss'%length, length, _x))
00205 _v3 = val1.pose
00206 _v4 = _v3.position
00207 _x = _v4
00208 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00209 _v5 = _v3.orientation
00210 _x = _v5
00211 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
00212 length = len(self.attempted_location_results)
00213 buff.write(_struct_I.pack(length))
00214 for val1 in self.attempted_location_results:
00215 _x = val1
00216 buff.write(_struct_iB.pack(_x.result_code, _x.continuation_possible))
00217 except struct.error as se: self._check_types(se)
00218 except TypeError as te: self._check_types(te)
00219
00220 def deserialize(self, str):
00221 """
00222 unpack serialized message in str into this message instance
00223 @param str: byte array of serialized message
00224 @type str: str
00225 """
00226 try:
00227 if self.manipulation_result is None:
00228 self.manipulation_result = object_manipulation_msgs.msg.ManipulationResult()
00229 if self.place_location is None:
00230 self.place_location = geometry_msgs.msg.PoseStamped()
00231 end = 0
00232 _x = self
00233 start = end
00234 end += 16
00235 (_x.manipulation_result.value, _x.place_location.header.seq, _x.place_location.header.stamp.secs, _x.place_location.header.stamp.nsecs,) = _struct_i3I.unpack(str[start:end])
00236 start = end
00237 end += 4
00238 (length,) = _struct_I.unpack(str[start:end])
00239 start = end
00240 end += length
00241 self.place_location.header.frame_id = str[start:end]
00242 _x = self
00243 start = end
00244 end += 56
00245 (_x.place_location.pose.position.x, _x.place_location.pose.position.y, _x.place_location.pose.position.z, _x.place_location.pose.orientation.x, _x.place_location.pose.orientation.y, _x.place_location.pose.orientation.z, _x.place_location.pose.orientation.w,) = _struct_7d.unpack(str[start:end])
00246 start = end
00247 end += 4
00248 (length,) = _struct_I.unpack(str[start:end])
00249 self.attempted_locations = []
00250 for i in range(0, length):
00251 val1 = geometry_msgs.msg.PoseStamped()
00252 _v6 = val1.header
00253 start = end
00254 end += 4
00255 (_v6.seq,) = _struct_I.unpack(str[start:end])
00256 _v7 = _v6.stamp
00257 _x = _v7
00258 start = end
00259 end += 8
00260 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
00261 start = end
00262 end += 4
00263 (length,) = _struct_I.unpack(str[start:end])
00264 start = end
00265 end += length
00266 _v6.frame_id = str[start:end]
00267 _v8 = val1.pose
00268 _v9 = _v8.position
00269 _x = _v9
00270 start = end
00271 end += 24
00272 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
00273 _v10 = _v8.orientation
00274 _x = _v10
00275 start = end
00276 end += 32
00277 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
00278 self.attempted_locations.append(val1)
00279 start = end
00280 end += 4
00281 (length,) = _struct_I.unpack(str[start:end])
00282 self.attempted_location_results = []
00283 for i in range(0, length):
00284 val1 = object_manipulation_msgs.msg.PlaceLocationResult()
00285 _x = val1
00286 start = end
00287 end += 5
00288 (_x.result_code, _x.continuation_possible,) = _struct_iB.unpack(str[start:end])
00289 val1.continuation_possible = bool(val1.continuation_possible)
00290 self.attempted_location_results.append(val1)
00291 return self
00292 except struct.error as e:
00293 raise roslib.message.DeserializationError(e)
00294
00295
00296 def serialize_numpy(self, buff, numpy):
00297 """
00298 serialize message with numpy array types into buffer
00299 @param buff: buffer
00300 @type buff: StringIO
00301 @param numpy: numpy python module
00302 @type numpy module
00303 """
00304 try:
00305 _x = self
00306 buff.write(_struct_i3I.pack(_x.manipulation_result.value, _x.place_location.header.seq, _x.place_location.header.stamp.secs, _x.place_location.header.stamp.nsecs))
00307 _x = self.place_location.header.frame_id
00308 length = len(_x)
00309 buff.write(struct.pack('<I%ss'%length, length, _x))
00310 _x = self
00311 buff.write(_struct_7d.pack(_x.place_location.pose.position.x, _x.place_location.pose.position.y, _x.place_location.pose.position.z, _x.place_location.pose.orientation.x, _x.place_location.pose.orientation.y, _x.place_location.pose.orientation.z, _x.place_location.pose.orientation.w))
00312 length = len(self.attempted_locations)
00313 buff.write(_struct_I.pack(length))
00314 for val1 in self.attempted_locations:
00315 _v11 = val1.header
00316 buff.write(_struct_I.pack(_v11.seq))
00317 _v12 = _v11.stamp
00318 _x = _v12
00319 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
00320 _x = _v11.frame_id
00321 length = len(_x)
00322 buff.write(struct.pack('<I%ss'%length, length, _x))
00323 _v13 = val1.pose
00324 _v14 = _v13.position
00325 _x = _v14
00326 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00327 _v15 = _v13.orientation
00328 _x = _v15
00329 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
00330 length = len(self.attempted_location_results)
00331 buff.write(_struct_I.pack(length))
00332 for val1 in self.attempted_location_results:
00333 _x = val1
00334 buff.write(_struct_iB.pack(_x.result_code, _x.continuation_possible))
00335 except struct.error as se: self._check_types(se)
00336 except TypeError as te: self._check_types(te)
00337
00338 def deserialize_numpy(self, str, numpy):
00339 """
00340 unpack serialized message in str into this message instance using numpy for array types
00341 @param str: byte array of serialized message
00342 @type str: str
00343 @param numpy: numpy python module
00344 @type numpy: module
00345 """
00346 try:
00347 if self.manipulation_result is None:
00348 self.manipulation_result = object_manipulation_msgs.msg.ManipulationResult()
00349 if self.place_location is None:
00350 self.place_location = geometry_msgs.msg.PoseStamped()
00351 end = 0
00352 _x = self
00353 start = end
00354 end += 16
00355 (_x.manipulation_result.value, _x.place_location.header.seq, _x.place_location.header.stamp.secs, _x.place_location.header.stamp.nsecs,) = _struct_i3I.unpack(str[start:end])
00356 start = end
00357 end += 4
00358 (length,) = _struct_I.unpack(str[start:end])
00359 start = end
00360 end += length
00361 self.place_location.header.frame_id = str[start:end]
00362 _x = self
00363 start = end
00364 end += 56
00365 (_x.place_location.pose.position.x, _x.place_location.pose.position.y, _x.place_location.pose.position.z, _x.place_location.pose.orientation.x, _x.place_location.pose.orientation.y, _x.place_location.pose.orientation.z, _x.place_location.pose.orientation.w,) = _struct_7d.unpack(str[start:end])
00366 start = end
00367 end += 4
00368 (length,) = _struct_I.unpack(str[start:end])
00369 self.attempted_locations = []
00370 for i in range(0, length):
00371 val1 = geometry_msgs.msg.PoseStamped()
00372 _v16 = val1.header
00373 start = end
00374 end += 4
00375 (_v16.seq,) = _struct_I.unpack(str[start:end])
00376 _v17 = _v16.stamp
00377 _x = _v17
00378 start = end
00379 end += 8
00380 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
00381 start = end
00382 end += 4
00383 (length,) = _struct_I.unpack(str[start:end])
00384 start = end
00385 end += length
00386 _v16.frame_id = str[start:end]
00387 _v18 = val1.pose
00388 _v19 = _v18.position
00389 _x = _v19
00390 start = end
00391 end += 24
00392 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
00393 _v20 = _v18.orientation
00394 _x = _v20
00395 start = end
00396 end += 32
00397 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
00398 self.attempted_locations.append(val1)
00399 start = end
00400 end += 4
00401 (length,) = _struct_I.unpack(str[start:end])
00402 self.attempted_location_results = []
00403 for i in range(0, length):
00404 val1 = object_manipulation_msgs.msg.PlaceLocationResult()
00405 _x = val1
00406 start = end
00407 end += 5
00408 (_x.result_code, _x.continuation_possible,) = _struct_iB.unpack(str[start:end])
00409 val1.continuation_possible = bool(val1.continuation_possible)
00410 self.attempted_location_results.append(val1)
00411 return self
00412 except struct.error as e:
00413 raise roslib.message.DeserializationError(e)
00414
00415 _struct_I = roslib.message.struct_I
00416 _struct_7d = struct.Struct("<7d")
00417 _struct_i3I = struct.Struct("<i3I")
00418 _struct_4d = struct.Struct("<4d")
00419 _struct_iB = struct.Struct("<iB")
00420 _struct_2I = struct.Struct("<2I")
00421 _struct_3d = struct.Struct("<3d")