def gradients(self, frameids=None, static=True, moving=True, time=None):
"""
function determines all gradients within view distance
:param frameids: frame IDs to be considered looking for gradients, None considers all frameids.
: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: list of gradients []
"""
if not self.ev_thread:
self._evaporate_buffer(time=time)
gradients = []
# if view distance is infinite, return list of all gradients
if self._view_distance == np.inf:
return self.all_gradients(frameids, static, moving)
# determine frames to consider
if not frameids:
frameids = []
if static:
frameids += self._static.keys()
if moving:
frameids += self._moving.keys()
pose = self.get_last_position()
# no own position available
if pose is None:
return gradients
if static:
staticbuffer = copy.deepcopy(self._static)
if moving:
movingbuffer = copy.deepcopy(self._moving)
for fid in frameids:
# static gradients
if static and fid in staticbuffer.keys():
for element in staticbuffer[fid]:
if calc.get_gradient_distance(element.p, pose.p) <= \
element.diffusion + \
element.goal_radius + \
self._view_distance:
gradients.append(element)
# moving gradients
if moving and fid in movingbuffer.keys() \
and movingbuffer[fid]:
for pid in movingbuffer[fid].keys():
if movingbuffer[fid][pid]:
element = movingbuffer[fid][pid][-1]
if calc.get_gradient_distance(element.p, pose.p) \
<= element.diffusion + element.goal_radius + \
self._view_distance:
gradients.append(element)
return gradients
def calc_repulsive_gradient_ge(gradient, goal, pose):
"""
calculation of movement vector based on repulsive gradient and
goal gradient
:param gradient: repulsive gradient message
:param goal: attractive gradient message (goal)
:param pose: position SoMessage of the robot
:return: movement vector
distance of influence of obstacle = goal_radius + diffusion
"""
v = Vector3()
# distance obstacle - agent
tmp = calc.delta_vector(pose.p, gradient.p)
# shortest distance considered (goal_radius of agent == size of agent)
d = np.linalg.norm([tmp.x, tmp.y, tmp.z]) - pose.goal_radius \
- gradient.goal_radius
if d <= 0:
v = Vector3(np.inf, np.inf, np.inf)
# calculate norm vector for direction
tmp = calc.unit_vector3(tmp)
# calculate repulsion vector - adjust sign/direction
if tmp.x != 0.0:
v.x *= tmp.x
if tmp.y != 0.0:
v.y *= tmp.y
if tmp.z != 0.0:
v.z *= tmp.z
elif 0 < d <= gradient.diffusion:
# unit vector obstacle - agent
tmp = calc.unit_vector3(tmp)
# distance agent - goal
d_goal = calc.get_gradient_distance(pose.p, goal.p) - \
pose.goal_radius - goal.goal_radius
# unit vector agent - goal
ag = calc.delta_vector(goal.p, pose.p)
ag = calc.unit_vector3(ag)
# closest distance to obstacle - diffusion
d_obs_diff = (1.0 / d) - (1.0 / gradient.diffusion)
# parameters
n = 1.0
eta = 1.0
# weighting rep1 and rep2
f_rep1 = eta * d_obs_diff * ((d_goal**n) / (d**n))
f_rep2 = eta * (n / 2.0) * np.square(d_obs_diff) * (d_goal**(n - 1))
v.x = f_rep1 * tmp.x + f_rep2 * ag.x
v.y = f_rep1 * tmp.y + f_rep2 * ag.y
v.z = f_rep1 * tmp.z + f_rep2 * ag.z
elif d > gradient.diffusion:
v = Vector3()
return v
def move(self):
"""
calculates repulsion vector based on Fernandez-Marquez
:return: repulsion movement vector
"""
pose = self._buffer.get_own_pose()
view = self._buffer.agent_list(self.frames)
mov = None
if pose and view:
repulsion_radius = pose.diffusion + pose.goal_radius
mov = Vector3()
for el in view:
distance = calc.get_gradient_distance(el.p, pose.p) \
- el.goal_radius
if distance <= self._buffer.view_distance:
if distance > 0:
diff = repulsion_radius - distance
mov.x += (pose.p.x - el.p.x) * diff / distance
mov.y += (pose.p.y - el.p.y) * diff / distance
mov.z += (pose.p.z - el.p.z) * diff / distance
# enhancement of formulas to cover case when gradients are
# collided (agent is in goal radius of neighbor)
elif distance <= 0:
# create random vector with length=repulsion radius
if distance == - el.goal_radius:
mov = calc.add_vectors(mov, calc.random_vector(
repulsion_radius))
# use direction leading away if available
else:
mov = calc.add_vectors(mov, calc.adjust_length(
calc.delta_vector(pose.p, el.p),
repulsion_radius))
if not mov:
return None
if calc.vector_length(mov) > repulsion_radius:
mov = calc.adjust_length(mov, repulsion_radius)
return mov
def agent_set(self, frameids=None, include_own=True, time=None):
"""
function determines all moving gradients within view distance with a
certain frame ID, excluding all gradients from agent itself
:param frame: frame ID of agent data
:param time: optional time stamp for evaporation calculations, if None time=rospy.Time.now()
:return: list of gradients
"""
if not self.ev_thread:
self._evaporate_buffer(time=time)
gradients = []
pose = self.get_last_position()
# no own position available
if pose is None:
return gradients
# determine frames to consider
if not frameids:
frameids = self._moving.keys()
moving = self._moving
moving_keys = moving.keys()
for frame in frameids:
if frame in moving_keys and moving[frame]:
moving_frame_keys = moving[frame].keys()
for pid in moving_frame_keys:
if (include_own or pid != self._id) and moving[frame][pid]:
for g in moving[frame][pid]:
if calc.get_gradient_distance(g.p, pose.p) <= \
g.diffusion + g.goal_radius + self._view_distance:
gradients.append(g)
return gradients
def agent_list(self, frameids=None, time=None):
"""
function determines all moving gradients within view distance with a
certain frame ID, excluding all gradients from agent itself
:param frame: frame ID of agent data
:param moving: consider moving gradients
:param time: optional time stamp for evaporation calculations, if None time=rospy.Time.now()
:return: list of gradients
"""
if not self.ev_thread:
self._evaporate_buffer(time=time)
gradients = []
pose = self.get_last_position()
# no own position available
if pose is None:
return gradients
# determine frames to consider
if not frameids:
frameids = self._moving.keys()
moving = copy.deepcopy(self._moving)
for frame in frameids:
if frame in moving.keys() and moving[frame]:
for pid in moving[frame].keys():
if pid != self._id and moving[frame][pid]:
if calc.get_gradient_distance(moving[frame][pid][-1].p,
pose.p) \
<= moving[frame][pid][-1].diffusion + \
moving[frame][pid][-1].goal_radius + \
self._view_distance:
gradients.append(moving[frame][pid][-1])
return gradients
def attractive_gradients(self,
frameids=None,
static=True,
moving=True,
time=None):
"""
function determines which attractive gradients are currently within
view distance
:param frameids: frame IDs to be considered looking for attractive
gradients
: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: list of attractive gradients within view distance
"""
# check if moving and / or static attractive gradients are
# within view distance
if not self.ev_thread:
self._evaporate_buffer(time=time)
gradients_attractive = []
pose = self.get_last_position()
if pose is None:
return gradients_attractive
# determine frames to consider
if not frameids:
frameids = []
if static:
frameids += self._static.keys()
if moving:
frameids += self._moving.keys()
if static:
staticbuffer = copy.deepcopy(self._static)
if moving:
movingbuffer = copy.deepcopy(self._moving)
for fid in frameids:
# static gradients
if static and fid in staticbuffer.keys():
for element in staticbuffer[fid]:
if calc.get_gradient_distance(element.p, pose.p) <= \
element.diffusion + \
element.goal_radius + \
self._view_distance:
if element.attraction == 1:
gradients_attractive.append(element)
# moving gradients
if moving and fid in movingbuffer.keys():
for pid in movingbuffer[fid].keys():
if movingbuffer[fid][pid]:
element = movingbuffer[fid][pid][-1]
if calc.get_gradient_distance(element.p, pose.p) \
<= element.diffusion + element.goal_radius + \
self._view_distance:
if element.attraction == 1:
gradients_attractive.append(element)
return gradients_attractive
def store_static(self, msg):
"""
method to store static gradients
:param msg: received SoMessage
"""
# set aggregation option
aggregation = self.aggregation_option(msg.header.frame_id)
if aggregation is None:
return
if msg.header.frame_id in self._static:
# aggregation based on parent frame
if aggregation == AGGREGATION.NEWPARENT:
self.aggregation_newparent(msg)
elif aggregation == AGGREGATION.NEWFRAME:
self.aggregation_newframe(msg)
# aggregation based on position
else:
# indicates whether data point at same position / within
# aggregation radius is already stored
view = {}
# find all points which are in aggregation range of message
for i in xrange(
len(self._static[msg.header.frame_id]) - 1, -1, -1):
distance = calc.get_gradient_distance(
self._static[msg.header.frame_id][i].p, msg.p)
# data point lies within aggregation distance
if distance <= self._aggregation_distance:
view[i] = distance
# find minimum distance to gradient centers within aggregation
# distance
if view:
min_val = min(view.values()) # get minimum distance
result = [j for j, v in view.items() if v == min_val]
# several with same distance - random value of list
if len(result) > 1:
k = random.choice(result)
# only one minimum - use found index
else:
k = result[0]
# keep data with max reach
if aggregation == AGGREGATION.MAX:
self.aggregation_max(msg, k)
# keep data with min reach
elif aggregation == AGGREGATION.MIN:
self.aggregation_min(msg, k)
# keep average gradient
elif aggregation == AGGREGATION.AVG:
self.aggregation_average(msg, k)
# keep last received gradient at one position
elif aggregation == AGGREGATION.NEW:
self.aggregation_new(msg, k)
else:
self._static[msg.header.frame_id].append(msg)
else:
self._static[msg.header.frame_id] = [msg]