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