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