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