def test_store_data_ev(self):
"""
test store_data method, evaporation of received data
"""
bffr = SoBuffer(aggregation={'DEFAULT': AGGREGATION.MAX})
testlist = {'None': []}
now = rospy.Time(secs=500000)
# not to be stored - ev time is zero, no goal radius
msg = SoMessage(None, None, Vector3(2, 2, 0), Quaternion(), Vector3(),
1, 4.0, 0.0, 0.3, 0, None, False, [])
bffr.store_data(msg, now)
# received data is older than stored one, will be evaporated
# and than compared
msg = SoMessage(Header(None, now - rospy.Duration(10), None), None,
Vector3(2, 2, 0), Quaternion(), Vector3(), 1, 4.0, 1.0,
0.8, 5, now - rospy.Duration(10), False, [])
result = SoMessage(Header(None, now - rospy.Duration(10), 'None'),
'None', Vector3(2, 2, 0), Quaternion(), Vector3(),
1, 4.0 * (0.8 ** 2), 1.0, 0.8, 5, now, False, [])
testlist['None'].append(result)
bffr.store_data(msg, now)
msg = SoMessage(Header(None, now - rospy.Duration(15), None), None,
Vector3(2, 2, 0), Quaternion(), Vector3(), 1, 4.0, 1.0,
0.8, 5, now - rospy.Duration(15), False, [])
bffr.store_data(msg, now)
self.assertEqual(bffr._static, testlist)
def test_calc_attractive_gradient_ge(self):
"""
test calc attractive gradient method based on Ge & Cui paper
:return:
"""
# robot within diffusion radius + goal radius of gradient
grad = SoMessage(None, None, Vector3(3, 5, 10), Quaternion(),
Vector3(), -1, 10.0, 2.0, 1.0, 0, None, False, [])
pose = SoMessage(None, None, Vector3(0, 0, 0), Quaternion(), Vector3(),
-1, 3.0, 0.0, 1.0, 0, None, False, [])
result = gradient.calc_attractive_gradient_ge(grad, pose)
result.x = round(result.x, 2)
result.y = round(result.y, 2)
result.z = round(result.z, 2)
self.assertEqual(result, Vector3(2.48, 4.14, 8.27))
# robot within goal radius of gradient
pose = SoMessage(None, None, Vector3(3, 7, 10), Quaternion(),
Vector3(), -1, 3.0, 0.0, 1.0, 0, None, False, [])
self.assertEqual(gradient.calc_attractive_gradient_ge(grad, pose),
Vector3(0, 0, 0))
# robot without radius + goal radius of gradient, but gradient is
# within view_distance
grad = SoMessage(None, None, Vector3(2, 3, 6), Quaternion(), Vector3(),
-1, 4.0, 2.0, 1.0, 0, None, False, [])
pose = SoMessage(None, None, Vector3(0, 0, 0), Quaternion(), Vector3(),
-1, 3.0, 0.0, 1.0, 0, None, False, [])
self.assertEqual(
gradient.calc_attractive_gradient_ge(grad, pose),
Vector3((2.0 / 7.0) * 4.0, (3.0 / 7.0) * 4.0, (6.0 / 7.0) * 4.0))
def test_aggregation_newframe(self):
"""
test aggregation_newframe method
:return:
"""
bffr = SoBuffer(aggregation={'DEFAULT': AGGREGATION.NEWFRAME})
now = rospy.Time(secs=500000)
testlist = {'robot': []}
# replaced by fourth robot1 message
msg = SoMessage(Header(None, now - rospy.Duration(10), 'robot'),
'robot1', Vector3(2.1, 2.2, 0), Quaternion(),
Vector3(), 1, 4.0, 1.0, 0.8, 5, None, False, [])
bffr.store_data(msg, now)
msg = SoMessage(Header(None, now - rospy.Duration(5), 'robot'),
'robot5', Vector3(2.1, 2.2, 0), Quaternion(),
Vector3(), 1, 4.0, 1.0, 0.8, 5, None, False, [])
bffr.store_data(msg, now)
msg = SoMessage(Header(None, now, 'robot'), 'robot3',
Vector3(2.1, 2.2, 0), Quaternion(), Vector3(), 1, 4.0,
1.0, 0.8, 5, None, False, [])
bffr.store_data(msg, now)
msg = SoMessage(Header(None, now, 'robot'), 'robot1',
Vector3(2.1, 2.2, 0), Quaternion(), Vector3(), 1, 4.0,
1.0, 0.8, 5, None, False, [])
testlist['robot'].append(msg)
bffr.store_data(msg, now)
self.assertEqual(bffr._static, testlist)
def test_repulsive_gradient_ge(self):
"""
test repulsion vector calculation based on Ge
:return:
"""
grad = SoMessage(None, None, Vector3(4, 2, 0), Quaternion(), Vector3(),
-1, 4.0, 2.0, 1.0, 0, None, False, [])
goal = SoMessage(None, None, Vector3(1, 0, 0), Quaternion(), Vector3(),
1, 3.0, 1.0, 1.0, 0, None, False, [])
# diffusion and goal_radius of gradient shorter than distance
pose = SoMessage(None, None, Vector3(8, 8, 0), Quaternion(), Vector3(),
-1, 3.0, 0.0, 1.0, 0, None, False, [])
self.assertEqual(gradient.calc_repulsive_gradient_ge(grad, goal, pose),
Vector3())
# agent within goal area of repulsive gradient
pose = SoMessage(None, None, Vector3(3, 2, 0), Quaternion(), Vector3(),
-1, 3.0, 0.0, 1.0, 0, None, False, [])
self.assertEqual(gradient.calc_repulsive_gradient_ge(grad, goal, pose),
Vector3(-np.inf, np.inf, np.inf))
# robot within reach of gradient
pose = SoMessage(None, None, Vector3(1, -2, 0), Quaternion(),
Vector3(), -1, 3.0, 0.0, 1.0, 0, None, False, [])
v = gradient.calc_repulsive_gradient_ge(grad, goal, pose)
v.x = round(v.x, 2)
v.y = round(v.y, 2)
v.z = round(v.z, 2)
self.assertEqual(v, Vector3(-0.02, -0.02, 0.0))
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_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_calc_attractive_gradient(self):
"""
test _calc_attractive_gradient method for 2D and 3D
"""
# 2D - D < r <= C
grad = SoMessage(None, None, Vector3(3, 4, 0), Quaternion(), Vector3(),
1, 5.0, 1.0, 1.0, 0, None, False, [])
pose = SoMessage(None, None, Vector3(0, 0, 0), Quaternion(), Vector3(),
-1, 3.0, 0.0, 1.0, 0, None, False, [])
self.assertEqual(gradient.calc_attractive_gradient(grad, pose),
Vector3(0.6 * 0.8, 0.8 * 0.8, 0))
# 2D - r > C
grad = SoMessage(None, None, Vector3(3, 4, 0), Quaternion(), Vector3(),
1, 2.0, 1.0, 1.0, 0, None, False, [])
self.assertEqual(
gradient.calc_attractive_gradient(grad, pose),
# Vector3(0.6, 0.8, 0)) # former version was normalizing to a unit vector
Vector3(3, 4, 0))
# 2D - r <= D
grad = SoMessage(None, None, Vector3(3, 4, 0), Quaternion(), Vector3(),
1, 2.0, 5.0, 1.0, 0, None, False, [])
self.assertEqual(gradient.calc_attractive_gradient(grad, pose),
Vector3(0, 0, 0))
# 2D - r > C - non zero robot pose
grad = SoMessage(None, None, Vector3(4, 5, 0), Quaternion(), Vector3(),
1, 2.0, 1.0, 1.0, 0, None, False, [])
pose = SoMessage(None, None, Vector3(1, 1, 0), Quaternion(), Vector3(),
-1, 3.0, 0.0, 1.0, 0, None, False, [])
self.assertEqual(
gradient.calc_attractive_gradient(grad, pose),
# Vector3(0.6, 0.8, 0)) # former version was normalizing to a unit vector
Vector3(3, 4, 0))
# 3D - D < r <= C
grad = SoMessage(None, None, Vector3(3, 5, 10), Quaternion(),
Vector3(), 1, 6.0, 2.0, 1.0, 0, None, False, [])
pose = SoMessage(None, None, Vector3(1, 2, 4), Quaternion(), Vector3(),
-1, 3.0, 0.0, 1.0, 0, None, False, [])
result = Vector3((2.0 / 7.0) * (5.0 / 6.0), (3.0 / 7.0) * (5.0 / 6.0),
(6.0 / 7.0) * (5.0 / 6.0))
self.assertEqual(gradient.calc_attractive_gradient(grad, pose), result)
# 3D - r > C
grad = SoMessage(None, None, Vector3(3, 5, 10), Quaternion(),
Vector3(), 1, 5.0, 2.0, 1.0, 0, None, False, [])
result = Vector3((2.0 / 7.0), (3.0 / 7.0), (6.0 / 7.0))
self.assertEqual(gradient.calc_attractive_gradient(grad, pose), result)
# 3D - r <= D
grad = SoMessage(None, None, Vector3(3, 5, 10), Quaternion(),
Vector3(), 1, 5.0, 7.0, 1.0, 0, None, False, [])
self.assertEqual(gradient.calc_attractive_gradient(grad, pose),
Vector3(0, 0, 0))
def test_calc_repulsive_gradient(self):
"""
test _calc_repulsive_gradient method
"""
# 2D - D < r <= C
grad = SoMessage(None, None, Vector3(3, 4, 0), Quaternion(), Vector3(),
-1, 5.0, 1.0, 1.0, 0, None, False, [])
pose = SoMessage(None, None, Vector3(0, 0, 0), Quaternion(), Vector3(),
-1, 3.0, 0.0, 1.0, 0, None, False, [])
self.assertEqual(gradient.calc_repulsive_gradient(grad, pose),
Vector3(-0.6 * 0.2, -0.8 * 0.2, 0))
# 2D - r > C
grad = SoMessage(None, None, Vector3(3, 4, 0), Quaternion(), Vector3(),
-1, 2.0, 1.0, 1.0, 0, None, False, [])
self.assertEqual(gradient.calc_repulsive_gradient(grad, pose),
Vector3(0, 0, 0))
# 2D - r <= D
grad = SoMessage(None, None, Vector3(3, 4, 0), Quaternion(), Vector3(),
-1, 2.0, 5.0, 1.0, 0, None, False, [])
self.assertEqual(gradient.calc_repulsive_gradient(grad, pose),
Vector3(-1.0 * np.inf, -1.0 * np.inf, np.inf))
# 2D - r > C - non zero robot pose
grad = SoMessage(None, None, Vector3(4, 5, 0), Quaternion(), Vector3(),
-1, 2.0, 1.0, 1.0, 0, None, False, [])
pose = SoMessage(None, None, Vector3(1, 1, 0), Quaternion(), Vector3(),
-1, 3.0, 0.0, 1.0, 0, None, False, [])
self.assertEqual(gradient.calc_repulsive_gradient(grad, pose),
Vector3(0, 0, 0))
# 3D - D < r <= C
grad = SoMessage(None, None, Vector3(3, 5, 10), Quaternion(),
Vector3(), -1, 6.0, 2.0, 1.0, 0, None, False, [])
pose = SoMessage(None, None, Vector3(1, 2, 4), Quaternion(), Vector3(),
-1, 3.0, 0.0, 1.0, 0, None, False, [])
result = Vector3((-2.0 / 7.0) * (1.0 / 6.0),
(-3.0 / 7.0) * (1.0 / 6.0),
(-6.0 / 7.0) * (1.0 / 6.0))
self.assertEqual(gradient.calc_repulsive_gradient(grad, pose), result)
# 3D - r > C
grad = SoMessage(None, None, Vector3(3, 5, 10), Quaternion(),
Vector3(), -1, 5.0, 2.0, 1.0, 0, None, False, [])
self.assertEqual(gradient.calc_repulsive_gradient(grad, pose),
Vector3(0, 0, 0))
# 3D - r <= D
grad = SoMessage(None, None, Vector3(3, 5, 10), Quaternion(),
Vector3(), -1, 5.0, 7.0, 1.0, 0, None, False, [])
self.assertEqual(gradient.calc_repulsive_gradient(grad, pose),
Vector3(-1 * np.inf, -1 * np.inf, -1 * np.inf))
def test_agent_velocity(self):
"""
unit test for agent velocity function
"""
# no movement at all
p1 = SoMessage()
p2 = SoMessage()
self.assertEqual(so_data.flocking.agent_velocity(p1, p2), Vector3())
p1 = SoMessage(Header(None, rospy.Duration(0), ''), None,
Vector3(2, 2, 0), Quaternion(), Vector3(), 1, 4.0, 1.0,
0.8, 5, rospy.Duration(0), True, [])
p2 = SoMessage(Header(None, rospy.Duration(5), ''), None,
Vector3(2, 2, 0), Quaternion(), Vector3(), 1, 4.0, 1.0,
0.8, 5, rospy.Duration(5), True, [])
self.assertEqual(so_data.flocking.agent_velocity(p1, p2),
Vector3(0, 0, 0))
# 5 seconds passed
p1 = SoMessage(Header(None, rospy.Duration(0), ''), None,
Vector3(0, 0, 0), Quaternion(), Vector3(), 1, 4.0, 1.0,
0.8, 5, rospy.Duration(0), True, [])
p2 = SoMessage(Header(None, rospy.Duration(5), ''), None,
Vector3(2, 2, 0), Quaternion(), Vector3(), 1, 4.0, 1.0,
0.8, 5, rospy.Duration(5), True, [])
self.assertEqual(so_data.flocking.agent_velocity(p1, p2),
Vector3(0.4, 0.4, 0))
# nanoseconds passed
p1 = SoMessage(Header(None, rospy.Duration(0, 0), ''), None,
Vector3(0, 0, 0), Quaternion(), Vector3(), 1, 4.0, 1.0,
0.8, 5, rospy.Duration(0, 0), True, [])
p2 = SoMessage(Header(None, rospy.Duration(0, 100000), ''), None,
Vector3(2, 2, 0), Quaternion(), Vector3(), 1, 4.0, 1.0,
0.8, 5, rospy.Duration(0, 100000), True, [])
self.assertEqual(so_data.flocking.agent_velocity(p1, p2),
Vector3(20000.0, 20000.0, 0))
# negative velocity
p1 = SoMessage(Header(None, rospy.Duration(0, 0), ''), None,
Vector3(2, 2, 0), Quaternion(), Vector3(), 1, 4.0, 1.0,
0.8, 5, rospy.Duration(0, 0), True, [])
p2 = SoMessage(Header(None, rospy.Duration(0, 100000), ''), None,
Vector3(0, 0, 0), Quaternion(), Vector3(), 1, 4.0, 1.0,
0.8, 5, rospy.Duration(0, 100000), True, [])
self.assertEqual(so_data.flocking.agent_velocity(p1, p2),
Vector3(-20000.0, -20000.0, 0))
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 __init__(self,
topic,
frame,
id,
message_type=None,
p=Vector3(),
attraction=-1,
diffusion=1.0,
goal_radius=0.5,
ev_factor=1.0,
ev_time=0,
quaternion=Quaternion(),
moving=True,
payload='',
direction=Vector3(1, 0, 0)):
"""
subscription to topic and variable initialization
:param topic: topic to subscribe to
:param frame: header frame id used in SoMessage
:param id: soMessage id used for parent frame
:param message_type: message type of topic
:param p: soMessage pose
:param attraction: soMessage attraction (1) or repulsion (-1)
:param diffusion: soMessage diffusion radius
:param goal_radius: soMessage goal radius
:param ev_factor: soMessage evaporation factor
:param ev_time: soMessage evaporation (delta) time
:param quaternion: soMessage orientation
:param direction: soMessage initial heading direction
:param moving: soMessage boolean to specify static / moving gradient
:param payload: soMessage payload data
"""
# broadcaster for transformed data
self._broadcast = self.init_so_broadcaster()
self._current_msg = SoMessage()
self._id = id
self._frame = frame
self.p = p
self.q = quaternion
self.attraction = attraction
self.diffusion = diffusion
self.goal_radius = goal_radius
self.ev_factor = ev_factor
self.ev_time = ev_time
self.direction = direction
self.moving = moving
self.payload = payload
if message_type is None:
message_type = get_topic_type(topic)
if message_type is not None:
self._sub = rospy.Subscriber(topic, message_type, self.callback)
else:
rospy.logerr("Could not determine message type of: %s. %s", topic,
traceback.format_stack())
def test_de_serialization(self):
test_str = "FoooBArr\n"
test_float = np.pi
test_dict = {'bla': test_str, 'foo': test_float}
msg = SoMessage(None, None, Vector3(2, 2, 0), Quaternion(), Vector3(),
1, 1.0, 1.0, 1.0, 0, None, True, [])
msg.payload = test_dict
original_msg = deepcopy(msg)
bffr = SoBuffer()
broadcaster = SoBroadcaster()
rospy.sleep(0.5)
broadcaster.send_data(msg)
rospy.sleep(0.5)
gradients = bffr.all_gradients()
self.assertEqual(gradients[0].payload, original_msg.payload)
# other simple payload
msg.payload = np.pi
msg.parent_frame = 'float_msg'
original_msg = deepcopy(msg)
broadcaster.send_data(msg)
rospy.sleep(0.5)
gradients = bffr.all_gradients()
self.assertEqual(gradients[0].payload, original_msg.payload)
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_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_store_data_fids(self):
"""
test store_data method with different frame IDs (all)
"""
bffr = SoBuffer(aggregation={'DEFAULT': AGGREGATION.MAX})
testlist = {'None': [], 'grad': []}
now = rospy.Time(secs=500000)
msg = SoMessage(Header(None, now, 'None'), 'None', Vector3(2, 2, 0),
Quaternion(), Vector3(), 1, 4.0, 1.0, 1.0, 0, None,
False, [])
bffr.store_data(msg, now)
msg = SoMessage(Header(None, now, 'grad'), 'None', Vector3(3, 3, 0),
Quaternion(), Vector3(), 1, 4.0, 1.0, 1.0, 0, None,
False, [])
testlist['grad'].append(msg)
bffr.store_data(msg, now)
msg = SoMessage(Header(None, now, 'None'), 'None', Vector3(3, 3, 0),
Quaternion(), Vector3(), 1, 3.0, 1.0, 1.0, 0, None,
False, [])
testlist['None'].append(deepcopy(msg))
bffr.store_data(msg, now)
msg = SoMessage(Header(None, now, 'grad'), 'None', Vector3(5, 5, 0),
Quaternion(), Vector3(), 1, 4.0, 1.0, 1.0, 0, None,
False, [])
testlist['grad'].append(deepcopy(msg))
bffr.store_data(msg, now)
msg = SoMessage(Header(None, now, 'None'), 'None', Vector3(2, 2, 0),
Quaternion(), Vector3(), 1, 5.0, 1.0, 1.0, 0, None,
False, [])
testlist['None'].append(deepcopy(msg))
bffr.store_data(msg, now)
self.assertEqual(bffr._static, testlist)
def test_store_data_min(self):
"""
test store_data method aggregation option = min
"""
bffr = SoBuffer(aggregation={'DEFAULT': AGGREGATION.MIN},
store_all=True)
now = rospy.Time(secs=500000)
testlist = []
msg = SoMessage(None, None, Vector3(2, 2, 0), Quaternion(), Vector3(),
1, 4.0, 1.0, 1.0, 0, None, False, [])
testlist.append(deepcopy(msg))
bffr.store_data(msg, now)
msg = SoMessage(None, None, Vector3(3, 3, 0), Quaternion(), Vector3(),
1, 4.0, 1.0, 1.0, 0, None, False, [])
bffr.store_data(msg, now)
msg = SoMessage(None, None, Vector3(3, 3, 0), Quaternion(), Vector3(),
1, 3.0, 1.0, 1.0, 0, None, False, [])
testlist.append(deepcopy(msg))
bffr.store_data(msg, now)
msg = SoMessage(None, None, Vector3(5, 5, 0), Quaternion(), Vector3(),
1, 4.0, 1.0, 1.0, 0, None, False, [])
testlist.append(deepcopy(msg))
bffr.store_data(msg, now)
msg = SoMessage(None, None, Vector3(2, 2, 0), Quaternion(), Vector3(),
1, 5.0, 1.0, 1.0, 0, None, False, [])
bffr.store_data(msg, now)
for el in testlist:
el.header.frame_id = 'None'
el.parent_frame = 'None'
self.assertEqual(bffr._static, {'None': testlist})
def test_store_data_max(self):
"""
test store_data method aggregation option = max
"""
bffr = SoBuffer(aggregation={'DEFAULT': AGGREGATION.MAX},
aggregation_distance=1.0)
now = rospy.Time(secs=500000)
testlist = []
msg = SoMessage(None, None, Vector3(2, 0.8, 0), Quaternion(),
Vector3(), 1, 4.0, 1.0, 1.0, 0, None, False, [])
testlist.append(deepcopy(msg))
bffr.store_data(msg, now)
msg = SoMessage(None, None, Vector3(2, 2, 0), Quaternion(), Vector3(),
1, 4.0, 1.0, 1.0, 0, None, False, [])
bffr.store_data(msg, now)
msg = SoMessage(None, None, Vector3(2, 2, 0), Quaternion(), Vector3(),
1, 5.0, 1.0, 1.0, 0, None, False, [])
bffr.store_data(msg, now)
# store max value within aggregation distance!
msg = SoMessage(None, None, Vector3(2, 1.5, 0), Quaternion(),
Vector3(), 1, 6.0, 1.0, 1.0, 0, None, False, [])
testlist.append(deepcopy(msg))
bffr.store_data(msg, now)
# keep max of both
msg = SoMessage(None, None, Vector3(3, 3, 0), Quaternion(), Vector3(),
1, 4.0, 1.0, 1.0, 0, None, False, [])
testlist.append(deepcopy(msg))
bffr.store_data(msg, now)
msg = SoMessage(None, None, Vector3(3, 3, 0), Quaternion(), Vector3(),
1, 3.0, 1.0, 1.0, 0, None, False, [])
bffr.store_data(msg, now)
# only value at this postion / area
msg = SoMessage(None, None, Vector3(5, 5, 0), Quaternion(), Vector3(),
1, 4.0, 1.0, 1.0, 0, None, False, [])
testlist.append(deepcopy(msg))
bffr.store_data(msg, now)
for el in testlist:
el.header.frame_id = 'None'
el.parent_frame = 'None'
self.assertEqual(bffr._static, {'None': testlist})
def test_store_data_avg(self):
"""
test store_data method aggregation option = avg
"""
bffr = SoBuffer(aggregation={'DEFAULT': AGGREGATION.AVG},
min_diffusion=1.0)
now = rospy.Time(secs=500000)
testlist = []
msg = SoMessage(None, None, Vector3(2, 2, 0), Quaternion(), Vector3(),
1, 4.0, 1.0, 1.0, 0, None, False, [])
bffr.store_data(msg, now)
msg = SoMessage(None, None, Vector3(3, 3, 0), Quaternion(), Vector3(),
1, 4.0, 1.0, 1.0, 0, None, False, [])
bffr.store_data(msg, now)
msg = SoMessage(None, None, Vector3(3, 3, 0), Quaternion(), Vector3(),
1, 3.0, 1.0, 1.0, 0, None, False, [])
testlist.append(
SoMessage(None, None, Vector3(3, 3, 0), Quaternion(), Vector3(), 1,
3.5, 1.0, 1.0, 0, None, False, []))
bffr.store_data(msg, now)
msg = SoMessage(None, None, Vector3(5, 5, 0), Quaternion(), Vector3(),
1, 4.0, 1.0, 1.0, 0, None, False, [])
bffr.store_data(msg, now)
msg = SoMessage(None, None, Vector3(2, 1, 0), Quaternion(), Vector3(),
-1, 5.0, 1.0, 1.0, 0, None, False, [])
testlist.append(
SoMessage(None, None, Vector3(2, 1.5, 0), Quaternion(), Vector3(),
-1, 1.0, 0.0, 1.0, 0, None, False, []))
bffr.store_data(msg, now)
msg = SoMessage(None, None, Vector3(5, 5, 0), Quaternion(), Vector3(),
-1, 4.0, 1.0, 1.0, 0, None, False, [])
bffr.store_data(msg, now)
for el in testlist:
el.header.frame_id = 'None'
el.parent_frame = 'None'
self.assertEqual(bffr._static, {'None': testlist})
def test_evaporation_msg(self):
"""
test the evaporation of one specified message
:return:
"""
bffr = SoBuffer(min_diffusion=1.0)
now = rospy.Time(secs=500000)
# with goal radius --> should be kept
msg = 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, [])
result = 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, [])
self.assertEqual(bffr._evaporate_msg(msg, now), result)
# following tests do not seem to be valid!
# ########################################
# without goal radius --> should be deleted
# msg = SoMessage(Header(None, now - rospy.Duration(45), 'None'), None,
# Vector3(1, 1, 0), Quaternion(), Vector3(), 1, 4.0, 0.0,
# 0.8, 5, now - rospy.Duration(45), False, [])
# msg = bffr._evaporate_msg(msg)
# self.assertEqual(msg, None)
# without goal radius & ev time is 0, ev factor < 1
# --> should be deleted
# msg = SoMessage(Header(None, now - rospy.Duration(45), 'None'), None,
# Vector3(1, 1, 0), Quaternion(), Vector3(), 1, 4.0, 0.0,
# 0.8, 0, now - rospy.Duration(45), False, [])
# msg = bffr._evaporate_msg(msg)
# self.assertEqual(msg, None)
# without goal radius & ev time is 0, ev factor == 1.0
# --> kept as it is
msg = 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, [])
result = 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,
[])
self.assertEqual(bffr._evaporate_msg(msg, now), result)
# with goal radius & ev time is 0, ev factor < 1.0
msg = SoMessage(Header(None, now - rospy.Duration(45), 'None'), None,
Vector3(1, 1, 0), Quaternion(), Vector3(), 1, 4.0, 1.0,
0.0, 0, now - rospy.Duration(45), False, [])
result = SoMessage(Header(None, now - rospy.Duration(45), 'None'),
None, Vector3(1, 1, 0), Quaternion(), Vector3(), 1,
0.0, 1.0, 0.0, 0, now - rospy.Duration(45), False,
[])
self.assertEqual(bffr._evaporate_msg(msg, now), result)
def test_store_data_neighbors(self):
"""
test store_data method, store neighbor data and own pos
"""
bffr = SoBuffer(aggregation={'DEFAULT':AGGREGATION.MAX}, id='robot3')
testlist = {'robot': {'robot1': [], 'robot2': []}}
ownpos = []
now = rospy.Time(secs=500000)
# replaced by fourth robot1 message
msg = SoMessage(Header(None, now - rospy.Duration(10), 'robot'),
'robot1', Vector3(2.1, 2.2, 0), Quaternion(),
Vector3(), 1, 4.0, 1.0, 0.8, 5, None, True, [])
bffr.store_data(msg, now)
# position older than newest stored position --> ignore
msg = SoMessage(Header(None, now - rospy.Duration(15), 'robot'),
'robot1', Vector3(2, 2, 0), Quaternion(), Vector3(), 1,
4.0, 1.0, 0.8, 5, None, True, [])
bffr.store_data(msg, now)
# add to position list
msg = SoMessage(Header(None, now - rospy.Duration(5), 'robot'),
'robot1', Vector3(2, 2, 0), Quaternion(), Vector3(), 1,
4.0, 1.0, 0.8, 5, None, True, [])
testlist['robot']['robot1'].append(msg)
bffr.store_data(msg, now)
# add to position list
msg = SoMessage(Header(None, now, 'robot'), 'robot1', Vector3(2, 2, 0),
Quaternion(), Vector3(), 1, 4.0, 1.0, 0.8, 5, None,
True, [])
testlist['robot']['robot1'].append(msg)
bffr.store_data(msg, now)
msg = SoMessage(Header(None, now - rospy.Duration(15), 'robot'),
'robot2', Vector3(2, 2, 0), Quaternion(), Vector3(), 1,
4.0, 1.0, 0.8, 5, None, True, [])
testlist['robot']['robot2'].append(msg)
bffr.store_data(msg, now)
# own position
msg = SoMessage(Header(None, now - rospy.Duration(15), 'robot'),
'robot3', Vector3(2, 2, 0), Quaternion(), Vector3(), 1,
4.0, 1.0, 0.8, 5, None, True, [])
ownpos.append(msg)
bffr.store_data(msg, now)
self.assertEqual(bffr._moving, testlist)
self.assertEqual(bffr._own_pos, ownpos)
def test_store_data_neighbors_False(self):
"""
test store_data method, neighbor gradients will not be stored
:return:
"""
bffr = SoBuffer(aggregation={'DEFAULT': AGGREGATION.MAX},
moving_storage_size=0, id='')
now = rospy.Time(secs=500000)
# replaced by fourth robot1 message
msg = SoMessage(Header(None, now - rospy.Duration(10), 'robot1'), None,
Vector3(2.1, 2.2, 0), Quaternion(), Vector3(), 1, 4.0,
1.0, 0.8, 5, None, True, [])
bffr.store_data(msg, now)
# position older than newest stored position --> ignore
msg = SoMessage(Header(None, now - rospy.Duration(15), 'robot1'), None,
Vector3(2, 2, 0), Quaternion(), Vector3(), 1, 4.0, 1.0,
0.8, 5, None, True, [])
bffr.store_data(msg, now)
# add to position list
msg = SoMessage(Header(None, now - rospy.Duration(5), 'robot1'), None,
Vector3(2, 2, 0), Quaternion(), Vector3(), 1, 4.0, 1.0,
0.8, 5, None, True, [])
bffr.store_data(msg, now)
# add to position list
msg = SoMessage(Header(None, now, 'robot1'), None, Vector3(2, 2, 0),
Quaternion(), Vector3(), 1, 4.0, 1.0, 0.8, 5, None,
True, [])
bffr.store_data(msg, now)
msg = SoMessage(Header(None, now - rospy.Duration(15), 'robot2'), None,
Vector3(2, 2, 0), Quaternion(), Vector3(), 1, 4.0, 1.0,
0.8, 5, None, True, [])
bffr.store_data(msg, now)
# own position
msg = SoMessage(Header(None, now - rospy.Duration(15), 'robot3'), None,
Vector3(2, 2, 0), Quaternion(), Vector3(), 1, 4.0, 1.0,
0.8, 5, None, True, [])
bffr.store_data(msg, now)
self.assertEqual(bffr._moving, {})
self.assertEqual(bffr._own_pos, [])
def test_store_data_fids_selection(self):
"""
test store_data method, only store gradients with specified frameids
"""
bffr = SoBuffer(aggregation={'DEFAULT': AGGREGATION.MAX},
store_all=False, framestorage=['grad', 'silk'])
testlist = {'grad': [], 'silk': []}
now = rospy.Time(secs=500000)
msg = SoMessage(Header(None, now, 'pheromone'), 'None',
Vector3(2, 2, 0), Quaternion(), Vector3(), 1, 4.0, 1.0,
1.0, 0, None, False, [])
bffr.store_data(msg, now)
msg = SoMessage(Header(None, now, 'grad'), 'None', Vector3(3, 3, 0),
Quaternion(), Vector3(), 1, 4.0, 1.0, 1.0, 0, None,
False, [])
testlist['grad'].append(deepcopy(msg))
bffr.store_data(msg, now)
msg = SoMessage(None, None, Vector3(3, 3, 0), Quaternion(), Vector3(),
1, 3.0, 1.0, 1.0, 0, None, False, [])
bffr.store_data(msg, now)
msg = SoMessage(Header(None, now, 'grad'), 'None', Vector3(5, 5, 0),
Quaternion(), Vector3(), 1, 4.0, 1.0, 1.0, 0, None,
False, [])
testlist['grad'].append(deepcopy(msg))
bffr.store_data(msg, now)
msg = SoMessage(Header(None, now, 'silk'), 'None', Vector3(5, 5, 0),
Quaternion(), Vector3(), 1, 4.0, 1.0, 1.0, 0, None,
False, [])
testlist['silk'].append(deepcopy(msg))
bffr.store_data(msg, now)
msg = SoMessage(None, None, Vector3(2, 2, 0), Quaternion(), Vector3(),
1, 5.0, 1.0, 1.0, 0, None, False, [])
bffr.store_data(msg, now)
self.assertEqual(bffr._static, testlist)
def test_payload_serialization(self):
# simple payload
msg = SoMessage(None, None, Vector3(2, 2, 0), Quaternion(), Vector3(), 1, 1.0, 1.0, 1.0, 0, None, True, [])
msg.payload = math.pi
original_msg = deepcopy(msg)
serialized_msg = serialize_msg_payload(msg)
self.assertIsInstance(serialized_msg.payload, str, "payload not serialized")
deserialized_msg = deserialize_msg_payload(serialized_msg)
self.assertEqual(original_msg, deserialized_msg, "de/serialization failed")
# Complex payload
test_str = "FoooBArr\n"
test_float = math.pi
test_dict = {'bla': test_str, 'foo': test_float}
msg = SoMessage(None, None, Vector3(2, 2, 0), Quaternion(), Vector3(), 1, 1.0, 1.0, 1.0, 0, None, True, [])
msg.payload = test_dict
original_msg = deepcopy(msg)
serialized_msg = serialize_msg_payload(msg)
self.assertIsInstance(serialized_msg.payload, str, "payload not serialized")
deserialized_msg = deserialize_msg_payload(serialized_msg)
self.assertEqual(original_msg, deserialized_msg, "de/serialization failed")
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 create_gradient(position,
attraction=0,
diffusion=3.0,
q=Quaternion(),
moving=False,
direction=Vector3(1, 0, 0),
goal_radius=1.0,
payload='',
ev_time=0,
ev_factor=1.0,
frameid='',
parentframe='',
time=None):
"""
creates a soMessage to specify a gradient
:param position: agent position
:param attraction: repulsion(-1), attraction(1)
:param diffusion: diffusion radius
:param q: Quaternion defining rotation
:param direction: initial direction to specify current gradient orientation
:param goal_radius: goal radius of gradient
:param payload: payload data
:param ev_time: evaporation time
:param ev_factor: evaporation factor
:param frameid: header frame id of gradient
:param: parentframe: parent frame id of gradient
:param time: optional time stamp for evaporation calculations, if None time=rospy.Time.now()
:return: soMessage
"""
if time is None:
time = rospy.Time.now()
msg = SoMessage()
msg.p = position
msg.q = q
msg.direction = direction
msg.attraction = attraction
msg.header.stamp = time
msg.header.frame_id = frameid
msg.parent_frame = parentframe
msg.diffusion = diffusion
msg.goal_radius = goal_radius
msg.ev_factor = ev_factor
msg.ev_time = ev_time
msg.ev_stamp = time
msg.moving = moving
msg.payload = payload
return msg
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())
