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
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
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)
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)
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")