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
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
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, se: self._check_types(se)
00214 except TypeError, 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 xrange(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 xrange(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 xrange(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 xrange(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, e:
00345 raise roslib.message.DeserializationError(e)
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, se: self._check_types(se)
00414 except TypeError, 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 xrange(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 xrange(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 xrange(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 xrange(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, e:
00547 raise roslib.message.DeserializationError(e)
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")