def test_gossip(self):
"""
test class gossip max
"""
bffr = SoBuffer()
gossip = GossipMax(bffr, 'gossip', 'max')
bffr._moving = {
'gossip': {
'robot1': [
SoMessage(None, None, Vector3(2, 2, 0), Quaternion(),
Vector3(), 1, 1.0, 1.0, 1.0, 0, None, True,
{'max': 1.0})
],
'robot2': [
SoMessage(None, None, Vector3(1, 2, 0), Quaternion(),
Vector3(), 1, 2.0, 1.0, 1.0, 0, None, True,
{'max': 4.0})
],
'robot4': [
SoMessage(None, None, Vector3(5, 6, 0), Quaternion(),
Vector3(), 1, 2.0, 1.0, 1.0, 0, None, True,
{'max': 8.9})
]
}
}
bffr._own_pos = [
SoMessage(None, None, Vector3(1, 1, 0), Quaternion(), Vector3(), 1,
1.0, 1.0, 1.0, 0, None, False, {'max': '3.0'})
]
self.assertEqual(gossip.calc_value()[0], 4.0)
def test_morphogenesis(self):
"""
test class morphogenesis barycenter
"""
bffr = SoBuffer()
morph = MorphogenesisBarycenter(bffr, 'morphogenesis', 'dist')
bffr._moving = {
'morphogenesis': {
'robot1': [
SoMessage(None, None, Vector3(2, 2, 0), Quaternion(),
Vector3(), 1, 1.0, 1.0, 1.0, 0, None, True,
{'dist': 7.0})
],
'robto2': [
SoMessage(None, None, Vector3(1, 2, 0), Quaternion(),
Vector3(), 1, 2.0, 1.0, 1.0, 0, None, True,
{'dist': 4.0})
]
}
}
bffr._own_pos = [
SoMessage(None, None, Vector3(1, 1, 0), Quaternion(), Vector3(), 1,
1.0, 1.0, 1.0, 0, None, False, {'dist': 5.0})
]
self.assertEqual(morph.state, 'None')
self.assertEqual(morph.calc_value()[0], np.sqrt(2) + 1)
self.assertEqual(morph.calc_value()[1], 'Center')
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_set(self):
"""
test agent set function
:return:
"""
bffr = SoBuffer(view_distance=2.0)
self.assertEqual(bffr.agent_set([bffr.pose_frame], time=self.now), [])
# 2 neighbors within view, one outside view
bffr._moving = {
'robot': {
'robot1': [SoMessage(None, None, Vector3(0, 2, 0),
Quaternion(), Vector3(), 1, 1.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, [])],
'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._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(0, 2, 0), Quaternion(), Vector3(),
1, 1.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, [])
]
agent_set = bffr.agent_set([bffr.pose_frame], time=self.now)
self.assertEqual(agent_set, result)
def test_repulsion_fernandez(self):
"""
test gradient repulsion method based on Fernandez-Marquez et al.
:return:
"""
# Mechanism
bffr = SoBuffer(id='robot1', view_distance=2.0)
repulsion = RepulsionFernandez(bffr)
# no own position specified
self.assertEqual(repulsion.move(), None)
bffr._own_pos = [
SoMessage(Header(None, rospy.Time.now(), 'None'), None,
Vector3(2, 4, 0), Quaternion(), Vector3(), -1, 4.0, 1.0,
1.0, 0, rospy.Time.now(), False, []),
SoMessage(Header(None, rospy.Time.now(), 'None'), None,
Vector3(2, 2, 0), Quaternion(), Vector3(), -1, 1.0, 0.0,
1.0, 0, rospy.Time.now(), False, [])
]
# no neighbors specified
self.assertEqual(repulsion.move(), None)
bffr._moving = {
'robot': {
'robot2': [
SoMessage(Header(None, rospy.Time.now(), 'None'), 'None',
Vector3(1, 3, 0), Quaternion(), Vector3(), -1,
1.0, 1.0, 1.0, 0, rospy.Time.now(), False, [])
],
'robot3': [
SoMessage(Header(None, rospy.Time.now(), 'None'), 'None',
Vector3(2, 2, 0), Quaternion(), Vector3(), -1,
4.0, 1.0, 1.0, 0, rospy.Time.now(), False, []),
SoMessage(Header(None, rospy.Time.now(), 'None'), 'None',
Vector3(3, 2, 0), Quaternion(), Vector3(), -1,
1.0, 0.8, 1.0, 0, rospy.Time.now(), False, [])
],
'robot4': []
}
}
# calculate resulting vector
result = repulsion.move()
result.x = round(result.x, 2)
result.y = round(result.y, 2)
result.z = round(result.z, 2)
# calculate vector
self.assertEqual(result, Vector3(-0.88, -0.48, 0))
# neighbor within goal_radius - returns vector with
# ||vector|| = repulsion_radius
bffr._moving = {
'robot': {
'robot2': [
SoMessage(Header(None, rospy.Time.now(), 'None'), None,
Vector3(2, 2, 0), Quaternion(), Vector3(), -1,
2.0, 1.0, 1.0, 0, rospy.Time.now(), False, [])
]
}
}
d = round(so_data.calc.vector_length(repulsion.move()), 0)
# calculate vector
self.assertEqual(d, 1.0)
def test_gradient_repulsion(self):
"""
test gradient repulsion method based on gradients
:return:
"""
bffr = SoBuffer(id='robot1')
repulsion = RepulsionGradient(bffr)
# no own position specified
self.assertEqual(repulsion.move(), None)
bffr._own_pos = [
SoMessage(Header(None, rospy.Time.now(), 'None'), None,
Vector3(2, 4, 0), Quaternion(), Vector3(), 1, 4.0, 0.0,
1.0, 0, None, False, []),
SoMessage(Header(None, rospy.Time.now(), 'None'), None,
Vector3(2, 2, 0), Quaternion(), Vector3(), 1, 2.0, 0.0,
1.0, 0, None, False, [])
]
# no neighbors specified
self.assertEqual(repulsion.move(), None)
bffr._moving = {
'robot': {
'robot2': [
SoMessage(Header(None, rospy.Time.now(), 'robot'),
'robot2', Vector3(1, 3, 0), Quaternion(),
Vector3(), -1, 1.0, 1.0, 1.0, 0, None, False, [])
],
'robot3': [
SoMessage(Header(None, rospy.Time.now(),
'robot'), 'robot3', Vector3(2, 2, 0),
Quaternion(), Vector3(), -1, 4.0, 1.0, 1.0, 0,
None, False, []),
SoMessage(Header(None, rospy.Time.now(), 'robot'),
'robot3', Vector3(3, 2, 0), Quaternion(),
Vector3(), -1, 1.0, 0.8, 1.0, 0, None, False, [])
]
}
}
# calculate resulting vector
result = repulsion.move()
result.x = round(result.x, 2)
result.y = round(result.y, 2)
result.z = round(result.z, 2)
# calculate vector
self.assertEqual(result, Vector3(-0.39, -0.41, 0.0))
# neighbor within goal_radius - returns vector with
# ||vector|| = repulsion_radius
bffr._moving = {
'robot': {
'robot2': [
SoMessage(Header(None, rospy.Time.now(), 'robot'),
'robot2', Vector3(2, 1.5, 0), Quaternion(),
Vector3(), -1, 2.0, 1.0, 1.0, 0, None, False, [])
]
}
}
d = round(so_data.calc.vector_length(repulsion.move()), 0)
# calculate vector
self.assertEqual(d, 2.0)
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())
