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