def __call__(self, sensor, landmark):
p = sensor.position
nh = sensor.orientation.xh
q = landmark.position
mh = landmark.orientation.xh
vec = q - p
if (vec * nh) <= 0.0 or (nh * mh) <= 0.0:
return 0.0, gmi.Vector(), gmi.Vector()
return self.spline(vec.norm) * (vec * nh) / vec.norm * (
nh * mh), gmi.Vector(), gmi.Vector()
def coverage_controller():
value = 0.0
pos_grad = gmi.Vector()
ori_grad = gmi.Vector()
for landmark in landmarks:
if landmark.coverage < landmark.TARGET_COVERAGE:
val, pg, og = FOOTPRINT(pose, landmark.POSE)
value += val
landmark.coverage += val
landmark.contributed_coverage += val
pos_grad += pg
ori_grad += og
return value, pos_grad, ori_grad
def pose_controller():
pos_grad = gmi.Vector()
ori_grad = gmi.Vector()
if not reference_landmark is None:
ref_pose = reference_landmark.POSE
pos_grad = GAIN*((ref_pose.position-pose.position)-pose.orientation.xh)
ori_grad = 0.3*GAIN*(pose.orientation.xh.cross(ref_pose.orientation.xh))
value = 0.0
for landmark in landmarks:
if landmark.coverage < landmark.TARGET_COVERAGE:
val, _, _ = FOOTPRINT(pose, landmark.POSE)
landmark.coverage += val
landmark.contributed_coverage += val
value += val
return value, pos_grad, ori_grad
def __call__(self, sns, lmk):
R = self.__BIG_RADIUS
r = self.__SMALL_RADIUS
q = lmk.position
p = sns.position
nh = sns.orientation.xh
vec = (q - p - r * nh)
xi = vec * nh
zeta2_mi2 = vec.norm**2 - xi**2
if xi > 0:
val = 1.0 - vec.norm**2 / R**2
pg = 2 * vec / R**2
og = 2 * vec / R**2
if val < 0.0:
return 0.0, gmi.Vector(), gmi.Vector()
else:
return val, pg, gmi.Vector()
else:
val = 1.0 - xi**2 / r**2 - zeta2_mi2 / R**2
pg = 2 * xi * (1 / r**2 - 1 / R**2) * nh + 2 / R**2 * vec
og = (2 * xi * (1 / R**2 - 1 / r**2) + 2 / R**2) * vec
if val < 0.0:
return 0.0, gmi.Vector(), gmi.Vector()
else:
return val, pg, gmi.Vector()
def outward_orthogonal(self, point):
if -self._RADIUS < point.x < self._RADIUS:
vec = gmi.Vector(point.x, point.y, 0.0)
return vec / vec.norm
else:
return Vector(0, 1, 0)
def outward_orthogonal(self, point):
vec = gmi.Vector(point.x - self._X0, point.y - self._Y0, 0.0)
return vec / vec.norm
def __call__(self, sns, lmk):
return max(
(0.0, sns.orientation.xh *
lmk.orientation.xh)), gmi.Vector(), sns.orientation.xh.cross(
lmk.orientation.xh)