$search
00001 """autogenerated by genmsg_py from JointTrajectoryAction.msg. Do not edit.""" 00002 import roslib.message 00003 import struct 00004 00005 import trajectory_msgs.msg 00006 import roslib.rostime 00007 import actionlib_msgs.msg 00008 import sensor_msgs.msg 00009 import nao_msgs.msg 00010 import std_msgs.msg 00011 00012 class JointTrajectoryAction(roslib.message.Message): 00013 _md5sum = "47b9ba22fa50be8763fd3e5c3b06e11f" 00014 _type = "nao_msgs/JointTrajectoryAction" 00015 _has_header = False #flag to mark the presence of a Header object 00016 _full_text = """# ====== DO NOT MODIFY! AUTOGENERATED FROM AN ACTION DEFINITION ====== 00017 00018 JointTrajectoryActionGoal action_goal 00019 JointTrajectoryActionResult action_result 00020 JointTrajectoryActionFeedback action_feedback 00021 00022 ================================================================================ 00023 MSG: nao_msgs/JointTrajectoryActionGoal 00024 # ====== DO NOT MODIFY! AUTOGENERATED FROM AN ACTION DEFINITION ====== 00025 00026 Header header 00027 actionlib_msgs/GoalID goal_id 00028 JointTrajectoryGoal goal 00029 00030 ================================================================================ 00031 MSG: std_msgs/Header 00032 # Standard metadata for higher-level stamped data types. 00033 # This is generally used to communicate timestamped data 00034 # in a particular coordinate frame. 00035 # 00036 # sequence ID: consecutively increasing ID 00037 uint32 seq 00038 #Two-integer timestamp that is expressed as: 00039 # * stamp.secs: seconds (stamp_secs) since epoch 00040 # * stamp.nsecs: nanoseconds since stamp_secs 00041 # time-handling sugar is provided by the client library 00042 time stamp 00043 #Frame this data is associated with 00044 # 0: no frame 00045 # 1: global frame 00046 string frame_id 00047 00048 ================================================================================ 00049 MSG: actionlib_msgs/GoalID 00050 # The stamp should store the time at which this goal was requested. 00051 # It is used by an action server when it tries to preempt all 00052 # goals that were requested before a certain time 00053 time stamp 00054 00055 # The id provides a way to associate feedback and 00056 # result message with specific goal requests. The id 00057 # specified must be unique. 00058 string id 00059 00060 00061 ================================================================================ 00062 MSG: nao_msgs/JointTrajectoryGoal 00063 # ====== DO NOT MODIFY! AUTOGENERATED FROM AN ACTION DEFINITION ====== 00064 # goal: a joint angle trajectory 00065 trajectory_msgs/JointTrajectory trajectory 00066 # flag whether motion is absolute (=0, default) or relative (=1) 00067 uint8 relative 00068 00069 ================================================================================ 00070 MSG: trajectory_msgs/JointTrajectory 00071 Header header 00072 string[] joint_names 00073 JointTrajectoryPoint[] points 00074 ================================================================================ 00075 MSG: trajectory_msgs/JointTrajectoryPoint 00076 float64[] positions 00077 float64[] velocities 00078 float64[] accelerations 00079 duration time_from_start 00080 ================================================================================ 00081 MSG: nao_msgs/JointTrajectoryActionResult 00082 # ====== DO NOT MODIFY! AUTOGENERATED FROM AN ACTION DEFINITION ====== 00083 00084 Header header 00085 actionlib_msgs/GoalStatus status 00086 JointTrajectoryResult result 00087 00088 ================================================================================ 00089 MSG: actionlib_msgs/GoalStatus 00090 GoalID goal_id 00091 uint8 status 00092 uint8 PENDING = 0 # The goal has yet to be processed by the action server 00093 uint8 ACTIVE = 1 # The goal is currently being processed by the action server 00094 uint8 PREEMPTED = 2 # The goal received a cancel request after it started executing 00095 # and has since completed its execution (Terminal State) 00096 uint8 SUCCEEDED = 3 # The goal was achieved successfully by the action server (Terminal State) 00097 uint8 ABORTED = 4 # The goal was aborted during execution by the action server due 00098 # to some failure (Terminal State) 00099 uint8 REJECTED = 5 # The goal was rejected by the action server without being processed, 00100 # because the goal was unattainable or invalid (Terminal State) 00101 uint8 PREEMPTING = 6 # The goal received a cancel request after it started executing 00102 # and has not yet completed execution 00103 uint8 RECALLING = 7 # The goal received a cancel request before it started executing, 00104 # but the action server has not yet confirmed that the goal is canceled 00105 uint8 RECALLED = 8 # The goal received a cancel request before it started executing 00106 # and was successfully cancelled (Terminal State) 00107 uint8 LOST = 9 # An action client can determine that a goal is LOST. This should not be 00108 # sent over the wire by an action server 00109 00110 #Allow for the user to associate a string with GoalStatus for debugging 00111 string text 00112 00113 00114 ================================================================================ 00115 MSG: nao_msgs/JointTrajectoryResult 00116 # ====== DO NOT MODIFY! AUTOGENERATED FROM AN ACTION DEFINITION ====== 00117 # result is the actually reached position 00118 sensor_msgs/JointState goal_position 00119 00120 ================================================================================ 00121 MSG: sensor_msgs/JointState 00122 # This is a message that holds data to describe the state of a set of torque controlled joints. 00123 # 00124 # The state of each joint (revolute or prismatic) is defined by: 00125 # * the position of the joint (rad or m), 00126 # * the velocity of the joint (rad/s or m/s) and 00127 # * the effort that is applied in the joint (Nm or N). 00128 # 00129 # Each joint is uniquely identified by its name 00130 # The header specifies the time at which the joint states were recorded. All the joint states 00131 # in one message have to be recorded at the same time. 00132 # 00133 # This message consists of a multiple arrays, one for each part of the joint state. 00134 # The goal is to make each of the fields optional. When e.g. your joints have no 00135 # effort associated with them, you can leave the effort array empty. 00136 # 00137 # All arrays in this message should have the same size, or be empty. 00138 # This is the only way to uniquely associate the joint name with the correct 00139 # states. 00140 00141 00142 Header header 00143 00144 string[] name 00145 float64[] position 00146 float64[] velocity 00147 float64[] effort 00148 00149 ================================================================================ 00150 MSG: nao_msgs/JointTrajectoryActionFeedback 00151 # ====== DO NOT MODIFY! AUTOGENERATED FROM AN ACTION DEFINITION ====== 00152 00153 Header header 00154 actionlib_msgs/GoalStatus status 00155 JointTrajectoryFeedback feedback 00156 00157 ================================================================================ 00158 MSG: nao_msgs/JointTrajectoryFeedback 00159 # ====== DO NOT MODIFY! AUTOGENERATED FROM AN ACTION DEFINITION ====== 00160 # no feedback currently 00161 00162 """ 00163 __slots__ = ['action_goal','action_result','action_feedback'] 00164 _slot_types = ['nao_msgs/JointTrajectoryActionGoal','nao_msgs/JointTrajectoryActionResult','nao_msgs/JointTrajectoryActionFeedback'] 00165 00166 def __init__(self, *args, **kwds): 00167 """ 00168 Constructor. Any message fields that are implicitly/explicitly 00169 set to None will be assigned a default value. The recommend 00170 use is keyword arguments as this is more robust to future message 00171 changes. You cannot mix in-order arguments and keyword arguments. 00172 00173 The available fields are: 00174 action_goal,action_result,action_feedback 00175 00176 @param args: complete set of field values, in .msg order 00177 @param kwds: use keyword arguments corresponding to message field names 00178 to set specific fields. 00179 """ 00180 if args or kwds: 00181 super(JointTrajectoryAction, self).__init__(*args, **kwds) 00182 #message fields cannot be None, assign default values for those that are 00183 if self.action_goal is None: 00184 self.action_goal = nao_msgs.msg.JointTrajectoryActionGoal() 00185 if self.action_result is None: 00186 self.action_result = nao_msgs.msg.JointTrajectoryActionResult() 00187 if self.action_feedback is None: 00188 self.action_feedback = nao_msgs.msg.JointTrajectoryActionFeedback() 00189 else: 00190 self.action_goal = nao_msgs.msg.JointTrajectoryActionGoal() 00191 self.action_result = nao_msgs.msg.JointTrajectoryActionResult() 00192 self.action_feedback = nao_msgs.msg.JointTrajectoryActionFeedback() 00193 00194 def _get_types(self): 00195 """ 00196 internal API method 00197 """ 00198 return self._slot_types 00199 00200 def serialize(self, buff): 00201 """ 00202 serialize message into buffer 00203 @param buff: buffer 00204 @type buff: StringIO 00205 """ 00206 try: 00207 _x = self 00208 buff.write(_struct_3I.pack(_x.action_goal.header.seq, _x.action_goal.header.stamp.secs, _x.action_goal.header.stamp.nsecs)) 00209 _x = self.action_goal.header.frame_id 00210 length = len(_x) 00211 buff.write(struct.pack('<I%ss'%length, length, _x)) 00212 _x = self 00213 buff.write(_struct_2I.pack(_x.action_goal.goal_id.stamp.secs, _x.action_goal.goal_id.stamp.nsecs)) 00214 _x = self.action_goal.goal_id.id 00215 length = len(_x) 00216 buff.write(struct.pack('<I%ss'%length, length, _x)) 00217 _x = self 00218 buff.write(_struct_3I.pack(_x.action_goal.goal.trajectory.header.seq, _x.action_goal.goal.trajectory.header.stamp.secs, _x.action_goal.goal.trajectory.header.stamp.nsecs)) 00219 _x = self.action_goal.goal.trajectory.header.frame_id 00220 length = len(_x) 00221 buff.write(struct.pack('<I%ss'%length, length, _x)) 00222 length = len(self.action_goal.goal.trajectory.joint_names) 00223 buff.write(_struct_I.pack(length)) 00224 for val1 in self.action_goal.goal.trajectory.joint_names: 00225 length = len(val1) 00226 buff.write(struct.pack('<I%ss'%length, length, val1)) 00227 length = len(self.action_goal.goal.trajectory.points) 00228 buff.write(_struct_I.pack(length)) 00229 for val1 in self.action_goal.goal.trajectory.points: 00230 length = len(val1.positions) 00231 buff.write(_struct_I.pack(length)) 00232 pattern = '<%sd'%length 00233 buff.write(struct.pack(pattern, *val1.positions)) 00234 length = len(val1.velocities) 00235 buff.write(_struct_I.pack(length)) 00236 pattern = '<%sd'%length 00237 buff.write(struct.pack(pattern, *val1.velocities)) 00238 length = len(val1.accelerations) 00239 buff.write(_struct_I.pack(length)) 00240 pattern = '<%sd'%length 00241 buff.write(struct.pack(pattern, *val1.accelerations)) 00242 _v1 = val1.time_from_start 00243 _x = _v1 00244 buff.write(_struct_2i.pack(_x.secs, _x.nsecs)) 00245 _x = self 00246 buff.write(_struct_B3I.pack(_x.action_goal.goal.relative, _x.action_result.header.seq, _x.action_result.header.stamp.secs, _x.action_result.header.stamp.nsecs)) 00247 _x = self.action_result.header.frame_id 00248 length = len(_x) 00249 buff.write(struct.pack('<I%ss'%length, length, _x)) 00250 _x = self 00251 buff.write(_struct_2I.pack(_x.action_result.status.goal_id.stamp.secs, _x.action_result.status.goal_id.stamp.nsecs)) 00252 _x = self.action_result.status.goal_id.id 00253 length = len(_x) 00254 buff.write(struct.pack('<I%ss'%length, length, _x)) 00255 buff.write(_struct_B.pack(self.action_result.status.status)) 00256 _x = self.action_result.status.text 00257 length = len(_x) 00258 buff.write(struct.pack('<I%ss'%length, length, _x)) 00259 _x = self 00260 buff.write(_struct_3I.pack(_x.action_result.result.goal_position.header.seq, _x.action_result.result.goal_position.header.stamp.secs, _x.action_result.result.goal_position.header.stamp.nsecs)) 00261 _x = self.action_result.result.goal_position.header.frame_id 00262 length = len(_x) 00263 buff.write(struct.pack('<I%ss'%length, length, _x)) 00264 length = len(self.action_result.result.goal_position.name) 00265 buff.write(_struct_I.pack(length)) 00266 for val1 in self.action_result.result.goal_position.name: 00267 length = len(val1) 00268 buff.write(struct.pack('<I%ss'%length, length, val1)) 00269 length = len(self.action_result.result.goal_position.position) 00270 buff.write(_struct_I.pack(length)) 00271 pattern = '<%sd'%length 00272 buff.write(struct.pack(pattern, *self.action_result.result.goal_position.position)) 00273 length = len(self.action_result.result.goal_position.velocity) 00274 buff.write(_struct_I.pack(length)) 00275 pattern = '<%sd'%length 00276 buff.write(struct.pack(pattern, *self.action_result.result.goal_position.velocity)) 00277 length = len(self.action_result.result.goal_position.effort) 00278 buff.write(_struct_I.pack(length)) 00279 pattern = '<%sd'%length 00280 buff.write(struct.pack(pattern, *self.action_result.result.goal_position.effort)) 00281 _x = self 00282 buff.write(_struct_3I.pack(_x.action_feedback.header.seq, _x.action_feedback.header.stamp.secs, _x.action_feedback.header.stamp.nsecs)) 00283 _x = self.action_feedback.header.frame_id 00284 length = len(_x) 00285 buff.write(struct.pack('<I%ss'%length, length, _x)) 00286 _x = self 00287 buff.write(_struct_2I.pack(_x.action_feedback.status.goal_id.stamp.secs, _x.action_feedback.status.goal_id.stamp.nsecs)) 00288 _x = self.action_feedback.status.goal_id.id 00289 length = len(_x) 00290 buff.write(struct.pack('<I%ss'%length, length, _x)) 00291 buff.write(_struct_B.pack(self.action_feedback.status.status)) 00292 _x = self.action_feedback.status.text 00293 length = len(_x) 00294 buff.write(struct.pack('<I%ss'%length, length, _x)) 00295 except struct.error as se: self._check_types(se) 00296 except TypeError as te: self._check_types(te) 00297 00298 def deserialize(self, str): 00299 """ 00300 unpack serialized message in str into this message instance 00301 @param str: byte array of serialized message 00302 @type str: str 00303 """ 00304 try: 00305 if self.action_goal is None: 00306 self.action_goal = nao_msgs.msg.JointTrajectoryActionGoal() 00307 if self.action_result is None: 00308 self.action_result = nao_msgs.msg.JointTrajectoryActionResult() 00309 if self.action_feedback is None: 00310 self.action_feedback = nao_msgs.msg.JointTrajectoryActionFeedback() 00311 end = 0 00312 _x = self 00313 start = end 00314 end += 12 00315 (_x.action_goal.header.seq, _x.action_goal.header.stamp.secs, _x.action_goal.header.stamp.nsecs,) = _struct_3I.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 self.action_goal.header.frame_id = str[start:end] 00322 _x = self 00323 start = end 00324 end += 8 00325 (_x.action_goal.goal_id.stamp.secs, _x.action_goal.goal_id.stamp.nsecs,) = _struct_2I.unpack(str[start:end]) 00326 start = end 00327 end += 4 00328 (length,) = _struct_I.unpack(str[start:end]) 00329 start = end 00330 end += length 00331 self.action_goal.goal_id.id = str[start:end] 00332 _x = self 00333 start = end 00334 end += 12 00335 (_x.action_goal.goal.trajectory.header.seq, _x.action_goal.goal.trajectory.header.stamp.secs, _x.action_goal.goal.trajectory.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end]) 00336 start = end 00337 end += 4 00338 (length,) = _struct_I.unpack(str[start:end]) 00339 start = end 00340 end += length 00341 self.action_goal.goal.trajectory.header.frame_id = str[start:end] 00342 start = end 00343 end += 4 00344 (length,) = _struct_I.unpack(str[start:end]) 00345 self.action_goal.goal.trajectory.joint_names = [] 00346 for i in range(0, length): 00347 start = end 00348 end += 4 00349 (length,) = _struct_I.unpack(str[start:end]) 00350 start = end 00351 end += length 00352 val1 = str[start:end] 00353 self.action_goal.goal.trajectory.joint_names.append(val1) 00354 start = end 00355 end += 4 00356 (length,) = _struct_I.unpack(str[start:end]) 00357 self.action_goal.goal.trajectory.points = [] 00358 for i in range(0, length): 00359 val1 = trajectory_msgs.msg.JointTrajectoryPoint() 00360 start = end 00361 end += 4 00362 (length,) = _struct_I.unpack(str[start:end]) 00363 pattern = '<%sd'%length 00364 start = end 00365 end += struct.calcsize(pattern) 00366 val1.positions = struct.unpack(pattern, str[start:end]) 00367 start = end 00368 end += 4 00369 (length,) = _struct_I.unpack(str[start:end]) 00370 pattern = '<%sd'%length 00371 start = end 00372 end += struct.calcsize(pattern) 00373 val1.velocities = struct.unpack(pattern, str[start:end]) 00374 start = end 00375 end += 4 00376 (length,) = _struct_I.unpack(str[start:end]) 00377 pattern = '<%sd'%length 00378 start = end 00379 end += struct.calcsize(pattern) 00380 val1.accelerations = struct.unpack(pattern, str[start:end]) 00381 _v2 = val1.time_from_start 00382 _x = _v2 00383 start = end 00384 end += 8 00385 (_x.secs, _x.nsecs,) = _struct_2i.unpack(str[start:end]) 00386 self.action_goal.goal.trajectory.points.append(val1) 00387 _x = self 00388 start = end 00389 end += 13 00390 (_x.action_goal.goal.relative, _x.action_result.header.seq, _x.action_result.header.stamp.secs, _x.action_result.header.stamp.nsecs,) = _struct_B3I.unpack(str[start:end]) 00391 start = end 00392 end += 4 00393 (length,) = _struct_I.unpack(str[start:end]) 00394 start = end 00395 end += length 00396 self.action_result.header.frame_id = str[start:end] 00397 _x = self 00398 start = end 00399 end += 8 00400 (_x.action_result.status.goal_id.stamp.secs, _x.action_result.status.goal_id.stamp.nsecs,) = _struct_2I.unpack(str[start:end]) 00401 start = end 00402 end += 4 00403 (length,) = _struct_I.unpack(str[start:end]) 00404 start = end 00405 end += length 00406 self.action_result.status.goal_id.id = str[start:end] 00407 start = end 00408 end += 1 00409 (self.action_result.status.status,) = _struct_B.unpack(str[start:end]) 00410 start = end 00411 end += 4 00412 (length,) = _struct_I.unpack(str[start:end]) 00413 start = end 00414 end += length 00415 self.action_result.status.text = str[start:end] 00416 _x = self 00417 start = end 00418 end += 12 00419 (_x.action_result.result.goal_position.header.seq, _x.action_result.result.goal_position.header.stamp.secs, _x.action_result.result.goal_position.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end]) 00420 start = end 00421 end += 4 00422 (length,) = _struct_I.unpack(str[start:end]) 00423 start = end 00424 end += length 00425 self.action_result.result.goal_position.header.frame_id = str[start:end] 00426 start = end 00427 end += 4 00428 (length,) = _struct_I.unpack(str[start:end]) 00429 self.action_result.result.goal_position.name = [] 00430 for i in range(0, length): 00431 start = end 00432 end += 4 00433 (length,) = _struct_I.unpack(str[start:end]) 00434 start = end 00435 end += length 00436 val1 = str[start:end] 00437 self.action_result.result.goal_position.name.append(val1) 00438 start = end 00439 end += 4 00440 (length,) = _struct_I.unpack(str[start:end]) 00441 pattern = '<%sd'%length 00442 start = end 00443 end += struct.calcsize(pattern) 00444 self.action_result.result.goal_position.position = struct.unpack(pattern, str[start:end]) 00445 start = end 00446 end += 4 00447 (length,) = _struct_I.unpack(str[start:end]) 00448 pattern = '<%sd'%length 00449 start = end 00450 end += struct.calcsize(pattern) 00451 self.action_result.result.goal_position.velocity = struct.unpack(pattern, str[start:end]) 00452 start = end 00453 end += 4 00454 (length,) = _struct_I.unpack(str[start:end]) 00455 pattern = '<%sd'%length 00456 start = end 00457 end += struct.calcsize(pattern) 00458 self.action_result.result.goal_position.effort = struct.unpack(pattern, str[start:end]) 00459 _x = self 00460 start = end 00461 end += 12 00462 (_x.action_feedback.header.seq, _x.action_feedback.header.stamp.secs, _x.action_feedback.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end]) 00463 start = end 00464 end += 4 00465 (length,) = _struct_I.unpack(str[start:end]) 00466 start = end 00467 end += length 00468 self.action_feedback.header.frame_id = str[start:end] 00469 _x = self 00470 start = end 00471 end += 8 00472 (_x.action_feedback.status.goal_id.stamp.secs, _x.action_feedback.status.goal_id.stamp.nsecs,) = _struct_2I.unpack(str[start:end]) 00473 start = end 00474 end += 4 00475 (length,) = _struct_I.unpack(str[start:end]) 00476 start = end 00477 end += length 00478 self.action_feedback.status.goal_id.id = str[start:end] 00479 start = end 00480 end += 1 00481 (self.action_feedback.status.status,) = _struct_B.unpack(str[start:end]) 00482 start = end 00483 end += 4 00484 (length,) = _struct_I.unpack(str[start:end]) 00485 start = end 00486 end += length 00487 self.action_feedback.status.text = str[start:end] 00488 return self 00489 except struct.error as e: 00490 raise roslib.message.DeserializationError(e) #most likely buffer underfill 00491 00492 00493 def serialize_numpy(self, buff, numpy): 00494 """ 00495 serialize message with numpy array types into buffer 00496 @param buff: buffer 00497 @type buff: StringIO 00498 @param numpy: numpy python module 00499 @type numpy module 00500 """ 00501 try: 00502 _x = self 00503 buff.write(_struct_3I.pack(_x.action_goal.header.seq, _x.action_goal.header.stamp.secs, _x.action_goal.header.stamp.nsecs)) 00504 _x = self.action_goal.header.frame_id 00505 length = len(_x) 00506 buff.write(struct.pack('<I%ss'%length, length, _x)) 00507 _x = self 00508 buff.write(_struct_2I.pack(_x.action_goal.goal_id.stamp.secs, _x.action_goal.goal_id.stamp.nsecs)) 00509 _x = self.action_goal.goal_id.id 00510 length = len(_x) 00511 buff.write(struct.pack('<I%ss'%length, length, _x)) 00512 _x = self 00513 buff.write(_struct_3I.pack(_x.action_goal.goal.trajectory.header.seq, _x.action_goal.goal.trajectory.header.stamp.secs, _x.action_goal.goal.trajectory.header.stamp.nsecs)) 00514 _x = self.action_goal.goal.trajectory.header.frame_id 00515 length = len(_x) 00516 buff.write(struct.pack('<I%ss'%length, length, _x)) 00517 length = len(self.action_goal.goal.trajectory.joint_names) 00518 buff.write(_struct_I.pack(length)) 00519 for val1 in self.action_goal.goal.trajectory.joint_names: 00520 length = len(val1) 00521 buff.write(struct.pack('<I%ss'%length, length, val1)) 00522 length = len(self.action_goal.goal.trajectory.points) 00523 buff.write(_struct_I.pack(length)) 00524 for val1 in self.action_goal.goal.trajectory.points: 00525 length = len(val1.positions) 00526 buff.write(_struct_I.pack(length)) 00527 pattern = '<%sd'%length 00528 buff.write(val1.positions.tostring()) 00529 length = len(val1.velocities) 00530 buff.write(_struct_I.pack(length)) 00531 pattern = '<%sd'%length 00532 buff.write(val1.velocities.tostring()) 00533 length = len(val1.accelerations) 00534 buff.write(_struct_I.pack(length)) 00535 pattern = '<%sd'%length 00536 buff.write(val1.accelerations.tostring()) 00537 _v3 = val1.time_from_start 00538 _x = _v3 00539 buff.write(_struct_2i.pack(_x.secs, _x.nsecs)) 00540 _x = self 00541 buff.write(_struct_B3I.pack(_x.action_goal.goal.relative, _x.action_result.header.seq, _x.action_result.header.stamp.secs, _x.action_result.header.stamp.nsecs)) 00542 _x = self.action_result.header.frame_id 00543 length = len(_x) 00544 buff.write(struct.pack('<I%ss'%length, length, _x)) 00545 _x = self 00546 buff.write(_struct_2I.pack(_x.action_result.status.goal_id.stamp.secs, _x.action_result.status.goal_id.stamp.nsecs)) 00547 _x = self.action_result.status.goal_id.id 00548 length = len(_x) 00549 buff.write(struct.pack('<I%ss'%length, length, _x)) 00550 buff.write(_struct_B.pack(self.action_result.status.status)) 00551 _x = self.action_result.status.text 00552 length = len(_x) 00553 buff.write(struct.pack('<I%ss'%length, length, _x)) 00554 _x = self 00555 buff.write(_struct_3I.pack(_x.action_result.result.goal_position.header.seq, _x.action_result.result.goal_position.header.stamp.secs, _x.action_result.result.goal_position.header.stamp.nsecs)) 00556 _x = self.action_result.result.goal_position.header.frame_id 00557 length = len(_x) 00558 buff.write(struct.pack('<I%ss'%length, length, _x)) 00559 length = len(self.action_result.result.goal_position.name) 00560 buff.write(_struct_I.pack(length)) 00561 for val1 in self.action_result.result.goal_position.name: 00562 length = len(val1) 00563 buff.write(struct.pack('<I%ss'%length, length, val1)) 00564 length = len(self.action_result.result.goal_position.position) 00565 buff.write(_struct_I.pack(length)) 00566 pattern = '<%sd'%length 00567 buff.write(self.action_result.result.goal_position.position.tostring()) 00568 length = len(self.action_result.result.goal_position.velocity) 00569 buff.write(_struct_I.pack(length)) 00570 pattern = '<%sd'%length 00571 buff.write(self.action_result.result.goal_position.velocity.tostring()) 00572 length = len(self.action_result.result.goal_position.effort) 00573 buff.write(_struct_I.pack(length)) 00574 pattern = '<%sd'%length 00575 buff.write(self.action_result.result.goal_position.effort.tostring()) 00576 _x = self 00577 buff.write(_struct_3I.pack(_x.action_feedback.header.seq, _x.action_feedback.header.stamp.secs, _x.action_feedback.header.stamp.nsecs)) 00578 _x = self.action_feedback.header.frame_id 00579 length = len(_x) 00580 buff.write(struct.pack('<I%ss'%length, length, _x)) 00581 _x = self 00582 buff.write(_struct_2I.pack(_x.action_feedback.status.goal_id.stamp.secs, _x.action_feedback.status.goal_id.stamp.nsecs)) 00583 _x = self.action_feedback.status.goal_id.id 00584 length = len(_x) 00585 buff.write(struct.pack('<I%ss'%length, length, _x)) 00586 buff.write(_struct_B.pack(self.action_feedback.status.status)) 00587 _x = self.action_feedback.status.text 00588 length = len(_x) 00589 buff.write(struct.pack('<I%ss'%length, length, _x)) 00590 except struct.error as se: self._check_types(se) 00591 except TypeError as te: self._check_types(te) 00592 00593 def deserialize_numpy(self, str, numpy): 00594 """ 00595 unpack serialized message in str into this message instance using numpy for array types 00596 @param str: byte array of serialized message 00597 @type str: str 00598 @param numpy: numpy python module 00599 @type numpy: module 00600 """ 00601 try: 00602 if self.action_goal is None: 00603 self.action_goal = nao_msgs.msg.JointTrajectoryActionGoal() 00604 if self.action_result is None: 00605 self.action_result = nao_msgs.msg.JointTrajectoryActionResult() 00606 if self.action_feedback is None: 00607 self.action_feedback = nao_msgs.msg.JointTrajectoryActionFeedback() 00608 end = 0 00609 _x = self 00610 start = end 00611 end += 12 00612 (_x.action_goal.header.seq, _x.action_goal.header.stamp.secs, _x.action_goal.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end]) 00613 start = end 00614 end += 4 00615 (length,) = _struct_I.unpack(str[start:end]) 00616 start = end 00617 end += length 00618 self.action_goal.header.frame_id = str[start:end] 00619 _x = self 00620 start = end 00621 end += 8 00622 (_x.action_goal.goal_id.stamp.secs, _x.action_goal.goal_id.stamp.nsecs,) = _struct_2I.unpack(str[start:end]) 00623 start = end 00624 end += 4 00625 (length,) = _struct_I.unpack(str[start:end]) 00626 start = end 00627 end += length 00628 self.action_goal.goal_id.id = str[start:end] 00629 _x = self 00630 start = end 00631 end += 12 00632 (_x.action_goal.goal.trajectory.header.seq, _x.action_goal.goal.trajectory.header.stamp.secs, _x.action_goal.goal.trajectory.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end]) 00633 start = end 00634 end += 4 00635 (length,) = _struct_I.unpack(str[start:end]) 00636 start = end 00637 end += length 00638 self.action_goal.goal.trajectory.header.frame_id = str[start:end] 00639 start = end 00640 end += 4 00641 (length,) = _struct_I.unpack(str[start:end]) 00642 self.action_goal.goal.trajectory.joint_names = [] 00643 for i in range(0, length): 00644 start = end 00645 end += 4 00646 (length,) = _struct_I.unpack(str[start:end]) 00647 start = end 00648 end += length 00649 val1 = str[start:end] 00650 self.action_goal.goal.trajectory.joint_names.append(val1) 00651 start = end 00652 end += 4 00653 (length,) = _struct_I.unpack(str[start:end]) 00654 self.action_goal.goal.trajectory.points = [] 00655 for i in range(0, length): 00656 val1 = trajectory_msgs.msg.JointTrajectoryPoint() 00657 start = end 00658 end += 4 00659 (length,) = _struct_I.unpack(str[start:end]) 00660 pattern = '<%sd'%length 00661 start = end 00662 end += struct.calcsize(pattern) 00663 val1.positions = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length) 00664 start = end 00665 end += 4 00666 (length,) = _struct_I.unpack(str[start:end]) 00667 pattern = '<%sd'%length 00668 start = end 00669 end += struct.calcsize(pattern) 00670 val1.velocities = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length) 00671 start = end 00672 end += 4 00673 (length,) = _struct_I.unpack(str[start:end]) 00674 pattern = '<%sd'%length 00675 start = end 00676 end += struct.calcsize(pattern) 00677 val1.accelerations = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length) 00678 _v4 = val1.time_from_start 00679 _x = _v4 00680 start = end 00681 end += 8 00682 (_x.secs, _x.nsecs,) = _struct_2i.unpack(str[start:end]) 00683 self.action_goal.goal.trajectory.points.append(val1) 00684 _x = self 00685 start = end 00686 end += 13 00687 (_x.action_goal.goal.relative, _x.action_result.header.seq, _x.action_result.header.stamp.secs, _x.action_result.header.stamp.nsecs,) = _struct_B3I.unpack(str[start:end]) 00688 start = end 00689 end += 4 00690 (length,) = _struct_I.unpack(str[start:end]) 00691 start = end 00692 end += length 00693 self.action_result.header.frame_id = str[start:end] 00694 _x = self 00695 start = end 00696 end += 8 00697 (_x.action_result.status.goal_id.stamp.secs, _x.action_result.status.goal_id.stamp.nsecs,) = _struct_2I.unpack(str[start:end]) 00698 start = end 00699 end += 4 00700 (length,) = _struct_I.unpack(str[start:end]) 00701 start = end 00702 end += length 00703 self.action_result.status.goal_id.id = str[start:end] 00704 start = end 00705 end += 1 00706 (self.action_result.status.status,) = _struct_B.unpack(str[start:end]) 00707 start = end 00708 end += 4 00709 (length,) = _struct_I.unpack(str[start:end]) 00710 start = end 00711 end += length 00712 self.action_result.status.text = str[start:end] 00713 _x = self 00714 start = end 00715 end += 12 00716 (_x.action_result.result.goal_position.header.seq, _x.action_result.result.goal_position.header.stamp.secs, _x.action_result.result.goal_position.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end]) 00717 start = end 00718 end += 4 00719 (length,) = _struct_I.unpack(str[start:end]) 00720 start = end 00721 end += length 00722 self.action_result.result.goal_position.header.frame_id = str[start:end] 00723 start = end 00724 end += 4 00725 (length,) = _struct_I.unpack(str[start:end]) 00726 self.action_result.result.goal_position.name = [] 00727 for i in range(0, length): 00728 start = end 00729 end += 4 00730 (length,) = _struct_I.unpack(str[start:end]) 00731 start = end 00732 end += length 00733 val1 = str[start:end] 00734 self.action_result.result.goal_position.name.append(val1) 00735 start = end 00736 end += 4 00737 (length,) = _struct_I.unpack(str[start:end]) 00738 pattern = '<%sd'%length 00739 start = end 00740 end += struct.calcsize(pattern) 00741 self.action_result.result.goal_position.position = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length) 00742 start = end 00743 end += 4 00744 (length,) = _struct_I.unpack(str[start:end]) 00745 pattern = '<%sd'%length 00746 start = end 00747 end += struct.calcsize(pattern) 00748 self.action_result.result.goal_position.velocity = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length) 00749 start = end 00750 end += 4 00751 (length,) = _struct_I.unpack(str[start:end]) 00752 pattern = '<%sd'%length 00753 start = end 00754 end += struct.calcsize(pattern) 00755 self.action_result.result.goal_position.effort = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length) 00756 _x = self 00757 start = end 00758 end += 12 00759 (_x.action_feedback.header.seq, _x.action_feedback.header.stamp.secs, _x.action_feedback.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end]) 00760 start = end 00761 end += 4 00762 (length,) = _struct_I.unpack(str[start:end]) 00763 start = end 00764 end += length 00765 self.action_feedback.header.frame_id = str[start:end] 00766 _x = self 00767 start = end 00768 end += 8 00769 (_x.action_feedback.status.goal_id.stamp.secs, _x.action_feedback.status.goal_id.stamp.nsecs,) = _struct_2I.unpack(str[start:end]) 00770 start = end 00771 end += 4 00772 (length,) = _struct_I.unpack(str[start:end]) 00773 start = end 00774 end += length 00775 self.action_feedback.status.goal_id.id = str[start:end] 00776 start = end 00777 end += 1 00778 (self.action_feedback.status.status,) = _struct_B.unpack(str[start:end]) 00779 start = end 00780 end += 4 00781 (length,) = _struct_I.unpack(str[start:end]) 00782 start = end 00783 end += length 00784 self.action_feedback.status.text = str[start:end] 00785 return self 00786 except struct.error as e: 00787 raise roslib.message.DeserializationError(e) #most likely buffer underfill 00788 00789 _struct_I = roslib.message.struct_I 00790 _struct_3I = struct.Struct("<3I") 00791 _struct_B3I = struct.Struct("<B3I") 00792 _struct_2I = struct.Struct("<2I") 00793 _struct_B = struct.Struct("<B") 00794 _struct_2i = struct.Struct("<2i")