$search
00001 """autogenerated by genmsg_py from FollowJointTrajectoryActionGoal.msg. Do not edit.""" 00002 import roslib.message 00003 import struct 00004 00005 import trajectory_msgs.msg 00006 import control_msgs.msg 00007 import roslib.rostime 00008 import actionlib_msgs.msg 00009 import std_msgs.msg 00010 00011 class FollowJointTrajectoryActionGoal(roslib.message.Message): 00012 _md5sum = "8f3e00277a7b5b7c60e1ac5be35ddfa2" 00013 _type = "control_msgs/FollowJointTrajectoryActionGoal" 00014 _has_header = True #flag to mark the presence of a Header object 00015 _full_text = """# ====== DO NOT MODIFY! AUTOGENERATED FROM AN ACTION DEFINITION ====== 00016 00017 Header header 00018 actionlib_msgs/GoalID goal_id 00019 FollowJointTrajectoryGoal goal 00020 00021 ================================================================================ 00022 MSG: std_msgs/Header 00023 # Standard metadata for higher-level stamped data types. 00024 # This is generally used to communicate timestamped data 00025 # in a particular coordinate frame. 00026 # 00027 # sequence ID: consecutively increasing ID 00028 uint32 seq 00029 #Two-integer timestamp that is expressed as: 00030 # * stamp.secs: seconds (stamp_secs) since epoch 00031 # * stamp.nsecs: nanoseconds since stamp_secs 00032 # time-handling sugar is provided by the client library 00033 time stamp 00034 #Frame this data is associated with 00035 # 0: no frame 00036 # 1: global frame 00037 string frame_id 00038 00039 ================================================================================ 00040 MSG: actionlib_msgs/GoalID 00041 # The stamp should store the time at which this goal was requested. 00042 # It is used by an action server when it tries to preempt all 00043 # goals that were requested before a certain time 00044 time stamp 00045 00046 # The id provides a way to associate feedback and 00047 # result message with specific goal requests. The id 00048 # specified must be unique. 00049 string id 00050 00051 00052 ================================================================================ 00053 MSG: control_msgs/FollowJointTrajectoryGoal 00054 # ====== DO NOT MODIFY! AUTOGENERATED FROM AN ACTION DEFINITION ====== 00055 # The joint trajectory to follow 00056 trajectory_msgs/JointTrajectory trajectory 00057 00058 # Tolerances for the trajectory. If the measured joint values fall 00059 # outside the tolerances the trajectory goal is aborted. Any 00060 # tolerances that are not specified (by being omitted or set to 0) are 00061 # set to the defaults for the action server (often taken from the 00062 # parameter server). 00063 00064 # Tolerances applied to the joints as the trajectory is executed. If 00065 # violated, the goal aborts with error_code set to 00066 # PATH_TOLERANCE_VIOLATED. 00067 JointTolerance[] path_tolerance 00068 00069 # To report success, the joints must be within goal_tolerance of the 00070 # final trajectory value. The goal must be achieved by time the 00071 # trajectory ends plus goal_time_tolerance. (goal_time_tolerance 00072 # allows some leeway in time, so that the trajectory goal can still 00073 # succeed even if the joints reach the goal some time after the 00074 # precise end time of the trajectory). 00075 # 00076 # If the joints are not within goal_tolerance after "trajectory finish 00077 # time" + goal_time_tolerance, the goal aborts with error_code set to 00078 # GOAL_TOLERANCE_VIOLATED 00079 JointTolerance[] goal_tolerance 00080 duration goal_time_tolerance 00081 00082 00083 ================================================================================ 00084 MSG: trajectory_msgs/JointTrajectory 00085 Header header 00086 string[] joint_names 00087 JointTrajectoryPoint[] points 00088 ================================================================================ 00089 MSG: trajectory_msgs/JointTrajectoryPoint 00090 float64[] positions 00091 float64[] velocities 00092 float64[] accelerations 00093 duration time_from_start 00094 ================================================================================ 00095 MSG: control_msgs/JointTolerance 00096 # The tolerances specify the amount the position, velocity, and 00097 # accelerations can vary from the setpoints. For example, in the case 00098 # of trajectory control, when the actual position varies beyond 00099 # (desired position + position tolerance), the trajectory goal may 00100 # abort. 00101 # 00102 # There are two special values for tolerances: 00103 # * 0 - The tolerance is unspecified and will remain at whatever the default is 00104 # * -1 - The tolerance is "erased". If there was a default, the joint will be 00105 # allowed to move without restriction. 00106 00107 string name 00108 float64 position # in radians or meters (for a revolute or prismatic joint, respectively) 00109 float64 velocity # in rad/sec or m/sec 00110 float64 acceleration # in rad/sec^2 or m/sec^2 00111 00112 """ 00113 __slots__ = ['header','goal_id','goal'] 00114 _slot_types = ['Header','actionlib_msgs/GoalID','control_msgs/FollowJointTrajectoryGoal'] 00115 00116 def __init__(self, *args, **kwds): 00117 """ 00118 Constructor. Any message fields that are implicitly/explicitly 00119 set to None will be assigned a default value. The recommend 00120 use is keyword arguments as this is more robust to future message 00121 changes. You cannot mix in-order arguments and keyword arguments. 00122 00123 The available fields are: 00124 header,goal_id,goal 00125 00126 @param args: complete set of field values, in .msg order 00127 @param kwds: use keyword arguments corresponding to message field names 00128 to set specific fields. 00129 """ 00130 if args or kwds: 00131 super(FollowJointTrajectoryActionGoal, self).__init__(*args, **kwds) 00132 #message fields cannot be None, assign default values for those that are 00133 if self.header is None: 00134 self.header = std_msgs.msg._Header.Header() 00135 if self.goal_id is None: 00136 self.goal_id = actionlib_msgs.msg.GoalID() 00137 if self.goal is None: 00138 self.goal = control_msgs.msg.FollowJointTrajectoryGoal() 00139 else: 00140 self.header = std_msgs.msg._Header.Header() 00141 self.goal_id = actionlib_msgs.msg.GoalID() 00142 self.goal = control_msgs.msg.FollowJointTrajectoryGoal() 00143 00144 def _get_types(self): 00145 """ 00146 internal API method 00147 """ 00148 return self._slot_types 00149 00150 def serialize(self, buff): 00151 """ 00152 serialize message into buffer 00153 @param buff: buffer 00154 @type buff: StringIO 00155 """ 00156 try: 00157 _x = self 00158 buff.write(_struct_3I.pack(_x.header.seq, _x.header.stamp.secs, _x.header.stamp.nsecs)) 00159 _x = self.header.frame_id 00160 length = len(_x) 00161 buff.write(struct.pack('<I%ss'%length, length, _x)) 00162 _x = self 00163 buff.write(_struct_2I.pack(_x.goal_id.stamp.secs, _x.goal_id.stamp.nsecs)) 00164 _x = self.goal_id.id 00165 length = len(_x) 00166 buff.write(struct.pack('<I%ss'%length, length, _x)) 00167 _x = self 00168 buff.write(_struct_3I.pack(_x.goal.trajectory.header.seq, _x.goal.trajectory.header.stamp.secs, _x.goal.trajectory.header.stamp.nsecs)) 00169 _x = self.goal.trajectory.header.frame_id 00170 length = len(_x) 00171 buff.write(struct.pack('<I%ss'%length, length, _x)) 00172 length = len(self.goal.trajectory.joint_names) 00173 buff.write(_struct_I.pack(length)) 00174 for val1 in self.goal.trajectory.joint_names: 00175 length = len(val1) 00176 buff.write(struct.pack('<I%ss'%length, length, val1)) 00177 length = len(self.goal.trajectory.points) 00178 buff.write(_struct_I.pack(length)) 00179 for val1 in self.goal.trajectory.points: 00180 length = len(val1.positions) 00181 buff.write(_struct_I.pack(length)) 00182 pattern = '<%sd'%length 00183 buff.write(struct.pack(pattern, *val1.positions)) 00184 length = len(val1.velocities) 00185 buff.write(_struct_I.pack(length)) 00186 pattern = '<%sd'%length 00187 buff.write(struct.pack(pattern, *val1.velocities)) 00188 length = len(val1.accelerations) 00189 buff.write(_struct_I.pack(length)) 00190 pattern = '<%sd'%length 00191 buff.write(struct.pack(pattern, *val1.accelerations)) 00192 _v1 = val1.time_from_start 00193 _x = _v1 00194 buff.write(_struct_2i.pack(_x.secs, _x.nsecs)) 00195 length = len(self.goal.path_tolerance) 00196 buff.write(_struct_I.pack(length)) 00197 for val1 in self.goal.path_tolerance: 00198 _x = val1.name 00199 length = len(_x) 00200 buff.write(struct.pack('<I%ss'%length, length, _x)) 00201 _x = val1 00202 buff.write(_struct_3d.pack(_x.position, _x.velocity, _x.acceleration)) 00203 length = len(self.goal.goal_tolerance) 00204 buff.write(_struct_I.pack(length)) 00205 for val1 in self.goal.goal_tolerance: 00206 _x = val1.name 00207 length = len(_x) 00208 buff.write(struct.pack('<I%ss'%length, length, _x)) 00209 _x = val1 00210 buff.write(_struct_3d.pack(_x.position, _x.velocity, _x.acceleration)) 00211 _x = self 00212 buff.write(_struct_2i.pack(_x.goal.goal_time_tolerance.secs, _x.goal.goal_time_tolerance.nsecs)) 00213 except struct.error as se: self._check_types(se) 00214 except TypeError as te: self._check_types(te) 00215 00216 def deserialize(self, str): 00217 """ 00218 unpack serialized message in str into this message instance 00219 @param str: byte array of serialized message 00220 @type str: str 00221 """ 00222 try: 00223 if self.header is None: 00224 self.header = std_msgs.msg._Header.Header() 00225 if self.goal_id is None: 00226 self.goal_id = actionlib_msgs.msg.GoalID() 00227 if self.goal is None: 00228 self.goal = control_msgs.msg.FollowJointTrajectoryGoal() 00229 end = 0 00230 _x = self 00231 start = end 00232 end += 12 00233 (_x.header.seq, _x.header.stamp.secs, _x.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end]) 00234 start = end 00235 end += 4 00236 (length,) = _struct_I.unpack(str[start:end]) 00237 start = end 00238 end += length 00239 self.header.frame_id = str[start:end] 00240 _x = self 00241 start = end 00242 end += 8 00243 (_x.goal_id.stamp.secs, _x.goal_id.stamp.nsecs,) = _struct_2I.unpack(str[start:end]) 00244 start = end 00245 end += 4 00246 (length,) = _struct_I.unpack(str[start:end]) 00247 start = end 00248 end += length 00249 self.goal_id.id = str[start:end] 00250 _x = self 00251 start = end 00252 end += 12 00253 (_x.goal.trajectory.header.seq, _x.goal.trajectory.header.stamp.secs, _x.goal.trajectory.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end]) 00254 start = end 00255 end += 4 00256 (length,) = _struct_I.unpack(str[start:end]) 00257 start = end 00258 end += length 00259 self.goal.trajectory.header.frame_id = str[start:end] 00260 start = end 00261 end += 4 00262 (length,) = _struct_I.unpack(str[start:end]) 00263 self.goal.trajectory.joint_names = [] 00264 for i in range(0, length): 00265 start = end 00266 end += 4 00267 (length,) = _struct_I.unpack(str[start:end]) 00268 start = end 00269 end += length 00270 val1 = str[start:end] 00271 self.goal.trajectory.joint_names.append(val1) 00272 start = end 00273 end += 4 00274 (length,) = _struct_I.unpack(str[start:end]) 00275 self.goal.trajectory.points = [] 00276 for i in range(0, length): 00277 val1 = trajectory_msgs.msg.JointTrajectoryPoint() 00278 start = end 00279 end += 4 00280 (length,) = _struct_I.unpack(str[start:end]) 00281 pattern = '<%sd'%length 00282 start = end 00283 end += struct.calcsize(pattern) 00284 val1.positions = struct.unpack(pattern, str[start:end]) 00285 start = end 00286 end += 4 00287 (length,) = _struct_I.unpack(str[start:end]) 00288 pattern = '<%sd'%length 00289 start = end 00290 end += struct.calcsize(pattern) 00291 val1.velocities = struct.unpack(pattern, str[start:end]) 00292 start = end 00293 end += 4 00294 (length,) = _struct_I.unpack(str[start:end]) 00295 pattern = '<%sd'%length 00296 start = end 00297 end += struct.calcsize(pattern) 00298 val1.accelerations = struct.unpack(pattern, str[start:end]) 00299 _v2 = val1.time_from_start 00300 _x = _v2 00301 start = end 00302 end += 8 00303 (_x.secs, _x.nsecs,) = _struct_2i.unpack(str[start:end]) 00304 self.goal.trajectory.points.append(val1) 00305 start = end 00306 end += 4 00307 (length,) = _struct_I.unpack(str[start:end]) 00308 self.goal.path_tolerance = [] 00309 for i in range(0, length): 00310 val1 = control_msgs.msg.JointTolerance() 00311 start = end 00312 end += 4 00313 (length,) = _struct_I.unpack(str[start:end]) 00314 start = end 00315 end += length 00316 val1.name = str[start:end] 00317 _x = val1 00318 start = end 00319 end += 24 00320 (_x.position, _x.velocity, _x.acceleration,) = _struct_3d.unpack(str[start:end]) 00321 self.goal.path_tolerance.append(val1) 00322 start = end 00323 end += 4 00324 (length,) = _struct_I.unpack(str[start:end]) 00325 self.goal.goal_tolerance = [] 00326 for i in range(0, length): 00327 val1 = control_msgs.msg.JointTolerance() 00328 start = end 00329 end += 4 00330 (length,) = _struct_I.unpack(str[start:end]) 00331 start = end 00332 end += length 00333 val1.name = str[start:end] 00334 _x = val1 00335 start = end 00336 end += 24 00337 (_x.position, _x.velocity, _x.acceleration,) = _struct_3d.unpack(str[start:end]) 00338 self.goal.goal_tolerance.append(val1) 00339 _x = self 00340 start = end 00341 end += 8 00342 (_x.goal.goal_time_tolerance.secs, _x.goal.goal_time_tolerance.nsecs,) = _struct_2i.unpack(str[start:end]) 00343 return self 00344 except struct.error as e: 00345 raise roslib.message.DeserializationError(e) #most likely buffer underfill 00346 00347 00348 def serialize_numpy(self, buff, numpy): 00349 """ 00350 serialize message with numpy array types into buffer 00351 @param buff: buffer 00352 @type buff: StringIO 00353 @param numpy: numpy python module 00354 @type numpy module 00355 """ 00356 try: 00357 _x = self 00358 buff.write(_struct_3I.pack(_x.header.seq, _x.header.stamp.secs, _x.header.stamp.nsecs)) 00359 _x = self.header.frame_id 00360 length = len(_x) 00361 buff.write(struct.pack('<I%ss'%length, length, _x)) 00362 _x = self 00363 buff.write(_struct_2I.pack(_x.goal_id.stamp.secs, _x.goal_id.stamp.nsecs)) 00364 _x = self.goal_id.id 00365 length = len(_x) 00366 buff.write(struct.pack('<I%ss'%length, length, _x)) 00367 _x = self 00368 buff.write(_struct_3I.pack(_x.goal.trajectory.header.seq, _x.goal.trajectory.header.stamp.secs, _x.goal.trajectory.header.stamp.nsecs)) 00369 _x = self.goal.trajectory.header.frame_id 00370 length = len(_x) 00371 buff.write(struct.pack('<I%ss'%length, length, _x)) 00372 length = len(self.goal.trajectory.joint_names) 00373 buff.write(_struct_I.pack(length)) 00374 for val1 in self.goal.trajectory.joint_names: 00375 length = len(val1) 00376 buff.write(struct.pack('<I%ss'%length, length, val1)) 00377 length = len(self.goal.trajectory.points) 00378 buff.write(_struct_I.pack(length)) 00379 for val1 in self.goal.trajectory.points: 00380 length = len(val1.positions) 00381 buff.write(_struct_I.pack(length)) 00382 pattern = '<%sd'%length 00383 buff.write(val1.positions.tostring()) 00384 length = len(val1.velocities) 00385 buff.write(_struct_I.pack(length)) 00386 pattern = '<%sd'%length 00387 buff.write(val1.velocities.tostring()) 00388 length = len(val1.accelerations) 00389 buff.write(_struct_I.pack(length)) 00390 pattern = '<%sd'%length 00391 buff.write(val1.accelerations.tostring()) 00392 _v3 = val1.time_from_start 00393 _x = _v3 00394 buff.write(_struct_2i.pack(_x.secs, _x.nsecs)) 00395 length = len(self.goal.path_tolerance) 00396 buff.write(_struct_I.pack(length)) 00397 for val1 in self.goal.path_tolerance: 00398 _x = val1.name 00399 length = len(_x) 00400 buff.write(struct.pack('<I%ss'%length, length, _x)) 00401 _x = val1 00402 buff.write(_struct_3d.pack(_x.position, _x.velocity, _x.acceleration)) 00403 length = len(self.goal.goal_tolerance) 00404 buff.write(_struct_I.pack(length)) 00405 for val1 in self.goal.goal_tolerance: 00406 _x = val1.name 00407 length = len(_x) 00408 buff.write(struct.pack('<I%ss'%length, length, _x)) 00409 _x = val1 00410 buff.write(_struct_3d.pack(_x.position, _x.velocity, _x.acceleration)) 00411 _x = self 00412 buff.write(_struct_2i.pack(_x.goal.goal_time_tolerance.secs, _x.goal.goal_time_tolerance.nsecs)) 00413 except struct.error as se: self._check_types(se) 00414 except TypeError as te: self._check_types(te) 00415 00416 def deserialize_numpy(self, str, numpy): 00417 """ 00418 unpack serialized message in str into this message instance using numpy for array types 00419 @param str: byte array of serialized message 00420 @type str: str 00421 @param numpy: numpy python module 00422 @type numpy: module 00423 """ 00424 try: 00425 if self.header is None: 00426 self.header = std_msgs.msg._Header.Header() 00427 if self.goal_id is None: 00428 self.goal_id = actionlib_msgs.msg.GoalID() 00429 if self.goal is None: 00430 self.goal = control_msgs.msg.FollowJointTrajectoryGoal() 00431 end = 0 00432 _x = self 00433 start = end 00434 end += 12 00435 (_x.header.seq, _x.header.stamp.secs, _x.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end]) 00436 start = end 00437 end += 4 00438 (length,) = _struct_I.unpack(str[start:end]) 00439 start = end 00440 end += length 00441 self.header.frame_id = str[start:end] 00442 _x = self 00443 start = end 00444 end += 8 00445 (_x.goal_id.stamp.secs, _x.goal_id.stamp.nsecs,) = _struct_2I.unpack(str[start:end]) 00446 start = end 00447 end += 4 00448 (length,) = _struct_I.unpack(str[start:end]) 00449 start = end 00450 end += length 00451 self.goal_id.id = str[start:end] 00452 _x = self 00453 start = end 00454 end += 12 00455 (_x.goal.trajectory.header.seq, _x.goal.trajectory.header.stamp.secs, _x.goal.trajectory.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end]) 00456 start = end 00457 end += 4 00458 (length,) = _struct_I.unpack(str[start:end]) 00459 start = end 00460 end += length 00461 self.goal.trajectory.header.frame_id = str[start:end] 00462 start = end 00463 end += 4 00464 (length,) = _struct_I.unpack(str[start:end]) 00465 self.goal.trajectory.joint_names = [] 00466 for i in range(0, length): 00467 start = end 00468 end += 4 00469 (length,) = _struct_I.unpack(str[start:end]) 00470 start = end 00471 end += length 00472 val1 = str[start:end] 00473 self.goal.trajectory.joint_names.append(val1) 00474 start = end 00475 end += 4 00476 (length,) = _struct_I.unpack(str[start:end]) 00477 self.goal.trajectory.points = [] 00478 for i in range(0, length): 00479 val1 = trajectory_msgs.msg.JointTrajectoryPoint() 00480 start = end 00481 end += 4 00482 (length,) = _struct_I.unpack(str[start:end]) 00483 pattern = '<%sd'%length 00484 start = end 00485 end += struct.calcsize(pattern) 00486 val1.positions = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length) 00487 start = end 00488 end += 4 00489 (length,) = _struct_I.unpack(str[start:end]) 00490 pattern = '<%sd'%length 00491 start = end 00492 end += struct.calcsize(pattern) 00493 val1.velocities = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length) 00494 start = end 00495 end += 4 00496 (length,) = _struct_I.unpack(str[start:end]) 00497 pattern = '<%sd'%length 00498 start = end 00499 end += struct.calcsize(pattern) 00500 val1.accelerations = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length) 00501 _v4 = val1.time_from_start 00502 _x = _v4 00503 start = end 00504 end += 8 00505 (_x.secs, _x.nsecs,) = _struct_2i.unpack(str[start:end]) 00506 self.goal.trajectory.points.append(val1) 00507 start = end 00508 end += 4 00509 (length,) = _struct_I.unpack(str[start:end]) 00510 self.goal.path_tolerance = [] 00511 for i in range(0, length): 00512 val1 = control_msgs.msg.JointTolerance() 00513 start = end 00514 end += 4 00515 (length,) = _struct_I.unpack(str[start:end]) 00516 start = end 00517 end += length 00518 val1.name = str[start:end] 00519 _x = val1 00520 start = end 00521 end += 24 00522 (_x.position, _x.velocity, _x.acceleration,) = _struct_3d.unpack(str[start:end]) 00523 self.goal.path_tolerance.append(val1) 00524 start = end 00525 end += 4 00526 (length,) = _struct_I.unpack(str[start:end]) 00527 self.goal.goal_tolerance = [] 00528 for i in range(0, length): 00529 val1 = control_msgs.msg.JointTolerance() 00530 start = end 00531 end += 4 00532 (length,) = _struct_I.unpack(str[start:end]) 00533 start = end 00534 end += length 00535 val1.name = str[start:end] 00536 _x = val1 00537 start = end 00538 end += 24 00539 (_x.position, _x.velocity, _x.acceleration,) = _struct_3d.unpack(str[start:end]) 00540 self.goal.goal_tolerance.append(val1) 00541 _x = self 00542 start = end 00543 end += 8 00544 (_x.goal.goal_time_tolerance.secs, _x.goal.goal_time_tolerance.nsecs,) = _struct_2i.unpack(str[start:end]) 00545 return self 00546 except struct.error as e: 00547 raise roslib.message.DeserializationError(e) #most likely buffer underfill 00548 00549 _struct_I = roslib.message.struct_I 00550 _struct_3I = struct.Struct("<3I") 00551 _struct_3d = struct.Struct("<3d") 00552 _struct_2I = struct.Struct("<2I") 00553 _struct_2i = struct.Struct("<2i")