00001 """autogenerated by genmsg_py from GraspHandPostureExecutionGoal.msg. Do not edit."""
00002 import roslib.message
00003 import struct
00004
00005 import geometry_msgs.msg
00006 import std_msgs.msg
00007 import object_manipulation_msgs.msg
00008 import sensor_msgs.msg
00009
00010 class GraspHandPostureExecutionGoal(roslib.message.Message):
00011 _md5sum = "f3fac748dba3210f45147d583f26a85c"
00012 _type = "object_manipulation_msgs/GraspHandPostureExecutionGoal"
00013 _has_header = False
00014 _full_text = """# ====== DO NOT MODIFY! AUTOGENERATED FROM AN ACTION DEFINITION ======
00015 # an action for requesting the finger posture part of grasp be physically carried out by a hand
00016 # the name of the arm being used is not in here, as this will be sent to a specific action server
00017 # for each arm
00018
00019 # the grasp to be executed
00020 Grasp grasp
00021
00022 # the goal of this action
00023 # requests that the hand be set in the pre-grasp posture
00024 int32 PRE_GRASP=1
00025 # requests that the hand execute the actual grasp
00026 int32 GRASP=2
00027 # requests that the hand open to release the object
00028 int32 RELEASE=3
00029 int32 goal
00030
00031
00032 ================================================================================
00033 MSG: object_manipulation_msgs/Grasp
00034
00035 # The internal posture of the hand for the pre-grasp
00036 # only positions are used
00037 sensor_msgs/JointState pre_grasp_posture
00038
00039 # The internal posture of the hand for the grasp
00040 # positions and efforts are used
00041 sensor_msgs/JointState grasp_posture
00042
00043 # The position of the end-effector for the grasp relative to the object
00044 geometry_msgs/Pose grasp_pose
00045
00046 # The estimated probability of success for this grasp
00047 float64 success_probability
00048
00049 # Debug flag to indicate that this grasp would be the best in its cluster
00050 bool cluster_rep
00051 ================================================================================
00052 MSG: sensor_msgs/JointState
00053 # This is a message that holds data to describe the state of a set of torque controlled joints.
00054 #
00055 # The state of each joint (revolute or prismatic) is defined by:
00056 # * the position of the joint (rad or m),
00057 # * the velocity of the joint (rad/s or m/s) and
00058 # * the effort that is applied in the joint (Nm or N).
00059 #
00060 # Each joint is uniquely identified by its name
00061 # The header specifies the time at which the joint states were recorded. All the joint states
00062 # in one message have to be recorded at the same time.
00063 #
00064 # This message consists of a multiple arrays, one for each part of the joint state.
00065 # The goal is to make each of the fields optional. When e.g. your joints have no
00066 # effort associated with them, you can leave the effort array empty.
00067 #
00068 # All arrays in this message should have the same size, or be empty.
00069 # This is the only way to uniquely associate the joint name with the correct
00070 # states.
00071
00072
00073 Header header
00074
00075 string[] name
00076 float64[] position
00077 float64[] velocity
00078 float64[] effort
00079
00080 ================================================================================
00081 MSG: std_msgs/Header
00082 # Standard metadata for higher-level stamped data types.
00083 # This is generally used to communicate timestamped data
00084 # in a particular coordinate frame.
00085 #
00086 # sequence ID: consecutively increasing ID
00087 uint32 seq
00088 #Two-integer timestamp that is expressed as:
00089 # * stamp.secs: seconds (stamp_secs) since epoch
00090 # * stamp.nsecs: nanoseconds since stamp_secs
00091 # time-handling sugar is provided by the client library
00092 time stamp
00093 #Frame this data is associated with
00094 # 0: no frame
00095 # 1: global frame
00096 string frame_id
00097
00098 ================================================================================
00099 MSG: geometry_msgs/Pose
00100 # A representation of pose in free space, composed of postion and orientation.
00101 Point position
00102 Quaternion orientation
00103
00104 ================================================================================
00105 MSG: geometry_msgs/Point
00106 # This contains the position of a point in free space
00107 float64 x
00108 float64 y
00109 float64 z
00110
00111 ================================================================================
00112 MSG: geometry_msgs/Quaternion
00113 # This represents an orientation in free space in quaternion form.
00114
00115 float64 x
00116 float64 y
00117 float64 z
00118 float64 w
00119
00120 """
00121
00122 PRE_GRASP = 1
00123 GRASP = 2
00124 RELEASE = 3
00125
00126 __slots__ = ['grasp','goal']
00127 _slot_types = ['object_manipulation_msgs/Grasp','int32']
00128
00129 def __init__(self, *args, **kwds):
00130 """
00131 Constructor. Any message fields that are implicitly/explicitly
00132 set to None will be assigned a default value. The recommend
00133 use is keyword arguments as this is more robust to future message
00134 changes. You cannot mix in-order arguments and keyword arguments.
00135
00136 The available fields are:
00137 grasp,goal
00138
00139 @param args: complete set of field values, in .msg order
00140 @param kwds: use keyword arguments corresponding to message field names
00141 to set specific fields.
00142 """
00143 if args or kwds:
00144 super(GraspHandPostureExecutionGoal, self).__init__(*args, **kwds)
00145
00146 if self.grasp is None:
00147 self.grasp = object_manipulation_msgs.msg.Grasp()
00148 if self.goal is None:
00149 self.goal = 0
00150 else:
00151 self.grasp = object_manipulation_msgs.msg.Grasp()
00152 self.goal = 0
00153
00154 def _get_types(self):
00155 """
00156 internal API method
00157 """
00158 return self._slot_types
00159
00160 def serialize(self, buff):
00161 """
00162 serialize message into buffer
00163 @param buff: buffer
00164 @type buff: StringIO
00165 """
00166 try:
00167 _x = self
00168 buff.write(_struct_3I.pack(_x.grasp.pre_grasp_posture.header.seq, _x.grasp.pre_grasp_posture.header.stamp.secs, _x.grasp.pre_grasp_posture.header.stamp.nsecs))
00169 _x = self.grasp.pre_grasp_posture.header.frame_id
00170 length = len(_x)
00171 buff.write(struct.pack('<I%ss'%length, length, _x))
00172 length = len(self.grasp.pre_grasp_posture.name)
00173 buff.write(_struct_I.pack(length))
00174 for val1 in self.grasp.pre_grasp_posture.name:
00175 length = len(val1)
00176 buff.write(struct.pack('<I%ss'%length, length, val1))
00177 length = len(self.grasp.pre_grasp_posture.position)
00178 buff.write(_struct_I.pack(length))
00179 pattern = '<%sd'%length
00180 buff.write(struct.pack(pattern, *self.grasp.pre_grasp_posture.position))
00181 length = len(self.grasp.pre_grasp_posture.velocity)
00182 buff.write(_struct_I.pack(length))
00183 pattern = '<%sd'%length
00184 buff.write(struct.pack(pattern, *self.grasp.pre_grasp_posture.velocity))
00185 length = len(self.grasp.pre_grasp_posture.effort)
00186 buff.write(_struct_I.pack(length))
00187 pattern = '<%sd'%length
00188 buff.write(struct.pack(pattern, *self.grasp.pre_grasp_posture.effort))
00189 _x = self
00190 buff.write(_struct_3I.pack(_x.grasp.grasp_posture.header.seq, _x.grasp.grasp_posture.header.stamp.secs, _x.grasp.grasp_posture.header.stamp.nsecs))
00191 _x = self.grasp.grasp_posture.header.frame_id
00192 length = len(_x)
00193 buff.write(struct.pack('<I%ss'%length, length, _x))
00194 length = len(self.grasp.grasp_posture.name)
00195 buff.write(_struct_I.pack(length))
00196 for val1 in self.grasp.grasp_posture.name:
00197 length = len(val1)
00198 buff.write(struct.pack('<I%ss'%length, length, val1))
00199 length = len(self.grasp.grasp_posture.position)
00200 buff.write(_struct_I.pack(length))
00201 pattern = '<%sd'%length
00202 buff.write(struct.pack(pattern, *self.grasp.grasp_posture.position))
00203 length = len(self.grasp.grasp_posture.velocity)
00204 buff.write(_struct_I.pack(length))
00205 pattern = '<%sd'%length
00206 buff.write(struct.pack(pattern, *self.grasp.grasp_posture.velocity))
00207 length = len(self.grasp.grasp_posture.effort)
00208 buff.write(_struct_I.pack(length))
00209 pattern = '<%sd'%length
00210 buff.write(struct.pack(pattern, *self.grasp.grasp_posture.effort))
00211 _x = self
00212 buff.write(_struct_8dBi.pack(_x.grasp.grasp_pose.position.x, _x.grasp.grasp_pose.position.y, _x.grasp.grasp_pose.position.z, _x.grasp.grasp_pose.orientation.x, _x.grasp.grasp_pose.orientation.y, _x.grasp.grasp_pose.orientation.z, _x.grasp.grasp_pose.orientation.w, _x.grasp.success_probability, _x.grasp.cluster_rep, _x.goal))
00213 except struct.error, se: self._check_types(se)
00214 except TypeError, te: self._check_types(te)
00215
00216 def deserialize(self, str):
00217 """
00218 unpack serialized message in str into this message instance
00219 @param str: byte array of serialized message
00220 @type str: str
00221 """
00222 try:
00223 if self.grasp is None:
00224 self.grasp = object_manipulation_msgs.msg.Grasp()
00225 end = 0
00226 _x = self
00227 start = end
00228 end += 12
00229 (_x.grasp.pre_grasp_posture.header.seq, _x.grasp.pre_grasp_posture.header.stamp.secs, _x.grasp.pre_grasp_posture.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
00230 start = end
00231 end += 4
00232 (length,) = _struct_I.unpack(str[start:end])
00233 start = end
00234 end += length
00235 self.grasp.pre_grasp_posture.header.frame_id = str[start:end]
00236 start = end
00237 end += 4
00238 (length,) = _struct_I.unpack(str[start:end])
00239 self.grasp.pre_grasp_posture.name = []
00240 for i in xrange(0, length):
00241 start = end
00242 end += 4
00243 (length,) = _struct_I.unpack(str[start:end])
00244 start = end
00245 end += length
00246 val1 = str[start:end]
00247 self.grasp.pre_grasp_posture.name.append(val1)
00248 start = end
00249 end += 4
00250 (length,) = _struct_I.unpack(str[start:end])
00251 pattern = '<%sd'%length
00252 start = end
00253 end += struct.calcsize(pattern)
00254 self.grasp.pre_grasp_posture.position = struct.unpack(pattern, str[start:end])
00255 start = end
00256 end += 4
00257 (length,) = _struct_I.unpack(str[start:end])
00258 pattern = '<%sd'%length
00259 start = end
00260 end += struct.calcsize(pattern)
00261 self.grasp.pre_grasp_posture.velocity = struct.unpack(pattern, str[start:end])
00262 start = end
00263 end += 4
00264 (length,) = _struct_I.unpack(str[start:end])
00265 pattern = '<%sd'%length
00266 start = end
00267 end += struct.calcsize(pattern)
00268 self.grasp.pre_grasp_posture.effort = struct.unpack(pattern, str[start:end])
00269 _x = self
00270 start = end
00271 end += 12
00272 (_x.grasp.grasp_posture.header.seq, _x.grasp.grasp_posture.header.stamp.secs, _x.grasp.grasp_posture.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
00273 start = end
00274 end += 4
00275 (length,) = _struct_I.unpack(str[start:end])
00276 start = end
00277 end += length
00278 self.grasp.grasp_posture.header.frame_id = str[start:end]
00279 start = end
00280 end += 4
00281 (length,) = _struct_I.unpack(str[start:end])
00282 self.grasp.grasp_posture.name = []
00283 for i in xrange(0, length):
00284 start = end
00285 end += 4
00286 (length,) = _struct_I.unpack(str[start:end])
00287 start = end
00288 end += length
00289 val1 = str[start:end]
00290 self.grasp.grasp_posture.name.append(val1)
00291 start = end
00292 end += 4
00293 (length,) = _struct_I.unpack(str[start:end])
00294 pattern = '<%sd'%length
00295 start = end
00296 end += struct.calcsize(pattern)
00297 self.grasp.grasp_posture.position = struct.unpack(pattern, str[start:end])
00298 start = end
00299 end += 4
00300 (length,) = _struct_I.unpack(str[start:end])
00301 pattern = '<%sd'%length
00302 start = end
00303 end += struct.calcsize(pattern)
00304 self.grasp.grasp_posture.velocity = struct.unpack(pattern, str[start:end])
00305 start = end
00306 end += 4
00307 (length,) = _struct_I.unpack(str[start:end])
00308 pattern = '<%sd'%length
00309 start = end
00310 end += struct.calcsize(pattern)
00311 self.grasp.grasp_posture.effort = struct.unpack(pattern, str[start:end])
00312 _x = self
00313 start = end
00314 end += 69
00315 (_x.grasp.grasp_pose.position.x, _x.grasp.grasp_pose.position.y, _x.grasp.grasp_pose.position.z, _x.grasp.grasp_pose.orientation.x, _x.grasp.grasp_pose.orientation.y, _x.grasp.grasp_pose.orientation.z, _x.grasp.grasp_pose.orientation.w, _x.grasp.success_probability, _x.grasp.cluster_rep, _x.goal,) = _struct_8dBi.unpack(str[start:end])
00316 self.grasp.cluster_rep = bool(self.grasp.cluster_rep)
00317 return self
00318 except struct.error, e:
00319 raise roslib.message.DeserializationError(e)
00320
00321
00322 def serialize_numpy(self, buff, numpy):
00323 """
00324 serialize message with numpy array types into buffer
00325 @param buff: buffer
00326 @type buff: StringIO
00327 @param numpy: numpy python module
00328 @type numpy module
00329 """
00330 try:
00331 _x = self
00332 buff.write(_struct_3I.pack(_x.grasp.pre_grasp_posture.header.seq, _x.grasp.pre_grasp_posture.header.stamp.secs, _x.grasp.pre_grasp_posture.header.stamp.nsecs))
00333 _x = self.grasp.pre_grasp_posture.header.frame_id
00334 length = len(_x)
00335 buff.write(struct.pack('<I%ss'%length, length, _x))
00336 length = len(self.grasp.pre_grasp_posture.name)
00337 buff.write(_struct_I.pack(length))
00338 for val1 in self.grasp.pre_grasp_posture.name:
00339 length = len(val1)
00340 buff.write(struct.pack('<I%ss'%length, length, val1))
00341 length = len(self.grasp.pre_grasp_posture.position)
00342 buff.write(_struct_I.pack(length))
00343 pattern = '<%sd'%length
00344 buff.write(self.grasp.pre_grasp_posture.position.tostring())
00345 length = len(self.grasp.pre_grasp_posture.velocity)
00346 buff.write(_struct_I.pack(length))
00347 pattern = '<%sd'%length
00348 buff.write(self.grasp.pre_grasp_posture.velocity.tostring())
00349 length = len(self.grasp.pre_grasp_posture.effort)
00350 buff.write(_struct_I.pack(length))
00351 pattern = '<%sd'%length
00352 buff.write(self.grasp.pre_grasp_posture.effort.tostring())
00353 _x = self
00354 buff.write(_struct_3I.pack(_x.grasp.grasp_posture.header.seq, _x.grasp.grasp_posture.header.stamp.secs, _x.grasp.grasp_posture.header.stamp.nsecs))
00355 _x = self.grasp.grasp_posture.header.frame_id
00356 length = len(_x)
00357 buff.write(struct.pack('<I%ss'%length, length, _x))
00358 length = len(self.grasp.grasp_posture.name)
00359 buff.write(_struct_I.pack(length))
00360 for val1 in self.grasp.grasp_posture.name:
00361 length = len(val1)
00362 buff.write(struct.pack('<I%ss'%length, length, val1))
00363 length = len(self.grasp.grasp_posture.position)
00364 buff.write(_struct_I.pack(length))
00365 pattern = '<%sd'%length
00366 buff.write(self.grasp.grasp_posture.position.tostring())
00367 length = len(self.grasp.grasp_posture.velocity)
00368 buff.write(_struct_I.pack(length))
00369 pattern = '<%sd'%length
00370 buff.write(self.grasp.grasp_posture.velocity.tostring())
00371 length = len(self.grasp.grasp_posture.effort)
00372 buff.write(_struct_I.pack(length))
00373 pattern = '<%sd'%length
00374 buff.write(self.grasp.grasp_posture.effort.tostring())
00375 _x = self
00376 buff.write(_struct_8dBi.pack(_x.grasp.grasp_pose.position.x, _x.grasp.grasp_pose.position.y, _x.grasp.grasp_pose.position.z, _x.grasp.grasp_pose.orientation.x, _x.grasp.grasp_pose.orientation.y, _x.grasp.grasp_pose.orientation.z, _x.grasp.grasp_pose.orientation.w, _x.grasp.success_probability, _x.grasp.cluster_rep, _x.goal))
00377 except struct.error, se: self._check_types(se)
00378 except TypeError, te: self._check_types(te)
00379
00380 def deserialize_numpy(self, str, numpy):
00381 """
00382 unpack serialized message in str into this message instance using numpy for array types
00383 @param str: byte array of serialized message
00384 @type str: str
00385 @param numpy: numpy python module
00386 @type numpy: module
00387 """
00388 try:
00389 if self.grasp is None:
00390 self.grasp = object_manipulation_msgs.msg.Grasp()
00391 end = 0
00392 _x = self
00393 start = end
00394 end += 12
00395 (_x.grasp.pre_grasp_posture.header.seq, _x.grasp.pre_grasp_posture.header.stamp.secs, _x.grasp.pre_grasp_posture.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
00396 start = end
00397 end += 4
00398 (length,) = _struct_I.unpack(str[start:end])
00399 start = end
00400 end += length
00401 self.grasp.pre_grasp_posture.header.frame_id = str[start:end]
00402 start = end
00403 end += 4
00404 (length,) = _struct_I.unpack(str[start:end])
00405 self.grasp.pre_grasp_posture.name = []
00406 for i in xrange(0, length):
00407 start = end
00408 end += 4
00409 (length,) = _struct_I.unpack(str[start:end])
00410 start = end
00411 end += length
00412 val1 = str[start:end]
00413 self.grasp.pre_grasp_posture.name.append(val1)
00414 start = end
00415 end += 4
00416 (length,) = _struct_I.unpack(str[start:end])
00417 pattern = '<%sd'%length
00418 start = end
00419 end += struct.calcsize(pattern)
00420 self.grasp.pre_grasp_posture.position = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
00421 start = end
00422 end += 4
00423 (length,) = _struct_I.unpack(str[start:end])
00424 pattern = '<%sd'%length
00425 start = end
00426 end += struct.calcsize(pattern)
00427 self.grasp.pre_grasp_posture.velocity = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
00428 start = end
00429 end += 4
00430 (length,) = _struct_I.unpack(str[start:end])
00431 pattern = '<%sd'%length
00432 start = end
00433 end += struct.calcsize(pattern)
00434 self.grasp.pre_grasp_posture.effort = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
00435 _x = self
00436 start = end
00437 end += 12
00438 (_x.grasp.grasp_posture.header.seq, _x.grasp.grasp_posture.header.stamp.secs, _x.grasp.grasp_posture.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
00439 start = end
00440 end += 4
00441 (length,) = _struct_I.unpack(str[start:end])
00442 start = end
00443 end += length
00444 self.grasp.grasp_posture.header.frame_id = str[start:end]
00445 start = end
00446 end += 4
00447 (length,) = _struct_I.unpack(str[start:end])
00448 self.grasp.grasp_posture.name = []
00449 for i in xrange(0, length):
00450 start = end
00451 end += 4
00452 (length,) = _struct_I.unpack(str[start:end])
00453 start = end
00454 end += length
00455 val1 = str[start:end]
00456 self.grasp.grasp_posture.name.append(val1)
00457 start = end
00458 end += 4
00459 (length,) = _struct_I.unpack(str[start:end])
00460 pattern = '<%sd'%length
00461 start = end
00462 end += struct.calcsize(pattern)
00463 self.grasp.grasp_posture.position = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
00464 start = end
00465 end += 4
00466 (length,) = _struct_I.unpack(str[start:end])
00467 pattern = '<%sd'%length
00468 start = end
00469 end += struct.calcsize(pattern)
00470 self.grasp.grasp_posture.velocity = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
00471 start = end
00472 end += 4
00473 (length,) = _struct_I.unpack(str[start:end])
00474 pattern = '<%sd'%length
00475 start = end
00476 end += struct.calcsize(pattern)
00477 self.grasp.grasp_posture.effort = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
00478 _x = self
00479 start = end
00480 end += 69
00481 (_x.grasp.grasp_pose.position.x, _x.grasp.grasp_pose.position.y, _x.grasp.grasp_pose.position.z, _x.grasp.grasp_pose.orientation.x, _x.grasp.grasp_pose.orientation.y, _x.grasp.grasp_pose.orientation.z, _x.grasp.grasp_pose.orientation.w, _x.grasp.success_probability, _x.grasp.cluster_rep, _x.goal,) = _struct_8dBi.unpack(str[start:end])
00482 self.grasp.cluster_rep = bool(self.grasp.cluster_rep)
00483 return self
00484 except struct.error, e:
00485 raise roslib.message.DeserializationError(e)
00486
00487 _struct_I = roslib.message.struct_I
00488 _struct_3I = struct.Struct("<3I")
00489 _struct_8dBi = struct.Struct("<8dBi")