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