00001 """autogenerated by genmsg_py from FilterJointTrajectoryRequest.msg. Do not edit."""
00002 import roslib.message
00003 import struct
00004
00005 import trajectory_msgs.msg
00006 import motion_planning_msgs.msg
00007 import roslib.rostime
00008 import std_msgs.msg
00009
00010 class FilterJointTrajectoryRequest(roslib.message.Message):
00011 _md5sum = "e9d059896dc21ec7575231a5b0553f2c"
00012 _type = "motion_planning_msgs/FilterJointTrajectoryRequest"
00013 _has_header = False
00014 _full_text = """
00015 trajectory_msgs/JointTrajectory trajectory
00016
00017
00018 motion_planning_msgs/JointLimits[] limits
00019
00020 duration allowed_time
00021
00022 ================================================================================
00023 MSG: trajectory_msgs/JointTrajectory
00024 Header header
00025 string[] joint_names
00026 JointTrajectoryPoint[] points
00027 ================================================================================
00028 MSG: std_msgs/Header
00029 # Standard metadata for higher-level stamped data types.
00030 # This is generally used to communicate timestamped data
00031 # in a particular coordinate frame.
00032 #
00033 # sequence ID: consecutively increasing ID
00034 uint32 seq
00035 #Two-integer timestamp that is expressed as:
00036 # * stamp.secs: seconds (stamp_secs) since epoch
00037 # * stamp.nsecs: nanoseconds since stamp_secs
00038 # time-handling sugar is provided by the client library
00039 time stamp
00040 #Frame this data is associated with
00041 # 0: no frame
00042 # 1: global frame
00043 string frame_id
00044
00045 ================================================================================
00046 MSG: trajectory_msgs/JointTrajectoryPoint
00047 float64[] positions
00048 float64[] velocities
00049 float64[] accelerations
00050 duration time_from_start
00051 ================================================================================
00052 MSG: motion_planning_msgs/JointLimits
00053 # This message contains information about limits of a particular joint (or control dimension)
00054 string joint_name
00055
00056 # true if the joint has position limits
00057 uint8 has_position_limits
00058
00059 # min and max position limits
00060 float64 min_position
00061 float64 max_position
00062
00063 # true if joint has velocity limits
00064 uint8 has_velocity_limits
00065
00066 # max velocity limit
00067 float64 max_velocity
00068 # min_velocity is assumed to be -max_velocity
00069
00070 # true if joint has acceleration limits
00071 uint8 has_acceleration_limits
00072 # max acceleration limit
00073 float64 max_acceleration
00074 # min_acceleration is assumed to be -max_acceleration
00075
00076 """
00077 __slots__ = ['trajectory','limits','allowed_time']
00078 _slot_types = ['trajectory_msgs/JointTrajectory','motion_planning_msgs/JointLimits[]','duration']
00079
00080 def __init__(self, *args, **kwds):
00081 """
00082 Constructor. Any message fields that are implicitly/explicitly
00083 set to None will be assigned a default value. The recommend
00084 use is keyword arguments as this is more robust to future message
00085 changes. You cannot mix in-order arguments and keyword arguments.
00086
00087 The available fields are:
00088 trajectory,limits,allowed_time
00089
00090 @param args: complete set of field values, in .msg order
00091 @param kwds: use keyword arguments corresponding to message field names
00092 to set specific fields.
00093 """
00094 if args or kwds:
00095 super(FilterJointTrajectoryRequest, self).__init__(*args, **kwds)
00096
00097 if self.trajectory is None:
00098 self.trajectory = trajectory_msgs.msg.JointTrajectory()
00099 if self.limits is None:
00100 self.limits = []
00101 if self.allowed_time is None:
00102 self.allowed_time = roslib.rostime.Duration()
00103 else:
00104 self.trajectory = trajectory_msgs.msg.JointTrajectory()
00105 self.limits = []
00106 self.allowed_time = roslib.rostime.Duration()
00107
00108 def _get_types(self):
00109 """
00110 internal API method
00111 """
00112 return self._slot_types
00113
00114 def serialize(self, buff):
00115 """
00116 serialize message into buffer
00117 @param buff: buffer
00118 @type buff: StringIO
00119 """
00120 try:
00121 _x = self
00122 buff.write(_struct_3I.pack(_x.trajectory.header.seq, _x.trajectory.header.stamp.secs, _x.trajectory.header.stamp.nsecs))
00123 _x = self.trajectory.header.frame_id
00124 length = len(_x)
00125 buff.write(struct.pack('<I%ss'%length, length, _x))
00126 length = len(self.trajectory.joint_names)
00127 buff.write(_struct_I.pack(length))
00128 for val1 in self.trajectory.joint_names:
00129 length = len(val1)
00130 buff.write(struct.pack('<I%ss'%length, length, val1))
00131 length = len(self.trajectory.points)
00132 buff.write(_struct_I.pack(length))
00133 for val1 in self.trajectory.points:
00134 length = len(val1.positions)
00135 buff.write(_struct_I.pack(length))
00136 pattern = '<%sd'%length
00137 buff.write(struct.pack(pattern, *val1.positions))
00138 length = len(val1.velocities)
00139 buff.write(_struct_I.pack(length))
00140 pattern = '<%sd'%length
00141 buff.write(struct.pack(pattern, *val1.velocities))
00142 length = len(val1.accelerations)
00143 buff.write(_struct_I.pack(length))
00144 pattern = '<%sd'%length
00145 buff.write(struct.pack(pattern, *val1.accelerations))
00146 _v1 = val1.time_from_start
00147 _x = _v1
00148 buff.write(_struct_2i.pack(_x.secs, _x.nsecs))
00149 length = len(self.limits)
00150 buff.write(_struct_I.pack(length))
00151 for val1 in self.limits:
00152 _x = val1.joint_name
00153 length = len(_x)
00154 buff.write(struct.pack('<I%ss'%length, length, _x))
00155 _x = val1
00156 buff.write(_struct_B2dBdBd.pack(_x.has_position_limits, _x.min_position, _x.max_position, _x.has_velocity_limits, _x.max_velocity, _x.has_acceleration_limits, _x.max_acceleration))
00157 _x = self
00158 buff.write(_struct_2i.pack(_x.allowed_time.secs, _x.allowed_time.nsecs))
00159 except struct.error, se: self._check_types(se)
00160 except TypeError, te: self._check_types(te)
00161
00162 def deserialize(self, str):
00163 """
00164 unpack serialized message in str into this message instance
00165 @param str: byte array of serialized message
00166 @type str: str
00167 """
00168 try:
00169 if self.trajectory is None:
00170 self.trajectory = trajectory_msgs.msg.JointTrajectory()
00171 if self.allowed_time is None:
00172 self.allowed_time = roslib.rostime.Duration()
00173 end = 0
00174 _x = self
00175 start = end
00176 end += 12
00177 (_x.trajectory.header.seq, _x.trajectory.header.stamp.secs, _x.trajectory.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
00178 start = end
00179 end += 4
00180 (length,) = _struct_I.unpack(str[start:end])
00181 start = end
00182 end += length
00183 self.trajectory.header.frame_id = str[start:end]
00184 start = end
00185 end += 4
00186 (length,) = _struct_I.unpack(str[start:end])
00187 self.trajectory.joint_names = []
00188 for i in xrange(0, length):
00189 start = end
00190 end += 4
00191 (length,) = _struct_I.unpack(str[start:end])
00192 start = end
00193 end += length
00194 val1 = str[start:end]
00195 self.trajectory.joint_names.append(val1)
00196 start = end
00197 end += 4
00198 (length,) = _struct_I.unpack(str[start:end])
00199 self.trajectory.points = []
00200 for i in xrange(0, length):
00201 val1 = trajectory_msgs.msg.JointTrajectoryPoint()
00202 start = end
00203 end += 4
00204 (length,) = _struct_I.unpack(str[start:end])
00205 pattern = '<%sd'%length
00206 start = end
00207 end += struct.calcsize(pattern)
00208 val1.positions = struct.unpack(pattern, str[start:end])
00209 start = end
00210 end += 4
00211 (length,) = _struct_I.unpack(str[start:end])
00212 pattern = '<%sd'%length
00213 start = end
00214 end += struct.calcsize(pattern)
00215 val1.velocities = struct.unpack(pattern, str[start:end])
00216 start = end
00217 end += 4
00218 (length,) = _struct_I.unpack(str[start:end])
00219 pattern = '<%sd'%length
00220 start = end
00221 end += struct.calcsize(pattern)
00222 val1.accelerations = struct.unpack(pattern, str[start:end])
00223 _v2 = val1.time_from_start
00224 _x = _v2
00225 start = end
00226 end += 8
00227 (_x.secs, _x.nsecs,) = _struct_2i.unpack(str[start:end])
00228 self.trajectory.points.append(val1)
00229 start = end
00230 end += 4
00231 (length,) = _struct_I.unpack(str[start:end])
00232 self.limits = []
00233 for i in xrange(0, length):
00234 val1 = motion_planning_msgs.msg.JointLimits()
00235 start = end
00236 end += 4
00237 (length,) = _struct_I.unpack(str[start:end])
00238 start = end
00239 end += length
00240 val1.joint_name = str[start:end]
00241 _x = val1
00242 start = end
00243 end += 35
00244 (_x.has_position_limits, _x.min_position, _x.max_position, _x.has_velocity_limits, _x.max_velocity, _x.has_acceleration_limits, _x.max_acceleration,) = _struct_B2dBdBd.unpack(str[start:end])
00245 self.limits.append(val1)
00246 _x = self
00247 start = end
00248 end += 8
00249 (_x.allowed_time.secs, _x.allowed_time.nsecs,) = _struct_2i.unpack(str[start:end])
00250 self.allowed_time.canon()
00251 return self
00252 except struct.error, e:
00253 raise roslib.message.DeserializationError(e)
00254
00255
00256 def serialize_numpy(self, buff, numpy):
00257 """
00258 serialize message with numpy array types into buffer
00259 @param buff: buffer
00260 @type buff: StringIO
00261 @param numpy: numpy python module
00262 @type numpy module
00263 """
00264 try:
00265 _x = self
00266 buff.write(_struct_3I.pack(_x.trajectory.header.seq, _x.trajectory.header.stamp.secs, _x.trajectory.header.stamp.nsecs))
00267 _x = self.trajectory.header.frame_id
00268 length = len(_x)
00269 buff.write(struct.pack('<I%ss'%length, length, _x))
00270 length = len(self.trajectory.joint_names)
00271 buff.write(_struct_I.pack(length))
00272 for val1 in self.trajectory.joint_names:
00273 length = len(val1)
00274 buff.write(struct.pack('<I%ss'%length, length, val1))
00275 length = len(self.trajectory.points)
00276 buff.write(_struct_I.pack(length))
00277 for val1 in self.trajectory.points:
00278 length = len(val1.positions)
00279 buff.write(_struct_I.pack(length))
00280 pattern = '<%sd'%length
00281 buff.write(val1.positions.tostring())
00282 length = len(val1.velocities)
00283 buff.write(_struct_I.pack(length))
00284 pattern = '<%sd'%length
00285 buff.write(val1.velocities.tostring())
00286 length = len(val1.accelerations)
00287 buff.write(_struct_I.pack(length))
00288 pattern = '<%sd'%length
00289 buff.write(val1.accelerations.tostring())
00290 _v3 = val1.time_from_start
00291 _x = _v3
00292 buff.write(_struct_2i.pack(_x.secs, _x.nsecs))
00293 length = len(self.limits)
00294 buff.write(_struct_I.pack(length))
00295 for val1 in self.limits:
00296 _x = val1.joint_name
00297 length = len(_x)
00298 buff.write(struct.pack('<I%ss'%length, length, _x))
00299 _x = val1
00300 buff.write(_struct_B2dBdBd.pack(_x.has_position_limits, _x.min_position, _x.max_position, _x.has_velocity_limits, _x.max_velocity, _x.has_acceleration_limits, _x.max_acceleration))
00301 _x = self
00302 buff.write(_struct_2i.pack(_x.allowed_time.secs, _x.allowed_time.nsecs))
00303 except struct.error, se: self._check_types(se)
00304 except TypeError, te: self._check_types(te)
00305
00306 def deserialize_numpy(self, str, numpy):
00307 """
00308 unpack serialized message in str into this message instance using numpy for array types
00309 @param str: byte array of serialized message
00310 @type str: str
00311 @param numpy: numpy python module
00312 @type numpy: module
00313 """
00314 try:
00315 if self.trajectory is None:
00316 self.trajectory = trajectory_msgs.msg.JointTrajectory()
00317 if self.allowed_time is None:
00318 self.allowed_time = roslib.rostime.Duration()
00319 end = 0
00320 _x = self
00321 start = end
00322 end += 12
00323 (_x.trajectory.header.seq, _x.trajectory.header.stamp.secs, _x.trajectory.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
00324 start = end
00325 end += 4
00326 (length,) = _struct_I.unpack(str[start:end])
00327 start = end
00328 end += length
00329 self.trajectory.header.frame_id = str[start:end]
00330 start = end
00331 end += 4
00332 (length,) = _struct_I.unpack(str[start:end])
00333 self.trajectory.joint_names = []
00334 for i in xrange(0, length):
00335 start = end
00336 end += 4
00337 (length,) = _struct_I.unpack(str[start:end])
00338 start = end
00339 end += length
00340 val1 = str[start:end]
00341 self.trajectory.joint_names.append(val1)
00342 start = end
00343 end += 4
00344 (length,) = _struct_I.unpack(str[start:end])
00345 self.trajectory.points = []
00346 for i in xrange(0, length):
00347 val1 = trajectory_msgs.msg.JointTrajectoryPoint()
00348 start = end
00349 end += 4
00350 (length,) = _struct_I.unpack(str[start:end])
00351 pattern = '<%sd'%length
00352 start = end
00353 end += struct.calcsize(pattern)
00354 val1.positions = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
00355 start = end
00356 end += 4
00357 (length,) = _struct_I.unpack(str[start:end])
00358 pattern = '<%sd'%length
00359 start = end
00360 end += struct.calcsize(pattern)
00361 val1.velocities = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
00362 start = end
00363 end += 4
00364 (length,) = _struct_I.unpack(str[start:end])
00365 pattern = '<%sd'%length
00366 start = end
00367 end += struct.calcsize(pattern)
00368 val1.accelerations = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
00369 _v4 = val1.time_from_start
00370 _x = _v4
00371 start = end
00372 end += 8
00373 (_x.secs, _x.nsecs,) = _struct_2i.unpack(str[start:end])
00374 self.trajectory.points.append(val1)
00375 start = end
00376 end += 4
00377 (length,) = _struct_I.unpack(str[start:end])
00378 self.limits = []
00379 for i in xrange(0, length):
00380 val1 = motion_planning_msgs.msg.JointLimits()
00381 start = end
00382 end += 4
00383 (length,) = _struct_I.unpack(str[start:end])
00384 start = end
00385 end += length
00386 val1.joint_name = str[start:end]
00387 _x = val1
00388 start = end
00389 end += 35
00390 (_x.has_position_limits, _x.min_position, _x.max_position, _x.has_velocity_limits, _x.max_velocity, _x.has_acceleration_limits, _x.max_acceleration,) = _struct_B2dBdBd.unpack(str[start:end])
00391 self.limits.append(val1)
00392 _x = self
00393 start = end
00394 end += 8
00395 (_x.allowed_time.secs, _x.allowed_time.nsecs,) = _struct_2i.unpack(str[start:end])
00396 self.allowed_time.canon()
00397 return self
00398 except struct.error, e:
00399 raise roslib.message.DeserializationError(e)
00400
00401 _struct_I = roslib.message.struct_I
00402 _struct_3I = struct.Struct("<3I")
00403 _struct_2i = struct.Struct("<2i")
00404 _struct_B2dBdBd = struct.Struct("<B2dBdBd")
00405 """autogenerated by genmsg_py from FilterJointTrajectoryResponse.msg. Do not edit."""
00406 import roslib.message
00407 import struct
00408
00409 import trajectory_msgs.msg
00410 import motion_planning_msgs.msg
00411 import roslib.rostime
00412 import std_msgs.msg
00413
00414 class FilterJointTrajectoryResponse(roslib.message.Message):
00415 _md5sum = "5b4da90f4032f9ac3da9abfb05f766cc"
00416 _type = "motion_planning_msgs/FilterJointTrajectoryResponse"
00417 _has_header = False
00418 _full_text = """trajectory_msgs/JointTrajectory trajectory
00419 ArmNavigationErrorCodes error_code
00420
00421 ================================================================================
00422 MSG: trajectory_msgs/JointTrajectory
00423 Header header
00424 string[] joint_names
00425 JointTrajectoryPoint[] points
00426 ================================================================================
00427 MSG: std_msgs/Header
00428 # Standard metadata for higher-level stamped data types.
00429 # This is generally used to communicate timestamped data
00430 # in a particular coordinate frame.
00431 #
00432 # sequence ID: consecutively increasing ID
00433 uint32 seq
00434 #Two-integer timestamp that is expressed as:
00435 # * stamp.secs: seconds (stamp_secs) since epoch
00436 # * stamp.nsecs: nanoseconds since stamp_secs
00437 # time-handling sugar is provided by the client library
00438 time stamp
00439 #Frame this data is associated with
00440 # 0: no frame
00441 # 1: global frame
00442 string frame_id
00443
00444 ================================================================================
00445 MSG: trajectory_msgs/JointTrajectoryPoint
00446 float64[] positions
00447 float64[] velocities
00448 float64[] accelerations
00449 duration time_from_start
00450 ================================================================================
00451 MSG: motion_planning_msgs/ArmNavigationErrorCodes
00452 int32 val
00453
00454 # overall behavior
00455 int32 PLANNING_FAILED=-1
00456 int32 SUCCESS=1
00457 int32 TIMED_OUT=-2
00458
00459 # start state errors
00460 int32 START_STATE_IN_COLLISION=-3
00461 int32 START_STATE_VIOLATES_PATH_CONSTRAINTS=-4
00462
00463 # goal errors
00464 int32 GOAL_IN_COLLISION=-5
00465 int32 GOAL_VIOLATES_PATH_CONSTRAINTS=-6
00466
00467 # robot state
00468 int32 INVALID_ROBOT_STATE=-7
00469 int32 INCOMPLETE_ROBOT_STATE=-8
00470
00471 # planning request errors
00472 int32 INVALID_PLANNER_ID=-9
00473 int32 INVALID_NUM_PLANNING_ATTEMPTS=-10
00474 int32 INVALID_ALLOWED_PLANNING_TIME=-11
00475 int32 INVALID_GROUP_NAME=-12
00476 int32 INVALID_GOAL_JOINT_CONSTRAINTS=-13
00477 int32 INVALID_GOAL_POSITION_CONSTRAINTS=-14
00478 int32 INVALID_GOAL_ORIENTATION_CONSTRAINTS=-15
00479 int32 INVALID_PATH_JOINT_CONSTRAINTS=-16
00480 int32 INVALID_PATH_POSITION_CONSTRAINTS=-17
00481 int32 INVALID_PATH_ORIENTATION_CONSTRAINTS=-18
00482
00483 # state/trajectory monitor errors
00484 int32 INVALID_TRAJECTORY=-19
00485 int32 INVALID_INDEX=-20
00486 int32 JOINT_LIMITS_VIOLATED=-21
00487 int32 PATH_CONSTRAINTS_VIOLATED=-22
00488 int32 COLLISION_CONSTRAINTS_VIOLATED=-23
00489 int32 GOAL_CONSTRAINTS_VIOLATED=-24
00490 int32 JOINTS_NOT_MOVING=-25
00491 int32 TRAJECTORY_CONTROLLER_FAILED=-26
00492
00493 # system errors
00494 int32 FRAME_TRANSFORM_FAILURE=-27
00495 int32 COLLISION_CHECKING_UNAVAILABLE=-28
00496 int32 ROBOT_STATE_STALE=-29
00497 int32 SENSOR_INFO_STALE=-30
00498
00499 # kinematics errors
00500 int32 NO_IK_SOLUTION=-31
00501 int32 INVALID_LINK_NAME=-32
00502 int32 IK_LINK_IN_COLLISION=-33
00503 int32 NO_FK_SOLUTION=-34
00504 int32 KINEMATICS_STATE_IN_COLLISION=-35
00505
00506 # general errors
00507 int32 INVALID_TIMEOUT=-36
00508
00509
00510 """
00511 __slots__ = ['trajectory','error_code']
00512 _slot_types = ['trajectory_msgs/JointTrajectory','motion_planning_msgs/ArmNavigationErrorCodes']
00513
00514 def __init__(self, *args, **kwds):
00515 """
00516 Constructor. Any message fields that are implicitly/explicitly
00517 set to None will be assigned a default value. The recommend
00518 use is keyword arguments as this is more robust to future message
00519 changes. You cannot mix in-order arguments and keyword arguments.
00520
00521 The available fields are:
00522 trajectory,error_code
00523
00524 @param args: complete set of field values, in .msg order
00525 @param kwds: use keyword arguments corresponding to message field names
00526 to set specific fields.
00527 """
00528 if args or kwds:
00529 super(FilterJointTrajectoryResponse, self).__init__(*args, **kwds)
00530
00531 if self.trajectory is None:
00532 self.trajectory = trajectory_msgs.msg.JointTrajectory()
00533 if self.error_code is None:
00534 self.error_code = motion_planning_msgs.msg.ArmNavigationErrorCodes()
00535 else:
00536 self.trajectory = trajectory_msgs.msg.JointTrajectory()
00537 self.error_code = motion_planning_msgs.msg.ArmNavigationErrorCodes()
00538
00539 def _get_types(self):
00540 """
00541 internal API method
00542 """
00543 return self._slot_types
00544
00545 def serialize(self, buff):
00546 """
00547 serialize message into buffer
00548 @param buff: buffer
00549 @type buff: StringIO
00550 """
00551 try:
00552 _x = self
00553 buff.write(_struct_3I.pack(_x.trajectory.header.seq, _x.trajectory.header.stamp.secs, _x.trajectory.header.stamp.nsecs))
00554 _x = self.trajectory.header.frame_id
00555 length = len(_x)
00556 buff.write(struct.pack('<I%ss'%length, length, _x))
00557 length = len(self.trajectory.joint_names)
00558 buff.write(_struct_I.pack(length))
00559 for val1 in self.trajectory.joint_names:
00560 length = len(val1)
00561 buff.write(struct.pack('<I%ss'%length, length, val1))
00562 length = len(self.trajectory.points)
00563 buff.write(_struct_I.pack(length))
00564 for val1 in self.trajectory.points:
00565 length = len(val1.positions)
00566 buff.write(_struct_I.pack(length))
00567 pattern = '<%sd'%length
00568 buff.write(struct.pack(pattern, *val1.positions))
00569 length = len(val1.velocities)
00570 buff.write(_struct_I.pack(length))
00571 pattern = '<%sd'%length
00572 buff.write(struct.pack(pattern, *val1.velocities))
00573 length = len(val1.accelerations)
00574 buff.write(_struct_I.pack(length))
00575 pattern = '<%sd'%length
00576 buff.write(struct.pack(pattern, *val1.accelerations))
00577 _v5 = val1.time_from_start
00578 _x = _v5
00579 buff.write(_struct_2i.pack(_x.secs, _x.nsecs))
00580 buff.write(_struct_i.pack(self.error_code.val))
00581 except struct.error, se: self._check_types(se)
00582 except TypeError, te: self._check_types(te)
00583
00584 def deserialize(self, str):
00585 """
00586 unpack serialized message in str into this message instance
00587 @param str: byte array of serialized message
00588 @type str: str
00589 """
00590 try:
00591 if self.trajectory is None:
00592 self.trajectory = trajectory_msgs.msg.JointTrajectory()
00593 if self.error_code is None:
00594 self.error_code = motion_planning_msgs.msg.ArmNavigationErrorCodes()
00595 end = 0
00596 _x = self
00597 start = end
00598 end += 12
00599 (_x.trajectory.header.seq, _x.trajectory.header.stamp.secs, _x.trajectory.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
00600 start = end
00601 end += 4
00602 (length,) = _struct_I.unpack(str[start:end])
00603 start = end
00604 end += length
00605 self.trajectory.header.frame_id = str[start:end]
00606 start = end
00607 end += 4
00608 (length,) = _struct_I.unpack(str[start:end])
00609 self.trajectory.joint_names = []
00610 for i in xrange(0, length):
00611 start = end
00612 end += 4
00613 (length,) = _struct_I.unpack(str[start:end])
00614 start = end
00615 end += length
00616 val1 = str[start:end]
00617 self.trajectory.joint_names.append(val1)
00618 start = end
00619 end += 4
00620 (length,) = _struct_I.unpack(str[start:end])
00621 self.trajectory.points = []
00622 for i in xrange(0, length):
00623 val1 = trajectory_msgs.msg.JointTrajectoryPoint()
00624 start = end
00625 end += 4
00626 (length,) = _struct_I.unpack(str[start:end])
00627 pattern = '<%sd'%length
00628 start = end
00629 end += struct.calcsize(pattern)
00630 val1.positions = struct.unpack(pattern, str[start:end])
00631 start = end
00632 end += 4
00633 (length,) = _struct_I.unpack(str[start:end])
00634 pattern = '<%sd'%length
00635 start = end
00636 end += struct.calcsize(pattern)
00637 val1.velocities = struct.unpack(pattern, str[start:end])
00638 start = end
00639 end += 4
00640 (length,) = _struct_I.unpack(str[start:end])
00641 pattern = '<%sd'%length
00642 start = end
00643 end += struct.calcsize(pattern)
00644 val1.accelerations = struct.unpack(pattern, str[start:end])
00645 _v6 = val1.time_from_start
00646 _x = _v6
00647 start = end
00648 end += 8
00649 (_x.secs, _x.nsecs,) = _struct_2i.unpack(str[start:end])
00650 self.trajectory.points.append(val1)
00651 start = end
00652 end += 4
00653 (self.error_code.val,) = _struct_i.unpack(str[start:end])
00654 return self
00655 except struct.error, e:
00656 raise roslib.message.DeserializationError(e)
00657
00658
00659 def serialize_numpy(self, buff, numpy):
00660 """
00661 serialize message with numpy array types into buffer
00662 @param buff: buffer
00663 @type buff: StringIO
00664 @param numpy: numpy python module
00665 @type numpy module
00666 """
00667 try:
00668 _x = self
00669 buff.write(_struct_3I.pack(_x.trajectory.header.seq, _x.trajectory.header.stamp.secs, _x.trajectory.header.stamp.nsecs))
00670 _x = self.trajectory.header.frame_id
00671 length = len(_x)
00672 buff.write(struct.pack('<I%ss'%length, length, _x))
00673 length = len(self.trajectory.joint_names)
00674 buff.write(_struct_I.pack(length))
00675 for val1 in self.trajectory.joint_names:
00676 length = len(val1)
00677 buff.write(struct.pack('<I%ss'%length, length, val1))
00678 length = len(self.trajectory.points)
00679 buff.write(_struct_I.pack(length))
00680 for val1 in self.trajectory.points:
00681 length = len(val1.positions)
00682 buff.write(_struct_I.pack(length))
00683 pattern = '<%sd'%length
00684 buff.write(val1.positions.tostring())
00685 length = len(val1.velocities)
00686 buff.write(_struct_I.pack(length))
00687 pattern = '<%sd'%length
00688 buff.write(val1.velocities.tostring())
00689 length = len(val1.accelerations)
00690 buff.write(_struct_I.pack(length))
00691 pattern = '<%sd'%length
00692 buff.write(val1.accelerations.tostring())
00693 _v7 = val1.time_from_start
00694 _x = _v7
00695 buff.write(_struct_2i.pack(_x.secs, _x.nsecs))
00696 buff.write(_struct_i.pack(self.error_code.val))
00697 except struct.error, se: self._check_types(se)
00698 except TypeError, te: self._check_types(te)
00699
00700 def deserialize_numpy(self, str, numpy):
00701 """
00702 unpack serialized message in str into this message instance using numpy for array types
00703 @param str: byte array of serialized message
00704 @type str: str
00705 @param numpy: numpy python module
00706 @type numpy: module
00707 """
00708 try:
00709 if self.trajectory is None:
00710 self.trajectory = trajectory_msgs.msg.JointTrajectory()
00711 if self.error_code is None:
00712 self.error_code = motion_planning_msgs.msg.ArmNavigationErrorCodes()
00713 end = 0
00714 _x = self
00715 start = end
00716 end += 12
00717 (_x.trajectory.header.seq, _x.trajectory.header.stamp.secs, _x.trajectory.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
00718 start = end
00719 end += 4
00720 (length,) = _struct_I.unpack(str[start:end])
00721 start = end
00722 end += length
00723 self.trajectory.header.frame_id = str[start:end]
00724 start = end
00725 end += 4
00726 (length,) = _struct_I.unpack(str[start:end])
00727 self.trajectory.joint_names = []
00728 for i in xrange(0, length):
00729 start = end
00730 end += 4
00731 (length,) = _struct_I.unpack(str[start:end])
00732 start = end
00733 end += length
00734 val1 = str[start:end]
00735 self.trajectory.joint_names.append(val1)
00736 start = end
00737 end += 4
00738 (length,) = _struct_I.unpack(str[start:end])
00739 self.trajectory.points = []
00740 for i in xrange(0, length):
00741 val1 = trajectory_msgs.msg.JointTrajectoryPoint()
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 val1.positions = 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 val1.velocities = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
00756 start = end
00757 end += 4
00758 (length,) = _struct_I.unpack(str[start:end])
00759 pattern = '<%sd'%length
00760 start = end
00761 end += struct.calcsize(pattern)
00762 val1.accelerations = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
00763 _v8 = val1.time_from_start
00764 _x = _v8
00765 start = end
00766 end += 8
00767 (_x.secs, _x.nsecs,) = _struct_2i.unpack(str[start:end])
00768 self.trajectory.points.append(val1)
00769 start = end
00770 end += 4
00771 (self.error_code.val,) = _struct_i.unpack(str[start:end])
00772 return self
00773 except struct.error, e:
00774 raise roslib.message.DeserializationError(e)
00775
00776 _struct_I = roslib.message.struct_I
00777 _struct_i = struct.Struct("<i")
00778 _struct_3I = struct.Struct("<3I")
00779 _struct_2i = struct.Struct("<2i")
00780 class FilterJointTrajectory(roslib.message.ServiceDefinition):
00781 _type = 'motion_planning_msgs/FilterJointTrajectory'
00782 _md5sum = 'ca9260f17206fb6e1f0667bb6feb1521'
00783 _request_class = FilterJointTrajectoryRequest
00784 _response_class = FilterJointTrajectoryResponse