00001 """autogenerated by genpy from halfsteps_pattern_generator/GetPathRequest.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 halfsteps_pattern_generator.msg
00008 import walk_msgs.msg
00009 import geometry_msgs.msg
00010 import genpy
00011
00012 class GetPathRequest(genpy.Message):
00013 _md5sum = "e063e957046b30786d0c14db34db37f1"
00014 _type = "halfsteps_pattern_generator/GetPathRequest"
00015 _has_header = False
00016 _full_text = """geometry_msgs/Pose initial_left_foot_position
00017 geometry_msgs/Pose initial_right_foot_position
00018 geometry_msgs/Point initial_center_of_mass_position
00019
00020 geometry_msgs/Pose final_left_foot_position
00021 geometry_msgs/Pose final_right_foot_position
00022 geometry_msgs/Point final_center_of_mass_position
00023
00024 bool start_with_left_foot
00025
00026 halfsteps_pattern_generator/Footprint[] footprints
00027
00028 ================================================================================
00029 MSG: geometry_msgs/Pose
00030 # A representation of pose in free space, composed of postion and orientation.
00031 Point position
00032 Quaternion orientation
00033
00034 ================================================================================
00035 MSG: geometry_msgs/Point
00036 # This contains the position of a point in free space
00037 float64 x
00038 float64 y
00039 float64 z
00040
00041 ================================================================================
00042 MSG: geometry_msgs/Quaternion
00043 # This represents an orientation in free space in quaternion form.
00044
00045 float64 x
00046 float64 y
00047 float64 z
00048 float64 w
00049
00050 ================================================================================
00051 MSG: halfsteps_pattern_generator/Footprint
00052 walk_msgs/Footprint2d footprint
00053 float64 slideUp
00054 float64 slideDown
00055 float64 horizontalDistance
00056 float64 stepHeight
00057
00058 ================================================================================
00059 MSG: walk_msgs/Footprint2d
00060 time beginTime
00061 duration duration
00062 float64 x
00063 float64 y
00064 float64 theta
00065
00066 """
00067 __slots__ = ['initial_left_foot_position','initial_right_foot_position','initial_center_of_mass_position','final_left_foot_position','final_right_foot_position','final_center_of_mass_position','start_with_left_foot','footprints']
00068 _slot_types = ['geometry_msgs/Pose','geometry_msgs/Pose','geometry_msgs/Point','geometry_msgs/Pose','geometry_msgs/Pose','geometry_msgs/Point','bool','halfsteps_pattern_generator/Footprint[]']
00069
00070 def __init__(self, *args, **kwds):
00071 """
00072 Constructor. Any message fields that are implicitly/explicitly
00073 set to None will be assigned a default value. The recommend
00074 use is keyword arguments as this is more robust to future message
00075 changes. You cannot mix in-order arguments and keyword arguments.
00076
00077 The available fields are:
00078 initial_left_foot_position,initial_right_foot_position,initial_center_of_mass_position,final_left_foot_position,final_right_foot_position,final_center_of_mass_position,start_with_left_foot,footprints
00079
00080 :param args: complete set of field values, in .msg order
00081 :param kwds: use keyword arguments corresponding to message field names
00082 to set specific fields.
00083 """
00084 if args or kwds:
00085 super(GetPathRequest, self).__init__(*args, **kwds)
00086
00087 if self.initial_left_foot_position is None:
00088 self.initial_left_foot_position = geometry_msgs.msg.Pose()
00089 if self.initial_right_foot_position is None:
00090 self.initial_right_foot_position = geometry_msgs.msg.Pose()
00091 if self.initial_center_of_mass_position is None:
00092 self.initial_center_of_mass_position = geometry_msgs.msg.Point()
00093 if self.final_left_foot_position is None:
00094 self.final_left_foot_position = geometry_msgs.msg.Pose()
00095 if self.final_right_foot_position is None:
00096 self.final_right_foot_position = geometry_msgs.msg.Pose()
00097 if self.final_center_of_mass_position is None:
00098 self.final_center_of_mass_position = geometry_msgs.msg.Point()
00099 if self.start_with_left_foot is None:
00100 self.start_with_left_foot = False
00101 if self.footprints is None:
00102 self.footprints = []
00103 else:
00104 self.initial_left_foot_position = geometry_msgs.msg.Pose()
00105 self.initial_right_foot_position = geometry_msgs.msg.Pose()
00106 self.initial_center_of_mass_position = geometry_msgs.msg.Point()
00107 self.final_left_foot_position = geometry_msgs.msg.Pose()
00108 self.final_right_foot_position = geometry_msgs.msg.Pose()
00109 self.final_center_of_mass_position = geometry_msgs.msg.Point()
00110 self.start_with_left_foot = False
00111 self.footprints = []
00112
00113 def _get_types(self):
00114 """
00115 internal API method
00116 """
00117 return self._slot_types
00118
00119 def serialize(self, buff):
00120 """
00121 serialize message into buffer
00122 :param buff: buffer, ``StringIO``
00123 """
00124 try:
00125 _x = self
00126 buff.write(_struct_34dB.pack(_x.initial_left_foot_position.position.x, _x.initial_left_foot_position.position.y, _x.initial_left_foot_position.position.z, _x.initial_left_foot_position.orientation.x, _x.initial_left_foot_position.orientation.y, _x.initial_left_foot_position.orientation.z, _x.initial_left_foot_position.orientation.w, _x.initial_right_foot_position.position.x, _x.initial_right_foot_position.position.y, _x.initial_right_foot_position.position.z, _x.initial_right_foot_position.orientation.x, _x.initial_right_foot_position.orientation.y, _x.initial_right_foot_position.orientation.z, _x.initial_right_foot_position.orientation.w, _x.initial_center_of_mass_position.x, _x.initial_center_of_mass_position.y, _x.initial_center_of_mass_position.z, _x.final_left_foot_position.position.x, _x.final_left_foot_position.position.y, _x.final_left_foot_position.position.z, _x.final_left_foot_position.orientation.x, _x.final_left_foot_position.orientation.y, _x.final_left_foot_position.orientation.z, _x.final_left_foot_position.orientation.w, _x.final_right_foot_position.position.x, _x.final_right_foot_position.position.y, _x.final_right_foot_position.position.z, _x.final_right_foot_position.orientation.x, _x.final_right_foot_position.orientation.y, _x.final_right_foot_position.orientation.z, _x.final_right_foot_position.orientation.w, _x.final_center_of_mass_position.x, _x.final_center_of_mass_position.y, _x.final_center_of_mass_position.z, _x.start_with_left_foot))
00127 length = len(self.footprints)
00128 buff.write(_struct_I.pack(length))
00129 for val1 in self.footprints:
00130 _v1 = val1.footprint
00131 _v2 = _v1.beginTime
00132 _x = _v2
00133 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
00134 _v3 = _v1.duration
00135 _x = _v3
00136 buff.write(_struct_2i.pack(_x.secs, _x.nsecs))
00137 _x = _v1
00138 buff.write(_struct_3d.pack(_x.x, _x.y, _x.theta))
00139 _x = val1
00140 buff.write(_struct_4d.pack(_x.slideUp, _x.slideDown, _x.horizontalDistance, _x.stepHeight))
00141 except struct.error as se: self._check_types(se)
00142 except TypeError as te: self._check_types(te)
00143
00144 def deserialize(self, str):
00145 """
00146 unpack serialized message in str into this message instance
00147 :param str: byte array of serialized message, ``str``
00148 """
00149 try:
00150 if self.initial_left_foot_position is None:
00151 self.initial_left_foot_position = geometry_msgs.msg.Pose()
00152 if self.initial_right_foot_position is None:
00153 self.initial_right_foot_position = geometry_msgs.msg.Pose()
00154 if self.initial_center_of_mass_position is None:
00155 self.initial_center_of_mass_position = geometry_msgs.msg.Point()
00156 if self.final_left_foot_position is None:
00157 self.final_left_foot_position = geometry_msgs.msg.Pose()
00158 if self.final_right_foot_position is None:
00159 self.final_right_foot_position = geometry_msgs.msg.Pose()
00160 if self.final_center_of_mass_position is None:
00161 self.final_center_of_mass_position = geometry_msgs.msg.Point()
00162 if self.footprints is None:
00163 self.footprints = None
00164 end = 0
00165 _x = self
00166 start = end
00167 end += 273
00168 (_x.initial_left_foot_position.position.x, _x.initial_left_foot_position.position.y, _x.initial_left_foot_position.position.z, _x.initial_left_foot_position.orientation.x, _x.initial_left_foot_position.orientation.y, _x.initial_left_foot_position.orientation.z, _x.initial_left_foot_position.orientation.w, _x.initial_right_foot_position.position.x, _x.initial_right_foot_position.position.y, _x.initial_right_foot_position.position.z, _x.initial_right_foot_position.orientation.x, _x.initial_right_foot_position.orientation.y, _x.initial_right_foot_position.orientation.z, _x.initial_right_foot_position.orientation.w, _x.initial_center_of_mass_position.x, _x.initial_center_of_mass_position.y, _x.initial_center_of_mass_position.z, _x.final_left_foot_position.position.x, _x.final_left_foot_position.position.y, _x.final_left_foot_position.position.z, _x.final_left_foot_position.orientation.x, _x.final_left_foot_position.orientation.y, _x.final_left_foot_position.orientation.z, _x.final_left_foot_position.orientation.w, _x.final_right_foot_position.position.x, _x.final_right_foot_position.position.y, _x.final_right_foot_position.position.z, _x.final_right_foot_position.orientation.x, _x.final_right_foot_position.orientation.y, _x.final_right_foot_position.orientation.z, _x.final_right_foot_position.orientation.w, _x.final_center_of_mass_position.x, _x.final_center_of_mass_position.y, _x.final_center_of_mass_position.z, _x.start_with_left_foot,) = _struct_34dB.unpack(str[start:end])
00169 self.start_with_left_foot = bool(self.start_with_left_foot)
00170 start = end
00171 end += 4
00172 (length,) = _struct_I.unpack(str[start:end])
00173 self.footprints = []
00174 for i in range(0, length):
00175 val1 = halfsteps_pattern_generator.msg.Footprint()
00176 _v4 = val1.footprint
00177 _v5 = _v4.beginTime
00178 _x = _v5
00179 start = end
00180 end += 8
00181 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
00182 _v6 = _v4.duration
00183 _x = _v6
00184 start = end
00185 end += 8
00186 (_x.secs, _x.nsecs,) = _struct_2i.unpack(str[start:end])
00187 _x = _v4
00188 start = end
00189 end += 24
00190 (_x.x, _x.y, _x.theta,) = _struct_3d.unpack(str[start:end])
00191 _x = val1
00192 start = end
00193 end += 32
00194 (_x.slideUp, _x.slideDown, _x.horizontalDistance, _x.stepHeight,) = _struct_4d.unpack(str[start:end])
00195 self.footprints.append(val1)
00196 return self
00197 except struct.error as e:
00198 raise genpy.DeserializationError(e)
00199
00200
00201 def serialize_numpy(self, buff, numpy):
00202 """
00203 serialize message with numpy array types into buffer
00204 :param buff: buffer, ``StringIO``
00205 :param numpy: numpy python module
00206 """
00207 try:
00208 _x = self
00209 buff.write(_struct_34dB.pack(_x.initial_left_foot_position.position.x, _x.initial_left_foot_position.position.y, _x.initial_left_foot_position.position.z, _x.initial_left_foot_position.orientation.x, _x.initial_left_foot_position.orientation.y, _x.initial_left_foot_position.orientation.z, _x.initial_left_foot_position.orientation.w, _x.initial_right_foot_position.position.x, _x.initial_right_foot_position.position.y, _x.initial_right_foot_position.position.z, _x.initial_right_foot_position.orientation.x, _x.initial_right_foot_position.orientation.y, _x.initial_right_foot_position.orientation.z, _x.initial_right_foot_position.orientation.w, _x.initial_center_of_mass_position.x, _x.initial_center_of_mass_position.y, _x.initial_center_of_mass_position.z, _x.final_left_foot_position.position.x, _x.final_left_foot_position.position.y, _x.final_left_foot_position.position.z, _x.final_left_foot_position.orientation.x, _x.final_left_foot_position.orientation.y, _x.final_left_foot_position.orientation.z, _x.final_left_foot_position.orientation.w, _x.final_right_foot_position.position.x, _x.final_right_foot_position.position.y, _x.final_right_foot_position.position.z, _x.final_right_foot_position.orientation.x, _x.final_right_foot_position.orientation.y, _x.final_right_foot_position.orientation.z, _x.final_right_foot_position.orientation.w, _x.final_center_of_mass_position.x, _x.final_center_of_mass_position.y, _x.final_center_of_mass_position.z, _x.start_with_left_foot))
00210 length = len(self.footprints)
00211 buff.write(_struct_I.pack(length))
00212 for val1 in self.footprints:
00213 _v7 = val1.footprint
00214 _v8 = _v7.beginTime
00215 _x = _v8
00216 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
00217 _v9 = _v7.duration
00218 _x = _v9
00219 buff.write(_struct_2i.pack(_x.secs, _x.nsecs))
00220 _x = _v7
00221 buff.write(_struct_3d.pack(_x.x, _x.y, _x.theta))
00222 _x = val1
00223 buff.write(_struct_4d.pack(_x.slideUp, _x.slideDown, _x.horizontalDistance, _x.stepHeight))
00224 except struct.error as se: self._check_types(se)
00225 except TypeError as te: self._check_types(te)
00226
00227 def deserialize_numpy(self, str, numpy):
00228 """
00229 unpack serialized message in str into this message instance using numpy for array types
00230 :param str: byte array of serialized message, ``str``
00231 :param numpy: numpy python module
00232 """
00233 try:
00234 if self.initial_left_foot_position is None:
00235 self.initial_left_foot_position = geometry_msgs.msg.Pose()
00236 if self.initial_right_foot_position is None:
00237 self.initial_right_foot_position = geometry_msgs.msg.Pose()
00238 if self.initial_center_of_mass_position is None:
00239 self.initial_center_of_mass_position = geometry_msgs.msg.Point()
00240 if self.final_left_foot_position is None:
00241 self.final_left_foot_position = geometry_msgs.msg.Pose()
00242 if self.final_right_foot_position is None:
00243 self.final_right_foot_position = geometry_msgs.msg.Pose()
00244 if self.final_center_of_mass_position is None:
00245 self.final_center_of_mass_position = geometry_msgs.msg.Point()
00246 if self.footprints is None:
00247 self.footprints = None
00248 end = 0
00249 _x = self
00250 start = end
00251 end += 273
00252 (_x.initial_left_foot_position.position.x, _x.initial_left_foot_position.position.y, _x.initial_left_foot_position.position.z, _x.initial_left_foot_position.orientation.x, _x.initial_left_foot_position.orientation.y, _x.initial_left_foot_position.orientation.z, _x.initial_left_foot_position.orientation.w, _x.initial_right_foot_position.position.x, _x.initial_right_foot_position.position.y, _x.initial_right_foot_position.position.z, _x.initial_right_foot_position.orientation.x, _x.initial_right_foot_position.orientation.y, _x.initial_right_foot_position.orientation.z, _x.initial_right_foot_position.orientation.w, _x.initial_center_of_mass_position.x, _x.initial_center_of_mass_position.y, _x.initial_center_of_mass_position.z, _x.final_left_foot_position.position.x, _x.final_left_foot_position.position.y, _x.final_left_foot_position.position.z, _x.final_left_foot_position.orientation.x, _x.final_left_foot_position.orientation.y, _x.final_left_foot_position.orientation.z, _x.final_left_foot_position.orientation.w, _x.final_right_foot_position.position.x, _x.final_right_foot_position.position.y, _x.final_right_foot_position.position.z, _x.final_right_foot_position.orientation.x, _x.final_right_foot_position.orientation.y, _x.final_right_foot_position.orientation.z, _x.final_right_foot_position.orientation.w, _x.final_center_of_mass_position.x, _x.final_center_of_mass_position.y, _x.final_center_of_mass_position.z, _x.start_with_left_foot,) = _struct_34dB.unpack(str[start:end])
00253 self.start_with_left_foot = bool(self.start_with_left_foot)
00254 start = end
00255 end += 4
00256 (length,) = _struct_I.unpack(str[start:end])
00257 self.footprints = []
00258 for i in range(0, length):
00259 val1 = halfsteps_pattern_generator.msg.Footprint()
00260 _v10 = val1.footprint
00261 _v11 = _v10.beginTime
00262 _x = _v11
00263 start = end
00264 end += 8
00265 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
00266 _v12 = _v10.duration
00267 _x = _v12
00268 start = end
00269 end += 8
00270 (_x.secs, _x.nsecs,) = _struct_2i.unpack(str[start:end])
00271 _x = _v10
00272 start = end
00273 end += 24
00274 (_x.x, _x.y, _x.theta,) = _struct_3d.unpack(str[start:end])
00275 _x = val1
00276 start = end
00277 end += 32
00278 (_x.slideUp, _x.slideDown, _x.horizontalDistance, _x.stepHeight,) = _struct_4d.unpack(str[start:end])
00279 self.footprints.append(val1)
00280 return self
00281 except struct.error as e:
00282 raise genpy.DeserializationError(e)
00283
00284 _struct_I = genpy.struct_I
00285 _struct_2i = struct.Struct("<2i")
00286 _struct_4d = struct.Struct("<4d")
00287 _struct_3d = struct.Struct("<3d")
00288 _struct_2I = struct.Struct("<2I")
00289 _struct_34dB = struct.Struct("<34dB")
00290 """autogenerated by genpy from halfsteps_pattern_generator/GetPathResponse.msg. Do not edit."""
00291 import sys
00292 python3 = True if sys.hexversion > 0x03000000 else False
00293 import genpy
00294 import struct
00295
00296 import geometry_msgs.msg
00297 import walk_msgs.msg
00298 import nav_msgs.msg
00299 import std_msgs.msg
00300
00301 class GetPathResponse(genpy.Message):
00302 _md5sum = "76d3891998acfa84511d5b8b9e0f9b97"
00303 _type = "halfsteps_pattern_generator/GetPathResponse"
00304 _has_header = False
00305 _full_text = """walk_msgs/WalkPath path
00306
00307
00308 ================================================================================
00309 MSG: walk_msgs/WalkPath
00310 nav_msgs/Path left_foot
00311 nav_msgs/Path right_foot
00312 walk_msgs/PathPoint3d center_of_mass
00313 walk_msgs/PathPoint2d zmp
00314
00315 ================================================================================
00316 MSG: nav_msgs/Path
00317 #An array of poses that represents a Path for a robot to follow
00318 Header header
00319 geometry_msgs/PoseStamped[] poses
00320
00321 ================================================================================
00322 MSG: std_msgs/Header
00323 # Standard metadata for higher-level stamped data types.
00324 # This is generally used to communicate timestamped data
00325 # in a particular coordinate frame.
00326 #
00327 # sequence ID: consecutively increasing ID
00328 uint32 seq
00329 #Two-integer timestamp that is expressed as:
00330 # * stamp.secs: seconds (stamp_secs) since epoch
00331 # * stamp.nsecs: nanoseconds since stamp_secs
00332 # time-handling sugar is provided by the client library
00333 time stamp
00334 #Frame this data is associated with
00335 # 0: no frame
00336 # 1: global frame
00337 string frame_id
00338
00339 ================================================================================
00340 MSG: geometry_msgs/PoseStamped
00341 # A Pose with reference coordinate frame and timestamp
00342 Header header
00343 Pose pose
00344
00345 ================================================================================
00346 MSG: geometry_msgs/Pose
00347 # A representation of pose in free space, composed of postion and orientation.
00348 Point position
00349 Quaternion orientation
00350
00351 ================================================================================
00352 MSG: geometry_msgs/Point
00353 # This contains the position of a point in free space
00354 float64 x
00355 float64 y
00356 float64 z
00357
00358 ================================================================================
00359 MSG: geometry_msgs/Quaternion
00360 # This represents an orientation in free space in quaternion form.
00361
00362 float64 x
00363 float64 y
00364 float64 z
00365 float64 w
00366
00367 ================================================================================
00368 MSG: walk_msgs/PathPoint3d
00369 Header header
00370 geometry_msgs/PointStamped[] points
00371
00372 ================================================================================
00373 MSG: geometry_msgs/PointStamped
00374 # This represents a Point with reference coordinate frame and timestamp
00375 Header header
00376 Point point
00377
00378 ================================================================================
00379 MSG: walk_msgs/PathPoint2d
00380 Header header
00381 walk_msgs/Point2dStamped[] points
00382
00383 ================================================================================
00384 MSG: walk_msgs/Point2dStamped
00385 Header header
00386 Point2d point
00387
00388 ================================================================================
00389 MSG: walk_msgs/Point2d
00390 float64 x
00391 float64 y
00392
00393 """
00394 __slots__ = ['path']
00395 _slot_types = ['walk_msgs/WalkPath']
00396
00397 def __init__(self, *args, **kwds):
00398 """
00399 Constructor. Any message fields that are implicitly/explicitly
00400 set to None will be assigned a default value. The recommend
00401 use is keyword arguments as this is more robust to future message
00402 changes. You cannot mix in-order arguments and keyword arguments.
00403
00404 The available fields are:
00405 path
00406
00407 :param args: complete set of field values, in .msg order
00408 :param kwds: use keyword arguments corresponding to message field names
00409 to set specific fields.
00410 """
00411 if args or kwds:
00412 super(GetPathResponse, self).__init__(*args, **kwds)
00413
00414 if self.path is None:
00415 self.path = walk_msgs.msg.WalkPath()
00416 else:
00417 self.path = walk_msgs.msg.WalkPath()
00418
00419 def _get_types(self):
00420 """
00421 internal API method
00422 """
00423 return self._slot_types
00424
00425 def serialize(self, buff):
00426 """
00427 serialize message into buffer
00428 :param buff: buffer, ``StringIO``
00429 """
00430 try:
00431 _x = self
00432 buff.write(_struct_3I.pack(_x.path.left_foot.header.seq, _x.path.left_foot.header.stamp.secs, _x.path.left_foot.header.stamp.nsecs))
00433 _x = self.path.left_foot.header.frame_id
00434 length = len(_x)
00435 if python3 or type(_x) == unicode:
00436 _x = _x.encode('utf-8')
00437 length = len(_x)
00438 buff.write(struct.pack('<I%ss'%length, length, _x))
00439 length = len(self.path.left_foot.poses)
00440 buff.write(_struct_I.pack(length))
00441 for val1 in self.path.left_foot.poses:
00442 _v13 = val1.header
00443 buff.write(_struct_I.pack(_v13.seq))
00444 _v14 = _v13.stamp
00445 _x = _v14
00446 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
00447 _x = _v13.frame_id
00448 length = len(_x)
00449 if python3 or type(_x) == unicode:
00450 _x = _x.encode('utf-8')
00451 length = len(_x)
00452 buff.write(struct.pack('<I%ss'%length, length, _x))
00453 _v15 = val1.pose
00454 _v16 = _v15.position
00455 _x = _v16
00456 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00457 _v17 = _v15.orientation
00458 _x = _v17
00459 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
00460 _x = self
00461 buff.write(_struct_3I.pack(_x.path.right_foot.header.seq, _x.path.right_foot.header.stamp.secs, _x.path.right_foot.header.stamp.nsecs))
00462 _x = self.path.right_foot.header.frame_id
00463 length = len(_x)
00464 if python3 or type(_x) == unicode:
00465 _x = _x.encode('utf-8')
00466 length = len(_x)
00467 buff.write(struct.pack('<I%ss'%length, length, _x))
00468 length = len(self.path.right_foot.poses)
00469 buff.write(_struct_I.pack(length))
00470 for val1 in self.path.right_foot.poses:
00471 _v18 = val1.header
00472 buff.write(_struct_I.pack(_v18.seq))
00473 _v19 = _v18.stamp
00474 _x = _v19
00475 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
00476 _x = _v18.frame_id
00477 length = len(_x)
00478 if python3 or type(_x) == unicode:
00479 _x = _x.encode('utf-8')
00480 length = len(_x)
00481 buff.write(struct.pack('<I%ss'%length, length, _x))
00482 _v20 = val1.pose
00483 _v21 = _v20.position
00484 _x = _v21
00485 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00486 _v22 = _v20.orientation
00487 _x = _v22
00488 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
00489 _x = self
00490 buff.write(_struct_3I.pack(_x.path.center_of_mass.header.seq, _x.path.center_of_mass.header.stamp.secs, _x.path.center_of_mass.header.stamp.nsecs))
00491 _x = self.path.center_of_mass.header.frame_id
00492 length = len(_x)
00493 if python3 or type(_x) == unicode:
00494 _x = _x.encode('utf-8')
00495 length = len(_x)
00496 buff.write(struct.pack('<I%ss'%length, length, _x))
00497 length = len(self.path.center_of_mass.points)
00498 buff.write(_struct_I.pack(length))
00499 for val1 in self.path.center_of_mass.points:
00500 _v23 = val1.header
00501 buff.write(_struct_I.pack(_v23.seq))
00502 _v24 = _v23.stamp
00503 _x = _v24
00504 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
00505 _x = _v23.frame_id
00506 length = len(_x)
00507 if python3 or type(_x) == unicode:
00508 _x = _x.encode('utf-8')
00509 length = len(_x)
00510 buff.write(struct.pack('<I%ss'%length, length, _x))
00511 _v25 = val1.point
00512 _x = _v25
00513 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00514 _x = self
00515 buff.write(_struct_3I.pack(_x.path.zmp.header.seq, _x.path.zmp.header.stamp.secs, _x.path.zmp.header.stamp.nsecs))
00516 _x = self.path.zmp.header.frame_id
00517 length = len(_x)
00518 if python3 or type(_x) == unicode:
00519 _x = _x.encode('utf-8')
00520 length = len(_x)
00521 buff.write(struct.pack('<I%ss'%length, length, _x))
00522 length = len(self.path.zmp.points)
00523 buff.write(_struct_I.pack(length))
00524 for val1 in self.path.zmp.points:
00525 _v26 = val1.header
00526 buff.write(_struct_I.pack(_v26.seq))
00527 _v27 = _v26.stamp
00528 _x = _v27
00529 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
00530 _x = _v26.frame_id
00531 length = len(_x)
00532 if python3 or type(_x) == unicode:
00533 _x = _x.encode('utf-8')
00534 length = len(_x)
00535 buff.write(struct.pack('<I%ss'%length, length, _x))
00536 _v28 = val1.point
00537 _x = _v28
00538 buff.write(_struct_2d.pack(_x.x, _x.y))
00539 except struct.error as se: self._check_types(se)
00540 except TypeError as te: self._check_types(te)
00541
00542 def deserialize(self, str):
00543 """
00544 unpack serialized message in str into this message instance
00545 :param str: byte array of serialized message, ``str``
00546 """
00547 try:
00548 if self.path is None:
00549 self.path = walk_msgs.msg.WalkPath()
00550 end = 0
00551 _x = self
00552 start = end
00553 end += 12
00554 (_x.path.left_foot.header.seq, _x.path.left_foot.header.stamp.secs, _x.path.left_foot.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
00555 start = end
00556 end += 4
00557 (length,) = _struct_I.unpack(str[start:end])
00558 start = end
00559 end += length
00560 if python3:
00561 self.path.left_foot.header.frame_id = str[start:end].decode('utf-8')
00562 else:
00563 self.path.left_foot.header.frame_id = str[start:end]
00564 start = end
00565 end += 4
00566 (length,) = _struct_I.unpack(str[start:end])
00567 self.path.left_foot.poses = []
00568 for i in range(0, length):
00569 val1 = geometry_msgs.msg.PoseStamped()
00570 _v29 = val1.header
00571 start = end
00572 end += 4
00573 (_v29.seq,) = _struct_I.unpack(str[start:end])
00574 _v30 = _v29.stamp
00575 _x = _v30
00576 start = end
00577 end += 8
00578 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
00579 start = end
00580 end += 4
00581 (length,) = _struct_I.unpack(str[start:end])
00582 start = end
00583 end += length
00584 if python3:
00585 _v29.frame_id = str[start:end].decode('utf-8')
00586 else:
00587 _v29.frame_id = str[start:end]
00588 _v31 = val1.pose
00589 _v32 = _v31.position
00590 _x = _v32
00591 start = end
00592 end += 24
00593 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
00594 _v33 = _v31.orientation
00595 _x = _v33
00596 start = end
00597 end += 32
00598 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
00599 self.path.left_foot.poses.append(val1)
00600 _x = self
00601 start = end
00602 end += 12
00603 (_x.path.right_foot.header.seq, _x.path.right_foot.header.stamp.secs, _x.path.right_foot.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
00604 start = end
00605 end += 4
00606 (length,) = _struct_I.unpack(str[start:end])
00607 start = end
00608 end += length
00609 if python3:
00610 self.path.right_foot.header.frame_id = str[start:end].decode('utf-8')
00611 else:
00612 self.path.right_foot.header.frame_id = str[start:end]
00613 start = end
00614 end += 4
00615 (length,) = _struct_I.unpack(str[start:end])
00616 self.path.right_foot.poses = []
00617 for i in range(0, length):
00618 val1 = geometry_msgs.msg.PoseStamped()
00619 _v34 = val1.header
00620 start = end
00621 end += 4
00622 (_v34.seq,) = _struct_I.unpack(str[start:end])
00623 _v35 = _v34.stamp
00624 _x = _v35
00625 start = end
00626 end += 8
00627 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
00628 start = end
00629 end += 4
00630 (length,) = _struct_I.unpack(str[start:end])
00631 start = end
00632 end += length
00633 if python3:
00634 _v34.frame_id = str[start:end].decode('utf-8')
00635 else:
00636 _v34.frame_id = str[start:end]
00637 _v36 = val1.pose
00638 _v37 = _v36.position
00639 _x = _v37
00640 start = end
00641 end += 24
00642 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
00643 _v38 = _v36.orientation
00644 _x = _v38
00645 start = end
00646 end += 32
00647 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
00648 self.path.right_foot.poses.append(val1)
00649 _x = self
00650 start = end
00651 end += 12
00652 (_x.path.center_of_mass.header.seq, _x.path.center_of_mass.header.stamp.secs, _x.path.center_of_mass.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
00653 start = end
00654 end += 4
00655 (length,) = _struct_I.unpack(str[start:end])
00656 start = end
00657 end += length
00658 if python3:
00659 self.path.center_of_mass.header.frame_id = str[start:end].decode('utf-8')
00660 else:
00661 self.path.center_of_mass.header.frame_id = str[start:end]
00662 start = end
00663 end += 4
00664 (length,) = _struct_I.unpack(str[start:end])
00665 self.path.center_of_mass.points = []
00666 for i in range(0, length):
00667 val1 = geometry_msgs.msg.PointStamped()
00668 _v39 = val1.header
00669 start = end
00670 end += 4
00671 (_v39.seq,) = _struct_I.unpack(str[start:end])
00672 _v40 = _v39.stamp
00673 _x = _v40
00674 start = end
00675 end += 8
00676 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
00677 start = end
00678 end += 4
00679 (length,) = _struct_I.unpack(str[start:end])
00680 start = end
00681 end += length
00682 if python3:
00683 _v39.frame_id = str[start:end].decode('utf-8')
00684 else:
00685 _v39.frame_id = str[start:end]
00686 _v41 = val1.point
00687 _x = _v41
00688 start = end
00689 end += 24
00690 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
00691 self.path.center_of_mass.points.append(val1)
00692 _x = self
00693 start = end
00694 end += 12
00695 (_x.path.zmp.header.seq, _x.path.zmp.header.stamp.secs, _x.path.zmp.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
00696 start = end
00697 end += 4
00698 (length,) = _struct_I.unpack(str[start:end])
00699 start = end
00700 end += length
00701 if python3:
00702 self.path.zmp.header.frame_id = str[start:end].decode('utf-8')
00703 else:
00704 self.path.zmp.header.frame_id = str[start:end]
00705 start = end
00706 end += 4
00707 (length,) = _struct_I.unpack(str[start:end])
00708 self.path.zmp.points = []
00709 for i in range(0, length):
00710 val1 = walk_msgs.msg.Point2dStamped()
00711 _v42 = val1.header
00712 start = end
00713 end += 4
00714 (_v42.seq,) = _struct_I.unpack(str[start:end])
00715 _v43 = _v42.stamp
00716 _x = _v43
00717 start = end
00718 end += 8
00719 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
00720 start = end
00721 end += 4
00722 (length,) = _struct_I.unpack(str[start:end])
00723 start = end
00724 end += length
00725 if python3:
00726 _v42.frame_id = str[start:end].decode('utf-8')
00727 else:
00728 _v42.frame_id = str[start:end]
00729 _v44 = val1.point
00730 _x = _v44
00731 start = end
00732 end += 16
00733 (_x.x, _x.y,) = _struct_2d.unpack(str[start:end])
00734 self.path.zmp.points.append(val1)
00735 return self
00736 except struct.error as e:
00737 raise genpy.DeserializationError(e)
00738
00739
00740 def serialize_numpy(self, buff, numpy):
00741 """
00742 serialize message with numpy array types into buffer
00743 :param buff: buffer, ``StringIO``
00744 :param numpy: numpy python module
00745 """
00746 try:
00747 _x = self
00748 buff.write(_struct_3I.pack(_x.path.left_foot.header.seq, _x.path.left_foot.header.stamp.secs, _x.path.left_foot.header.stamp.nsecs))
00749 _x = self.path.left_foot.header.frame_id
00750 length = len(_x)
00751 if python3 or type(_x) == unicode:
00752 _x = _x.encode('utf-8')
00753 length = len(_x)
00754 buff.write(struct.pack('<I%ss'%length, length, _x))
00755 length = len(self.path.left_foot.poses)
00756 buff.write(_struct_I.pack(length))
00757 for val1 in self.path.left_foot.poses:
00758 _v45 = val1.header
00759 buff.write(_struct_I.pack(_v45.seq))
00760 _v46 = _v45.stamp
00761 _x = _v46
00762 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
00763 _x = _v45.frame_id
00764 length = len(_x)
00765 if python3 or type(_x) == unicode:
00766 _x = _x.encode('utf-8')
00767 length = len(_x)
00768 buff.write(struct.pack('<I%ss'%length, length, _x))
00769 _v47 = val1.pose
00770 _v48 = _v47.position
00771 _x = _v48
00772 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00773 _v49 = _v47.orientation
00774 _x = _v49
00775 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
00776 _x = self
00777 buff.write(_struct_3I.pack(_x.path.right_foot.header.seq, _x.path.right_foot.header.stamp.secs, _x.path.right_foot.header.stamp.nsecs))
00778 _x = self.path.right_foot.header.frame_id
00779 length = len(_x)
00780 if python3 or type(_x) == unicode:
00781 _x = _x.encode('utf-8')
00782 length = len(_x)
00783 buff.write(struct.pack('<I%ss'%length, length, _x))
00784 length = len(self.path.right_foot.poses)
00785 buff.write(_struct_I.pack(length))
00786 for val1 in self.path.right_foot.poses:
00787 _v50 = val1.header
00788 buff.write(_struct_I.pack(_v50.seq))
00789 _v51 = _v50.stamp
00790 _x = _v51
00791 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
00792 _x = _v50.frame_id
00793 length = len(_x)
00794 if python3 or type(_x) == unicode:
00795 _x = _x.encode('utf-8')
00796 length = len(_x)
00797 buff.write(struct.pack('<I%ss'%length, length, _x))
00798 _v52 = val1.pose
00799 _v53 = _v52.position
00800 _x = _v53
00801 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00802 _v54 = _v52.orientation
00803 _x = _v54
00804 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
00805 _x = self
00806 buff.write(_struct_3I.pack(_x.path.center_of_mass.header.seq, _x.path.center_of_mass.header.stamp.secs, _x.path.center_of_mass.header.stamp.nsecs))
00807 _x = self.path.center_of_mass.header.frame_id
00808 length = len(_x)
00809 if python3 or type(_x) == unicode:
00810 _x = _x.encode('utf-8')
00811 length = len(_x)
00812 buff.write(struct.pack('<I%ss'%length, length, _x))
00813 length = len(self.path.center_of_mass.points)
00814 buff.write(_struct_I.pack(length))
00815 for val1 in self.path.center_of_mass.points:
00816 _v55 = val1.header
00817 buff.write(_struct_I.pack(_v55.seq))
00818 _v56 = _v55.stamp
00819 _x = _v56
00820 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
00821 _x = _v55.frame_id
00822 length = len(_x)
00823 if python3 or type(_x) == unicode:
00824 _x = _x.encode('utf-8')
00825 length = len(_x)
00826 buff.write(struct.pack('<I%ss'%length, length, _x))
00827 _v57 = val1.point
00828 _x = _v57
00829 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00830 _x = self
00831 buff.write(_struct_3I.pack(_x.path.zmp.header.seq, _x.path.zmp.header.stamp.secs, _x.path.zmp.header.stamp.nsecs))
00832 _x = self.path.zmp.header.frame_id
00833 length = len(_x)
00834 if python3 or type(_x) == unicode:
00835 _x = _x.encode('utf-8')
00836 length = len(_x)
00837 buff.write(struct.pack('<I%ss'%length, length, _x))
00838 length = len(self.path.zmp.points)
00839 buff.write(_struct_I.pack(length))
00840 for val1 in self.path.zmp.points:
00841 _v58 = val1.header
00842 buff.write(_struct_I.pack(_v58.seq))
00843 _v59 = _v58.stamp
00844 _x = _v59
00845 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
00846 _x = _v58.frame_id
00847 length = len(_x)
00848 if python3 or type(_x) == unicode:
00849 _x = _x.encode('utf-8')
00850 length = len(_x)
00851 buff.write(struct.pack('<I%ss'%length, length, _x))
00852 _v60 = val1.point
00853 _x = _v60
00854 buff.write(_struct_2d.pack(_x.x, _x.y))
00855 except struct.error as se: self._check_types(se)
00856 except TypeError as te: self._check_types(te)
00857
00858 def deserialize_numpy(self, str, numpy):
00859 """
00860 unpack serialized message in str into this message instance using numpy for array types
00861 :param str: byte array of serialized message, ``str``
00862 :param numpy: numpy python module
00863 """
00864 try:
00865 if self.path is None:
00866 self.path = walk_msgs.msg.WalkPath()
00867 end = 0
00868 _x = self
00869 start = end
00870 end += 12
00871 (_x.path.left_foot.header.seq, _x.path.left_foot.header.stamp.secs, _x.path.left_foot.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
00872 start = end
00873 end += 4
00874 (length,) = _struct_I.unpack(str[start:end])
00875 start = end
00876 end += length
00877 if python3:
00878 self.path.left_foot.header.frame_id = str[start:end].decode('utf-8')
00879 else:
00880 self.path.left_foot.header.frame_id = str[start:end]
00881 start = end
00882 end += 4
00883 (length,) = _struct_I.unpack(str[start:end])
00884 self.path.left_foot.poses = []
00885 for i in range(0, length):
00886 val1 = geometry_msgs.msg.PoseStamped()
00887 _v61 = val1.header
00888 start = end
00889 end += 4
00890 (_v61.seq,) = _struct_I.unpack(str[start:end])
00891 _v62 = _v61.stamp
00892 _x = _v62
00893 start = end
00894 end += 8
00895 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
00896 start = end
00897 end += 4
00898 (length,) = _struct_I.unpack(str[start:end])
00899 start = end
00900 end += length
00901 if python3:
00902 _v61.frame_id = str[start:end].decode('utf-8')
00903 else:
00904 _v61.frame_id = str[start:end]
00905 _v63 = val1.pose
00906 _v64 = _v63.position
00907 _x = _v64
00908 start = end
00909 end += 24
00910 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
00911 _v65 = _v63.orientation
00912 _x = _v65
00913 start = end
00914 end += 32
00915 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
00916 self.path.left_foot.poses.append(val1)
00917 _x = self
00918 start = end
00919 end += 12
00920 (_x.path.right_foot.header.seq, _x.path.right_foot.header.stamp.secs, _x.path.right_foot.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
00921 start = end
00922 end += 4
00923 (length,) = _struct_I.unpack(str[start:end])
00924 start = end
00925 end += length
00926 if python3:
00927 self.path.right_foot.header.frame_id = str[start:end].decode('utf-8')
00928 else:
00929 self.path.right_foot.header.frame_id = str[start:end]
00930 start = end
00931 end += 4
00932 (length,) = _struct_I.unpack(str[start:end])
00933 self.path.right_foot.poses = []
00934 for i in range(0, length):
00935 val1 = geometry_msgs.msg.PoseStamped()
00936 _v66 = val1.header
00937 start = end
00938 end += 4
00939 (_v66.seq,) = _struct_I.unpack(str[start:end])
00940 _v67 = _v66.stamp
00941 _x = _v67
00942 start = end
00943 end += 8
00944 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
00945 start = end
00946 end += 4
00947 (length,) = _struct_I.unpack(str[start:end])
00948 start = end
00949 end += length
00950 if python3:
00951 _v66.frame_id = str[start:end].decode('utf-8')
00952 else:
00953 _v66.frame_id = str[start:end]
00954 _v68 = val1.pose
00955 _v69 = _v68.position
00956 _x = _v69
00957 start = end
00958 end += 24
00959 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
00960 _v70 = _v68.orientation
00961 _x = _v70
00962 start = end
00963 end += 32
00964 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
00965 self.path.right_foot.poses.append(val1)
00966 _x = self
00967 start = end
00968 end += 12
00969 (_x.path.center_of_mass.header.seq, _x.path.center_of_mass.header.stamp.secs, _x.path.center_of_mass.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
00970 start = end
00971 end += 4
00972 (length,) = _struct_I.unpack(str[start:end])
00973 start = end
00974 end += length
00975 if python3:
00976 self.path.center_of_mass.header.frame_id = str[start:end].decode('utf-8')
00977 else:
00978 self.path.center_of_mass.header.frame_id = str[start:end]
00979 start = end
00980 end += 4
00981 (length,) = _struct_I.unpack(str[start:end])
00982 self.path.center_of_mass.points = []
00983 for i in range(0, length):
00984 val1 = geometry_msgs.msg.PointStamped()
00985 _v71 = val1.header
00986 start = end
00987 end += 4
00988 (_v71.seq,) = _struct_I.unpack(str[start:end])
00989 _v72 = _v71.stamp
00990 _x = _v72
00991 start = end
00992 end += 8
00993 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
00994 start = end
00995 end += 4
00996 (length,) = _struct_I.unpack(str[start:end])
00997 start = end
00998 end += length
00999 if python3:
01000 _v71.frame_id = str[start:end].decode('utf-8')
01001 else:
01002 _v71.frame_id = str[start:end]
01003 _v73 = val1.point
01004 _x = _v73
01005 start = end
01006 end += 24
01007 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
01008 self.path.center_of_mass.points.append(val1)
01009 _x = self
01010 start = end
01011 end += 12
01012 (_x.path.zmp.header.seq, _x.path.zmp.header.stamp.secs, _x.path.zmp.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
01013 start = end
01014 end += 4
01015 (length,) = _struct_I.unpack(str[start:end])
01016 start = end
01017 end += length
01018 if python3:
01019 self.path.zmp.header.frame_id = str[start:end].decode('utf-8')
01020 else:
01021 self.path.zmp.header.frame_id = str[start:end]
01022 start = end
01023 end += 4
01024 (length,) = _struct_I.unpack(str[start:end])
01025 self.path.zmp.points = []
01026 for i in range(0, length):
01027 val1 = walk_msgs.msg.Point2dStamped()
01028 _v74 = val1.header
01029 start = end
01030 end += 4
01031 (_v74.seq,) = _struct_I.unpack(str[start:end])
01032 _v75 = _v74.stamp
01033 _x = _v75
01034 start = end
01035 end += 8
01036 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
01037 start = end
01038 end += 4
01039 (length,) = _struct_I.unpack(str[start:end])
01040 start = end
01041 end += length
01042 if python3:
01043 _v74.frame_id = str[start:end].decode('utf-8')
01044 else:
01045 _v74.frame_id = str[start:end]
01046 _v76 = val1.point
01047 _x = _v76
01048 start = end
01049 end += 16
01050 (_x.x, _x.y,) = _struct_2d.unpack(str[start:end])
01051 self.path.zmp.points.append(val1)
01052 return self
01053 except struct.error as e:
01054 raise genpy.DeserializationError(e)
01055
01056 _struct_I = genpy.struct_I
01057 _struct_2d = struct.Struct("<2d")
01058 _struct_3I = struct.Struct("<3I")
01059 _struct_4d = struct.Struct("<4d")
01060 _struct_2I = struct.Struct("<2I")
01061 _struct_3d = struct.Struct("<3d")
01062 class GetPath(object):
01063 _type = 'halfsteps_pattern_generator/GetPath'
01064 _md5sum = '52f659c3ddcbd1a61d82d7784cec1a56'
01065 _request_class = GetPathRequest
01066 _response_class = GetPathResponse