$search
00001 """autogenerated by genmsg_py from WalkPath.msg. Do not edit.""" 00002 import roslib.message 00003 import struct 00004 00005 import geometry_msgs.msg 00006 import walk_msgs.msg 00007 import nav_msgs.msg 00008 import std_msgs.msg 00009 00010 class WalkPath(roslib.message.Message): 00011 _md5sum = "d0a0a99ccf67829c6bd2a9f0499ae76e" 00012 _type = "walk_msgs/WalkPath" 00013 _has_header = False #flag to mark the presence of a Header object 00014 _full_text = """nav_msgs/Path left_foot 00015 nav_msgs/Path right_foot 00016 walk_msgs/PathPoint3d center_of_mass 00017 walk_msgs/PathPoint2d zmp 00018 00019 ================================================================================ 00020 MSG: nav_msgs/Path 00021 #An array of poses that represents a Path for a robot to follow 00022 Header header 00023 geometry_msgs/PoseStamped[] poses 00024 00025 ================================================================================ 00026 MSG: std_msgs/Header 00027 # Standard metadata for higher-level stamped data types. 00028 # This is generally used to communicate timestamped data 00029 # in a particular coordinate frame. 00030 # 00031 # sequence ID: consecutively increasing ID 00032 uint32 seq 00033 #Two-integer timestamp that is expressed as: 00034 # * stamp.secs: seconds (stamp_secs) since epoch 00035 # * stamp.nsecs: nanoseconds since stamp_secs 00036 # time-handling sugar is provided by the client library 00037 time stamp 00038 #Frame this data is associated with 00039 # 0: no frame 00040 # 1: global frame 00041 string frame_id 00042 00043 ================================================================================ 00044 MSG: geometry_msgs/PoseStamped 00045 # A Pose with reference coordinate frame and timestamp 00046 Header header 00047 Pose pose 00048 00049 ================================================================================ 00050 MSG: geometry_msgs/Pose 00051 # A representation of pose in free space, composed of postion and orientation. 00052 Point position 00053 Quaternion orientation 00054 00055 ================================================================================ 00056 MSG: geometry_msgs/Point 00057 # This contains the position of a point in free space 00058 float64 x 00059 float64 y 00060 float64 z 00061 00062 ================================================================================ 00063 MSG: geometry_msgs/Quaternion 00064 # This represents an orientation in free space in quaternion form. 00065 00066 float64 x 00067 float64 y 00068 float64 z 00069 float64 w 00070 00071 ================================================================================ 00072 MSG: walk_msgs/PathPoint3d 00073 Header header 00074 geometry_msgs/PointStamped[] points 00075 00076 ================================================================================ 00077 MSG: geometry_msgs/PointStamped 00078 # This represents a Point with reference coordinate frame and timestamp 00079 Header header 00080 Point point 00081 00082 ================================================================================ 00083 MSG: walk_msgs/PathPoint2d 00084 Header header 00085 walk_msgs/Point2dStamped[] points 00086 00087 ================================================================================ 00088 MSG: walk_msgs/Point2dStamped 00089 Header header 00090 Point2d point 00091 00092 ================================================================================ 00093 MSG: walk_msgs/Point2d 00094 float64 x 00095 float64 y 00096 00097 """ 00098 __slots__ = ['left_foot','right_foot','center_of_mass','zmp'] 00099 _slot_types = ['nav_msgs/Path','nav_msgs/Path','walk_msgs/PathPoint3d','walk_msgs/PathPoint2d'] 00100 00101 def __init__(self, *args, **kwds): 00102 """ 00103 Constructor. Any message fields that are implicitly/explicitly 00104 set to None will be assigned a default value. The recommend 00105 use is keyword arguments as this is more robust to future message 00106 changes. You cannot mix in-order arguments and keyword arguments. 00107 00108 The available fields are: 00109 left_foot,right_foot,center_of_mass,zmp 00110 00111 @param args: complete set of field values, in .msg order 00112 @param kwds: use keyword arguments corresponding to message field names 00113 to set specific fields. 00114 """ 00115 if args or kwds: 00116 super(WalkPath, self).__init__(*args, **kwds) 00117 #message fields cannot be None, assign default values for those that are 00118 if self.left_foot is None: 00119 self.left_foot = nav_msgs.msg.Path() 00120 if self.right_foot is None: 00121 self.right_foot = nav_msgs.msg.Path() 00122 if self.center_of_mass is None: 00123 self.center_of_mass = walk_msgs.msg.PathPoint3d() 00124 if self.zmp is None: 00125 self.zmp = walk_msgs.msg.PathPoint2d() 00126 else: 00127 self.left_foot = nav_msgs.msg.Path() 00128 self.right_foot = nav_msgs.msg.Path() 00129 self.center_of_mass = walk_msgs.msg.PathPoint3d() 00130 self.zmp = walk_msgs.msg.PathPoint2d() 00131 00132 def _get_types(self): 00133 """ 00134 internal API method 00135 """ 00136 return self._slot_types 00137 00138 def serialize(self, buff): 00139 """ 00140 serialize message into buffer 00141 @param buff: buffer 00142 @type buff: StringIO 00143 """ 00144 try: 00145 _x = self 00146 buff.write(_struct_3I.pack(_x.left_foot.header.seq, _x.left_foot.header.stamp.secs, _x.left_foot.header.stamp.nsecs)) 00147 _x = self.left_foot.header.frame_id 00148 length = len(_x) 00149 buff.write(struct.pack('<I%ss'%length, length, _x)) 00150 length = len(self.left_foot.poses) 00151 buff.write(_struct_I.pack(length)) 00152 for val1 in self.left_foot.poses: 00153 _v1 = val1.header 00154 buff.write(_struct_I.pack(_v1.seq)) 00155 _v2 = _v1.stamp 00156 _x = _v2 00157 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 00158 _x = _v1.frame_id 00159 length = len(_x) 00160 buff.write(struct.pack('<I%ss'%length, length, _x)) 00161 _v3 = val1.pose 00162 _v4 = _v3.position 00163 _x = _v4 00164 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 00165 _v5 = _v3.orientation 00166 _x = _v5 00167 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 00168 _x = self 00169 buff.write(_struct_3I.pack(_x.right_foot.header.seq, _x.right_foot.header.stamp.secs, _x.right_foot.header.stamp.nsecs)) 00170 _x = self.right_foot.header.frame_id 00171 length = len(_x) 00172 buff.write(struct.pack('<I%ss'%length, length, _x)) 00173 length = len(self.right_foot.poses) 00174 buff.write(_struct_I.pack(length)) 00175 for val1 in self.right_foot.poses: 00176 _v6 = val1.header 00177 buff.write(_struct_I.pack(_v6.seq)) 00178 _v7 = _v6.stamp 00179 _x = _v7 00180 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 00181 _x = _v6.frame_id 00182 length = len(_x) 00183 buff.write(struct.pack('<I%ss'%length, length, _x)) 00184 _v8 = val1.pose 00185 _v9 = _v8.position 00186 _x = _v9 00187 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 00188 _v10 = _v8.orientation 00189 _x = _v10 00190 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 00191 _x = self 00192 buff.write(_struct_3I.pack(_x.center_of_mass.header.seq, _x.center_of_mass.header.stamp.secs, _x.center_of_mass.header.stamp.nsecs)) 00193 _x = self.center_of_mass.header.frame_id 00194 length = len(_x) 00195 buff.write(struct.pack('<I%ss'%length, length, _x)) 00196 length = len(self.center_of_mass.points) 00197 buff.write(_struct_I.pack(length)) 00198 for val1 in self.center_of_mass.points: 00199 _v11 = val1.header 00200 buff.write(_struct_I.pack(_v11.seq)) 00201 _v12 = _v11.stamp 00202 _x = _v12 00203 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 00204 _x = _v11.frame_id 00205 length = len(_x) 00206 buff.write(struct.pack('<I%ss'%length, length, _x)) 00207 _v13 = val1.point 00208 _x = _v13 00209 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 00210 _x = self 00211 buff.write(_struct_3I.pack(_x.zmp.header.seq, _x.zmp.header.stamp.secs, _x.zmp.header.stamp.nsecs)) 00212 _x = self.zmp.header.frame_id 00213 length = len(_x) 00214 buff.write(struct.pack('<I%ss'%length, length, _x)) 00215 length = len(self.zmp.points) 00216 buff.write(_struct_I.pack(length)) 00217 for val1 in self.zmp.points: 00218 _v14 = val1.header 00219 buff.write(_struct_I.pack(_v14.seq)) 00220 _v15 = _v14.stamp 00221 _x = _v15 00222 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 00223 _x = _v14.frame_id 00224 length = len(_x) 00225 buff.write(struct.pack('<I%ss'%length, length, _x)) 00226 _v16 = val1.point 00227 _x = _v16 00228 buff.write(_struct_2d.pack(_x.x, _x.y)) 00229 except struct.error as se: self._check_types(se) 00230 except TypeError as te: self._check_types(te) 00231 00232 def deserialize(self, str): 00233 """ 00234 unpack serialized message in str into this message instance 00235 @param str: byte array of serialized message 00236 @type str: str 00237 """ 00238 try: 00239 if self.left_foot is None: 00240 self.left_foot = nav_msgs.msg.Path() 00241 if self.right_foot is None: 00242 self.right_foot = nav_msgs.msg.Path() 00243 if self.center_of_mass is None: 00244 self.center_of_mass = walk_msgs.msg.PathPoint3d() 00245 if self.zmp is None: 00246 self.zmp = walk_msgs.msg.PathPoint2d() 00247 end = 0 00248 _x = self 00249 start = end 00250 end += 12 00251 (_x.left_foot.header.seq, _x.left_foot.header.stamp.secs, _x.left_foot.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end]) 00252 start = end 00253 end += 4 00254 (length,) = _struct_I.unpack(str[start:end]) 00255 start = end 00256 end += length 00257 self.left_foot.header.frame_id = str[start:end] 00258 start = end 00259 end += 4 00260 (length,) = _struct_I.unpack(str[start:end]) 00261 self.left_foot.poses = [] 00262 for i in range(0, length): 00263 val1 = geometry_msgs.msg.PoseStamped() 00264 _v17 = val1.header 00265 start = end 00266 end += 4 00267 (_v17.seq,) = _struct_I.unpack(str[start:end]) 00268 _v18 = _v17.stamp 00269 _x = _v18 00270 start = end 00271 end += 8 00272 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 00273 start = end 00274 end += 4 00275 (length,) = _struct_I.unpack(str[start:end]) 00276 start = end 00277 end += length 00278 _v17.frame_id = str[start:end] 00279 _v19 = val1.pose 00280 _v20 = _v19.position 00281 _x = _v20 00282 start = end 00283 end += 24 00284 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 00285 _v21 = _v19.orientation 00286 _x = _v21 00287 start = end 00288 end += 32 00289 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 00290 self.left_foot.poses.append(val1) 00291 _x = self 00292 start = end 00293 end += 12 00294 (_x.right_foot.header.seq, _x.right_foot.header.stamp.secs, _x.right_foot.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end]) 00295 start = end 00296 end += 4 00297 (length,) = _struct_I.unpack(str[start:end]) 00298 start = end 00299 end += length 00300 self.right_foot.header.frame_id = str[start:end] 00301 start = end 00302 end += 4 00303 (length,) = _struct_I.unpack(str[start:end]) 00304 self.right_foot.poses = [] 00305 for i in range(0, length): 00306 val1 = geometry_msgs.msg.PoseStamped() 00307 _v22 = val1.header 00308 start = end 00309 end += 4 00310 (_v22.seq,) = _struct_I.unpack(str[start:end]) 00311 _v23 = _v22.stamp 00312 _x = _v23 00313 start = end 00314 end += 8 00315 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 00316 start = end 00317 end += 4 00318 (length,) = _struct_I.unpack(str[start:end]) 00319 start = end 00320 end += length 00321 _v22.frame_id = str[start:end] 00322 _v24 = val1.pose 00323 _v25 = _v24.position 00324 _x = _v25 00325 start = end 00326 end += 24 00327 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 00328 _v26 = _v24.orientation 00329 _x = _v26 00330 start = end 00331 end += 32 00332 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 00333 self.right_foot.poses.append(val1) 00334 _x = self 00335 start = end 00336 end += 12 00337 (_x.center_of_mass.header.seq, _x.center_of_mass.header.stamp.secs, _x.center_of_mass.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end]) 00338 start = end 00339 end += 4 00340 (length,) = _struct_I.unpack(str[start:end]) 00341 start = end 00342 end += length 00343 self.center_of_mass.header.frame_id = str[start:end] 00344 start = end 00345 end += 4 00346 (length,) = _struct_I.unpack(str[start:end]) 00347 self.center_of_mass.points = [] 00348 for i in range(0, length): 00349 val1 = geometry_msgs.msg.PointStamped() 00350 _v27 = val1.header 00351 start = end 00352 end += 4 00353 (_v27.seq,) = _struct_I.unpack(str[start:end]) 00354 _v28 = _v27.stamp 00355 _x = _v28 00356 start = end 00357 end += 8 00358 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 00359 start = end 00360 end += 4 00361 (length,) = _struct_I.unpack(str[start:end]) 00362 start = end 00363 end += length 00364 _v27.frame_id = str[start:end] 00365 _v29 = val1.point 00366 _x = _v29 00367 start = end 00368 end += 24 00369 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 00370 self.center_of_mass.points.append(val1) 00371 _x = self 00372 start = end 00373 end += 12 00374 (_x.zmp.header.seq, _x.zmp.header.stamp.secs, _x.zmp.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end]) 00375 start = end 00376 end += 4 00377 (length,) = _struct_I.unpack(str[start:end]) 00378 start = end 00379 end += length 00380 self.zmp.header.frame_id = str[start:end] 00381 start = end 00382 end += 4 00383 (length,) = _struct_I.unpack(str[start:end]) 00384 self.zmp.points = [] 00385 for i in range(0, length): 00386 val1 = walk_msgs.msg.Point2dStamped() 00387 _v30 = val1.header 00388 start = end 00389 end += 4 00390 (_v30.seq,) = _struct_I.unpack(str[start:end]) 00391 _v31 = _v30.stamp 00392 _x = _v31 00393 start = end 00394 end += 8 00395 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 00396 start = end 00397 end += 4 00398 (length,) = _struct_I.unpack(str[start:end]) 00399 start = end 00400 end += length 00401 _v30.frame_id = str[start:end] 00402 _v32 = val1.point 00403 _x = _v32 00404 start = end 00405 end += 16 00406 (_x.x, _x.y,) = _struct_2d.unpack(str[start:end]) 00407 self.zmp.points.append(val1) 00408 return self 00409 except struct.error as e: 00410 raise roslib.message.DeserializationError(e) #most likely buffer underfill 00411 00412 00413 def serialize_numpy(self, buff, numpy): 00414 """ 00415 serialize message with numpy array types into buffer 00416 @param buff: buffer 00417 @type buff: StringIO 00418 @param numpy: numpy python module 00419 @type numpy module 00420 """ 00421 try: 00422 _x = self 00423 buff.write(_struct_3I.pack(_x.left_foot.header.seq, _x.left_foot.header.stamp.secs, _x.left_foot.header.stamp.nsecs)) 00424 _x = self.left_foot.header.frame_id 00425 length = len(_x) 00426 buff.write(struct.pack('<I%ss'%length, length, _x)) 00427 length = len(self.left_foot.poses) 00428 buff.write(_struct_I.pack(length)) 00429 for val1 in self.left_foot.poses: 00430 _v33 = val1.header 00431 buff.write(_struct_I.pack(_v33.seq)) 00432 _v34 = _v33.stamp 00433 _x = _v34 00434 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 00435 _x = _v33.frame_id 00436 length = len(_x) 00437 buff.write(struct.pack('<I%ss'%length, length, _x)) 00438 _v35 = val1.pose 00439 _v36 = _v35.position 00440 _x = _v36 00441 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 00442 _v37 = _v35.orientation 00443 _x = _v37 00444 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 00445 _x = self 00446 buff.write(_struct_3I.pack(_x.right_foot.header.seq, _x.right_foot.header.stamp.secs, _x.right_foot.header.stamp.nsecs)) 00447 _x = self.right_foot.header.frame_id 00448 length = len(_x) 00449 buff.write(struct.pack('<I%ss'%length, length, _x)) 00450 length = len(self.right_foot.poses) 00451 buff.write(_struct_I.pack(length)) 00452 for val1 in self.right_foot.poses: 00453 _v38 = val1.header 00454 buff.write(_struct_I.pack(_v38.seq)) 00455 _v39 = _v38.stamp 00456 _x = _v39 00457 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 00458 _x = _v38.frame_id 00459 length = len(_x) 00460 buff.write(struct.pack('<I%ss'%length, length, _x)) 00461 _v40 = val1.pose 00462 _v41 = _v40.position 00463 _x = _v41 00464 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 00465 _v42 = _v40.orientation 00466 _x = _v42 00467 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 00468 _x = self 00469 buff.write(_struct_3I.pack(_x.center_of_mass.header.seq, _x.center_of_mass.header.stamp.secs, _x.center_of_mass.header.stamp.nsecs)) 00470 _x = self.center_of_mass.header.frame_id 00471 length = len(_x) 00472 buff.write(struct.pack('<I%ss'%length, length, _x)) 00473 length = len(self.center_of_mass.points) 00474 buff.write(_struct_I.pack(length)) 00475 for val1 in self.center_of_mass.points: 00476 _v43 = val1.header 00477 buff.write(_struct_I.pack(_v43.seq)) 00478 _v44 = _v43.stamp 00479 _x = _v44 00480 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 00481 _x = _v43.frame_id 00482 length = len(_x) 00483 buff.write(struct.pack('<I%ss'%length, length, _x)) 00484 _v45 = val1.point 00485 _x = _v45 00486 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 00487 _x = self 00488 buff.write(_struct_3I.pack(_x.zmp.header.seq, _x.zmp.header.stamp.secs, _x.zmp.header.stamp.nsecs)) 00489 _x = self.zmp.header.frame_id 00490 length = len(_x) 00491 buff.write(struct.pack('<I%ss'%length, length, _x)) 00492 length = len(self.zmp.points) 00493 buff.write(_struct_I.pack(length)) 00494 for val1 in self.zmp.points: 00495 _v46 = val1.header 00496 buff.write(_struct_I.pack(_v46.seq)) 00497 _v47 = _v46.stamp 00498 _x = _v47 00499 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 00500 _x = _v46.frame_id 00501 length = len(_x) 00502 buff.write(struct.pack('<I%ss'%length, length, _x)) 00503 _v48 = val1.point 00504 _x = _v48 00505 buff.write(_struct_2d.pack(_x.x, _x.y)) 00506 except struct.error as se: self._check_types(se) 00507 except TypeError as te: self._check_types(te) 00508 00509 def deserialize_numpy(self, str, numpy): 00510 """ 00511 unpack serialized message in str into this message instance using numpy for array types 00512 @param str: byte array of serialized message 00513 @type str: str 00514 @param numpy: numpy python module 00515 @type numpy: module 00516 """ 00517 try: 00518 if self.left_foot is None: 00519 self.left_foot = nav_msgs.msg.Path() 00520 if self.right_foot is None: 00521 self.right_foot = nav_msgs.msg.Path() 00522 if self.center_of_mass is None: 00523 self.center_of_mass = walk_msgs.msg.PathPoint3d() 00524 if self.zmp is None: 00525 self.zmp = walk_msgs.msg.PathPoint2d() 00526 end = 0 00527 _x = self 00528 start = end 00529 end += 12 00530 (_x.left_foot.header.seq, _x.left_foot.header.stamp.secs, _x.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.left_foot.header.frame_id = str[start:end] 00537 start = end 00538 end += 4 00539 (length,) = _struct_I.unpack(str[start:end]) 00540 self.left_foot.poses = [] 00541 for i in range(0, length): 00542 val1 = geometry_msgs.msg.PoseStamped() 00543 _v49 = val1.header 00544 start = end 00545 end += 4 00546 (_v49.seq,) = _struct_I.unpack(str[start:end]) 00547 _v50 = _v49.stamp 00548 _x = _v50 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 _v49.frame_id = str[start:end] 00558 _v51 = val1.pose 00559 _v52 = _v51.position 00560 _x = _v52 00561 start = end 00562 end += 24 00563 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 00564 _v53 = _v51.orientation 00565 _x = _v53 00566 start = end 00567 end += 32 00568 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 00569 self.left_foot.poses.append(val1) 00570 _x = self 00571 start = end 00572 end += 12 00573 (_x.right_foot.header.seq, _x.right_foot.header.stamp.secs, _x.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.right_foot.header.frame_id = str[start:end] 00580 start = end 00581 end += 4 00582 (length,) = _struct_I.unpack(str[start:end]) 00583 self.right_foot.poses = [] 00584 for i in range(0, length): 00585 val1 = geometry_msgs.msg.PoseStamped() 00586 _v54 = val1.header 00587 start = end 00588 end += 4 00589 (_v54.seq,) = _struct_I.unpack(str[start:end]) 00590 _v55 = _v54.stamp 00591 _x = _v55 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 _v54.frame_id = str[start:end] 00601 _v56 = val1.pose 00602 _v57 = _v56.position 00603 _x = _v57 00604 start = end 00605 end += 24 00606 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 00607 _v58 = _v56.orientation 00608 _x = _v58 00609 start = end 00610 end += 32 00611 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 00612 self.right_foot.poses.append(val1) 00613 _x = self 00614 start = end 00615 end += 12 00616 (_x.center_of_mass.header.seq, _x.center_of_mass.header.stamp.secs, _x.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.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.center_of_mass.points = [] 00627 for i in range(0, length): 00628 val1 = geometry_msgs.msg.PointStamped() 00629 _v59 = val1.header 00630 start = end 00631 end += 4 00632 (_v59.seq,) = _struct_I.unpack(str[start:end]) 00633 _v60 = _v59.stamp 00634 _x = _v60 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 _v59.frame_id = str[start:end] 00644 _v61 = val1.point 00645 _x = _v61 00646 start = end 00647 end += 24 00648 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 00649 self.center_of_mass.points.append(val1) 00650 _x = self 00651 start = end 00652 end += 12 00653 (_x.zmp.header.seq, _x.zmp.header.stamp.secs, _x.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.zmp.header.frame_id = str[start:end] 00660 start = end 00661 end += 4 00662 (length,) = _struct_I.unpack(str[start:end]) 00663 self.zmp.points = [] 00664 for i in range(0, length): 00665 val1 = walk_msgs.msg.Point2dStamped() 00666 _v62 = val1.header 00667 start = end 00668 end += 4 00669 (_v62.seq,) = _struct_I.unpack(str[start:end]) 00670 _v63 = _v62.stamp 00671 _x = _v63 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 _v62.frame_id = str[start:end] 00681 _v64 = val1.point 00682 _x = _v64 00683 start = end 00684 end += 16 00685 (_x.x, _x.y,) = _struct_2d.unpack(str[start:end]) 00686 self.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 _struct_I = roslib.message.struct_I 00692 _struct_2d = struct.Struct("<2d") 00693 _struct_3I = struct.Struct("<3I") 00694 _struct_4d = struct.Struct("<4d") 00695 _struct_2I = struct.Struct("<2I") 00696 _struct_3d = struct.Struct("<3d")