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