00001 """autogenerated by genmsg_py from GetPositionFKRequest.msg. Do not edit."""
00002 import roslib.message
00003 import struct
00004
00005 import geometry_msgs.msg
00006 import roslib.rostime
00007 import sensor_msgs.msg
00008 import motion_planning_msgs.msg
00009 import std_msgs.msg
00010
00011 class GetPositionFKRequest(roslib.message.Message):
00012 _md5sum = "ddaa8b9932e60599795bcb983e28cf57"
00013 _type = "kinematics_msgs/GetPositionFKRequest"
00014 _has_header = True
00015 _full_text = """
00016
00017
00018 Header header
00019
00020
00021 string[] fk_link_names
00022
00023
00024 motion_planning_msgs/RobotState robot_state
00025
00026 ================================================================================
00027 MSG: std_msgs/Header
00028 # Standard metadata for higher-level stamped data types.
00029 # This is generally used to communicate timestamped data
00030 # in a particular coordinate frame.
00031 #
00032 # sequence ID: consecutively increasing ID
00033 uint32 seq
00034 #Two-integer timestamp that is expressed as:
00035 # * stamp.secs: seconds (stamp_secs) since epoch
00036 # * stamp.nsecs: nanoseconds since stamp_secs
00037 # time-handling sugar is provided by the client library
00038 time stamp
00039 #Frame this data is associated with
00040 # 0: no frame
00041 # 1: global frame
00042 string frame_id
00043
00044 ================================================================================
00045 MSG: motion_planning_msgs/RobotState
00046 # This message contains information about the robot state, i.e. the positions of its joints and links
00047 sensor_msgs/JointState joint_state
00048 motion_planning_msgs/MultiDOFJointState multi_dof_joint_state
00049 ================================================================================
00050 MSG: sensor_msgs/JointState
00051 # This is a message that holds data to describe the state of a set of torque controlled joints.
00052 #
00053 # The state of each joint (revolute or prismatic) is defined by:
00054 # * the position of the joint (rad or m),
00055 # * the velocity of the joint (rad/s or m/s) and
00056 # * the effort that is applied in the joint (Nm or N).
00057 #
00058 # Each joint is uniquely identified by its name
00059 # The header specifies the time at which the joint states were recorded. All the joint states
00060 # in one message have to be recorded at the same time.
00061 #
00062 # This message consists of a multiple arrays, one for each part of the joint state.
00063 # The goal is to make each of the fields optional. When e.g. your joints have no
00064 # effort associated with them, you can leave the effort array empty.
00065 #
00066 # All arrays in this message should have the same size, or be empty.
00067 # This is the only way to uniquely associate the joint name with the correct
00068 # states.
00069
00070
00071 Header header
00072
00073 string[] name
00074 float64[] position
00075 float64[] velocity
00076 float64[] effort
00077
00078 ================================================================================
00079 MSG: motion_planning_msgs/MultiDOFJointState
00080 #A representation of a multi-dof joint state
00081 time stamp
00082 string[] joint_names
00083 string[] frame_ids
00084 string[] child_frame_ids
00085 geometry_msgs/Pose[] poses
00086
00087 ================================================================================
00088 MSG: geometry_msgs/Pose
00089 # A representation of pose in free space, composed of postion and orientation.
00090 Point position
00091 Quaternion orientation
00092
00093 ================================================================================
00094 MSG: geometry_msgs/Point
00095 # This contains the position of a point in free space
00096 float64 x
00097 float64 y
00098 float64 z
00099
00100 ================================================================================
00101 MSG: geometry_msgs/Quaternion
00102 # This represents an orientation in free space in quaternion form.
00103
00104 float64 x
00105 float64 y
00106 float64 z
00107 float64 w
00108
00109 """
00110 __slots__ = ['header','fk_link_names','robot_state']
00111 _slot_types = ['Header','string[]','motion_planning_msgs/RobotState']
00112
00113 def __init__(self, *args, **kwds):
00114 """
00115 Constructor. Any message fields that are implicitly/explicitly
00116 set to None will be assigned a default value. The recommend
00117 use is keyword arguments as this is more robust to future message
00118 changes. You cannot mix in-order arguments and keyword arguments.
00119
00120 The available fields are:
00121 header,fk_link_names,robot_state
00122
00123 @param args: complete set of field values, in .msg order
00124 @param kwds: use keyword arguments corresponding to message field names
00125 to set specific fields.
00126 """
00127 if args or kwds:
00128 super(GetPositionFKRequest, self).__init__(*args, **kwds)
00129
00130 if self.header is None:
00131 self.header = std_msgs.msg._Header.Header()
00132 if self.fk_link_names is None:
00133 self.fk_link_names = []
00134 if self.robot_state is None:
00135 self.robot_state = motion_planning_msgs.msg.RobotState()
00136 else:
00137 self.header = std_msgs.msg._Header.Header()
00138 self.fk_link_names = []
00139 self.robot_state = motion_planning_msgs.msg.RobotState()
00140
00141 def _get_types(self):
00142 """
00143 internal API method
00144 """
00145 return self._slot_types
00146
00147 def serialize(self, buff):
00148 """
00149 serialize message into buffer
00150 @param buff: buffer
00151 @type buff: StringIO
00152 """
00153 try:
00154 _x = self
00155 buff.write(_struct_3I.pack(_x.header.seq, _x.header.stamp.secs, _x.header.stamp.nsecs))
00156 _x = self.header.frame_id
00157 length = len(_x)
00158 buff.write(struct.pack('<I%ss'%length, length, _x))
00159 length = len(self.fk_link_names)
00160 buff.write(_struct_I.pack(length))
00161 for val1 in self.fk_link_names:
00162 length = len(val1)
00163 buff.write(struct.pack('<I%ss'%length, length, val1))
00164 _x = self
00165 buff.write(_struct_3I.pack(_x.robot_state.joint_state.header.seq, _x.robot_state.joint_state.header.stamp.secs, _x.robot_state.joint_state.header.stamp.nsecs))
00166 _x = self.robot_state.joint_state.header.frame_id
00167 length = len(_x)
00168 buff.write(struct.pack('<I%ss'%length, length, _x))
00169 length = len(self.robot_state.joint_state.name)
00170 buff.write(_struct_I.pack(length))
00171 for val1 in self.robot_state.joint_state.name:
00172 length = len(val1)
00173 buff.write(struct.pack('<I%ss'%length, length, val1))
00174 length = len(self.robot_state.joint_state.position)
00175 buff.write(_struct_I.pack(length))
00176 pattern = '<%sd'%length
00177 buff.write(struct.pack(pattern, *self.robot_state.joint_state.position))
00178 length = len(self.robot_state.joint_state.velocity)
00179 buff.write(_struct_I.pack(length))
00180 pattern = '<%sd'%length
00181 buff.write(struct.pack(pattern, *self.robot_state.joint_state.velocity))
00182 length = len(self.robot_state.joint_state.effort)
00183 buff.write(_struct_I.pack(length))
00184 pattern = '<%sd'%length
00185 buff.write(struct.pack(pattern, *self.robot_state.joint_state.effort))
00186 _x = self
00187 buff.write(_struct_2I.pack(_x.robot_state.multi_dof_joint_state.stamp.secs, _x.robot_state.multi_dof_joint_state.stamp.nsecs))
00188 length = len(self.robot_state.multi_dof_joint_state.joint_names)
00189 buff.write(_struct_I.pack(length))
00190 for val1 in self.robot_state.multi_dof_joint_state.joint_names:
00191 length = len(val1)
00192 buff.write(struct.pack('<I%ss'%length, length, val1))
00193 length = len(self.robot_state.multi_dof_joint_state.frame_ids)
00194 buff.write(_struct_I.pack(length))
00195 for val1 in self.robot_state.multi_dof_joint_state.frame_ids:
00196 length = len(val1)
00197 buff.write(struct.pack('<I%ss'%length, length, val1))
00198 length = len(self.robot_state.multi_dof_joint_state.child_frame_ids)
00199 buff.write(_struct_I.pack(length))
00200 for val1 in self.robot_state.multi_dof_joint_state.child_frame_ids:
00201 length = len(val1)
00202 buff.write(struct.pack('<I%ss'%length, length, val1))
00203 length = len(self.robot_state.multi_dof_joint_state.poses)
00204 buff.write(_struct_I.pack(length))
00205 for val1 in self.robot_state.multi_dof_joint_state.poses:
00206 _v1 = val1.position
00207 _x = _v1
00208 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00209 _v2 = val1.orientation
00210 _x = _v2
00211 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
00212 except struct.error, se: self._check_types(se)
00213 except TypeError, te: self._check_types(te)
00214
00215 def deserialize(self, str):
00216 """
00217 unpack serialized message in str into this message instance
00218 @param str: byte array of serialized message
00219 @type str: str
00220 """
00221 try:
00222 if self.header is None:
00223 self.header = std_msgs.msg._Header.Header()
00224 if self.robot_state is None:
00225 self.robot_state = motion_planning_msgs.msg.RobotState()
00226 end = 0
00227 _x = self
00228 start = end
00229 end += 12
00230 (_x.header.seq, _x.header.stamp.secs, _x.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
00231 start = end
00232 end += 4
00233 (length,) = _struct_I.unpack(str[start:end])
00234 start = end
00235 end += length
00236 self.header.frame_id = str[start:end]
00237 start = end
00238 end += 4
00239 (length,) = _struct_I.unpack(str[start:end])
00240 self.fk_link_names = []
00241 for i in xrange(0, length):
00242 start = end
00243 end += 4
00244 (length,) = _struct_I.unpack(str[start:end])
00245 start = end
00246 end += length
00247 val1 = str[start:end]
00248 self.fk_link_names.append(val1)
00249 _x = self
00250 start = end
00251 end += 12
00252 (_x.robot_state.joint_state.header.seq, _x.robot_state.joint_state.header.stamp.secs, _x.robot_state.joint_state.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
00253 start = end
00254 end += 4
00255 (length,) = _struct_I.unpack(str[start:end])
00256 start = end
00257 end += length
00258 self.robot_state.joint_state.header.frame_id = str[start:end]
00259 start = end
00260 end += 4
00261 (length,) = _struct_I.unpack(str[start:end])
00262 self.robot_state.joint_state.name = []
00263 for i in xrange(0, length):
00264 start = end
00265 end += 4
00266 (length,) = _struct_I.unpack(str[start:end])
00267 start = end
00268 end += length
00269 val1 = str[start:end]
00270 self.robot_state.joint_state.name.append(val1)
00271 start = end
00272 end += 4
00273 (length,) = _struct_I.unpack(str[start:end])
00274 pattern = '<%sd'%length
00275 start = end
00276 end += struct.calcsize(pattern)
00277 self.robot_state.joint_state.position = struct.unpack(pattern, str[start:end])
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 self.robot_state.joint_state.velocity = 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 self.robot_state.joint_state.effort = struct.unpack(pattern, str[start:end])
00292 _x = self
00293 start = end
00294 end += 8
00295 (_x.robot_state.multi_dof_joint_state.stamp.secs, _x.robot_state.multi_dof_joint_state.stamp.nsecs,) = _struct_2I.unpack(str[start:end])
00296 start = end
00297 end += 4
00298 (length,) = _struct_I.unpack(str[start:end])
00299 self.robot_state.multi_dof_joint_state.joint_names = []
00300 for i in xrange(0, length):
00301 start = end
00302 end += 4
00303 (length,) = _struct_I.unpack(str[start:end])
00304 start = end
00305 end += length
00306 val1 = str[start:end]
00307 self.robot_state.multi_dof_joint_state.joint_names.append(val1)
00308 start = end
00309 end += 4
00310 (length,) = _struct_I.unpack(str[start:end])
00311 self.robot_state.multi_dof_joint_state.frame_ids = []
00312 for i in xrange(0, length):
00313 start = end
00314 end += 4
00315 (length,) = _struct_I.unpack(str[start:end])
00316 start = end
00317 end += length
00318 val1 = str[start:end]
00319 self.robot_state.multi_dof_joint_state.frame_ids.append(val1)
00320 start = end
00321 end += 4
00322 (length,) = _struct_I.unpack(str[start:end])
00323 self.robot_state.multi_dof_joint_state.child_frame_ids = []
00324 for i in xrange(0, length):
00325 start = end
00326 end += 4
00327 (length,) = _struct_I.unpack(str[start:end])
00328 start = end
00329 end += length
00330 val1 = str[start:end]
00331 self.robot_state.multi_dof_joint_state.child_frame_ids.append(val1)
00332 start = end
00333 end += 4
00334 (length,) = _struct_I.unpack(str[start:end])
00335 self.robot_state.multi_dof_joint_state.poses = []
00336 for i in xrange(0, length):
00337 val1 = geometry_msgs.msg.Pose()
00338 _v3 = val1.position
00339 _x = _v3
00340 start = end
00341 end += 24
00342 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
00343 _v4 = val1.orientation
00344 _x = _v4
00345 start = end
00346 end += 32
00347 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
00348 self.robot_state.multi_dof_joint_state.poses.append(val1)
00349 return self
00350 except struct.error, e:
00351 raise roslib.message.DeserializationError(e)
00352
00353
00354 def serialize_numpy(self, buff, numpy):
00355 """
00356 serialize message with numpy array types into buffer
00357 @param buff: buffer
00358 @type buff: StringIO
00359 @param numpy: numpy python module
00360 @type numpy module
00361 """
00362 try:
00363 _x = self
00364 buff.write(_struct_3I.pack(_x.header.seq, _x.header.stamp.secs, _x.header.stamp.nsecs))
00365 _x = self.header.frame_id
00366 length = len(_x)
00367 buff.write(struct.pack('<I%ss'%length, length, _x))
00368 length = len(self.fk_link_names)
00369 buff.write(_struct_I.pack(length))
00370 for val1 in self.fk_link_names:
00371 length = len(val1)
00372 buff.write(struct.pack('<I%ss'%length, length, val1))
00373 _x = self
00374 buff.write(_struct_3I.pack(_x.robot_state.joint_state.header.seq, _x.robot_state.joint_state.header.stamp.secs, _x.robot_state.joint_state.header.stamp.nsecs))
00375 _x = self.robot_state.joint_state.header.frame_id
00376 length = len(_x)
00377 buff.write(struct.pack('<I%ss'%length, length, _x))
00378 length = len(self.robot_state.joint_state.name)
00379 buff.write(_struct_I.pack(length))
00380 for val1 in self.robot_state.joint_state.name:
00381 length = len(val1)
00382 buff.write(struct.pack('<I%ss'%length, length, val1))
00383 length = len(self.robot_state.joint_state.position)
00384 buff.write(_struct_I.pack(length))
00385 pattern = '<%sd'%length
00386 buff.write(self.robot_state.joint_state.position.tostring())
00387 length = len(self.robot_state.joint_state.velocity)
00388 buff.write(_struct_I.pack(length))
00389 pattern = '<%sd'%length
00390 buff.write(self.robot_state.joint_state.velocity.tostring())
00391 length = len(self.robot_state.joint_state.effort)
00392 buff.write(_struct_I.pack(length))
00393 pattern = '<%sd'%length
00394 buff.write(self.robot_state.joint_state.effort.tostring())
00395 _x = self
00396 buff.write(_struct_2I.pack(_x.robot_state.multi_dof_joint_state.stamp.secs, _x.robot_state.multi_dof_joint_state.stamp.nsecs))
00397 length = len(self.robot_state.multi_dof_joint_state.joint_names)
00398 buff.write(_struct_I.pack(length))
00399 for val1 in self.robot_state.multi_dof_joint_state.joint_names:
00400 length = len(val1)
00401 buff.write(struct.pack('<I%ss'%length, length, val1))
00402 length = len(self.robot_state.multi_dof_joint_state.frame_ids)
00403 buff.write(_struct_I.pack(length))
00404 for val1 in self.robot_state.multi_dof_joint_state.frame_ids:
00405 length = len(val1)
00406 buff.write(struct.pack('<I%ss'%length, length, val1))
00407 length = len(self.robot_state.multi_dof_joint_state.child_frame_ids)
00408 buff.write(_struct_I.pack(length))
00409 for val1 in self.robot_state.multi_dof_joint_state.child_frame_ids:
00410 length = len(val1)
00411 buff.write(struct.pack('<I%ss'%length, length, val1))
00412 length = len(self.robot_state.multi_dof_joint_state.poses)
00413 buff.write(_struct_I.pack(length))
00414 for val1 in self.robot_state.multi_dof_joint_state.poses:
00415 _v5 = val1.position
00416 _x = _v5
00417 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00418 _v6 = val1.orientation
00419 _x = _v6
00420 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
00421 except struct.error, se: self._check_types(se)
00422 except TypeError, te: self._check_types(te)
00423
00424 def deserialize_numpy(self, str, numpy):
00425 """
00426 unpack serialized message in str into this message instance using numpy for array types
00427 @param str: byte array of serialized message
00428 @type str: str
00429 @param numpy: numpy python module
00430 @type numpy: module
00431 """
00432 try:
00433 if self.header is None:
00434 self.header = std_msgs.msg._Header.Header()
00435 if self.robot_state is None:
00436 self.robot_state = motion_planning_msgs.msg.RobotState()
00437 end = 0
00438 _x = self
00439 start = end
00440 end += 12
00441 (_x.header.seq, _x.header.stamp.secs, _x.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
00442 start = end
00443 end += 4
00444 (length,) = _struct_I.unpack(str[start:end])
00445 start = end
00446 end += length
00447 self.header.frame_id = str[start:end]
00448 start = end
00449 end += 4
00450 (length,) = _struct_I.unpack(str[start:end])
00451 self.fk_link_names = []
00452 for i in xrange(0, length):
00453 start = end
00454 end += 4
00455 (length,) = _struct_I.unpack(str[start:end])
00456 start = end
00457 end += length
00458 val1 = str[start:end]
00459 self.fk_link_names.append(val1)
00460 _x = self
00461 start = end
00462 end += 12
00463 (_x.robot_state.joint_state.header.seq, _x.robot_state.joint_state.header.stamp.secs, _x.robot_state.joint_state.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
00464 start = end
00465 end += 4
00466 (length,) = _struct_I.unpack(str[start:end])
00467 start = end
00468 end += length
00469 self.robot_state.joint_state.header.frame_id = str[start:end]
00470 start = end
00471 end += 4
00472 (length,) = _struct_I.unpack(str[start:end])
00473 self.robot_state.joint_state.name = []
00474 for i in xrange(0, length):
00475 start = end
00476 end += 4
00477 (length,) = _struct_I.unpack(str[start:end])
00478 start = end
00479 end += length
00480 val1 = str[start:end]
00481 self.robot_state.joint_state.name.append(val1)
00482 start = end
00483 end += 4
00484 (length,) = _struct_I.unpack(str[start:end])
00485 pattern = '<%sd'%length
00486 start = end
00487 end += struct.calcsize(pattern)
00488 self.robot_state.joint_state.position = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
00489 start = end
00490 end += 4
00491 (length,) = _struct_I.unpack(str[start:end])
00492 pattern = '<%sd'%length
00493 start = end
00494 end += struct.calcsize(pattern)
00495 self.robot_state.joint_state.velocity = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
00496 start = end
00497 end += 4
00498 (length,) = _struct_I.unpack(str[start:end])
00499 pattern = '<%sd'%length
00500 start = end
00501 end += struct.calcsize(pattern)
00502 self.robot_state.joint_state.effort = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
00503 _x = self
00504 start = end
00505 end += 8
00506 (_x.robot_state.multi_dof_joint_state.stamp.secs, _x.robot_state.multi_dof_joint_state.stamp.nsecs,) = _struct_2I.unpack(str[start:end])
00507 start = end
00508 end += 4
00509 (length,) = _struct_I.unpack(str[start:end])
00510 self.robot_state.multi_dof_joint_state.joint_names = []
00511 for i in xrange(0, length):
00512 start = end
00513 end += 4
00514 (length,) = _struct_I.unpack(str[start:end])
00515 start = end
00516 end += length
00517 val1 = str[start:end]
00518 self.robot_state.multi_dof_joint_state.joint_names.append(val1)
00519 start = end
00520 end += 4
00521 (length,) = _struct_I.unpack(str[start:end])
00522 self.robot_state.multi_dof_joint_state.frame_ids = []
00523 for i in xrange(0, length):
00524 start = end
00525 end += 4
00526 (length,) = _struct_I.unpack(str[start:end])
00527 start = end
00528 end += length
00529 val1 = str[start:end]
00530 self.robot_state.multi_dof_joint_state.frame_ids.append(val1)
00531 start = end
00532 end += 4
00533 (length,) = _struct_I.unpack(str[start:end])
00534 self.robot_state.multi_dof_joint_state.child_frame_ids = []
00535 for i in xrange(0, length):
00536 start = end
00537 end += 4
00538 (length,) = _struct_I.unpack(str[start:end])
00539 start = end
00540 end += length
00541 val1 = str[start:end]
00542 self.robot_state.multi_dof_joint_state.child_frame_ids.append(val1)
00543 start = end
00544 end += 4
00545 (length,) = _struct_I.unpack(str[start:end])
00546 self.robot_state.multi_dof_joint_state.poses = []
00547 for i in xrange(0, length):
00548 val1 = geometry_msgs.msg.Pose()
00549 _v7 = val1.position
00550 _x = _v7
00551 start = end
00552 end += 24
00553 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
00554 _v8 = val1.orientation
00555 _x = _v8
00556 start = end
00557 end += 32
00558 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
00559 self.robot_state.multi_dof_joint_state.poses.append(val1)
00560 return self
00561 except struct.error, e:
00562 raise roslib.message.DeserializationError(e)
00563
00564 _struct_I = roslib.message.struct_I
00565 _struct_4d = struct.Struct("<4d")
00566 _struct_3I = struct.Struct("<3I")
00567 _struct_2I = struct.Struct("<2I")
00568 _struct_3d = struct.Struct("<3d")
00569 """autogenerated by genmsg_py from GetPositionFKResponse.msg. Do not edit."""
00570 import roslib.message
00571 import struct
00572
00573 import geometry_msgs.msg
00574 import motion_planning_msgs.msg
00575 import std_msgs.msg
00576
00577 class GetPositionFKResponse(roslib.message.Message):
00578 _md5sum = "4a3efc190f6ac6f248069cfa3cd94286"
00579 _type = "kinematics_msgs/GetPositionFKResponse"
00580 _has_header = False
00581 _full_text = """
00582 geometry_msgs/PoseStamped[] pose_stamped
00583
00584
00585 string[] fk_link_names
00586
00587 motion_planning_msgs/ArmNavigationErrorCodes error_code
00588
00589 ================================================================================
00590 MSG: geometry_msgs/PoseStamped
00591 # A Pose with reference coordinate frame and timestamp
00592 Header header
00593 Pose pose
00594
00595 ================================================================================
00596 MSG: std_msgs/Header
00597 # Standard metadata for higher-level stamped data types.
00598 # This is generally used to communicate timestamped data
00599 # in a particular coordinate frame.
00600 #
00601 # sequence ID: consecutively increasing ID
00602 uint32 seq
00603 #Two-integer timestamp that is expressed as:
00604 # * stamp.secs: seconds (stamp_secs) since epoch
00605 # * stamp.nsecs: nanoseconds since stamp_secs
00606 # time-handling sugar is provided by the client library
00607 time stamp
00608 #Frame this data is associated with
00609 # 0: no frame
00610 # 1: global frame
00611 string frame_id
00612
00613 ================================================================================
00614 MSG: geometry_msgs/Pose
00615 # A representation of pose in free space, composed of postion and orientation.
00616 Point position
00617 Quaternion orientation
00618
00619 ================================================================================
00620 MSG: geometry_msgs/Point
00621 # This contains the position of a point in free space
00622 float64 x
00623 float64 y
00624 float64 z
00625
00626 ================================================================================
00627 MSG: geometry_msgs/Quaternion
00628 # This represents an orientation in free space in quaternion form.
00629
00630 float64 x
00631 float64 y
00632 float64 z
00633 float64 w
00634
00635 ================================================================================
00636 MSG: motion_planning_msgs/ArmNavigationErrorCodes
00637 int32 val
00638
00639 # overall behavior
00640 int32 PLANNING_FAILED=-1
00641 int32 SUCCESS=1
00642 int32 TIMED_OUT=-2
00643
00644 # start state errors
00645 int32 START_STATE_IN_COLLISION=-3
00646 int32 START_STATE_VIOLATES_PATH_CONSTRAINTS=-4
00647
00648 # goal errors
00649 int32 GOAL_IN_COLLISION=-5
00650 int32 GOAL_VIOLATES_PATH_CONSTRAINTS=-6
00651
00652 # robot state
00653 int32 INVALID_ROBOT_STATE=-7
00654 int32 INCOMPLETE_ROBOT_STATE=-8
00655
00656 # planning request errors
00657 int32 INVALID_PLANNER_ID=-9
00658 int32 INVALID_NUM_PLANNING_ATTEMPTS=-10
00659 int32 INVALID_ALLOWED_PLANNING_TIME=-11
00660 int32 INVALID_GROUP_NAME=-12
00661 int32 INVALID_GOAL_JOINT_CONSTRAINTS=-13
00662 int32 INVALID_GOAL_POSITION_CONSTRAINTS=-14
00663 int32 INVALID_GOAL_ORIENTATION_CONSTRAINTS=-15
00664 int32 INVALID_PATH_JOINT_CONSTRAINTS=-16
00665 int32 INVALID_PATH_POSITION_CONSTRAINTS=-17
00666 int32 INVALID_PATH_ORIENTATION_CONSTRAINTS=-18
00667
00668 # state/trajectory monitor errors
00669 int32 INVALID_TRAJECTORY=-19
00670 int32 INVALID_INDEX=-20
00671 int32 JOINT_LIMITS_VIOLATED=-21
00672 int32 PATH_CONSTRAINTS_VIOLATED=-22
00673 int32 COLLISION_CONSTRAINTS_VIOLATED=-23
00674 int32 GOAL_CONSTRAINTS_VIOLATED=-24
00675 int32 JOINTS_NOT_MOVING=-25
00676 int32 TRAJECTORY_CONTROLLER_FAILED=-26
00677
00678 # system errors
00679 int32 FRAME_TRANSFORM_FAILURE=-27
00680 int32 COLLISION_CHECKING_UNAVAILABLE=-28
00681 int32 ROBOT_STATE_STALE=-29
00682 int32 SENSOR_INFO_STALE=-30
00683
00684 # kinematics errors
00685 int32 NO_IK_SOLUTION=-31
00686 int32 INVALID_LINK_NAME=-32
00687 int32 IK_LINK_IN_COLLISION=-33
00688 int32 NO_FK_SOLUTION=-34
00689 int32 KINEMATICS_STATE_IN_COLLISION=-35
00690
00691 # general errors
00692 int32 INVALID_TIMEOUT=-36
00693
00694
00695 """
00696 __slots__ = ['pose_stamped','fk_link_names','error_code']
00697 _slot_types = ['geometry_msgs/PoseStamped[]','string[]','motion_planning_msgs/ArmNavigationErrorCodes']
00698
00699 def __init__(self, *args, **kwds):
00700 """
00701 Constructor. Any message fields that are implicitly/explicitly
00702 set to None will be assigned a default value. The recommend
00703 use is keyword arguments as this is more robust to future message
00704 changes. You cannot mix in-order arguments and keyword arguments.
00705
00706 The available fields are:
00707 pose_stamped,fk_link_names,error_code
00708
00709 @param args: complete set of field values, in .msg order
00710 @param kwds: use keyword arguments corresponding to message field names
00711 to set specific fields.
00712 """
00713 if args or kwds:
00714 super(GetPositionFKResponse, self).__init__(*args, **kwds)
00715
00716 if self.pose_stamped is None:
00717 self.pose_stamped = []
00718 if self.fk_link_names is None:
00719 self.fk_link_names = []
00720 if self.error_code is None:
00721 self.error_code = motion_planning_msgs.msg.ArmNavigationErrorCodes()
00722 else:
00723 self.pose_stamped = []
00724 self.fk_link_names = []
00725 self.error_code = motion_planning_msgs.msg.ArmNavigationErrorCodes()
00726
00727 def _get_types(self):
00728 """
00729 internal API method
00730 """
00731 return self._slot_types
00732
00733 def serialize(self, buff):
00734 """
00735 serialize message into buffer
00736 @param buff: buffer
00737 @type buff: StringIO
00738 """
00739 try:
00740 length = len(self.pose_stamped)
00741 buff.write(_struct_I.pack(length))
00742 for val1 in self.pose_stamped:
00743 _v9 = val1.header
00744 buff.write(_struct_I.pack(_v9.seq))
00745 _v10 = _v9.stamp
00746 _x = _v10
00747 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
00748 _x = _v9.frame_id
00749 length = len(_x)
00750 buff.write(struct.pack('<I%ss'%length, length, _x))
00751 _v11 = val1.pose
00752 _v12 = _v11.position
00753 _x = _v12
00754 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00755 _v13 = _v11.orientation
00756 _x = _v13
00757 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
00758 length = len(self.fk_link_names)
00759 buff.write(_struct_I.pack(length))
00760 for val1 in self.fk_link_names:
00761 length = len(val1)
00762 buff.write(struct.pack('<I%ss'%length, length, val1))
00763 buff.write(_struct_i.pack(self.error_code.val))
00764 except struct.error, se: self._check_types(se)
00765 except TypeError, te: self._check_types(te)
00766
00767 def deserialize(self, str):
00768 """
00769 unpack serialized message in str into this message instance
00770 @param str: byte array of serialized message
00771 @type str: str
00772 """
00773 try:
00774 if self.error_code is None:
00775 self.error_code = motion_planning_msgs.msg.ArmNavigationErrorCodes()
00776 end = 0
00777 start = end
00778 end += 4
00779 (length,) = _struct_I.unpack(str[start:end])
00780 self.pose_stamped = []
00781 for i in xrange(0, length):
00782 val1 = geometry_msgs.msg.PoseStamped()
00783 _v14 = val1.header
00784 start = end
00785 end += 4
00786 (_v14.seq,) = _struct_I.unpack(str[start:end])
00787 _v15 = _v14.stamp
00788 _x = _v15
00789 start = end
00790 end += 8
00791 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
00792 start = end
00793 end += 4
00794 (length,) = _struct_I.unpack(str[start:end])
00795 start = end
00796 end += length
00797 _v14.frame_id = str[start:end]
00798 _v16 = val1.pose
00799 _v17 = _v16.position
00800 _x = _v17
00801 start = end
00802 end += 24
00803 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
00804 _v18 = _v16.orientation
00805 _x = _v18
00806 start = end
00807 end += 32
00808 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
00809 self.pose_stamped.append(val1)
00810 start = end
00811 end += 4
00812 (length,) = _struct_I.unpack(str[start:end])
00813 self.fk_link_names = []
00814 for i in xrange(0, length):
00815 start = end
00816 end += 4
00817 (length,) = _struct_I.unpack(str[start:end])
00818 start = end
00819 end += length
00820 val1 = str[start:end]
00821 self.fk_link_names.append(val1)
00822 start = end
00823 end += 4
00824 (self.error_code.val,) = _struct_i.unpack(str[start:end])
00825 return self
00826 except struct.error, e:
00827 raise roslib.message.DeserializationError(e)
00828
00829
00830 def serialize_numpy(self, buff, numpy):
00831 """
00832 serialize message with numpy array types into buffer
00833 @param buff: buffer
00834 @type buff: StringIO
00835 @param numpy: numpy python module
00836 @type numpy module
00837 """
00838 try:
00839 length = len(self.pose_stamped)
00840 buff.write(_struct_I.pack(length))
00841 for val1 in self.pose_stamped:
00842 _v19 = val1.header
00843 buff.write(_struct_I.pack(_v19.seq))
00844 _v20 = _v19.stamp
00845 _x = _v20
00846 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
00847 _x = _v19.frame_id
00848 length = len(_x)
00849 buff.write(struct.pack('<I%ss'%length, length, _x))
00850 _v21 = val1.pose
00851 _v22 = _v21.position
00852 _x = _v22
00853 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00854 _v23 = _v21.orientation
00855 _x = _v23
00856 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
00857 length = len(self.fk_link_names)
00858 buff.write(_struct_I.pack(length))
00859 for val1 in self.fk_link_names:
00860 length = len(val1)
00861 buff.write(struct.pack('<I%ss'%length, length, val1))
00862 buff.write(_struct_i.pack(self.error_code.val))
00863 except struct.error, se: self._check_types(se)
00864 except TypeError, te: self._check_types(te)
00865
00866 def deserialize_numpy(self, str, numpy):
00867 """
00868 unpack serialized message in str into this message instance using numpy for array types
00869 @param str: byte array of serialized message
00870 @type str: str
00871 @param numpy: numpy python module
00872 @type numpy: module
00873 """
00874 try:
00875 if self.error_code is None:
00876 self.error_code = motion_planning_msgs.msg.ArmNavigationErrorCodes()
00877 end = 0
00878 start = end
00879 end += 4
00880 (length,) = _struct_I.unpack(str[start:end])
00881 self.pose_stamped = []
00882 for i in xrange(0, length):
00883 val1 = geometry_msgs.msg.PoseStamped()
00884 _v24 = val1.header
00885 start = end
00886 end += 4
00887 (_v24.seq,) = _struct_I.unpack(str[start:end])
00888 _v25 = _v24.stamp
00889 _x = _v25
00890 start = end
00891 end += 8
00892 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
00893 start = end
00894 end += 4
00895 (length,) = _struct_I.unpack(str[start:end])
00896 start = end
00897 end += length
00898 _v24.frame_id = str[start:end]
00899 _v26 = val1.pose
00900 _v27 = _v26.position
00901 _x = _v27
00902 start = end
00903 end += 24
00904 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
00905 _v28 = _v26.orientation
00906 _x = _v28
00907 start = end
00908 end += 32
00909 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
00910 self.pose_stamped.append(val1)
00911 start = end
00912 end += 4
00913 (length,) = _struct_I.unpack(str[start:end])
00914 self.fk_link_names = []
00915 for i in xrange(0, length):
00916 start = end
00917 end += 4
00918 (length,) = _struct_I.unpack(str[start:end])
00919 start = end
00920 end += length
00921 val1 = str[start:end]
00922 self.fk_link_names.append(val1)
00923 start = end
00924 end += 4
00925 (self.error_code.val,) = _struct_i.unpack(str[start:end])
00926 return self
00927 except struct.error, e:
00928 raise roslib.message.DeserializationError(e)
00929
00930 _struct_I = roslib.message.struct_I
00931 _struct_i = struct.Struct("<i")
00932 _struct_4d = struct.Struct("<4d")
00933 _struct_2I = struct.Struct("<2I")
00934 _struct_3d = struct.Struct("<3d")
00935 class GetPositionFK(roslib.message.ServiceDefinition):
00936 _type = 'kinematics_msgs/GetPositionFK'
00937 _md5sum = '2088007c8963e2252a67c872affa0985'
00938 _request_class = GetPositionFKRequest
00939 _response_class = GetPositionFKResponse