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