00001 """autogenerated by genpy from tree_kinematics/get_tree_position_ikRequest.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 kinematics_msgs.msg
00009 import geometry_msgs.msg
00010 import sensor_msgs.msg
00011 import genpy
00012 import std_msgs.msg
00013
00014 class get_tree_position_ikRequest(genpy.Message):
00015 _md5sum = "ebff3a9548f87f46312b51a1358841ab"
00016 _type = "tree_kinematics/get_tree_position_ikRequest"
00017 _has_header = False
00018 _full_text = """
00019
00020
00021
00022 kinematics_msgs/PositionIKRequest[] pos_ik_request
00023
00024
00025 ================================================================================
00026 MSG: kinematics_msgs/PositionIKRequest
00027 # A Position IK request message
00028 # The name of the link for which we are computing IK
00029 string ik_link_name
00030
00031 # The (stamped) pose of the link
00032 geometry_msgs/PoseStamped pose_stamped
00033
00034 # A RobotState consisting of hint/seed positions for the IK computation.
00035 # These may be used to seed the IK search.
00036 # The seed state MUST contain state for all joints to be used by the IK solver
00037 # to compute IK. The list of joints that the IK solver deals with can be found using
00038 # the kinematics_msgs/GetKinematicSolverInfo
00039 arm_navigation_msgs/RobotState ik_seed_state
00040
00041 # Additional state information can be provided here to specify the starting positions
00042 # of other joints/links on the robot.
00043 arm_navigation_msgs/RobotState robot_state
00044
00045 ================================================================================
00046 MSG: geometry_msgs/PoseStamped
00047 # A Pose with reference coordinate frame and timestamp
00048 Header header
00049 Pose pose
00050
00051 ================================================================================
00052 MSG: std_msgs/Header
00053 # Standard metadata for higher-level stamped data types.
00054 # This is generally used to communicate timestamped data
00055 # in a particular coordinate frame.
00056 #
00057 # sequence ID: consecutively increasing ID
00058 uint32 seq
00059 #Two-integer timestamp that is expressed as:
00060 # * stamp.secs: seconds (stamp_secs) since epoch
00061 # * stamp.nsecs: nanoseconds since stamp_secs
00062 # time-handling sugar is provided by the client library
00063 time stamp
00064 #Frame this data is associated with
00065 # 0: no frame
00066 # 1: global frame
00067 string frame_id
00068
00069 ================================================================================
00070 MSG: geometry_msgs/Pose
00071 # A representation of pose in free space, composed of postion and orientation.
00072 Point position
00073 Quaternion orientation
00074
00075 ================================================================================
00076 MSG: geometry_msgs/Point
00077 # This contains the position of a point in free space
00078 float64 x
00079 float64 y
00080 float64 z
00081
00082 ================================================================================
00083 MSG: geometry_msgs/Quaternion
00084 # This represents an orientation in free space in quaternion form.
00085
00086 float64 x
00087 float64 y
00088 float64 z
00089 float64 w
00090
00091 ================================================================================
00092 MSG: arm_navigation_msgs/RobotState
00093 # This message contains information about the robot state, i.e. the positions of its joints and links
00094 sensor_msgs/JointState joint_state
00095 arm_navigation_msgs/MultiDOFJointState multi_dof_joint_state
00096
00097 ================================================================================
00098 MSG: sensor_msgs/JointState
00099 # This is a message that holds data to describe the state of a set of torque controlled joints.
00100 #
00101 # The state of each joint (revolute or prismatic) is defined by:
00102 # * the position of the joint (rad or m),
00103 # * the velocity of the joint (rad/s or m/s) and
00104 # * the effort that is applied in the joint (Nm or N).
00105 #
00106 # Each joint is uniquely identified by its name
00107 # The header specifies the time at which the joint states were recorded. All the joint states
00108 # in one message have to be recorded at the same time.
00109 #
00110 # This message consists of a multiple arrays, one for each part of the joint state.
00111 # The goal is to make each of the fields optional. When e.g. your joints have no
00112 # effort associated with them, you can leave the effort array empty.
00113 #
00114 # All arrays in this message should have the same size, or be empty.
00115 # This is the only way to uniquely associate the joint name with the correct
00116 # states.
00117
00118
00119 Header header
00120
00121 string[] name
00122 float64[] position
00123 float64[] velocity
00124 float64[] effort
00125
00126 ================================================================================
00127 MSG: arm_navigation_msgs/MultiDOFJointState
00128 #A representation of a multi-dof joint state
00129 time stamp
00130 string[] joint_names
00131 string[] frame_ids
00132 string[] child_frame_ids
00133 geometry_msgs/Pose[] poses
00134
00135 """
00136 __slots__ = ['pos_ik_request']
00137 _slot_types = ['kinematics_msgs/PositionIKRequest[]']
00138
00139 def __init__(self, *args, **kwds):
00140 """
00141 Constructor. Any message fields that are implicitly/explicitly
00142 set to None will be assigned a default value. The recommend
00143 use is keyword arguments as this is more robust to future message
00144 changes. You cannot mix in-order arguments and keyword arguments.
00145
00146 The available fields are:
00147 pos_ik_request
00148
00149 :param args: complete set of field values, in .msg order
00150 :param kwds: use keyword arguments corresponding to message field names
00151 to set specific fields.
00152 """
00153 if args or kwds:
00154 super(get_tree_position_ikRequest, self).__init__(*args, **kwds)
00155
00156 if self.pos_ik_request is None:
00157 self.pos_ik_request = []
00158 else:
00159 self.pos_ik_request = []
00160
00161 def _get_types(self):
00162 """
00163 internal API method
00164 """
00165 return self._slot_types
00166
00167 def serialize(self, buff):
00168 """
00169 serialize message into buffer
00170 :param buff: buffer, ``StringIO``
00171 """
00172 try:
00173 length = len(self.pos_ik_request)
00174 buff.write(_struct_I.pack(length))
00175 for val1 in self.pos_ik_request:
00176 _x = val1.ik_link_name
00177 length = len(_x)
00178 if python3 or type(_x) == unicode:
00179 _x = _x.encode('utf-8')
00180 length = len(_x)
00181 buff.write(struct.pack('<I%ss'%length, length, _x))
00182 _v1 = val1.pose_stamped
00183 _v2 = _v1.header
00184 buff.write(_struct_I.pack(_v2.seq))
00185 _v3 = _v2.stamp
00186 _x = _v3
00187 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
00188 _x = _v2.frame_id
00189 length = len(_x)
00190 if python3 or type(_x) == unicode:
00191 _x = _x.encode('utf-8')
00192 length = len(_x)
00193 buff.write(struct.pack('<I%ss'%length, length, _x))
00194 _v4 = _v1.pose
00195 _v5 = _v4.position
00196 _x = _v5
00197 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00198 _v6 = _v4.orientation
00199 _x = _v6
00200 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
00201 _v7 = val1.ik_seed_state
00202 _v8 = _v7.joint_state
00203 _v9 = _v8.header
00204 buff.write(_struct_I.pack(_v9.seq))
00205 _v10 = _v9.stamp
00206 _x = _v10
00207 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
00208 _x = _v9.frame_id
00209 length = len(_x)
00210 if python3 or type(_x) == unicode:
00211 _x = _x.encode('utf-8')
00212 length = len(_x)
00213 buff.write(struct.pack('<I%ss'%length, length, _x))
00214 length = len(_v8.name)
00215 buff.write(_struct_I.pack(length))
00216 for val4 in _v8.name:
00217 length = len(val4)
00218 if python3 or type(val4) == unicode:
00219 val4 = val4.encode('utf-8')
00220 length = len(val4)
00221 buff.write(struct.pack('<I%ss'%length, length, val4))
00222 length = len(_v8.position)
00223 buff.write(_struct_I.pack(length))
00224 pattern = '<%sd'%length
00225 buff.write(struct.pack(pattern, *_v8.position))
00226 length = len(_v8.velocity)
00227 buff.write(_struct_I.pack(length))
00228 pattern = '<%sd'%length
00229 buff.write(struct.pack(pattern, *_v8.velocity))
00230 length = len(_v8.effort)
00231 buff.write(_struct_I.pack(length))
00232 pattern = '<%sd'%length
00233 buff.write(struct.pack(pattern, *_v8.effort))
00234 _v11 = _v7.multi_dof_joint_state
00235 _v12 = _v11.stamp
00236 _x = _v12
00237 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
00238 length = len(_v11.joint_names)
00239 buff.write(_struct_I.pack(length))
00240 for val4 in _v11.joint_names:
00241 length = len(val4)
00242 if python3 or type(val4) == unicode:
00243 val4 = val4.encode('utf-8')
00244 length = len(val4)
00245 buff.write(struct.pack('<I%ss'%length, length, val4))
00246 length = len(_v11.frame_ids)
00247 buff.write(_struct_I.pack(length))
00248 for val4 in _v11.frame_ids:
00249 length = len(val4)
00250 if python3 or type(val4) == unicode:
00251 val4 = val4.encode('utf-8')
00252 length = len(val4)
00253 buff.write(struct.pack('<I%ss'%length, length, val4))
00254 length = len(_v11.child_frame_ids)
00255 buff.write(_struct_I.pack(length))
00256 for val4 in _v11.child_frame_ids:
00257 length = len(val4)
00258 if python3 or type(val4) == unicode:
00259 val4 = val4.encode('utf-8')
00260 length = len(val4)
00261 buff.write(struct.pack('<I%ss'%length, length, val4))
00262 length = len(_v11.poses)
00263 buff.write(_struct_I.pack(length))
00264 for val4 in _v11.poses:
00265 _v13 = val4.position
00266 _x = _v13
00267 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00268 _v14 = val4.orientation
00269 _x = _v14
00270 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
00271 _v15 = val1.robot_state
00272 _v16 = _v15.joint_state
00273 _v17 = _v16.header
00274 buff.write(_struct_I.pack(_v17.seq))
00275 _v18 = _v17.stamp
00276 _x = _v18
00277 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
00278 _x = _v17.frame_id
00279 length = len(_x)
00280 if python3 or type(_x) == unicode:
00281 _x = _x.encode('utf-8')
00282 length = len(_x)
00283 buff.write(struct.pack('<I%ss'%length, length, _x))
00284 length = len(_v16.name)
00285 buff.write(_struct_I.pack(length))
00286 for val4 in _v16.name:
00287 length = len(val4)
00288 if python3 or type(val4) == unicode:
00289 val4 = val4.encode('utf-8')
00290 length = len(val4)
00291 buff.write(struct.pack('<I%ss'%length, length, val4))
00292 length = len(_v16.position)
00293 buff.write(_struct_I.pack(length))
00294 pattern = '<%sd'%length
00295 buff.write(struct.pack(pattern, *_v16.position))
00296 length = len(_v16.velocity)
00297 buff.write(_struct_I.pack(length))
00298 pattern = '<%sd'%length
00299 buff.write(struct.pack(pattern, *_v16.velocity))
00300 length = len(_v16.effort)
00301 buff.write(_struct_I.pack(length))
00302 pattern = '<%sd'%length
00303 buff.write(struct.pack(pattern, *_v16.effort))
00304 _v19 = _v15.multi_dof_joint_state
00305 _v20 = _v19.stamp
00306 _x = _v20
00307 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
00308 length = len(_v19.joint_names)
00309 buff.write(_struct_I.pack(length))
00310 for val4 in _v19.joint_names:
00311 length = len(val4)
00312 if python3 or type(val4) == unicode:
00313 val4 = val4.encode('utf-8')
00314 length = len(val4)
00315 buff.write(struct.pack('<I%ss'%length, length, val4))
00316 length = len(_v19.frame_ids)
00317 buff.write(_struct_I.pack(length))
00318 for val4 in _v19.frame_ids:
00319 length = len(val4)
00320 if python3 or type(val4) == unicode:
00321 val4 = val4.encode('utf-8')
00322 length = len(val4)
00323 buff.write(struct.pack('<I%ss'%length, length, val4))
00324 length = len(_v19.child_frame_ids)
00325 buff.write(_struct_I.pack(length))
00326 for val4 in _v19.child_frame_ids:
00327 length = len(val4)
00328 if python3 or type(val4) == unicode:
00329 val4 = val4.encode('utf-8')
00330 length = len(val4)
00331 buff.write(struct.pack('<I%ss'%length, length, val4))
00332 length = len(_v19.poses)
00333 buff.write(_struct_I.pack(length))
00334 for val4 in _v19.poses:
00335 _v21 = val4.position
00336 _x = _v21
00337 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00338 _v22 = val4.orientation
00339 _x = _v22
00340 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
00341 except struct.error as se: self._check_types(se)
00342 except TypeError as te: self._check_types(te)
00343
00344 def deserialize(self, str):
00345 """
00346 unpack serialized message in str into this message instance
00347 :param str: byte array of serialized message, ``str``
00348 """
00349 try:
00350 if self.pos_ik_request is None:
00351 self.pos_ik_request = None
00352 end = 0
00353 start = end
00354 end += 4
00355 (length,) = _struct_I.unpack(str[start:end])
00356 self.pos_ik_request = []
00357 for i in range(0, length):
00358 val1 = kinematics_msgs.msg.PositionIKRequest()
00359 start = end
00360 end += 4
00361 (length,) = _struct_I.unpack(str[start:end])
00362 start = end
00363 end += length
00364 if python3:
00365 val1.ik_link_name = str[start:end].decode('utf-8')
00366 else:
00367 val1.ik_link_name = str[start:end]
00368 _v23 = val1.pose_stamped
00369 _v24 = _v23.header
00370 start = end
00371 end += 4
00372 (_v24.seq,) = _struct_I.unpack(str[start:end])
00373 _v25 = _v24.stamp
00374 _x = _v25
00375 start = end
00376 end += 8
00377 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
00378 start = end
00379 end += 4
00380 (length,) = _struct_I.unpack(str[start:end])
00381 start = end
00382 end += length
00383 if python3:
00384 _v24.frame_id = str[start:end].decode('utf-8')
00385 else:
00386 _v24.frame_id = str[start:end]
00387 _v26 = _v23.pose
00388 _v27 = _v26.position
00389 _x = _v27
00390 start = end
00391 end += 24
00392 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
00393 _v28 = _v26.orientation
00394 _x = _v28
00395 start = end
00396 end += 32
00397 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
00398 _v29 = val1.ik_seed_state
00399 _v30 = _v29.joint_state
00400 _v31 = _v30.header
00401 start = end
00402 end += 4
00403 (_v31.seq,) = _struct_I.unpack(str[start:end])
00404 _v32 = _v31.stamp
00405 _x = _v32
00406 start = end
00407 end += 8
00408 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
00409 start = end
00410 end += 4
00411 (length,) = _struct_I.unpack(str[start:end])
00412 start = end
00413 end += length
00414 if python3:
00415 _v31.frame_id = str[start:end].decode('utf-8')
00416 else:
00417 _v31.frame_id = str[start:end]
00418 start = end
00419 end += 4
00420 (length,) = _struct_I.unpack(str[start:end])
00421 _v30.name = []
00422 for i in range(0, length):
00423 start = end
00424 end += 4
00425 (length,) = _struct_I.unpack(str[start:end])
00426 start = end
00427 end += length
00428 if python3:
00429 val4 = str[start:end].decode('utf-8')
00430 else:
00431 val4 = str[start:end]
00432 _v30.name.append(val4)
00433 start = end
00434 end += 4
00435 (length,) = _struct_I.unpack(str[start:end])
00436 pattern = '<%sd'%length
00437 start = end
00438 end += struct.calcsize(pattern)
00439 _v30.position = struct.unpack(pattern, str[start:end])
00440 start = end
00441 end += 4
00442 (length,) = _struct_I.unpack(str[start:end])
00443 pattern = '<%sd'%length
00444 start = end
00445 end += struct.calcsize(pattern)
00446 _v30.velocity = struct.unpack(pattern, str[start:end])
00447 start = end
00448 end += 4
00449 (length,) = _struct_I.unpack(str[start:end])
00450 pattern = '<%sd'%length
00451 start = end
00452 end += struct.calcsize(pattern)
00453 _v30.effort = struct.unpack(pattern, str[start:end])
00454 _v33 = _v29.multi_dof_joint_state
00455 _v34 = _v33.stamp
00456 _x = _v34
00457 start = end
00458 end += 8
00459 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
00460 start = end
00461 end += 4
00462 (length,) = _struct_I.unpack(str[start:end])
00463 _v33.joint_names = []
00464 for i in range(0, length):
00465 start = end
00466 end += 4
00467 (length,) = _struct_I.unpack(str[start:end])
00468 start = end
00469 end += length
00470 if python3:
00471 val4 = str[start:end].decode('utf-8')
00472 else:
00473 val4 = str[start:end]
00474 _v33.joint_names.append(val4)
00475 start = end
00476 end += 4
00477 (length,) = _struct_I.unpack(str[start:end])
00478 _v33.frame_ids = []
00479 for i in range(0, length):
00480 start = end
00481 end += 4
00482 (length,) = _struct_I.unpack(str[start:end])
00483 start = end
00484 end += length
00485 if python3:
00486 val4 = str[start:end].decode('utf-8')
00487 else:
00488 val4 = str[start:end]
00489 _v33.frame_ids.append(val4)
00490 start = end
00491 end += 4
00492 (length,) = _struct_I.unpack(str[start:end])
00493 _v33.child_frame_ids = []
00494 for i in range(0, length):
00495 start = end
00496 end += 4
00497 (length,) = _struct_I.unpack(str[start:end])
00498 start = end
00499 end += length
00500 if python3:
00501 val4 = str[start:end].decode('utf-8')
00502 else:
00503 val4 = str[start:end]
00504 _v33.child_frame_ids.append(val4)
00505 start = end
00506 end += 4
00507 (length,) = _struct_I.unpack(str[start:end])
00508 _v33.poses = []
00509 for i in range(0, length):
00510 val4 = geometry_msgs.msg.Pose()
00511 _v35 = val4.position
00512 _x = _v35
00513 start = end
00514 end += 24
00515 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
00516 _v36 = val4.orientation
00517 _x = _v36
00518 start = end
00519 end += 32
00520 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
00521 _v33.poses.append(val4)
00522 _v37 = val1.robot_state
00523 _v38 = _v37.joint_state
00524 _v39 = _v38.header
00525 start = end
00526 end += 4
00527 (_v39.seq,) = _struct_I.unpack(str[start:end])
00528 _v40 = _v39.stamp
00529 _x = _v40
00530 start = end
00531 end += 8
00532 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
00533 start = end
00534 end += 4
00535 (length,) = _struct_I.unpack(str[start:end])
00536 start = end
00537 end += length
00538 if python3:
00539 _v39.frame_id = str[start:end].decode('utf-8')
00540 else:
00541 _v39.frame_id = str[start:end]
00542 start = end
00543 end += 4
00544 (length,) = _struct_I.unpack(str[start:end])
00545 _v38.name = []
00546 for i in range(0, length):
00547 start = end
00548 end += 4
00549 (length,) = _struct_I.unpack(str[start:end])
00550 start = end
00551 end += length
00552 if python3:
00553 val4 = str[start:end].decode('utf-8')
00554 else:
00555 val4 = str[start:end]
00556 _v38.name.append(val4)
00557 start = end
00558 end += 4
00559 (length,) = _struct_I.unpack(str[start:end])
00560 pattern = '<%sd'%length
00561 start = end
00562 end += struct.calcsize(pattern)
00563 _v38.position = struct.unpack(pattern, str[start:end])
00564 start = end
00565 end += 4
00566 (length,) = _struct_I.unpack(str[start:end])
00567 pattern = '<%sd'%length
00568 start = end
00569 end += struct.calcsize(pattern)
00570 _v38.velocity = struct.unpack(pattern, str[start:end])
00571 start = end
00572 end += 4
00573 (length,) = _struct_I.unpack(str[start:end])
00574 pattern = '<%sd'%length
00575 start = end
00576 end += struct.calcsize(pattern)
00577 _v38.effort = struct.unpack(pattern, str[start:end])
00578 _v41 = _v37.multi_dof_joint_state
00579 _v42 = _v41.stamp
00580 _x = _v42
00581 start = end
00582 end += 8
00583 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
00584 start = end
00585 end += 4
00586 (length,) = _struct_I.unpack(str[start:end])
00587 _v41.joint_names = []
00588 for i in range(0, length):
00589 start = end
00590 end += 4
00591 (length,) = _struct_I.unpack(str[start:end])
00592 start = end
00593 end += length
00594 if python3:
00595 val4 = str[start:end].decode('utf-8')
00596 else:
00597 val4 = str[start:end]
00598 _v41.joint_names.append(val4)
00599 start = end
00600 end += 4
00601 (length,) = _struct_I.unpack(str[start:end])
00602 _v41.frame_ids = []
00603 for i in range(0, length):
00604 start = end
00605 end += 4
00606 (length,) = _struct_I.unpack(str[start:end])
00607 start = end
00608 end += length
00609 if python3:
00610 val4 = str[start:end].decode('utf-8')
00611 else:
00612 val4 = str[start:end]
00613 _v41.frame_ids.append(val4)
00614 start = end
00615 end += 4
00616 (length,) = _struct_I.unpack(str[start:end])
00617 _v41.child_frame_ids = []
00618 for i in range(0, length):
00619 start = end
00620 end += 4
00621 (length,) = _struct_I.unpack(str[start:end])
00622 start = end
00623 end += length
00624 if python3:
00625 val4 = str[start:end].decode('utf-8')
00626 else:
00627 val4 = str[start:end]
00628 _v41.child_frame_ids.append(val4)
00629 start = end
00630 end += 4
00631 (length,) = _struct_I.unpack(str[start:end])
00632 _v41.poses = []
00633 for i in range(0, length):
00634 val4 = geometry_msgs.msg.Pose()
00635 _v43 = val4.position
00636 _x = _v43
00637 start = end
00638 end += 24
00639 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
00640 _v44 = val4.orientation
00641 _x = _v44
00642 start = end
00643 end += 32
00644 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
00645 _v41.poses.append(val4)
00646 self.pos_ik_request.append(val1)
00647 return self
00648 except struct.error as e:
00649 raise genpy.DeserializationError(e)
00650
00651
00652 def serialize_numpy(self, buff, numpy):
00653 """
00654 serialize message with numpy array types into buffer
00655 :param buff: buffer, ``StringIO``
00656 :param numpy: numpy python module
00657 """
00658 try:
00659 length = len(self.pos_ik_request)
00660 buff.write(_struct_I.pack(length))
00661 for val1 in self.pos_ik_request:
00662 _x = val1.ik_link_name
00663 length = len(_x)
00664 if python3 or type(_x) == unicode:
00665 _x = _x.encode('utf-8')
00666 length = len(_x)
00667 buff.write(struct.pack('<I%ss'%length, length, _x))
00668 _v45 = val1.pose_stamped
00669 _v46 = _v45.header
00670 buff.write(_struct_I.pack(_v46.seq))
00671 _v47 = _v46.stamp
00672 _x = _v47
00673 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
00674 _x = _v46.frame_id
00675 length = len(_x)
00676 if python3 or type(_x) == unicode:
00677 _x = _x.encode('utf-8')
00678 length = len(_x)
00679 buff.write(struct.pack('<I%ss'%length, length, _x))
00680 _v48 = _v45.pose
00681 _v49 = _v48.position
00682 _x = _v49
00683 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00684 _v50 = _v48.orientation
00685 _x = _v50
00686 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
00687 _v51 = val1.ik_seed_state
00688 _v52 = _v51.joint_state
00689 _v53 = _v52.header
00690 buff.write(_struct_I.pack(_v53.seq))
00691 _v54 = _v53.stamp
00692 _x = _v54
00693 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
00694 _x = _v53.frame_id
00695 length = len(_x)
00696 if python3 or type(_x) == unicode:
00697 _x = _x.encode('utf-8')
00698 length = len(_x)
00699 buff.write(struct.pack('<I%ss'%length, length, _x))
00700 length = len(_v52.name)
00701 buff.write(_struct_I.pack(length))
00702 for val4 in _v52.name:
00703 length = len(val4)
00704 if python3 or type(val4) == unicode:
00705 val4 = val4.encode('utf-8')
00706 length = len(val4)
00707 buff.write(struct.pack('<I%ss'%length, length, val4))
00708 length = len(_v52.position)
00709 buff.write(_struct_I.pack(length))
00710 pattern = '<%sd'%length
00711 buff.write(_v52.position.tostring())
00712 length = len(_v52.velocity)
00713 buff.write(_struct_I.pack(length))
00714 pattern = '<%sd'%length
00715 buff.write(_v52.velocity.tostring())
00716 length = len(_v52.effort)
00717 buff.write(_struct_I.pack(length))
00718 pattern = '<%sd'%length
00719 buff.write(_v52.effort.tostring())
00720 _v55 = _v51.multi_dof_joint_state
00721 _v56 = _v55.stamp
00722 _x = _v56
00723 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
00724 length = len(_v55.joint_names)
00725 buff.write(_struct_I.pack(length))
00726 for val4 in _v55.joint_names:
00727 length = len(val4)
00728 if python3 or type(val4) == unicode:
00729 val4 = val4.encode('utf-8')
00730 length = len(val4)
00731 buff.write(struct.pack('<I%ss'%length, length, val4))
00732 length = len(_v55.frame_ids)
00733 buff.write(_struct_I.pack(length))
00734 for val4 in _v55.frame_ids:
00735 length = len(val4)
00736 if python3 or type(val4) == unicode:
00737 val4 = val4.encode('utf-8')
00738 length = len(val4)
00739 buff.write(struct.pack('<I%ss'%length, length, val4))
00740 length = len(_v55.child_frame_ids)
00741 buff.write(_struct_I.pack(length))
00742 for val4 in _v55.child_frame_ids:
00743 length = len(val4)
00744 if python3 or type(val4) == unicode:
00745 val4 = val4.encode('utf-8')
00746 length = len(val4)
00747 buff.write(struct.pack('<I%ss'%length, length, val4))
00748 length = len(_v55.poses)
00749 buff.write(_struct_I.pack(length))
00750 for val4 in _v55.poses:
00751 _v57 = val4.position
00752 _x = _v57
00753 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00754 _v58 = val4.orientation
00755 _x = _v58
00756 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
00757 _v59 = val1.robot_state
00758 _v60 = _v59.joint_state
00759 _v61 = _v60.header
00760 buff.write(_struct_I.pack(_v61.seq))
00761 _v62 = _v61.stamp
00762 _x = _v62
00763 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
00764 _x = _v61.frame_id
00765 length = len(_x)
00766 if python3 or type(_x) == unicode:
00767 _x = _x.encode('utf-8')
00768 length = len(_x)
00769 buff.write(struct.pack('<I%ss'%length, length, _x))
00770 length = len(_v60.name)
00771 buff.write(_struct_I.pack(length))
00772 for val4 in _v60.name:
00773 length = len(val4)
00774 if python3 or type(val4) == unicode:
00775 val4 = val4.encode('utf-8')
00776 length = len(val4)
00777 buff.write(struct.pack('<I%ss'%length, length, val4))
00778 length = len(_v60.position)
00779 buff.write(_struct_I.pack(length))
00780 pattern = '<%sd'%length
00781 buff.write(_v60.position.tostring())
00782 length = len(_v60.velocity)
00783 buff.write(_struct_I.pack(length))
00784 pattern = '<%sd'%length
00785 buff.write(_v60.velocity.tostring())
00786 length = len(_v60.effort)
00787 buff.write(_struct_I.pack(length))
00788 pattern = '<%sd'%length
00789 buff.write(_v60.effort.tostring())
00790 _v63 = _v59.multi_dof_joint_state
00791 _v64 = _v63.stamp
00792 _x = _v64
00793 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
00794 length = len(_v63.joint_names)
00795 buff.write(_struct_I.pack(length))
00796 for val4 in _v63.joint_names:
00797 length = len(val4)
00798 if python3 or type(val4) == unicode:
00799 val4 = val4.encode('utf-8')
00800 length = len(val4)
00801 buff.write(struct.pack('<I%ss'%length, length, val4))
00802 length = len(_v63.frame_ids)
00803 buff.write(_struct_I.pack(length))
00804 for val4 in _v63.frame_ids:
00805 length = len(val4)
00806 if python3 or type(val4) == unicode:
00807 val4 = val4.encode('utf-8')
00808 length = len(val4)
00809 buff.write(struct.pack('<I%ss'%length, length, val4))
00810 length = len(_v63.child_frame_ids)
00811 buff.write(_struct_I.pack(length))
00812 for val4 in _v63.child_frame_ids:
00813 length = len(val4)
00814 if python3 or type(val4) == unicode:
00815 val4 = val4.encode('utf-8')
00816 length = len(val4)
00817 buff.write(struct.pack('<I%ss'%length, length, val4))
00818 length = len(_v63.poses)
00819 buff.write(_struct_I.pack(length))
00820 for val4 in _v63.poses:
00821 _v65 = val4.position
00822 _x = _v65
00823 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00824 _v66 = val4.orientation
00825 _x = _v66
00826 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
00827 except struct.error as se: self._check_types(se)
00828 except TypeError as te: self._check_types(te)
00829
00830 def deserialize_numpy(self, str, numpy):
00831 """
00832 unpack serialized message in str into this message instance using numpy for array types
00833 :param str: byte array of serialized message, ``str``
00834 :param numpy: numpy python module
00835 """
00836 try:
00837 if self.pos_ik_request is None:
00838 self.pos_ik_request = None
00839 end = 0
00840 start = end
00841 end += 4
00842 (length,) = _struct_I.unpack(str[start:end])
00843 self.pos_ik_request = []
00844 for i in range(0, length):
00845 val1 = kinematics_msgs.msg.PositionIKRequest()
00846 start = end
00847 end += 4
00848 (length,) = _struct_I.unpack(str[start:end])
00849 start = end
00850 end += length
00851 if python3:
00852 val1.ik_link_name = str[start:end].decode('utf-8')
00853 else:
00854 val1.ik_link_name = str[start:end]
00855 _v67 = val1.pose_stamped
00856 _v68 = _v67.header
00857 start = end
00858 end += 4
00859 (_v68.seq,) = _struct_I.unpack(str[start:end])
00860 _v69 = _v68.stamp
00861 _x = _v69
00862 start = end
00863 end += 8
00864 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
00865 start = end
00866 end += 4
00867 (length,) = _struct_I.unpack(str[start:end])
00868 start = end
00869 end += length
00870 if python3:
00871 _v68.frame_id = str[start:end].decode('utf-8')
00872 else:
00873 _v68.frame_id = str[start:end]
00874 _v70 = _v67.pose
00875 _v71 = _v70.position
00876 _x = _v71
00877 start = end
00878 end += 24
00879 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
00880 _v72 = _v70.orientation
00881 _x = _v72
00882 start = end
00883 end += 32
00884 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
00885 _v73 = val1.ik_seed_state
00886 _v74 = _v73.joint_state
00887 _v75 = _v74.header
00888 start = end
00889 end += 4
00890 (_v75.seq,) = _struct_I.unpack(str[start:end])
00891 _v76 = _v75.stamp
00892 _x = _v76
00893 start = end
00894 end += 8
00895 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
00896 start = end
00897 end += 4
00898 (length,) = _struct_I.unpack(str[start:end])
00899 start = end
00900 end += length
00901 if python3:
00902 _v75.frame_id = str[start:end].decode('utf-8')
00903 else:
00904 _v75.frame_id = str[start:end]
00905 start = end
00906 end += 4
00907 (length,) = _struct_I.unpack(str[start:end])
00908 _v74.name = []
00909 for i in range(0, length):
00910 start = end
00911 end += 4
00912 (length,) = _struct_I.unpack(str[start:end])
00913 start = end
00914 end += length
00915 if python3:
00916 val4 = str[start:end].decode('utf-8')
00917 else:
00918 val4 = str[start:end]
00919 _v74.name.append(val4)
00920 start = end
00921 end += 4
00922 (length,) = _struct_I.unpack(str[start:end])
00923 pattern = '<%sd'%length
00924 start = end
00925 end += struct.calcsize(pattern)
00926 _v74.position = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
00927 start = end
00928 end += 4
00929 (length,) = _struct_I.unpack(str[start:end])
00930 pattern = '<%sd'%length
00931 start = end
00932 end += struct.calcsize(pattern)
00933 _v74.velocity = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
00934 start = end
00935 end += 4
00936 (length,) = _struct_I.unpack(str[start:end])
00937 pattern = '<%sd'%length
00938 start = end
00939 end += struct.calcsize(pattern)
00940 _v74.effort = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
00941 _v77 = _v73.multi_dof_joint_state
00942 _v78 = _v77.stamp
00943 _x = _v78
00944 start = end
00945 end += 8
00946 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
00947 start = end
00948 end += 4
00949 (length,) = _struct_I.unpack(str[start:end])
00950 _v77.joint_names = []
00951 for i in range(0, length):
00952 start = end
00953 end += 4
00954 (length,) = _struct_I.unpack(str[start:end])
00955 start = end
00956 end += length
00957 if python3:
00958 val4 = str[start:end].decode('utf-8')
00959 else:
00960 val4 = str[start:end]
00961 _v77.joint_names.append(val4)
00962 start = end
00963 end += 4
00964 (length,) = _struct_I.unpack(str[start:end])
00965 _v77.frame_ids = []
00966 for i in range(0, length):
00967 start = end
00968 end += 4
00969 (length,) = _struct_I.unpack(str[start:end])
00970 start = end
00971 end += length
00972 if python3:
00973 val4 = str[start:end].decode('utf-8')
00974 else:
00975 val4 = str[start:end]
00976 _v77.frame_ids.append(val4)
00977 start = end
00978 end += 4
00979 (length,) = _struct_I.unpack(str[start:end])
00980 _v77.child_frame_ids = []
00981 for i in range(0, length):
00982 start = end
00983 end += 4
00984 (length,) = _struct_I.unpack(str[start:end])
00985 start = end
00986 end += length
00987 if python3:
00988 val4 = str[start:end].decode('utf-8')
00989 else:
00990 val4 = str[start:end]
00991 _v77.child_frame_ids.append(val4)
00992 start = end
00993 end += 4
00994 (length,) = _struct_I.unpack(str[start:end])
00995 _v77.poses = []
00996 for i in range(0, length):
00997 val4 = geometry_msgs.msg.Pose()
00998 _v79 = val4.position
00999 _x = _v79
01000 start = end
01001 end += 24
01002 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
01003 _v80 = val4.orientation
01004 _x = _v80
01005 start = end
01006 end += 32
01007 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
01008 _v77.poses.append(val4)
01009 _v81 = val1.robot_state
01010 _v82 = _v81.joint_state
01011 _v83 = _v82.header
01012 start = end
01013 end += 4
01014 (_v83.seq,) = _struct_I.unpack(str[start:end])
01015 _v84 = _v83.stamp
01016 _x = _v84
01017 start = end
01018 end += 8
01019 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
01020 start = end
01021 end += 4
01022 (length,) = _struct_I.unpack(str[start:end])
01023 start = end
01024 end += length
01025 if python3:
01026 _v83.frame_id = str[start:end].decode('utf-8')
01027 else:
01028 _v83.frame_id = str[start:end]
01029 start = end
01030 end += 4
01031 (length,) = _struct_I.unpack(str[start:end])
01032 _v82.name = []
01033 for i in range(0, length):
01034 start = end
01035 end += 4
01036 (length,) = _struct_I.unpack(str[start:end])
01037 start = end
01038 end += length
01039 if python3:
01040 val4 = str[start:end].decode('utf-8')
01041 else:
01042 val4 = str[start:end]
01043 _v82.name.append(val4)
01044 start = end
01045 end += 4
01046 (length,) = _struct_I.unpack(str[start:end])
01047 pattern = '<%sd'%length
01048 start = end
01049 end += struct.calcsize(pattern)
01050 _v82.position = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
01051 start = end
01052 end += 4
01053 (length,) = _struct_I.unpack(str[start:end])
01054 pattern = '<%sd'%length
01055 start = end
01056 end += struct.calcsize(pattern)
01057 _v82.velocity = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
01058 start = end
01059 end += 4
01060 (length,) = _struct_I.unpack(str[start:end])
01061 pattern = '<%sd'%length
01062 start = end
01063 end += struct.calcsize(pattern)
01064 _v82.effort = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
01065 _v85 = _v81.multi_dof_joint_state
01066 _v86 = _v85.stamp
01067 _x = _v86
01068 start = end
01069 end += 8
01070 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
01071 start = end
01072 end += 4
01073 (length,) = _struct_I.unpack(str[start:end])
01074 _v85.joint_names = []
01075 for i in range(0, length):
01076 start = end
01077 end += 4
01078 (length,) = _struct_I.unpack(str[start:end])
01079 start = end
01080 end += length
01081 if python3:
01082 val4 = str[start:end].decode('utf-8')
01083 else:
01084 val4 = str[start:end]
01085 _v85.joint_names.append(val4)
01086 start = end
01087 end += 4
01088 (length,) = _struct_I.unpack(str[start:end])
01089 _v85.frame_ids = []
01090 for i in range(0, length):
01091 start = end
01092 end += 4
01093 (length,) = _struct_I.unpack(str[start:end])
01094 start = end
01095 end += length
01096 if python3:
01097 val4 = str[start:end].decode('utf-8')
01098 else:
01099 val4 = str[start:end]
01100 _v85.frame_ids.append(val4)
01101 start = end
01102 end += 4
01103 (length,) = _struct_I.unpack(str[start:end])
01104 _v85.child_frame_ids = []
01105 for i in range(0, length):
01106 start = end
01107 end += 4
01108 (length,) = _struct_I.unpack(str[start:end])
01109 start = end
01110 end += length
01111 if python3:
01112 val4 = str[start:end].decode('utf-8')
01113 else:
01114 val4 = str[start:end]
01115 _v85.child_frame_ids.append(val4)
01116 start = end
01117 end += 4
01118 (length,) = _struct_I.unpack(str[start:end])
01119 _v85.poses = []
01120 for i in range(0, length):
01121 val4 = geometry_msgs.msg.Pose()
01122 _v87 = val4.position
01123 _x = _v87
01124 start = end
01125 end += 24
01126 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
01127 _v88 = val4.orientation
01128 _x = _v88
01129 start = end
01130 end += 32
01131 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
01132 _v85.poses.append(val4)
01133 self.pos_ik_request.append(val1)
01134 return self
01135 except struct.error as e:
01136 raise genpy.DeserializationError(e)
01137
01138 _struct_I = genpy.struct_I
01139 _struct_4d = struct.Struct("<4d")
01140 _struct_2I = struct.Struct("<2I")
01141 _struct_3d = struct.Struct("<3d")
01142 """autogenerated by genpy from tree_kinematics/get_tree_position_ikResponse.msg. Do not edit."""
01143 import sys
01144 python3 = True if sys.hexversion > 0x03000000 else False
01145 import genpy
01146 import struct
01147
01148 import arm_navigation_msgs.msg
01149 import geometry_msgs.msg
01150 import std_msgs.msg
01151 import genpy
01152 import sensor_msgs.msg
01153
01154 class get_tree_position_ikResponse(genpy.Message):
01155 _md5sum = "5a8bbc4eb2775fe00cbd09858fd3dc65"
01156 _type = "tree_kinematics/get_tree_position_ikResponse"
01157 _has_header = False
01158 _full_text = """
01159
01160
01161 arm_navigation_msgs/RobotState solution
01162
01163 arm_navigation_msgs/ArmNavigationErrorCodes error_code
01164
01165
01166 ================================================================================
01167 MSG: arm_navigation_msgs/RobotState
01168 # This message contains information about the robot state, i.e. the positions of its joints and links
01169 sensor_msgs/JointState joint_state
01170 arm_navigation_msgs/MultiDOFJointState multi_dof_joint_state
01171
01172 ================================================================================
01173 MSG: sensor_msgs/JointState
01174 # This is a message that holds data to describe the state of a set of torque controlled joints.
01175 #
01176 # The state of each joint (revolute or prismatic) is defined by:
01177 # * the position of the joint (rad or m),
01178 # * the velocity of the joint (rad/s or m/s) and
01179 # * the effort that is applied in the joint (Nm or N).
01180 #
01181 # Each joint is uniquely identified by its name
01182 # The header specifies the time at which the joint states were recorded. All the joint states
01183 # in one message have to be recorded at the same time.
01184 #
01185 # This message consists of a multiple arrays, one for each part of the joint state.
01186 # The goal is to make each of the fields optional. When e.g. your joints have no
01187 # effort associated with them, you can leave the effort array empty.
01188 #
01189 # All arrays in this message should have the same size, or be empty.
01190 # This is the only way to uniquely associate the joint name with the correct
01191 # states.
01192
01193
01194 Header header
01195
01196 string[] name
01197 float64[] position
01198 float64[] velocity
01199 float64[] effort
01200
01201 ================================================================================
01202 MSG: std_msgs/Header
01203 # Standard metadata for higher-level stamped data types.
01204 # This is generally used to communicate timestamped data
01205 # in a particular coordinate frame.
01206 #
01207 # sequence ID: consecutively increasing ID
01208 uint32 seq
01209 #Two-integer timestamp that is expressed as:
01210 # * stamp.secs: seconds (stamp_secs) since epoch
01211 # * stamp.nsecs: nanoseconds since stamp_secs
01212 # time-handling sugar is provided by the client library
01213 time stamp
01214 #Frame this data is associated with
01215 # 0: no frame
01216 # 1: global frame
01217 string frame_id
01218
01219 ================================================================================
01220 MSG: arm_navigation_msgs/MultiDOFJointState
01221 #A representation of a multi-dof joint state
01222 time stamp
01223 string[] joint_names
01224 string[] frame_ids
01225 string[] child_frame_ids
01226 geometry_msgs/Pose[] poses
01227
01228 ================================================================================
01229 MSG: geometry_msgs/Pose
01230 # A representation of pose in free space, composed of postion and orientation.
01231 Point position
01232 Quaternion orientation
01233
01234 ================================================================================
01235 MSG: geometry_msgs/Point
01236 # This contains the position of a point in free space
01237 float64 x
01238 float64 y
01239 float64 z
01240
01241 ================================================================================
01242 MSG: geometry_msgs/Quaternion
01243 # This represents an orientation in free space in quaternion form.
01244
01245 float64 x
01246 float64 y
01247 float64 z
01248 float64 w
01249
01250 ================================================================================
01251 MSG: arm_navigation_msgs/ArmNavigationErrorCodes
01252 int32 val
01253
01254 # overall behavior
01255 int32 PLANNING_FAILED=-1
01256 int32 SUCCESS=1
01257 int32 TIMED_OUT=-2
01258
01259 # start state errors
01260 int32 START_STATE_IN_COLLISION=-3
01261 int32 START_STATE_VIOLATES_PATH_CONSTRAINTS=-4
01262
01263 # goal errors
01264 int32 GOAL_IN_COLLISION=-5
01265 int32 GOAL_VIOLATES_PATH_CONSTRAINTS=-6
01266
01267 # robot state
01268 int32 INVALID_ROBOT_STATE=-7
01269 int32 INCOMPLETE_ROBOT_STATE=-8
01270
01271 # planning request errors
01272 int32 INVALID_PLANNER_ID=-9
01273 int32 INVALID_NUM_PLANNING_ATTEMPTS=-10
01274 int32 INVALID_ALLOWED_PLANNING_TIME=-11
01275 int32 INVALID_GROUP_NAME=-12
01276 int32 INVALID_GOAL_JOINT_CONSTRAINTS=-13
01277 int32 INVALID_GOAL_POSITION_CONSTRAINTS=-14
01278 int32 INVALID_GOAL_ORIENTATION_CONSTRAINTS=-15
01279 int32 INVALID_PATH_JOINT_CONSTRAINTS=-16
01280 int32 INVALID_PATH_POSITION_CONSTRAINTS=-17
01281 int32 INVALID_PATH_ORIENTATION_CONSTRAINTS=-18
01282
01283 # state/trajectory monitor errors
01284 int32 INVALID_TRAJECTORY=-19
01285 int32 INVALID_INDEX=-20
01286 int32 JOINT_LIMITS_VIOLATED=-21
01287 int32 PATH_CONSTRAINTS_VIOLATED=-22
01288 int32 COLLISION_CONSTRAINTS_VIOLATED=-23
01289 int32 GOAL_CONSTRAINTS_VIOLATED=-24
01290 int32 JOINTS_NOT_MOVING=-25
01291 int32 TRAJECTORY_CONTROLLER_FAILED=-26
01292
01293 # system errors
01294 int32 FRAME_TRANSFORM_FAILURE=-27
01295 int32 COLLISION_CHECKING_UNAVAILABLE=-28
01296 int32 ROBOT_STATE_STALE=-29
01297 int32 SENSOR_INFO_STALE=-30
01298
01299 # kinematics errors
01300 int32 NO_IK_SOLUTION=-31
01301 int32 INVALID_LINK_NAME=-32
01302 int32 IK_LINK_IN_COLLISION=-33
01303 int32 NO_FK_SOLUTION=-34
01304 int32 KINEMATICS_STATE_IN_COLLISION=-35
01305
01306 # general errors
01307 int32 INVALID_TIMEOUT=-36
01308
01309
01310 """
01311 __slots__ = ['solution','error_code']
01312 _slot_types = ['arm_navigation_msgs/RobotState','arm_navigation_msgs/ArmNavigationErrorCodes']
01313
01314 def __init__(self, *args, **kwds):
01315 """
01316 Constructor. Any message fields that are implicitly/explicitly
01317 set to None will be assigned a default value. The recommend
01318 use is keyword arguments as this is more robust to future message
01319 changes. You cannot mix in-order arguments and keyword arguments.
01320
01321 The available fields are:
01322 solution,error_code
01323
01324 :param args: complete set of field values, in .msg order
01325 :param kwds: use keyword arguments corresponding to message field names
01326 to set specific fields.
01327 """
01328 if args or kwds:
01329 super(get_tree_position_ikResponse, self).__init__(*args, **kwds)
01330
01331 if self.solution is None:
01332 self.solution = arm_navigation_msgs.msg.RobotState()
01333 if self.error_code is None:
01334 self.error_code = arm_navigation_msgs.msg.ArmNavigationErrorCodes()
01335 else:
01336 self.solution = arm_navigation_msgs.msg.RobotState()
01337 self.error_code = arm_navigation_msgs.msg.ArmNavigationErrorCodes()
01338
01339 def _get_types(self):
01340 """
01341 internal API method
01342 """
01343 return self._slot_types
01344
01345 def serialize(self, buff):
01346 """
01347 serialize message into buffer
01348 :param buff: buffer, ``StringIO``
01349 """
01350 try:
01351 _x = self
01352 buff.write(_struct_3I.pack(_x.solution.joint_state.header.seq, _x.solution.joint_state.header.stamp.secs, _x.solution.joint_state.header.stamp.nsecs))
01353 _x = self.solution.joint_state.header.frame_id
01354 length = len(_x)
01355 if python3 or type(_x) == unicode:
01356 _x = _x.encode('utf-8')
01357 length = len(_x)
01358 buff.write(struct.pack('<I%ss'%length, length, _x))
01359 length = len(self.solution.joint_state.name)
01360 buff.write(_struct_I.pack(length))
01361 for val1 in self.solution.joint_state.name:
01362 length = len(val1)
01363 if python3 or type(val1) == unicode:
01364 val1 = val1.encode('utf-8')
01365 length = len(val1)
01366 buff.write(struct.pack('<I%ss'%length, length, val1))
01367 length = len(self.solution.joint_state.position)
01368 buff.write(_struct_I.pack(length))
01369 pattern = '<%sd'%length
01370 buff.write(struct.pack(pattern, *self.solution.joint_state.position))
01371 length = len(self.solution.joint_state.velocity)
01372 buff.write(_struct_I.pack(length))
01373 pattern = '<%sd'%length
01374 buff.write(struct.pack(pattern, *self.solution.joint_state.velocity))
01375 length = len(self.solution.joint_state.effort)
01376 buff.write(_struct_I.pack(length))
01377 pattern = '<%sd'%length
01378 buff.write(struct.pack(pattern, *self.solution.joint_state.effort))
01379 _x = self
01380 buff.write(_struct_2I.pack(_x.solution.multi_dof_joint_state.stamp.secs, _x.solution.multi_dof_joint_state.stamp.nsecs))
01381 length = len(self.solution.multi_dof_joint_state.joint_names)
01382 buff.write(_struct_I.pack(length))
01383 for val1 in self.solution.multi_dof_joint_state.joint_names:
01384 length = len(val1)
01385 if python3 or type(val1) == unicode:
01386 val1 = val1.encode('utf-8')
01387 length = len(val1)
01388 buff.write(struct.pack('<I%ss'%length, length, val1))
01389 length = len(self.solution.multi_dof_joint_state.frame_ids)
01390 buff.write(_struct_I.pack(length))
01391 for val1 in self.solution.multi_dof_joint_state.frame_ids:
01392 length = len(val1)
01393 if python3 or type(val1) == unicode:
01394 val1 = val1.encode('utf-8')
01395 length = len(val1)
01396 buff.write(struct.pack('<I%ss'%length, length, val1))
01397 length = len(self.solution.multi_dof_joint_state.child_frame_ids)
01398 buff.write(_struct_I.pack(length))
01399 for val1 in self.solution.multi_dof_joint_state.child_frame_ids:
01400 length = len(val1)
01401 if python3 or type(val1) == unicode:
01402 val1 = val1.encode('utf-8')
01403 length = len(val1)
01404 buff.write(struct.pack('<I%ss'%length, length, val1))
01405 length = len(self.solution.multi_dof_joint_state.poses)
01406 buff.write(_struct_I.pack(length))
01407 for val1 in self.solution.multi_dof_joint_state.poses:
01408 _v89 = val1.position
01409 _x = _v89
01410 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
01411 _v90 = val1.orientation
01412 _x = _v90
01413 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
01414 buff.write(_struct_i.pack(self.error_code.val))
01415 except struct.error as se: self._check_types(se)
01416 except TypeError as te: self._check_types(te)
01417
01418 def deserialize(self, str):
01419 """
01420 unpack serialized message in str into this message instance
01421 :param str: byte array of serialized message, ``str``
01422 """
01423 try:
01424 if self.solution is None:
01425 self.solution = arm_navigation_msgs.msg.RobotState()
01426 if self.error_code is None:
01427 self.error_code = arm_navigation_msgs.msg.ArmNavigationErrorCodes()
01428 end = 0
01429 _x = self
01430 start = end
01431 end += 12
01432 (_x.solution.joint_state.header.seq, _x.solution.joint_state.header.stamp.secs, _x.solution.joint_state.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
01433 start = end
01434 end += 4
01435 (length,) = _struct_I.unpack(str[start:end])
01436 start = end
01437 end += length
01438 if python3:
01439 self.solution.joint_state.header.frame_id = str[start:end].decode('utf-8')
01440 else:
01441 self.solution.joint_state.header.frame_id = str[start:end]
01442 start = end
01443 end += 4
01444 (length,) = _struct_I.unpack(str[start:end])
01445 self.solution.joint_state.name = []
01446 for i in range(0, length):
01447 start = end
01448 end += 4
01449 (length,) = _struct_I.unpack(str[start:end])
01450 start = end
01451 end += length
01452 if python3:
01453 val1 = str[start:end].decode('utf-8')
01454 else:
01455 val1 = str[start:end]
01456 self.solution.joint_state.name.append(val1)
01457 start = end
01458 end += 4
01459 (length,) = _struct_I.unpack(str[start:end])
01460 pattern = '<%sd'%length
01461 start = end
01462 end += struct.calcsize(pattern)
01463 self.solution.joint_state.position = struct.unpack(pattern, str[start:end])
01464 start = end
01465 end += 4
01466 (length,) = _struct_I.unpack(str[start:end])
01467 pattern = '<%sd'%length
01468 start = end
01469 end += struct.calcsize(pattern)
01470 self.solution.joint_state.velocity = struct.unpack(pattern, str[start:end])
01471 start = end
01472 end += 4
01473 (length,) = _struct_I.unpack(str[start:end])
01474 pattern = '<%sd'%length
01475 start = end
01476 end += struct.calcsize(pattern)
01477 self.solution.joint_state.effort = struct.unpack(pattern, str[start:end])
01478 _x = self
01479 start = end
01480 end += 8
01481 (_x.solution.multi_dof_joint_state.stamp.secs, _x.solution.multi_dof_joint_state.stamp.nsecs,) = _struct_2I.unpack(str[start:end])
01482 start = end
01483 end += 4
01484 (length,) = _struct_I.unpack(str[start:end])
01485 self.solution.multi_dof_joint_state.joint_names = []
01486 for i in range(0, length):
01487 start = end
01488 end += 4
01489 (length,) = _struct_I.unpack(str[start:end])
01490 start = end
01491 end += length
01492 if python3:
01493 val1 = str[start:end].decode('utf-8')
01494 else:
01495 val1 = str[start:end]
01496 self.solution.multi_dof_joint_state.joint_names.append(val1)
01497 start = end
01498 end += 4
01499 (length,) = _struct_I.unpack(str[start:end])
01500 self.solution.multi_dof_joint_state.frame_ids = []
01501 for i in range(0, length):
01502 start = end
01503 end += 4
01504 (length,) = _struct_I.unpack(str[start:end])
01505 start = end
01506 end += length
01507 if python3:
01508 val1 = str[start:end].decode('utf-8')
01509 else:
01510 val1 = str[start:end]
01511 self.solution.multi_dof_joint_state.frame_ids.append(val1)
01512 start = end
01513 end += 4
01514 (length,) = _struct_I.unpack(str[start:end])
01515 self.solution.multi_dof_joint_state.child_frame_ids = []
01516 for i in range(0, length):
01517 start = end
01518 end += 4
01519 (length,) = _struct_I.unpack(str[start:end])
01520 start = end
01521 end += length
01522 if python3:
01523 val1 = str[start:end].decode('utf-8')
01524 else:
01525 val1 = str[start:end]
01526 self.solution.multi_dof_joint_state.child_frame_ids.append(val1)
01527 start = end
01528 end += 4
01529 (length,) = _struct_I.unpack(str[start:end])
01530 self.solution.multi_dof_joint_state.poses = []
01531 for i in range(0, length):
01532 val1 = geometry_msgs.msg.Pose()
01533 _v91 = val1.position
01534 _x = _v91
01535 start = end
01536 end += 24
01537 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
01538 _v92 = val1.orientation
01539 _x = _v92
01540 start = end
01541 end += 32
01542 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
01543 self.solution.multi_dof_joint_state.poses.append(val1)
01544 start = end
01545 end += 4
01546 (self.error_code.val,) = _struct_i.unpack(str[start:end])
01547 return self
01548 except struct.error as e:
01549 raise genpy.DeserializationError(e)
01550
01551
01552 def serialize_numpy(self, buff, numpy):
01553 """
01554 serialize message with numpy array types into buffer
01555 :param buff: buffer, ``StringIO``
01556 :param numpy: numpy python module
01557 """
01558 try:
01559 _x = self
01560 buff.write(_struct_3I.pack(_x.solution.joint_state.header.seq, _x.solution.joint_state.header.stamp.secs, _x.solution.joint_state.header.stamp.nsecs))
01561 _x = self.solution.joint_state.header.frame_id
01562 length = len(_x)
01563 if python3 or type(_x) == unicode:
01564 _x = _x.encode('utf-8')
01565 length = len(_x)
01566 buff.write(struct.pack('<I%ss'%length, length, _x))
01567 length = len(self.solution.joint_state.name)
01568 buff.write(_struct_I.pack(length))
01569 for val1 in self.solution.joint_state.name:
01570 length = len(val1)
01571 if python3 or type(val1) == unicode:
01572 val1 = val1.encode('utf-8')
01573 length = len(val1)
01574 buff.write(struct.pack('<I%ss'%length, length, val1))
01575 length = len(self.solution.joint_state.position)
01576 buff.write(_struct_I.pack(length))
01577 pattern = '<%sd'%length
01578 buff.write(self.solution.joint_state.position.tostring())
01579 length = len(self.solution.joint_state.velocity)
01580 buff.write(_struct_I.pack(length))
01581 pattern = '<%sd'%length
01582 buff.write(self.solution.joint_state.velocity.tostring())
01583 length = len(self.solution.joint_state.effort)
01584 buff.write(_struct_I.pack(length))
01585 pattern = '<%sd'%length
01586 buff.write(self.solution.joint_state.effort.tostring())
01587 _x = self
01588 buff.write(_struct_2I.pack(_x.solution.multi_dof_joint_state.stamp.secs, _x.solution.multi_dof_joint_state.stamp.nsecs))
01589 length = len(self.solution.multi_dof_joint_state.joint_names)
01590 buff.write(_struct_I.pack(length))
01591 for val1 in self.solution.multi_dof_joint_state.joint_names:
01592 length = len(val1)
01593 if python3 or type(val1) == unicode:
01594 val1 = val1.encode('utf-8')
01595 length = len(val1)
01596 buff.write(struct.pack('<I%ss'%length, length, val1))
01597 length = len(self.solution.multi_dof_joint_state.frame_ids)
01598 buff.write(_struct_I.pack(length))
01599 for val1 in self.solution.multi_dof_joint_state.frame_ids:
01600 length = len(val1)
01601 if python3 or type(val1) == unicode:
01602 val1 = val1.encode('utf-8')
01603 length = len(val1)
01604 buff.write(struct.pack('<I%ss'%length, length, val1))
01605 length = len(self.solution.multi_dof_joint_state.child_frame_ids)
01606 buff.write(_struct_I.pack(length))
01607 for val1 in self.solution.multi_dof_joint_state.child_frame_ids:
01608 length = len(val1)
01609 if python3 or type(val1) == unicode:
01610 val1 = val1.encode('utf-8')
01611 length = len(val1)
01612 buff.write(struct.pack('<I%ss'%length, length, val1))
01613 length = len(self.solution.multi_dof_joint_state.poses)
01614 buff.write(_struct_I.pack(length))
01615 for val1 in self.solution.multi_dof_joint_state.poses:
01616 _v93 = val1.position
01617 _x = _v93
01618 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
01619 _v94 = val1.orientation
01620 _x = _v94
01621 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
01622 buff.write(_struct_i.pack(self.error_code.val))
01623 except struct.error as se: self._check_types(se)
01624 except TypeError as te: self._check_types(te)
01625
01626 def deserialize_numpy(self, str, numpy):
01627 """
01628 unpack serialized message in str into this message instance using numpy for array types
01629 :param str: byte array of serialized message, ``str``
01630 :param numpy: numpy python module
01631 """
01632 try:
01633 if self.solution is None:
01634 self.solution = arm_navigation_msgs.msg.RobotState()
01635 if self.error_code is None:
01636 self.error_code = arm_navigation_msgs.msg.ArmNavigationErrorCodes()
01637 end = 0
01638 _x = self
01639 start = end
01640 end += 12
01641 (_x.solution.joint_state.header.seq, _x.solution.joint_state.header.stamp.secs, _x.solution.joint_state.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
01642 start = end
01643 end += 4
01644 (length,) = _struct_I.unpack(str[start:end])
01645 start = end
01646 end += length
01647 if python3:
01648 self.solution.joint_state.header.frame_id = str[start:end].decode('utf-8')
01649 else:
01650 self.solution.joint_state.header.frame_id = str[start:end]
01651 start = end
01652 end += 4
01653 (length,) = _struct_I.unpack(str[start:end])
01654 self.solution.joint_state.name = []
01655 for i in range(0, length):
01656 start = end
01657 end += 4
01658 (length,) = _struct_I.unpack(str[start:end])
01659 start = end
01660 end += length
01661 if python3:
01662 val1 = str[start:end].decode('utf-8')
01663 else:
01664 val1 = str[start:end]
01665 self.solution.joint_state.name.append(val1)
01666 start = end
01667 end += 4
01668 (length,) = _struct_I.unpack(str[start:end])
01669 pattern = '<%sd'%length
01670 start = end
01671 end += struct.calcsize(pattern)
01672 self.solution.joint_state.position = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
01673 start = end
01674 end += 4
01675 (length,) = _struct_I.unpack(str[start:end])
01676 pattern = '<%sd'%length
01677 start = end
01678 end += struct.calcsize(pattern)
01679 self.solution.joint_state.velocity = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
01680 start = end
01681 end += 4
01682 (length,) = _struct_I.unpack(str[start:end])
01683 pattern = '<%sd'%length
01684 start = end
01685 end += struct.calcsize(pattern)
01686 self.solution.joint_state.effort = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
01687 _x = self
01688 start = end
01689 end += 8
01690 (_x.solution.multi_dof_joint_state.stamp.secs, _x.solution.multi_dof_joint_state.stamp.nsecs,) = _struct_2I.unpack(str[start:end])
01691 start = end
01692 end += 4
01693 (length,) = _struct_I.unpack(str[start:end])
01694 self.solution.multi_dof_joint_state.joint_names = []
01695 for i in range(0, length):
01696 start = end
01697 end += 4
01698 (length,) = _struct_I.unpack(str[start:end])
01699 start = end
01700 end += length
01701 if python3:
01702 val1 = str[start:end].decode('utf-8')
01703 else:
01704 val1 = str[start:end]
01705 self.solution.multi_dof_joint_state.joint_names.append(val1)
01706 start = end
01707 end += 4
01708 (length,) = _struct_I.unpack(str[start:end])
01709 self.solution.multi_dof_joint_state.frame_ids = []
01710 for i in range(0, length):
01711 start = end
01712 end += 4
01713 (length,) = _struct_I.unpack(str[start:end])
01714 start = end
01715 end += length
01716 if python3:
01717 val1 = str[start:end].decode('utf-8')
01718 else:
01719 val1 = str[start:end]
01720 self.solution.multi_dof_joint_state.frame_ids.append(val1)
01721 start = end
01722 end += 4
01723 (length,) = _struct_I.unpack(str[start:end])
01724 self.solution.multi_dof_joint_state.child_frame_ids = []
01725 for i in range(0, length):
01726 start = end
01727 end += 4
01728 (length,) = _struct_I.unpack(str[start:end])
01729 start = end
01730 end += length
01731 if python3:
01732 val1 = str[start:end].decode('utf-8')
01733 else:
01734 val1 = str[start:end]
01735 self.solution.multi_dof_joint_state.child_frame_ids.append(val1)
01736 start = end
01737 end += 4
01738 (length,) = _struct_I.unpack(str[start:end])
01739 self.solution.multi_dof_joint_state.poses = []
01740 for i in range(0, length):
01741 val1 = geometry_msgs.msg.Pose()
01742 _v95 = val1.position
01743 _x = _v95
01744 start = end
01745 end += 24
01746 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
01747 _v96 = val1.orientation
01748 _x = _v96
01749 start = end
01750 end += 32
01751 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
01752 self.solution.multi_dof_joint_state.poses.append(val1)
01753 start = end
01754 end += 4
01755 (self.error_code.val,) = _struct_i.unpack(str[start:end])
01756 return self
01757 except struct.error as e:
01758 raise genpy.DeserializationError(e)
01759
01760 _struct_I = genpy.struct_I
01761 _struct_i = struct.Struct("<i")
01762 _struct_4d = struct.Struct("<4d")
01763 _struct_3I = struct.Struct("<3I")
01764 _struct_2I = struct.Struct("<2I")
01765 _struct_3d = struct.Struct("<3d")
01766 class get_tree_position_ik(object):
01767 _type = 'tree_kinematics/get_tree_position_ik'
01768 _md5sum = '2f9c4754bd93d04c0256c3f144caddff'
01769 _request_class = get_tree_position_ikRequest
01770 _response_class = get_tree_position_ikResponse