$search
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 #flag to mark the presence of a Header object 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 #message fields cannot be None, assign default values for those that are 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) #most likely buffer underfill 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) #most likely buffer underfill 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")