00001 """autogenerated by genpy from object_manipulation_msgs/ReactivePlaceGoal.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 trajectory_msgs.msg
00008 import geometry_msgs.msg
00009 import genpy
00010 import std_msgs.msg
00011
00012 class ReactivePlaceGoal(genpy.Message):
00013 _md5sum = "c40d270ade3a224752149b298986a70c"
00014 _type = "object_manipulation_msgs/ReactivePlaceGoal"
00015 _has_header = False
00016 _full_text = """# ====== DO NOT MODIFY! AUTOGENERATED FROM AN ACTION DEFINITION ======
00017 # an action for placing the object using tactile sensor feedback.
00018 # a reactive place starts from the current pose of the gripper and ends
00019 # at a desired place pose, presumably using the touch sensors along the way
00020
00021 # the name of the arm being used
00022 string arm_name
00023
00024 # the desired final place pose for the hand
00025 geometry_msgs/PoseStamped final_place_pose
00026
00027 # the joint trajectory to use for the place (if available)
00028 # this trajectory is expected to start at the current pose of the gripper
00029 # and end at the desired place pose
00030 trajectory_msgs/JointTrajectory trajectory
00031
00032 # the name of the support surface in the collision environment, if any
00033 string collision_support_surface_name
00034
00035 # the name in the collision environment of the object being placed, if any
00036 string collision_object_name
00037
00038 ================================================================================
00039 MSG: geometry_msgs/PoseStamped
00040 # A Pose with reference coordinate frame and timestamp
00041 Header header
00042 Pose pose
00043
00044 ================================================================================
00045 MSG: std_msgs/Header
00046 # Standard metadata for higher-level stamped data types.
00047 # This is generally used to communicate timestamped data
00048 # in a particular coordinate frame.
00049 #
00050 # sequence ID: consecutively increasing ID
00051 uint32 seq
00052 #Two-integer timestamp that is expressed as:
00053 # * stamp.secs: seconds (stamp_secs) since epoch
00054 # * stamp.nsecs: nanoseconds since stamp_secs
00055 # time-handling sugar is provided by the client library
00056 time stamp
00057 #Frame this data is associated with
00058 # 0: no frame
00059 # 1: global frame
00060 string frame_id
00061
00062 ================================================================================
00063 MSG: geometry_msgs/Pose
00064 # A representation of pose in free space, composed of postion and orientation.
00065 Point position
00066 Quaternion orientation
00067
00068 ================================================================================
00069 MSG: geometry_msgs/Point
00070 # This contains the position of a point in free space
00071 float64 x
00072 float64 y
00073 float64 z
00074
00075 ================================================================================
00076 MSG: geometry_msgs/Quaternion
00077 # This represents an orientation in free space in quaternion form.
00078
00079 float64 x
00080 float64 y
00081 float64 z
00082 float64 w
00083
00084 ================================================================================
00085 MSG: trajectory_msgs/JointTrajectory
00086 Header header
00087 string[] joint_names
00088 JointTrajectoryPoint[] points
00089 ================================================================================
00090 MSG: trajectory_msgs/JointTrajectoryPoint
00091 float64[] positions
00092 float64[] velocities
00093 float64[] accelerations
00094 duration time_from_start
00095 """
00096 __slots__ = ['arm_name','final_place_pose','trajectory','collision_support_surface_name','collision_object_name']
00097 _slot_types = ['string','geometry_msgs/PoseStamped','trajectory_msgs/JointTrajectory','string','string']
00098
00099 def __init__(self, *args, **kwds):
00100 """
00101 Constructor. Any message fields that are implicitly/explicitly
00102 set to None will be assigned a default value. The recommend
00103 use is keyword arguments as this is more robust to future message
00104 changes. You cannot mix in-order arguments and keyword arguments.
00105
00106 The available fields are:
00107 arm_name,final_place_pose,trajectory,collision_support_surface_name,collision_object_name
00108
00109 :param args: complete set of field values, in .msg order
00110 :param kwds: use keyword arguments corresponding to message field names
00111 to set specific fields.
00112 """
00113 if args or kwds:
00114 super(ReactivePlaceGoal, self).__init__(*args, **kwds)
00115
00116 if self.arm_name is None:
00117 self.arm_name = ''
00118 if self.final_place_pose is None:
00119 self.final_place_pose = geometry_msgs.msg.PoseStamped()
00120 if self.trajectory is None:
00121 self.trajectory = trajectory_msgs.msg.JointTrajectory()
00122 if self.collision_support_surface_name is None:
00123 self.collision_support_surface_name = ''
00124 if self.collision_object_name is None:
00125 self.collision_object_name = ''
00126 else:
00127 self.arm_name = ''
00128 self.final_place_pose = geometry_msgs.msg.PoseStamped()
00129 self.trajectory = trajectory_msgs.msg.JointTrajectory()
00130 self.collision_support_surface_name = ''
00131 self.collision_object_name = ''
00132
00133 def _get_types(self):
00134 """
00135 internal API method
00136 """
00137 return self._slot_types
00138
00139 def serialize(self, buff):
00140 """
00141 serialize message into buffer
00142 :param buff: buffer, ``StringIO``
00143 """
00144 try:
00145 _x = self.arm_name
00146 length = len(_x)
00147 if python3 or type(_x) == unicode:
00148 _x = _x.encode('utf-8')
00149 length = len(_x)
00150 buff.write(struct.pack('<I%ss'%length, length, _x))
00151 _x = self
00152 buff.write(_struct_3I.pack(_x.final_place_pose.header.seq, _x.final_place_pose.header.stamp.secs, _x.final_place_pose.header.stamp.nsecs))
00153 _x = self.final_place_pose.header.frame_id
00154 length = len(_x)
00155 if python3 or type(_x) == unicode:
00156 _x = _x.encode('utf-8')
00157 length = len(_x)
00158 buff.write(struct.pack('<I%ss'%length, length, _x))
00159 _x = self
00160 buff.write(_struct_7d3I.pack(_x.final_place_pose.pose.position.x, _x.final_place_pose.pose.position.y, _x.final_place_pose.pose.position.z, _x.final_place_pose.pose.orientation.x, _x.final_place_pose.pose.orientation.y, _x.final_place_pose.pose.orientation.z, _x.final_place_pose.pose.orientation.w, _x.trajectory.header.seq, _x.trajectory.header.stamp.secs, _x.trajectory.header.stamp.nsecs))
00161 _x = self.trajectory.header.frame_id
00162 length = len(_x)
00163 if python3 or type(_x) == unicode:
00164 _x = _x.encode('utf-8')
00165 length = len(_x)
00166 buff.write(struct.pack('<I%ss'%length, length, _x))
00167 length = len(self.trajectory.joint_names)
00168 buff.write(_struct_I.pack(length))
00169 for val1 in self.trajectory.joint_names:
00170 length = len(val1)
00171 if python3 or type(val1) == unicode:
00172 val1 = val1.encode('utf-8')
00173 length = len(val1)
00174 buff.write(struct.pack('<I%ss'%length, length, val1))
00175 length = len(self.trajectory.points)
00176 buff.write(_struct_I.pack(length))
00177 for val1 in self.trajectory.points:
00178 length = len(val1.positions)
00179 buff.write(_struct_I.pack(length))
00180 pattern = '<%sd'%length
00181 buff.write(struct.pack(pattern, *val1.positions))
00182 length = len(val1.velocities)
00183 buff.write(_struct_I.pack(length))
00184 pattern = '<%sd'%length
00185 buff.write(struct.pack(pattern, *val1.velocities))
00186 length = len(val1.accelerations)
00187 buff.write(_struct_I.pack(length))
00188 pattern = '<%sd'%length
00189 buff.write(struct.pack(pattern, *val1.accelerations))
00190 _v1 = val1.time_from_start
00191 _x = _v1
00192 buff.write(_struct_2i.pack(_x.secs, _x.nsecs))
00193 _x = self.collision_support_surface_name
00194 length = len(_x)
00195 if python3 or type(_x) == unicode:
00196 _x = _x.encode('utf-8')
00197 length = len(_x)
00198 buff.write(struct.pack('<I%ss'%length, length, _x))
00199 _x = self.collision_object_name
00200 length = len(_x)
00201 if python3 or type(_x) == unicode:
00202 _x = _x.encode('utf-8')
00203 length = len(_x)
00204 buff.write(struct.pack('<I%ss'%length, length, _x))
00205 except struct.error as se: self._check_types(struct.error("%s: '%s' when writing '%s'" % (type(se), str(se), str(_x))))
00206 except TypeError as te: self._check_types(ValueError("%s: '%s' when writing '%s'" % (type(te), str(te), str(_x))))
00207
00208 def deserialize(self, str):
00209 """
00210 unpack serialized message in str into this message instance
00211 :param str: byte array of serialized message, ``str``
00212 """
00213 try:
00214 if self.final_place_pose is None:
00215 self.final_place_pose = geometry_msgs.msg.PoseStamped()
00216 if self.trajectory is None:
00217 self.trajectory = trajectory_msgs.msg.JointTrajectory()
00218 end = 0
00219 start = end
00220 end += 4
00221 (length,) = _struct_I.unpack(str[start:end])
00222 start = end
00223 end += length
00224 if python3:
00225 self.arm_name = str[start:end].decode('utf-8')
00226 else:
00227 self.arm_name = str[start:end]
00228 _x = self
00229 start = end
00230 end += 12
00231 (_x.final_place_pose.header.seq, _x.final_place_pose.header.stamp.secs, _x.final_place_pose.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
00232 start = end
00233 end += 4
00234 (length,) = _struct_I.unpack(str[start:end])
00235 start = end
00236 end += length
00237 if python3:
00238 self.final_place_pose.header.frame_id = str[start:end].decode('utf-8')
00239 else:
00240 self.final_place_pose.header.frame_id = str[start:end]
00241 _x = self
00242 start = end
00243 end += 68
00244 (_x.final_place_pose.pose.position.x, _x.final_place_pose.pose.position.y, _x.final_place_pose.pose.position.z, _x.final_place_pose.pose.orientation.x, _x.final_place_pose.pose.orientation.y, _x.final_place_pose.pose.orientation.z, _x.final_place_pose.pose.orientation.w, _x.trajectory.header.seq, _x.trajectory.header.stamp.secs, _x.trajectory.header.stamp.nsecs,) = _struct_7d3I.unpack(str[start:end])
00245 start = end
00246 end += 4
00247 (length,) = _struct_I.unpack(str[start:end])
00248 start = end
00249 end += length
00250 if python3:
00251 self.trajectory.header.frame_id = str[start:end].decode('utf-8')
00252 else:
00253 self.trajectory.header.frame_id = str[start:end]
00254 start = end
00255 end += 4
00256 (length,) = _struct_I.unpack(str[start:end])
00257 self.trajectory.joint_names = []
00258 for i in range(0, length):
00259 start = end
00260 end += 4
00261 (length,) = _struct_I.unpack(str[start:end])
00262 start = end
00263 end += length
00264 if python3:
00265 val1 = str[start:end].decode('utf-8')
00266 else:
00267 val1 = str[start:end]
00268 self.trajectory.joint_names.append(val1)
00269 start = end
00270 end += 4
00271 (length,) = _struct_I.unpack(str[start:end])
00272 self.trajectory.points = []
00273 for i in range(0, length):
00274 val1 = trajectory_msgs.msg.JointTrajectoryPoint()
00275 start = end
00276 end += 4
00277 (length,) = _struct_I.unpack(str[start:end])
00278 pattern = '<%sd'%length
00279 start = end
00280 end += struct.calcsize(pattern)
00281 val1.positions = struct.unpack(pattern, str[start:end])
00282 start = end
00283 end += 4
00284 (length,) = _struct_I.unpack(str[start:end])
00285 pattern = '<%sd'%length
00286 start = end
00287 end += struct.calcsize(pattern)
00288 val1.velocities = struct.unpack(pattern, str[start:end])
00289 start = end
00290 end += 4
00291 (length,) = _struct_I.unpack(str[start:end])
00292 pattern = '<%sd'%length
00293 start = end
00294 end += struct.calcsize(pattern)
00295 val1.accelerations = struct.unpack(pattern, str[start:end])
00296 _v2 = val1.time_from_start
00297 _x = _v2
00298 start = end
00299 end += 8
00300 (_x.secs, _x.nsecs,) = _struct_2i.unpack(str[start:end])
00301 self.trajectory.points.append(val1)
00302 start = end
00303 end += 4
00304 (length,) = _struct_I.unpack(str[start:end])
00305 start = end
00306 end += length
00307 if python3:
00308 self.collision_support_surface_name = str[start:end].decode('utf-8')
00309 else:
00310 self.collision_support_surface_name = str[start:end]
00311 start = end
00312 end += 4
00313 (length,) = _struct_I.unpack(str[start:end])
00314 start = end
00315 end += length
00316 if python3:
00317 self.collision_object_name = str[start:end].decode('utf-8')
00318 else:
00319 self.collision_object_name = str[start:end]
00320 return self
00321 except struct.error as e:
00322 raise genpy.DeserializationError(e)
00323
00324
00325 def serialize_numpy(self, buff, numpy):
00326 """
00327 serialize message with numpy array types into buffer
00328 :param buff: buffer, ``StringIO``
00329 :param numpy: numpy python module
00330 """
00331 try:
00332 _x = self.arm_name
00333 length = len(_x)
00334 if python3 or type(_x) == unicode:
00335 _x = _x.encode('utf-8')
00336 length = len(_x)
00337 buff.write(struct.pack('<I%ss'%length, length, _x))
00338 _x = self
00339 buff.write(_struct_3I.pack(_x.final_place_pose.header.seq, _x.final_place_pose.header.stamp.secs, _x.final_place_pose.header.stamp.nsecs))
00340 _x = self.final_place_pose.header.frame_id
00341 length = len(_x)
00342 if python3 or type(_x) == unicode:
00343 _x = _x.encode('utf-8')
00344 length = len(_x)
00345 buff.write(struct.pack('<I%ss'%length, length, _x))
00346 _x = self
00347 buff.write(_struct_7d3I.pack(_x.final_place_pose.pose.position.x, _x.final_place_pose.pose.position.y, _x.final_place_pose.pose.position.z, _x.final_place_pose.pose.orientation.x, _x.final_place_pose.pose.orientation.y, _x.final_place_pose.pose.orientation.z, _x.final_place_pose.pose.orientation.w, _x.trajectory.header.seq, _x.trajectory.header.stamp.secs, _x.trajectory.header.stamp.nsecs))
00348 _x = self.trajectory.header.frame_id
00349 length = len(_x)
00350 if python3 or type(_x) == unicode:
00351 _x = _x.encode('utf-8')
00352 length = len(_x)
00353 buff.write(struct.pack('<I%ss'%length, length, _x))
00354 length = len(self.trajectory.joint_names)
00355 buff.write(_struct_I.pack(length))
00356 for val1 in self.trajectory.joint_names:
00357 length = len(val1)
00358 if python3 or type(val1) == unicode:
00359 val1 = val1.encode('utf-8')
00360 length = len(val1)
00361 buff.write(struct.pack('<I%ss'%length, length, val1))
00362 length = len(self.trajectory.points)
00363 buff.write(_struct_I.pack(length))
00364 for val1 in self.trajectory.points:
00365 length = len(val1.positions)
00366 buff.write(_struct_I.pack(length))
00367 pattern = '<%sd'%length
00368 buff.write(val1.positions.tostring())
00369 length = len(val1.velocities)
00370 buff.write(_struct_I.pack(length))
00371 pattern = '<%sd'%length
00372 buff.write(val1.velocities.tostring())
00373 length = len(val1.accelerations)
00374 buff.write(_struct_I.pack(length))
00375 pattern = '<%sd'%length
00376 buff.write(val1.accelerations.tostring())
00377 _v3 = val1.time_from_start
00378 _x = _v3
00379 buff.write(_struct_2i.pack(_x.secs, _x.nsecs))
00380 _x = self.collision_support_surface_name
00381 length = len(_x)
00382 if python3 or type(_x) == unicode:
00383 _x = _x.encode('utf-8')
00384 length = len(_x)
00385 buff.write(struct.pack('<I%ss'%length, length, _x))
00386 _x = self.collision_object_name
00387 length = len(_x)
00388 if python3 or type(_x) == unicode:
00389 _x = _x.encode('utf-8')
00390 length = len(_x)
00391 buff.write(struct.pack('<I%ss'%length, length, _x))
00392 except struct.error as se: self._check_types(struct.error("%s: '%s' when writing '%s'" % (type(se), str(se), str(_x))))
00393 except TypeError as te: self._check_types(ValueError("%s: '%s' when writing '%s'" % (type(te), str(te), str(_x))))
00394
00395 def deserialize_numpy(self, str, numpy):
00396 """
00397 unpack serialized message in str into this message instance using numpy for array types
00398 :param str: byte array of serialized message, ``str``
00399 :param numpy: numpy python module
00400 """
00401 try:
00402 if self.final_place_pose is None:
00403 self.final_place_pose = geometry_msgs.msg.PoseStamped()
00404 if self.trajectory is None:
00405 self.trajectory = trajectory_msgs.msg.JointTrajectory()
00406 end = 0
00407 start = end
00408 end += 4
00409 (length,) = _struct_I.unpack(str[start:end])
00410 start = end
00411 end += length
00412 if python3:
00413 self.arm_name = str[start:end].decode('utf-8')
00414 else:
00415 self.arm_name = str[start:end]
00416 _x = self
00417 start = end
00418 end += 12
00419 (_x.final_place_pose.header.seq, _x.final_place_pose.header.stamp.secs, _x.final_place_pose.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
00420 start = end
00421 end += 4
00422 (length,) = _struct_I.unpack(str[start:end])
00423 start = end
00424 end += length
00425 if python3:
00426 self.final_place_pose.header.frame_id = str[start:end].decode('utf-8')
00427 else:
00428 self.final_place_pose.header.frame_id = str[start:end]
00429 _x = self
00430 start = end
00431 end += 68
00432 (_x.final_place_pose.pose.position.x, _x.final_place_pose.pose.position.y, _x.final_place_pose.pose.position.z, _x.final_place_pose.pose.orientation.x, _x.final_place_pose.pose.orientation.y, _x.final_place_pose.pose.orientation.z, _x.final_place_pose.pose.orientation.w, _x.trajectory.header.seq, _x.trajectory.header.stamp.secs, _x.trajectory.header.stamp.nsecs,) = _struct_7d3I.unpack(str[start:end])
00433 start = end
00434 end += 4
00435 (length,) = _struct_I.unpack(str[start:end])
00436 start = end
00437 end += length
00438 if python3:
00439 self.trajectory.header.frame_id = str[start:end].decode('utf-8')
00440 else:
00441 self.trajectory.header.frame_id = str[start:end]
00442 start = end
00443 end += 4
00444 (length,) = _struct_I.unpack(str[start:end])
00445 self.trajectory.joint_names = []
00446 for i in range(0, length):
00447 start = end
00448 end += 4
00449 (length,) = _struct_I.unpack(str[start:end])
00450 start = end
00451 end += length
00452 if python3:
00453 val1 = str[start:end].decode('utf-8')
00454 else:
00455 val1 = str[start:end]
00456 self.trajectory.joint_names.append(val1)
00457 start = end
00458 end += 4
00459 (length,) = _struct_I.unpack(str[start:end])
00460 self.trajectory.points = []
00461 for i in range(0, length):
00462 val1 = trajectory_msgs.msg.JointTrajectoryPoint()
00463 start = end
00464 end += 4
00465 (length,) = _struct_I.unpack(str[start:end])
00466 pattern = '<%sd'%length
00467 start = end
00468 end += struct.calcsize(pattern)
00469 val1.positions = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
00470 start = end
00471 end += 4
00472 (length,) = _struct_I.unpack(str[start:end])
00473 pattern = '<%sd'%length
00474 start = end
00475 end += struct.calcsize(pattern)
00476 val1.velocities = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
00477 start = end
00478 end += 4
00479 (length,) = _struct_I.unpack(str[start:end])
00480 pattern = '<%sd'%length
00481 start = end
00482 end += struct.calcsize(pattern)
00483 val1.accelerations = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
00484 _v4 = val1.time_from_start
00485 _x = _v4
00486 start = end
00487 end += 8
00488 (_x.secs, _x.nsecs,) = _struct_2i.unpack(str[start:end])
00489 self.trajectory.points.append(val1)
00490 start = end
00491 end += 4
00492 (length,) = _struct_I.unpack(str[start:end])
00493 start = end
00494 end += length
00495 if python3:
00496 self.collision_support_surface_name = str[start:end].decode('utf-8')
00497 else:
00498 self.collision_support_surface_name = str[start:end]
00499 start = end
00500 end += 4
00501 (length,) = _struct_I.unpack(str[start:end])
00502 start = end
00503 end += length
00504 if python3:
00505 self.collision_object_name = str[start:end].decode('utf-8')
00506 else:
00507 self.collision_object_name = str[start:end]
00508 return self
00509 except struct.error as e:
00510 raise genpy.DeserializationError(e)
00511
00512 _struct_I = genpy.struct_I
00513 _struct_3I = struct.Struct("<3I")
00514 _struct_7d3I = struct.Struct("<7d3I")
00515 _struct_2i = struct.Struct("<2i")