def move(self):
"""
calculates movement vector
:return: movement vector
"""
pose = self._buffer.get_own_pose()
g = None
# strongest gradient
grad = self._buffer.max_reach_attractive_gradient(self.frames,
self.static,
self.moving)
if pose and grad:
g = gradient.calc_attractive_gradient(grad, pose)
if not g:
return None
# adjust length
d = calc.vector_length(g)
if d > self.maxvel:
g = calc.adjust_length(g, self.maxvel)
elif 0 < d < self.minvel:
g = calc.adjust_length(g, self.minvel)
return g
def goal_gradient(self):
"""
:return: vector to goal gradient
"""
grad = None
attractive = self._buffer.max_attractive_gradient(self.frames,
self.static,
self.moving)
pose = self._buffer.get_own_pose()
if attractive and pose:
grad = gradient.calc_attractive_gradient(attractive, pose)
return grad
def strongest_gradient(self,
frameids=None,
static=True,
moving=True,
time=None):
"""
return gradient with strongest potential on robot
:param frameids: frames to consider
:param static: bool, consider static gradients
:param moving: bool, consider moving gradients
:param time: optional time stamp for evaporation calculations, if None time=rospy.Time.now()
:return: soMessage gradient
"""
tmp_grad = None
if self.get_last_position() is None:
return tmp_grad
# all gradients within view distance
gradients = self.gradients(frameids, static, moving, time=time)
tmp_att = -1
if gradients:
for grad in gradients:
if grad.attraction == 1:
g = gradient.calc_attractive_gradient(
grad, self.get_last_position())
else:
g = gradient.calc_repulsive_gradient(
grad, self.get_last_position())
if abs(g.x) == np.inf or abs(g.y) == np.inf or \
abs(g.z) == np.inf:
att = np.inf
else:
att = calc.vector_length(g)
# attractive potential is defined inverse to repulsive
# potential --> adjust attractive value for comparison
if grad.attraction == 1:
att = 1 - att
if att > tmp_att:
tmp_grad = grad
tmp_att = att
return tmp_grad
def move(self):
"""
calculate movement vector
:return: movement vector
"""
pose = self._buffer.get_own_pose()
g = None
# strongest gradient
grad = self._buffer.strongest_gradient(self.frames, self.static,
self.moving)
if pose:
if grad:
if grad.attraction == 1:
g = gradient.calc_attractive_gradient(grad, pose)
elif grad.attraction == -1:
g = gradient.calc_repulsive_gradient(grad, pose)
# robot position is within obstacle goal radius
# handle infinite repulsion
if g.x == np.inf or g.x == -1 * np.inf:
# create random vector with length (goal_radius +
# gradient.diffusion)
dv = calc.delta_vector(pose.p, grad.p)
if calc.vector_length(dv) == 0:
g = calc.random_vector(grad.goal_radius +
grad.diffusion)
else:
g = calc.adjust_length(dv, grad.goal_radius
+ grad.diffusion)
if not g:
return None
# adjust length
d = calc.vector_length(g)
if d > self.maxvel:
g = calc.adjust_length(g, self.maxvel)
elif 0 < d < self.minvel:
g = calc.adjust_length(g, self.minvel)
return g
def max_attractive_gradient(self,
frameids=None,
static=True,
moving=True,
time=None):
"""
Method to return relatively nearest attractive gradient
(= min movement vector length)
based on attraction values of Balch & Hybinette
:param frameids: frames to consider
:param static: consider static gradients
:param moving: consider moving gradients
:param time: optional time stamp for evaporation calculations, if None time=rospy.Time.now()
:return: relatively nearest attractive gradient (Vector3)
"""
pose = self.get_last_position()
if pose is None:
return []
gradients = self.attractive_gradients(frameids,
static,
moving,
time=time)
tmp_grad = None
tmp_att = np.inf
if gradients:
for grad in gradients:
g = gradient.calc_attractive_gradient(grad, pose)
att = calc.vector_length(g)
# attraction decreases with being closer to gradient source
# / goal area
if att < tmp_att:
tmp_grad = grad
tmp_att = att
return tmp_grad
def min_attractive_gradient(self,
frameids=None,
static=True,
moving=True,
time=None):
"""
Method to return relatively furthest gradient
(= maximum attraction value length)
based on attraction values of Balch & Hybinette
:param frameids: frames to consider
:param static: consider static gradients
:param moving: consider moving gradients
:param time: optional time stamp for evaporation calculations, if None time=rospy.Time.now()
:return: gradient (Vector3)
"""
pose = self.get_last_position()
if pose is None:
return []
gradients = self.attractive_gradients(frameids,
static,
moving,
time=time)
tmp_grad = None
tmp_att = 0
if gradients:
for grad in gradients:
# gradient reach
g = gradient.calc_attractive_gradient(grad, pose)
att = calc.vector_length(g)
if att > tmp_att:
tmp_grad = grad
tmp_att = att
return tmp_grad
