00001 """autogenerated by genpy from nao_msgs/FollowPathActionGoal.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 nav_msgs.msg
00008 import nao_msgs.msg
00009 import actionlib_msgs.msg
00010 import geometry_msgs.msg
00011 import genpy
00012 import std_msgs.msg
00013
00014 class FollowPathActionGoal(genpy.Message):
00015 _md5sum = "fd6a86de9a61df23ce19e20d45649f18"
00016 _type = "nao_msgs/FollowPathActionGoal"
00017 _has_header = True
00018 _full_text = """# ====== DO NOT MODIFY! AUTOGENERATED FROM AN ACTION DEFINITION ======
00019
00020 Header header
00021 actionlib_msgs/GoalID goal_id
00022 FollowPathGoal goal
00023
00024 ================================================================================
00025 MSG: std_msgs/Header
00026 # Standard metadata for higher-level stamped data types.
00027 # This is generally used to communicate timestamped data
00028 # in a particular coordinate frame.
00029 #
00030 # sequence ID: consecutively increasing ID
00031 uint32 seq
00032 #Two-integer timestamp that is expressed as:
00033 # * stamp.secs: seconds (stamp_secs) since epoch
00034 # * stamp.nsecs: nanoseconds since stamp_secs
00035 # time-handling sugar is provided by the client library
00036 time stamp
00037 #Frame this data is associated with
00038 # 0: no frame
00039 # 1: global frame
00040 string frame_id
00041
00042 ================================================================================
00043 MSG: actionlib_msgs/GoalID
00044 # The stamp should store the time at which this goal was requested.
00045 # It is used by an action server when it tries to preempt all
00046 # goals that were requested before a certain time
00047 time stamp
00048
00049 # The id provides a way to associate feedback and
00050 # result message with specific goal requests. The id
00051 # specified must be unique.
00052 string id
00053
00054
00055 ================================================================================
00056 MSG: nao_msgs/FollowPathGoal
00057 # ====== DO NOT MODIFY! AUTOGENERATED FROM AN ACTION DEFINITION ======
00058 # goal: a path to follow
00059 nav_msgs/Path path
00060
00061 ================================================================================
00062 MSG: nav_msgs/Path
00063 #An array of poses that represents a Path for a robot to follow
00064 Header header
00065 geometry_msgs/PoseStamped[] poses
00066
00067 ================================================================================
00068 MSG: geometry_msgs/PoseStamped
00069 # A Pose with reference coordinate frame and timestamp
00070 Header header
00071 Pose pose
00072
00073 ================================================================================
00074 MSG: geometry_msgs/Pose
00075 # A representation of pose in free space, composed of postion and orientation.
00076 Point position
00077 Quaternion orientation
00078
00079 ================================================================================
00080 MSG: geometry_msgs/Point
00081 # This contains the position of a point in free space
00082 float64 x
00083 float64 y
00084 float64 z
00085
00086 ================================================================================
00087 MSG: geometry_msgs/Quaternion
00088 # This represents an orientation in free space in quaternion form.
00089
00090 float64 x
00091 float64 y
00092 float64 z
00093 float64 w
00094
00095 """
00096 __slots__ = ['header','goal_id','goal']
00097 _slot_types = ['std_msgs/Header','actionlib_msgs/GoalID','nao_msgs/FollowPathGoal']
00098
00099 def __init__(self, *args, **kwds):
00100 """
00101 Constructor. Any message fields that are implicitly/explicitly
00102 set to None will be assigned a default value. The recommend
00103 use is keyword arguments as this is more robust to future message
00104 changes. You cannot mix in-order arguments and keyword arguments.
00105
00106 The available fields are:
00107 header,goal_id,goal
00108
00109 :param args: complete set of field values, in .msg order
00110 :param kwds: use keyword arguments corresponding to message field names
00111 to set specific fields.
00112 """
00113 if args or kwds:
00114 super(FollowPathActionGoal, self).__init__(*args, **kwds)
00115
00116 if self.header is None:
00117 self.header = std_msgs.msg.Header()
00118 if self.goal_id is None:
00119 self.goal_id = actionlib_msgs.msg.GoalID()
00120 if self.goal is None:
00121 self.goal = nao_msgs.msg.FollowPathGoal()
00122 else:
00123 self.header = std_msgs.msg.Header()
00124 self.goal_id = actionlib_msgs.msg.GoalID()
00125 self.goal = nao_msgs.msg.FollowPathGoal()
00126
00127 def _get_types(self):
00128 """
00129 internal API method
00130 """
00131 return self._slot_types
00132
00133 def serialize(self, buff):
00134 """
00135 serialize message into buffer
00136 :param buff: buffer, ``StringIO``
00137 """
00138 try:
00139 _x = self
00140 buff.write(_struct_3I.pack(_x.header.seq, _x.header.stamp.secs, _x.header.stamp.nsecs))
00141 _x = self.header.frame_id
00142 length = len(_x)
00143 if python3 or type(_x) == unicode:
00144 _x = _x.encode('utf-8')
00145 length = len(_x)
00146 buff.write(struct.pack('<I%ss'%length, length, _x))
00147 _x = self
00148 buff.write(_struct_2I.pack(_x.goal_id.stamp.secs, _x.goal_id.stamp.nsecs))
00149 _x = self.goal_id.id
00150 length = len(_x)
00151 if python3 or type(_x) == unicode:
00152 _x = _x.encode('utf-8')
00153 length = len(_x)
00154 buff.write(struct.pack('<I%ss'%length, length, _x))
00155 _x = self
00156 buff.write(_struct_3I.pack(_x.goal.path.header.seq, _x.goal.path.header.stamp.secs, _x.goal.path.header.stamp.nsecs))
00157 _x = self.goal.path.header.frame_id
00158 length = len(_x)
00159 if python3 or type(_x) == unicode:
00160 _x = _x.encode('utf-8')
00161 length = len(_x)
00162 buff.write(struct.pack('<I%ss'%length, length, _x))
00163 length = len(self.goal.path.poses)
00164 buff.write(_struct_I.pack(length))
00165 for val1 in self.goal.path.poses:
00166 _v1 = val1.header
00167 buff.write(_struct_I.pack(_v1.seq))
00168 _v2 = _v1.stamp
00169 _x = _v2
00170 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
00171 _x = _v1.frame_id
00172 length = len(_x)
00173 if python3 or type(_x) == unicode:
00174 _x = _x.encode('utf-8')
00175 length = len(_x)
00176 buff.write(struct.pack('<I%ss'%length, length, _x))
00177 _v3 = val1.pose
00178 _v4 = _v3.position
00179 _x = _v4
00180 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00181 _v5 = _v3.orientation
00182 _x = _v5
00183 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
00184 except struct.error as se: self._check_types(se)
00185 except TypeError as te: self._check_types(te)
00186
00187 def deserialize(self, str):
00188 """
00189 unpack serialized message in str into this message instance
00190 :param str: byte array of serialized message, ``str``
00191 """
00192 try:
00193 if self.header is None:
00194 self.header = std_msgs.msg.Header()
00195 if self.goal_id is None:
00196 self.goal_id = actionlib_msgs.msg.GoalID()
00197 if self.goal is None:
00198 self.goal = nao_msgs.msg.FollowPathGoal()
00199 end = 0
00200 _x = self
00201 start = end
00202 end += 12
00203 (_x.header.seq, _x.header.stamp.secs, _x.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
00204 start = end
00205 end += 4
00206 (length,) = _struct_I.unpack(str[start:end])
00207 start = end
00208 end += length
00209 if python3:
00210 self.header.frame_id = str[start:end].decode('utf-8')
00211 else:
00212 self.header.frame_id = str[start:end]
00213 _x = self
00214 start = end
00215 end += 8
00216 (_x.goal_id.stamp.secs, _x.goal_id.stamp.nsecs,) = _struct_2I.unpack(str[start:end])
00217 start = end
00218 end += 4
00219 (length,) = _struct_I.unpack(str[start:end])
00220 start = end
00221 end += length
00222 if python3:
00223 self.goal_id.id = str[start:end].decode('utf-8')
00224 else:
00225 self.goal_id.id = str[start:end]
00226 _x = self
00227 start = end
00228 end += 12
00229 (_x.goal.path.header.seq, _x.goal.path.header.stamp.secs, _x.goal.path.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
00230 start = end
00231 end += 4
00232 (length,) = _struct_I.unpack(str[start:end])
00233 start = end
00234 end += length
00235 if python3:
00236 self.goal.path.header.frame_id = str[start:end].decode('utf-8')
00237 else:
00238 self.goal.path.header.frame_id = str[start:end]
00239 start = end
00240 end += 4
00241 (length,) = _struct_I.unpack(str[start:end])
00242 self.goal.path.poses = []
00243 for i in range(0, length):
00244 val1 = geometry_msgs.msg.PoseStamped()
00245 _v6 = val1.header
00246 start = end
00247 end += 4
00248 (_v6.seq,) = _struct_I.unpack(str[start:end])
00249 _v7 = _v6.stamp
00250 _x = _v7
00251 start = end
00252 end += 8
00253 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
00254 start = end
00255 end += 4
00256 (length,) = _struct_I.unpack(str[start:end])
00257 start = end
00258 end += length
00259 if python3:
00260 _v6.frame_id = str[start:end].decode('utf-8')
00261 else:
00262 _v6.frame_id = str[start:end]
00263 _v8 = val1.pose
00264 _v9 = _v8.position
00265 _x = _v9
00266 start = end
00267 end += 24
00268 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
00269 _v10 = _v8.orientation
00270 _x = _v10
00271 start = end
00272 end += 32
00273 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
00274 self.goal.path.poses.append(val1)
00275 return self
00276 except struct.error as e:
00277 raise genpy.DeserializationError(e)
00278
00279
00280 def serialize_numpy(self, buff, numpy):
00281 """
00282 serialize message with numpy array types into buffer
00283 :param buff: buffer, ``StringIO``
00284 :param numpy: numpy python module
00285 """
00286 try:
00287 _x = self
00288 buff.write(_struct_3I.pack(_x.header.seq, _x.header.stamp.secs, _x.header.stamp.nsecs))
00289 _x = self.header.frame_id
00290 length = len(_x)
00291 if python3 or type(_x) == unicode:
00292 _x = _x.encode('utf-8')
00293 length = len(_x)
00294 buff.write(struct.pack('<I%ss'%length, length, _x))
00295 _x = self
00296 buff.write(_struct_2I.pack(_x.goal_id.stamp.secs, _x.goal_id.stamp.nsecs))
00297 _x = self.goal_id.id
00298 length = len(_x)
00299 if python3 or type(_x) == unicode:
00300 _x = _x.encode('utf-8')
00301 length = len(_x)
00302 buff.write(struct.pack('<I%ss'%length, length, _x))
00303 _x = self
00304 buff.write(_struct_3I.pack(_x.goal.path.header.seq, _x.goal.path.header.stamp.secs, _x.goal.path.header.stamp.nsecs))
00305 _x = self.goal.path.header.frame_id
00306 length = len(_x)
00307 if python3 or type(_x) == unicode:
00308 _x = _x.encode('utf-8')
00309 length = len(_x)
00310 buff.write(struct.pack('<I%ss'%length, length, _x))
00311 length = len(self.goal.path.poses)
00312 buff.write(_struct_I.pack(length))
00313 for val1 in self.goal.path.poses:
00314 _v11 = val1.header
00315 buff.write(_struct_I.pack(_v11.seq))
00316 _v12 = _v11.stamp
00317 _x = _v12
00318 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
00319 _x = _v11.frame_id
00320 length = len(_x)
00321 if python3 or type(_x) == unicode:
00322 _x = _x.encode('utf-8')
00323 length = len(_x)
00324 buff.write(struct.pack('<I%ss'%length, length, _x))
00325 _v13 = val1.pose
00326 _v14 = _v13.position
00327 _x = _v14
00328 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00329 _v15 = _v13.orientation
00330 _x = _v15
00331 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
00332 except struct.error as se: self._check_types(se)
00333 except TypeError as te: self._check_types(te)
00334
00335 def deserialize_numpy(self, str, numpy):
00336 """
00337 unpack serialized message in str into this message instance using numpy for array types
00338 :param str: byte array of serialized message, ``str``
00339 :param numpy: numpy python module
00340 """
00341 try:
00342 if self.header is None:
00343 self.header = std_msgs.msg.Header()
00344 if self.goal_id is None:
00345 self.goal_id = actionlib_msgs.msg.GoalID()
00346 if self.goal is None:
00347 self.goal = nao_msgs.msg.FollowPathGoal()
00348 end = 0
00349 _x = self
00350 start = end
00351 end += 12
00352 (_x.header.seq, _x.header.stamp.secs, _x.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
00353 start = end
00354 end += 4
00355 (length,) = _struct_I.unpack(str[start:end])
00356 start = end
00357 end += length
00358 if python3:
00359 self.header.frame_id = str[start:end].decode('utf-8')
00360 else:
00361 self.header.frame_id = str[start:end]
00362 _x = self
00363 start = end
00364 end += 8
00365 (_x.goal_id.stamp.secs, _x.goal_id.stamp.nsecs,) = _struct_2I.unpack(str[start:end])
00366 start = end
00367 end += 4
00368 (length,) = _struct_I.unpack(str[start:end])
00369 start = end
00370 end += length
00371 if python3:
00372 self.goal_id.id = str[start:end].decode('utf-8')
00373 else:
00374 self.goal_id.id = str[start:end]
00375 _x = self
00376 start = end
00377 end += 12
00378 (_x.goal.path.header.seq, _x.goal.path.header.stamp.secs, _x.goal.path.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
00379 start = end
00380 end += 4
00381 (length,) = _struct_I.unpack(str[start:end])
00382 start = end
00383 end += length
00384 if python3:
00385 self.goal.path.header.frame_id = str[start:end].decode('utf-8')
00386 else:
00387 self.goal.path.header.frame_id = str[start:end]
00388 start = end
00389 end += 4
00390 (length,) = _struct_I.unpack(str[start:end])
00391 self.goal.path.poses = []
00392 for i in range(0, length):
00393 val1 = geometry_msgs.msg.PoseStamped()
00394 _v16 = val1.header
00395 start = end
00396 end += 4
00397 (_v16.seq,) = _struct_I.unpack(str[start:end])
00398 _v17 = _v16.stamp
00399 _x = _v17
00400 start = end
00401 end += 8
00402 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
00403 start = end
00404 end += 4
00405 (length,) = _struct_I.unpack(str[start:end])
00406 start = end
00407 end += length
00408 if python3:
00409 _v16.frame_id = str[start:end].decode('utf-8')
00410 else:
00411 _v16.frame_id = str[start:end]
00412 _v18 = val1.pose
00413 _v19 = _v18.position
00414 _x = _v19
00415 start = end
00416 end += 24
00417 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
00418 _v20 = _v18.orientation
00419 _x = _v20
00420 start = end
00421 end += 32
00422 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
00423 self.goal.path.poses.append(val1)
00424 return self
00425 except struct.error as e:
00426 raise genpy.DeserializationError(e)
00427
00428 _struct_I = genpy.struct_I
00429 _struct_4d = struct.Struct("<4d")
00430 _struct_3I = struct.Struct("<3I")
00431 _struct_2I = struct.Struct("<2I")
00432 _struct_3d = struct.Struct("<3d")