def test_follow_all(self):
"""
test aggregate all method
:return:
"""
bffr = SoBuffer()
chem = FollowAll(bffr, moving=False, static=True, maxvel=np.inf)
bffr._static = {
'gradient': [
SoMessage(None, None, Vector3(2, 3, 1), Quaternion(),
Vector3(), -1, 1.0, 1.0, 1.0, 0, None, False, []),
SoMessage(None, None, Vector3(2, 2, 2), Quaternion(),
Vector3(), 1, 1.0, 1.0, 1.0, 0, None, False, [])
],
'None': [
SoMessage(None, None, Vector3(0, 3, 2), Quaternion(),
Vector3(), -1, 3.0, 1.0, 1.0, 0, None, False, []),
SoMessage(None, None, Vector3(5, 6, 3), Quaternion(),
Vector3(), 1, 2.0, 1.0, 1.0, 0, None, False, [])
]
}
bffr._own_pos = [
SoMessage(None, None, Vector3(1, 2, 3), Quaternion(), Vector3(),
-1, 3.0, 0.0, 1.0, 0, None, False, [])
]
# only one frameID + repulsive gradient is not considered as outside
# view distance
chem.frames = ['gradient']
result = chem.move()
result.x = round(result.x, 2)
result.y = round(result.y, 2)
result.z = round(result.z, 2)
self.assertEqual(result, Vector3(0.29, 0.0, -0.29))
# all frameIDs - everything within view is aggregated
chem.frames = []
result = chem.move()
result.x = round(result.x, 2)
result.y = round(result.y, 2)
result.z = round(result.z, 2)
self.assertEqual(result, Vector3(0.73, -0.44, 0.14))
# noe gradient within view
chem.frames = ['test']
result = chem.move()
self.assertEqual(result, None)
bffr._static = {
'test': [
SoMessage(None, None, Vector3(2, 3, 1), Quaternion(),
Vector3(), -1, 1.0, 1.0, 1.0, 0, None, False, [])
]
}
chem.frames = ['test']
result = chem.move()
self.assertEqual(result, Vector3())
def test_goal_gradient(self):
"""
test goal gradient method of chemotaxisBalch Behaviour
:return:
"""
bffr = SoBuffer()
chem = ChemotaxisBalch(bffr, moving=False, static=True, maxvel=np.inf)
bffr._own_pos = [
SoMessage(None, None, Vector3(1, 2, 3), Quaternion(), Vector3(),
-1, 3.0, 0.0, 1.0, 0, None, False, [])
]
bffr._static = {
'gradient': [
SoMessage(None, None, Vector3(2, 3, 1), Quaternion(),
Vector3(), -1, 1.0, 1.0, 1.0, 0, None, False, []),
SoMessage(None, None, Vector3(2, 2, 2), Quaternion(),
Vector3(), 1, 1.0, 1.0, 1.0, 0, None, False, [])
],
'None': [
SoMessage(None, None, Vector3(0, 3, 2), Quaternion(),
Vector3(), -1, 3.0, 1.0, 10, 0, None, False, []),
SoMessage(None, None, Vector3(5, 6, 3), Quaternion(),
Vector3(), 1, 2.0, 1.0, 1.0, 0, None, False, [])
]
}
self.assertEqual(calc.vector_length(chem.goal_gradient()),
np.sqrt(2) - 1)
# no gradient within view
chem.frames = ['test']
result = chem.move()
self.assertEqual(result, None)
def test_gradients(self):
"""
test gradients method
"""
bffr = SoBuffer(view_distance=2.0)
# 2 neighbors within view, one outside view
bffr._moving = {
'robot': {
'robot1': [SoMessage(),
SoMessage(None, None, Vector3(2, 2, 0),
Quaternion(), Vector3(), 1, 1.0, 1.0, 1.0,
0, None, True, [])],
'robot2': [SoMessage(),
SoMessage(None, None, Vector3(5, 6, 0),
Quaternion(), Vector3(), 1, 2.0, 1.0, 1.0,
0, None, True, [])],
'robot3': [SoMessage(),
SoMessage(),
SoMessage(None, None, Vector3(1, 2, 0),
Quaternion(), Vector3(), -1, 4.0, 1.0,
1.0, 0, None, True, [])],
'robot4': []
}
}
bffr._static = {
'None': [SoMessage(None, None, Vector3(2, 1, 0), Quaternion(),
Vector3(), 1, 1.0, 1.0, 1.0, 0, None, False,
[]), SoMessage()],
}
bffr._own_pos = [
SoMessage(None, None, Vector3(1, 1, 1), Quaternion(), Vector3(), 1,
1.0, 1.0, 1.0, 0, None, False, [])]
result = [
SoMessage(None, None, Vector3(1, 2, 0), Quaternion(), Vector3(),
-1, 4.0, 1.0, 1.0, 0, None, True, []),
SoMessage(None, None, Vector3(2, 2, 0), Quaternion(), Vector3(), 1,
1.0, 1.0, 1.0, 0, None, True, [])
]
self.assertEqual(bffr.gradients(static=False, time=self.now), result)
result = [SoMessage(None, None, Vector3(2, 1, 0), Quaternion(),
Vector3(), 1, 1.0, 1.0, 1.0, 0, None, False, []),
SoMessage(None, None, Vector3(1, 2, 0), Quaternion(),
Vector3(), -1, 4.0, 1.0, 1.0, 0, None, True, []),
SoMessage(None, None, Vector3(2, 2, 0), Quaternion(),
Vector3(), 1, 1.0, 1.0, 1.0, 0, None, True, [])]
self.assertEqual(bffr.gradients(time=self.now), result)
def test_quorum(self):
"""
test quorum sensing class
:return:
"""
bffr = SoBuffer()
quorum = Quorum(bffr, 2)
self.assertEqual(quorum.calc_value()[1], False)
# 2 neighbors within view, one outside view
bffr._moving = {
'robot': {
'robot1': [
SoMessage(),
SoMessage(None, None, Vector3(2, 2, 0), Quaternion(),
Vector3(), 1, 1.0, 1.0, 1.0, 0, None, True, [])
],
'robot2': [
SoMessage(),
SoMessage(None, None, Vector3(5, 6, 0), Quaternion(),
Vector3(), 1, 2.0, 1.0, 1.0, 0, None, True, [])
],
'robot3': [
SoMessage(),
SoMessage(),
SoMessage(None, None, Vector3(1, 2, 0), Quaternion(),
Vector3(), 1, 4.0, 1.0, 1.0, 0, None, True, [])
],
'robot4': []
}
}
bffr._static = {
'None': [
SoMessage(None, None, Vector3(5, 6, 5), Quaternion(),
Vector3(), 1, 1.0, 1.0, 1.0, 0, None, False, [])
]
}
bffr._own_pos = [
SoMessage(None, None, Vector3(1, 1, 1), Quaternion(), Vector3(), 1,
1.0, 1.0, 1.0, 0, None, False, [])
]
self.assertEqual(quorum.calc_value()[1], True)
quorum.threshold = 3
self.assertEqual(quorum.calc_value()[1], False)
def test_agent_list(self):
"""
test agent list function
:return:
"""
bffr = SoBuffer(view_distance=2.0)
self.assertEqual(bffr.agent_list([bffr.pose_frame], time=self.now), [])
# 2 neighbors within view, one outside view
bffr._moving = {
'robot': {
'robot1': [SoMessage(),
SoMessage(None, None, Vector3(2, 2, 0),
Quaternion(), Vector3(), 1, 1.0, 1.0, 1.0,
0, None, True, [])],
'robot2': [SoMessage(),
SoMessage(None, None, Vector3(5, 6, 0),
Quaternion(), Vector3(), 1, 2.0, 1.0, 1.0,
0, None, True, [])],
'robot3': [SoMessage(),
SoMessage(),
SoMessage(None, None, Vector3(1, 2, 0),
Quaternion(), Vector3(), 1, 4.0, 1.0, 1.0,
0, None, True, [])],
'robot4': []
}
}
bffr._static = {
'None': [SoMessage(None, None, Vector3(5, 6, 5), Quaternion(),
Vector3(), 1, 1.0, 1.0, 1.0, 0, None, False,
[]), SoMessage()],
}
bffr._own_pos = [
SoMessage(None, None, Vector3(1, 1, 1), Quaternion(), Vector3(),
1, 1.0, 1.0, 1.0, 0, None, False, [])]
result = [
SoMessage(None, None, Vector3(1, 2, 0), Quaternion(), Vector3(), 1,
4.0, 1.0, 1.0, 0, None, True, []),
SoMessage(None, None, Vector3(2, 2, 0), Quaternion(), Vector3(), 1,
1.0, 1.0, 1.0, 0, None, True, [])]
self.assertEqual(bffr.agent_list([bffr.pose_frame], time=self.now), result)
self.assertEqual(bffr.agent_list([bffr.pose_frame], time=self.now), result)
def test_static_list_angle(self):
"""
test static_list_angle method
:return:
"""
bffr = SoBuffer(view_distance=2.0)
bffr._own_pos = [
SoMessage(None, None, Vector3(1, 1, 0), Quaternion(),
Vector3(1, 0, 0), 1, 1.0, 1.0, 1.0, 0, None, False, [])]
bffr._static = {
'None': [SoMessage(None, None, Vector3(2, 1, 3), Quaternion(),
Vector3(), 1, 1.0, 0.1, 1.0, 0, None, False,
[]),
SoMessage(None, None, Vector3(3, 3, 0), Quaternion(),
Vector3(), -1, 0.2, 0.5, 1.0, 0, None, False,
[]),
SoMessage(None, None, Vector3(2, 2, 0), Quaternion(),
Vector3(), 1, 5.0, 0.2, 1.0, 0, None, False,
[]),
],
'Center': [SoMessage(None, None, Vector3(1, 1.8, 0), Quaternion(),
Vector3(), -1, 8.0, 1.0, 1.0, 0, None, False,
[]),
SoMessage(None, None, Vector3(0, 0, 0), Quaternion(),
Vector3(), -1, 3.0, 0.1, 1.0, 0, None, False,
[])]
}
result = [SoMessage(None, None, Vector3(1, 1.8, 0), Quaternion(),
Vector3(), -1, 8.0, 1.0, 1.0, 0, None, False, []),
SoMessage(None, None, Vector3(0, 0, 0), Quaternion(),
Vector3(), -1, 3.0, 0.1, 1.0, 0, None, False, [])]
self.assertEqual(bffr.static_list_angle(['Center'], np.pi, time=self.now), result)
self.assertEqual(bffr.static_list_angle(['Center'], np.pi/2, time=self.now),
[SoMessage(None, None, Vector3(1, 1.8, 0),
Quaternion(), Vector3(), -1, 8.0, 1.0, 1.0,
0, None, False, [])])
self.assertEqual(bffr.static_list_angle(['Center'], 0, time=self.now), [])
def test_max_attractive_gradient(self):
"""
test max attractive and strongest gradient methods
"""
bffr = SoBuffer(view_distance=2.0)
bffr._static = {
'None': [SoMessage(None, None, Vector3(2, 1, 3), Quaternion(),
Vector3(), 1, 1.0, 0.1, 1.0, 0, None, False,
[]),
SoMessage(None, None, Vector3(3, 3, 0), Quaternion(),
Vector3(), -1, 0.2, 0.5, 1.0, 0, None, False,
[]),
SoMessage(None, None, Vector3(2, 2, 0), Quaternion(),
Vector3(), 1, 5.0, 0.2, 1.0, 0, None, False,
[]),
],
'Center': [SoMessage(None, None, Vector3(1, 1, 1), Quaternion(),
Vector3(), -1, 8.0, 1.0, 1.0, 0, None, False,
[]),
SoMessage(None, None, Vector3(3, 1, 2), Quaternion(),
Vector3(), -1, 3.0, 0.1, 1.0, 0, None, False,
[])]
}
bffr._own_pos = [
SoMessage(None, None, Vector3(1, 1, 1), Quaternion(), Vector3(), 1,
1.0, 0, 1.0, 0, None, False, [])]
self.assertEqual(bffr.max_attractive_gradient(time=self.now),
SoMessage(None, None, Vector3(2, 2, 0), Quaternion(),
Vector3(), 1, 5.0, 0.2, 1.0, 0, None, False,
[]))
self.assertEqual(bffr.min_attractive_gradient(time=self.now),
SoMessage(None, None, Vector3(2, 1, 3), Quaternion(),
Vector3(), 1, 1.0, 0.1, 1.0, 0, None, False,
[]))
self.assertEqual(bffr.strongest_gradient(time=self.now),
SoMessage(None, None, Vector3(1, 1, 1), Quaternion(),
Vector3(), -1, 8.0, 1.0, 1.0, 0, None,
False, []))
def test_follow_max_reach(self):
"""
test FollowMin Mechanism
:return:
"""
bffr = SoBuffer()
chem = FollowMaxReach(bffr, moving=False, static=True, maxvel=np.inf)
bffr._own_pos = [
SoMessage(None, None, Vector3(1, 2, 3), Quaternion(), Vector3(),
-1, 3.0, 0.0, 1.0, 0, None, False, [])
]
bffr._moving = {
'robot1': {
'None': [
SoMessage(None, None, Vector3(1, 2, 3), Quaternion(),
Vector3(), -1, 3.0, 0.0, 1.0, 0, None, False, [])
]
},
'robot2': {}
}
bffr._static = {
'gradient': [
SoMessage(None, None, Vector3(2, 3, 1), Quaternion(),
Vector3(), -1, 3.0, 1.0, 1.0, 0, None, False, []),
SoMessage(None, None, Vector3(2, 2, 2), Quaternion(),
Vector3(), 1, 1.0, 1.0, 1.0, 0, None, False, [])
],
'None': [
SoMessage(None, None, Vector3(7, 3, 2), Quaternion(),
Vector3(), -1, 3.0, 1.0, 1.0, 0, None, False, []),
SoMessage(None, None, Vector3(5, 6, 3), Quaternion(),
Vector3(), 1, 2.0, 1.0, 1.0, 0, None, False, [])
],
'test': [
SoMessage(None, None, Vector3(5, 3, 2), Quaternion(),
Vector3(), -1, 3.0, 1.0, 1.0, 0, None, False, []),
SoMessage(None, None, Vector3(7, 2, 3), Quaternion(),
Vector3(), -1, 3.0, 1.0, 1.0, 0, None, False, []),
SoMessage(None, None, Vector3(1, 2, 6), Quaternion(),
Vector3(), 1, 4.0, 1.0, 1.0, 0, None, False, [])
]
}
# with all frameIDs
result = chem.move()
result.x = round(result.x, 2)
result.y = round(result.y, 2)
result.z = round(result.z, 2)
self.assertEqual(result, Vector3(0.0, 0.0, 0.5))
# only one frameID considered - no gradient within view
chem.frames = ['None']
result = chem.move()
self.assertEqual(result, None)
# two frameIDs
chem.frames = ['gradient']
result = chem.move()
result.x = round(result.x, 2)
result.y = round(result.y, 2)
result.z = round(result.z, 2)
self.assertEqual(result, Vector3(0.29, 0.0, -0.29))
def test_chemotaxis_ge(self):
"""
test chemotaxis ge mechanism
:return:
"""
bffr = SoBuffer()
chem = ChemotaxisGe(bffr, moving=False, static=True, maxvel=np.inf)
bffr._own_pos = [
SoMessage(None, None, Vector3(1, -2, 0), Quaternion(), Vector3(),
-1, 3.0, 0.0, 1.0, 0, None, False, [])
]
bffr._static = {
'gradient': [
SoMessage(None, None, Vector3(2, 3, 1), Quaternion(),
Vector3(), -1, 4.0, 1.0, 1.0, 0, None, False, []),
SoMessage(None, None, Vector3(1, 0, 0), Quaternion(),
Vector3(), 1, 3.0, 1.0, 1.0, 0, None, False, [])
],
'None': [
SoMessage(None, None, Vector3(4, 2, 0), Quaternion(),
Vector3(), -1, 4.0, 2.0, 1.0, 0, None, False, []),
SoMessage(None, None, Vector3(5, 6, 3), Quaternion(),
Vector3(), 1, 2.0, 1.0, 1.0, 0, None, False, [])
]
}
bffr._moving = {
'robot1': {
'None': [
SoMessage(None, None, Vector3(1, -2, 1), Quaternion(),
Vector3(), -1, 4.0, 0.0, 1.0, 0, None, False, [])
]
},
'robot2': {}
}
# only one frameID + repulsive gradient is not considered
chem.frames = ['gradient']
result = chem.move()
result.x = round(result.x, 2)
result.y = round(result.y, 2)
result.z = round(result.z, 2)
self.assertEqual(result, Vector3(0.0, 1.0, 0.0))
# all frameIDs
chem.frames = []
result = chem.move()
result.x = round(result.x, 2)
result.y = round(result.y, 2)
result.z = round(result.z, 2)
self.assertEqual(result, Vector3(-0.02, 0.98, 0.0))
# no gradient within view
chem.frames = ['test']
result = chem.move()
self.assertEqual(result, None)
bffr._static = {
'test': [
SoMessage(None, None, Vector3(0, -1, 2), Quaternion(),
Vector3(), -1, 1.0, 1.0, 1.0, 0, None, False, [])
]
}
chem.frames = ['test']
result = chem.move()
self.assertEqual(result, Vector3())
def test_chemotaxis_balch(self):
"""
test chemotaxis balch mechanism
:return:
"""
bffr = SoBuffer()
chem = ChemotaxisBalch(bffr, moving=False, static=True, maxvel=np.inf)
bffr._own_pos = [
SoMessage(None, None, Vector3(1, 2, 3), Quaternion(), Vector3(),
-1, 3.0, 0.0, 1.0, 0, None, False, [])
]
bffr._static = {
'gradient': [
SoMessage(None, None, Vector3(2, 3, 1), Quaternion(),
Vector3(), -1, 1.0, 1.0, 1.0, 0, None, False, []),
SoMessage(None, None, Vector3(2, 2, 2), Quaternion(),
Vector3(), 1, 1.0, 1.0, 1.0, 0, None, False, [])
],
'None': [
SoMessage(None, None, Vector3(0, 3, 2), Quaternion(),
Vector3(), -1, 3.0, 1.0, 1.0, 0, None, False, []),
SoMessage(None, None, Vector3(5, 6, 3), Quaternion(),
Vector3(), 1, 2.0, 1.0, 1.0, 0, None, False, [])
]
}
# only one frameID + repulsive gradient is not considered
chem.frames = ['gradient']
result = chem.move()
result.x = round(result.x, 2)
result.y = round(result.y, 2)
result.z = round(result.z, 2)
self.assertEqual(result, Vector3(0.29, 0.0, -0.29))
# all frameIDs
chem.frames = []
result = chem.move()
result.x = round(result.x, 2)
result.y = round(result.y, 2)
result.z = round(result.z, 2)
self.assertEqual(result, Vector3(0.73, -0.44, 0.14))
# not existing frame ID - no gradient will be followed
chem.frames = ['test']
result = chem.move()
self.assertEqual(result, None)
bffr._static = {
'test': [
SoMessage(None, None, Vector3(2, 3, 1), Quaternion(),
Vector3(), -1, 1.0, 1.0, 1.0, 0, None, False, [])
]
}
chem.frames = ['test']
result = chem.move()
self.assertEqual(result, Vector3())
bffr._static = {
'test': [
SoMessage(None, None, Vector3(2, 3, 1), Quaternion(),
Vector3(), -1, 1.0, 1.0, 1.0, 0, None, False, [])
]
}
chem.frames = ['test']
result = chem.move()
self.assertEqual(result, Vector3())
def test_evaporation_buffer(self):
"""
test evaporation of buffer data using evaporate_buffer method
"""
bffr = SoBuffer(aggregation=AGGREGATION.MAX, min_diffusion=1.0)
now = rospy.Time(secs=500000)
data = {'None': [ # message has goal radius - should be kept
SoMessage(Header(None, now - rospy.Duration(45), 'None'), 'None',
Vector3(1, 1, 0), Quaternion(), Vector3(), 1, 4.0, 1.0,
0.8, 5, now - rospy.Duration(45), False, []),
# evaporation time is zero cases
SoMessage(Header(None, now - rospy.Duration(45), 'None'), 'None',
Vector3(1, 1, 0), Quaternion(), Vector3(), 1, 4.0, 1.0,
0.8, 0, now - rospy.Duration(45), False, []),
SoMessage(Header(None, now - rospy.Duration(45), 'None'), 'None',
Vector3(1, 1, 0), Quaternion(), Vector3(), 1, 4.0, 0.0,
1.0, 0, now - rospy.Duration(45), False, []),
SoMessage(Header(None, now - rospy.Duration(45), 'None'), 'None',
Vector3(1, 1, 0), Quaternion(), Vector3(), 1, 4.0, 1.0,
1.0, 0, now - rospy.Duration(45), False, []),
# messages without goal radius - will be sorted out based on
# min diffusion
SoMessage(Header(None, now - rospy.Duration(20), 'None'), 'None',
Vector3(2, 2, 0), Quaternion(), Vector3(), 1, 4.0, 0.0,
0.75, 5, now - rospy.Duration(20), False, []),
SoMessage(Header(None, now - rospy.Duration(5), 'None'), 'None',
Vector3(5, 5, 0), Quaternion(), Vector3(), 1, 4.0, 0.0,
0.8, 3, now - rospy.Duration(5), False, []),
SoMessage(Header(None, now, 'None'), 'None', Vector3(6, 6, 0),
Quaternion(), Vector3(), 1, 4.0, 0.0, 0.8, 5, now, False,
[])
], 'gradient': [
SoMessage(Header(None, now - rospy.Duration(45), 'gradient'),
'None', Vector3(1, 1, 0), Quaternion(), Vector3(), 1,
4.0, 0.0, 0.8, 5, now - rospy.Duration(45), False, []),
SoMessage(Header(None, now - rospy.Duration(15), 'gradient'),
'None', Vector3(3, 3, 0), Quaternion(), Vector3(), 1,
4.0, 0.0, 0.6, 5, now - rospy.Duration(15), False, [])
], 'robo': [
SoMessage(Header(None, now - rospy.Duration(10), 'robo'), 'None',
Vector3(4, 4, 0), Quaternion(), Vector3(), 1, 4.0, 0.0,
0.8, 4, now - rospy.Duration(10), False, [])]}
bffr._static = deepcopy(data)
bffr._evaporate_buffer(time=now)
data = {'None': [
SoMessage(Header(None, now - rospy.Duration(45), 'None'), 'None',
Vector3(1, 1, 0), Quaternion(), Vector3(), 1, 4.0 *
(0.8 ** 9), 1.0, 0.8, 5, now, False, []),
SoMessage(Header(None, now - rospy.Duration(45), 'None'), 'None',
Vector3(1, 1, 0), Quaternion(), Vector3(), 1, 0.0, 1.0,
0.8, 0, now - rospy.Duration(45), False, []),
SoMessage(Header(None, now - rospy.Duration(45), 'None'), 'None',
Vector3(1, 1, 0), Quaternion(), Vector3(), 1, 4.0, 0.0,
1.0, 0, now - rospy.Duration(45), False, []),
SoMessage(Header(None, now - rospy.Duration(45), 'None'), 'None',
Vector3(1, 1, 0), Quaternion(), Vector3(), 1, 4.0, 1.0,
1.0, 0, now - rospy.Duration(45), False, []),
SoMessage(Header(None, now - rospy.Duration(20), 'None'), 'None',
Vector3(2, 2, 0), Quaternion(), Vector3(), 1,
4.0 * (0.75 ** 4), 0.0, 0.75, 5, now, False, []),
SoMessage(Header(None, now - rospy.Duration(5), 'None'), 'None',
Vector3(5, 5, 0), Quaternion(), Vector3(), 1, 4.0 * 0.8,
0.0, 0.8, 3, now - rospy.Duration(2), False, []),
SoMessage(Header(None, now, 'None'), 'None', Vector3(6, 6, 0),
Quaternion(), Vector3(), 1, 4.0, 0.0, 0.8, 5, now,
False, [])],
'gradient': [],
'robo': [
SoMessage(Header(None, now - rospy.Duration(10), 'robo'),
'None', Vector3(4, 4, 0), Quaternion(), Vector3(), 1,
4.0 * (0.8 ** 2), 0.0, 0.8, 4,
now - rospy.Duration(2), False, [])
]}
self.assertEqual(bffr._static, data)