$search
00001 """autogenerated by genmsg_py from PositionConstraint.msg. Do not edit.""" 00002 import roslib.message 00003 import struct 00004 00005 import arm_navigation_msgs.msg 00006 import geometry_msgs.msg 00007 import std_msgs.msg 00008 00009 class PositionConstraint(roslib.message.Message): 00010 _md5sum = "7e3d9697e64b346b9d3cb7311bb88ccb" 00011 _type = "arm_navigation_msgs/PositionConstraint" 00012 _has_header = True #flag to mark the presence of a Header object 00013 _full_text = """# This message contains the definition of a position constraint. 00014 Header header 00015 00016 # The robot link this constraint refers to 00017 string link_name 00018 00019 # The offset (in the link frame) for the target point on the link we are planning for 00020 geometry_msgs/Point target_point_offset 00021 00022 # The nominal/target position for the point we are planning for 00023 geometry_msgs/Point position 00024 00025 # The shape of the bounded region that constrains the position of the end-effector 00026 # This region is always centered at the position defined above 00027 arm_navigation_msgs/Shape constraint_region_shape 00028 00029 # The orientation of the bounded region that constrains the position of the end-effector. 00030 # This allows the specification of non-axis aligned constraints 00031 geometry_msgs/Quaternion constraint_region_orientation 00032 00033 # Constraint weighting factor - a weight for this constraint 00034 float64 weight 00035 00036 ================================================================================ 00037 MSG: std_msgs/Header 00038 # Standard metadata for higher-level stamped data types. 00039 # This is generally used to communicate timestamped data 00040 # in a particular coordinate frame. 00041 # 00042 # sequence ID: consecutively increasing ID 00043 uint32 seq 00044 #Two-integer timestamp that is expressed as: 00045 # * stamp.secs: seconds (stamp_secs) since epoch 00046 # * stamp.nsecs: nanoseconds since stamp_secs 00047 # time-handling sugar is provided by the client library 00048 time stamp 00049 #Frame this data is associated with 00050 # 0: no frame 00051 # 1: global frame 00052 string frame_id 00053 00054 ================================================================================ 00055 MSG: geometry_msgs/Point 00056 # This contains the position of a point in free space 00057 float64 x 00058 float64 y 00059 float64 z 00060 00061 ================================================================================ 00062 MSG: arm_navigation_msgs/Shape 00063 byte SPHERE=0 00064 byte BOX=1 00065 byte CYLINDER=2 00066 byte MESH=3 00067 00068 byte type 00069 00070 00071 #### define sphere, box, cylinder #### 00072 # the origin of each shape is considered at the shape's center 00073 00074 # for sphere 00075 # radius := dimensions[0] 00076 00077 # for cylinder 00078 # radius := dimensions[0] 00079 # length := dimensions[1] 00080 # the length is along the Z axis 00081 00082 # for box 00083 # size_x := dimensions[0] 00084 # size_y := dimensions[1] 00085 # size_z := dimensions[2] 00086 float64[] dimensions 00087 00088 00089 #### define mesh #### 00090 00091 # list of triangles; triangle k is defined by tre vertices located 00092 # at indices triangles[3k], triangles[3k+1], triangles[3k+2] 00093 int32[] triangles 00094 geometry_msgs/Point[] vertices 00095 00096 ================================================================================ 00097 MSG: geometry_msgs/Quaternion 00098 # This represents an orientation in free space in quaternion form. 00099 00100 float64 x 00101 float64 y 00102 float64 z 00103 float64 w 00104 00105 """ 00106 __slots__ = ['header','link_name','target_point_offset','position','constraint_region_shape','constraint_region_orientation','weight'] 00107 _slot_types = ['Header','string','geometry_msgs/Point','geometry_msgs/Point','arm_navigation_msgs/Shape','geometry_msgs/Quaternion','float64'] 00108 00109 def __init__(self, *args, **kwds): 00110 """ 00111 Constructor. Any message fields that are implicitly/explicitly 00112 set to None will be assigned a default value. The recommend 00113 use is keyword arguments as this is more robust to future message 00114 changes. You cannot mix in-order arguments and keyword arguments. 00115 00116 The available fields are: 00117 header,link_name,target_point_offset,position,constraint_region_shape,constraint_region_orientation,weight 00118 00119 @param args: complete set of field values, in .msg order 00120 @param kwds: use keyword arguments corresponding to message field names 00121 to set specific fields. 00122 """ 00123 if args or kwds: 00124 super(PositionConstraint, self).__init__(*args, **kwds) 00125 #message fields cannot be None, assign default values for those that are 00126 if self.header is None: 00127 self.header = std_msgs.msg._Header.Header() 00128 if self.link_name is None: 00129 self.link_name = '' 00130 if self.target_point_offset is None: 00131 self.target_point_offset = geometry_msgs.msg.Point() 00132 if self.position is None: 00133 self.position = geometry_msgs.msg.Point() 00134 if self.constraint_region_shape is None: 00135 self.constraint_region_shape = arm_navigation_msgs.msg.Shape() 00136 if self.constraint_region_orientation is None: 00137 self.constraint_region_orientation = geometry_msgs.msg.Quaternion() 00138 if self.weight is None: 00139 self.weight = 0. 00140 else: 00141 self.header = std_msgs.msg._Header.Header() 00142 self.link_name = '' 00143 self.target_point_offset = geometry_msgs.msg.Point() 00144 self.position = geometry_msgs.msg.Point() 00145 self.constraint_region_shape = arm_navigation_msgs.msg.Shape() 00146 self.constraint_region_orientation = geometry_msgs.msg.Quaternion() 00147 self.weight = 0. 00148 00149 def _get_types(self): 00150 """ 00151 internal API method 00152 """ 00153 return self._slot_types 00154 00155 def serialize(self, buff): 00156 """ 00157 serialize message into buffer 00158 @param buff: buffer 00159 @type buff: StringIO 00160 """ 00161 try: 00162 _x = self 00163 buff.write(_struct_3I.pack(_x.header.seq, _x.header.stamp.secs, _x.header.stamp.nsecs)) 00164 _x = self.header.frame_id 00165 length = len(_x) 00166 buff.write(struct.pack('<I%ss'%length, length, _x)) 00167 _x = self.link_name 00168 length = len(_x) 00169 buff.write(struct.pack('<I%ss'%length, length, _x)) 00170 _x = self 00171 buff.write(_struct_6db.pack(_x.target_point_offset.x, _x.target_point_offset.y, _x.target_point_offset.z, _x.position.x, _x.position.y, _x.position.z, _x.constraint_region_shape.type)) 00172 length = len(self.constraint_region_shape.dimensions) 00173 buff.write(_struct_I.pack(length)) 00174 pattern = '<%sd'%length 00175 buff.write(struct.pack(pattern, *self.constraint_region_shape.dimensions)) 00176 length = len(self.constraint_region_shape.triangles) 00177 buff.write(_struct_I.pack(length)) 00178 pattern = '<%si'%length 00179 buff.write(struct.pack(pattern, *self.constraint_region_shape.triangles)) 00180 length = len(self.constraint_region_shape.vertices) 00181 buff.write(_struct_I.pack(length)) 00182 for val1 in self.constraint_region_shape.vertices: 00183 _x = val1 00184 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 00185 _x = self 00186 buff.write(_struct_5d.pack(_x.constraint_region_orientation.x, _x.constraint_region_orientation.y, _x.constraint_region_orientation.z, _x.constraint_region_orientation.w, _x.weight)) 00187 except struct.error as se: self._check_types(se) 00188 except TypeError as te: self._check_types(te) 00189 00190 def deserialize(self, str): 00191 """ 00192 unpack serialized message in str into this message instance 00193 @param str: byte array of serialized message 00194 @type str: str 00195 """ 00196 try: 00197 if self.header is None: 00198 self.header = std_msgs.msg._Header.Header() 00199 if self.target_point_offset is None: 00200 self.target_point_offset = geometry_msgs.msg.Point() 00201 if self.position is None: 00202 self.position = geometry_msgs.msg.Point() 00203 if self.constraint_region_shape is None: 00204 self.constraint_region_shape = arm_navigation_msgs.msg.Shape() 00205 if self.constraint_region_orientation is None: 00206 self.constraint_region_orientation = geometry_msgs.msg.Quaternion() 00207 end = 0 00208 _x = self 00209 start = end 00210 end += 12 00211 (_x.header.seq, _x.header.stamp.secs, _x.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end]) 00212 start = end 00213 end += 4 00214 (length,) = _struct_I.unpack(str[start:end]) 00215 start = end 00216 end += length 00217 self.header.frame_id = str[start:end] 00218 start = end 00219 end += 4 00220 (length,) = _struct_I.unpack(str[start:end]) 00221 start = end 00222 end += length 00223 self.link_name = str[start:end] 00224 _x = self 00225 start = end 00226 end += 49 00227 (_x.target_point_offset.x, _x.target_point_offset.y, _x.target_point_offset.z, _x.position.x, _x.position.y, _x.position.z, _x.constraint_region_shape.type,) = _struct_6db.unpack(str[start:end]) 00228 start = end 00229 end += 4 00230 (length,) = _struct_I.unpack(str[start:end]) 00231 pattern = '<%sd'%length 00232 start = end 00233 end += struct.calcsize(pattern) 00234 self.constraint_region_shape.dimensions = struct.unpack(pattern, str[start:end]) 00235 start = end 00236 end += 4 00237 (length,) = _struct_I.unpack(str[start:end]) 00238 pattern = '<%si'%length 00239 start = end 00240 end += struct.calcsize(pattern) 00241 self.constraint_region_shape.triangles = struct.unpack(pattern, str[start:end]) 00242 start = end 00243 end += 4 00244 (length,) = _struct_I.unpack(str[start:end]) 00245 self.constraint_region_shape.vertices = [] 00246 for i in range(0, length): 00247 val1 = geometry_msgs.msg.Point() 00248 _x = val1 00249 start = end 00250 end += 24 00251 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 00252 self.constraint_region_shape.vertices.append(val1) 00253 _x = self 00254 start = end 00255 end += 40 00256 (_x.constraint_region_orientation.x, _x.constraint_region_orientation.y, _x.constraint_region_orientation.z, _x.constraint_region_orientation.w, _x.weight,) = _struct_5d.unpack(str[start:end]) 00257 return self 00258 except struct.error as e: 00259 raise roslib.message.DeserializationError(e) #most likely buffer underfill 00260 00261 00262 def serialize_numpy(self, buff, numpy): 00263 """ 00264 serialize message with numpy array types into buffer 00265 @param buff: buffer 00266 @type buff: StringIO 00267 @param numpy: numpy python module 00268 @type numpy module 00269 """ 00270 try: 00271 _x = self 00272 buff.write(_struct_3I.pack(_x.header.seq, _x.header.stamp.secs, _x.header.stamp.nsecs)) 00273 _x = self.header.frame_id 00274 length = len(_x) 00275 buff.write(struct.pack('<I%ss'%length, length, _x)) 00276 _x = self.link_name 00277 length = len(_x) 00278 buff.write(struct.pack('<I%ss'%length, length, _x)) 00279 _x = self 00280 buff.write(_struct_6db.pack(_x.target_point_offset.x, _x.target_point_offset.y, _x.target_point_offset.z, _x.position.x, _x.position.y, _x.position.z, _x.constraint_region_shape.type)) 00281 length = len(self.constraint_region_shape.dimensions) 00282 buff.write(_struct_I.pack(length)) 00283 pattern = '<%sd'%length 00284 buff.write(self.constraint_region_shape.dimensions.tostring()) 00285 length = len(self.constraint_region_shape.triangles) 00286 buff.write(_struct_I.pack(length)) 00287 pattern = '<%si'%length 00288 buff.write(self.constraint_region_shape.triangles.tostring()) 00289 length = len(self.constraint_region_shape.vertices) 00290 buff.write(_struct_I.pack(length)) 00291 for val1 in self.constraint_region_shape.vertices: 00292 _x = val1 00293 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 00294 _x = self 00295 buff.write(_struct_5d.pack(_x.constraint_region_orientation.x, _x.constraint_region_orientation.y, _x.constraint_region_orientation.z, _x.constraint_region_orientation.w, _x.weight)) 00296 except struct.error as se: self._check_types(se) 00297 except TypeError as te: self._check_types(te) 00298 00299 def deserialize_numpy(self, str, numpy): 00300 """ 00301 unpack serialized message in str into this message instance using numpy for array types 00302 @param str: byte array of serialized message 00303 @type str: str 00304 @param numpy: numpy python module 00305 @type numpy: module 00306 """ 00307 try: 00308 if self.header is None: 00309 self.header = std_msgs.msg._Header.Header() 00310 if self.target_point_offset is None: 00311 self.target_point_offset = geometry_msgs.msg.Point() 00312 if self.position is None: 00313 self.position = geometry_msgs.msg.Point() 00314 if self.constraint_region_shape is None: 00315 self.constraint_region_shape = arm_navigation_msgs.msg.Shape() 00316 if self.constraint_region_orientation is None: 00317 self.constraint_region_orientation = geometry_msgs.msg.Quaternion() 00318 end = 0 00319 _x = self 00320 start = end 00321 end += 12 00322 (_x.header.seq, _x.header.stamp.secs, _x.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end]) 00323 start = end 00324 end += 4 00325 (length,) = _struct_I.unpack(str[start:end]) 00326 start = end 00327 end += length 00328 self.header.frame_id = str[start:end] 00329 start = end 00330 end += 4 00331 (length,) = _struct_I.unpack(str[start:end]) 00332 start = end 00333 end += length 00334 self.link_name = str[start:end] 00335 _x = self 00336 start = end 00337 end += 49 00338 (_x.target_point_offset.x, _x.target_point_offset.y, _x.target_point_offset.z, _x.position.x, _x.position.y, _x.position.z, _x.constraint_region_shape.type,) = _struct_6db.unpack(str[start:end]) 00339 start = end 00340 end += 4 00341 (length,) = _struct_I.unpack(str[start:end]) 00342 pattern = '<%sd'%length 00343 start = end 00344 end += struct.calcsize(pattern) 00345 self.constraint_region_shape.dimensions = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length) 00346 start = end 00347 end += 4 00348 (length,) = _struct_I.unpack(str[start:end]) 00349 pattern = '<%si'%length 00350 start = end 00351 end += struct.calcsize(pattern) 00352 self.constraint_region_shape.triangles = numpy.frombuffer(str[start:end], dtype=numpy.int32, count=length) 00353 start = end 00354 end += 4 00355 (length,) = _struct_I.unpack(str[start:end]) 00356 self.constraint_region_shape.vertices = [] 00357 for i in range(0, length): 00358 val1 = geometry_msgs.msg.Point() 00359 _x = val1 00360 start = end 00361 end += 24 00362 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 00363 self.constraint_region_shape.vertices.append(val1) 00364 _x = self 00365 start = end 00366 end += 40 00367 (_x.constraint_region_orientation.x, _x.constraint_region_orientation.y, _x.constraint_region_orientation.z, _x.constraint_region_orientation.w, _x.weight,) = _struct_5d.unpack(str[start:end]) 00368 return self 00369 except struct.error as e: 00370 raise roslib.message.DeserializationError(e) #most likely buffer underfill 00371 00372 _struct_I = roslib.message.struct_I 00373 _struct_5d = struct.Struct("<5d") 00374 _struct_3I = struct.Struct("<3I") 00375 _struct_6db = struct.Struct("<6db") 00376 _struct_3d = struct.Struct("<3d")