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_store_data_various_aggregations(self):
"""
test store data method with different aggregation options for
different frame IDs
:return:
"""
bffr = SoBuffer(aggregation={'DEFAULT': AGGREGATION.MAX,
'grad': AGGREGATION.AVG})
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=msg, time=now)
msg = SoMessage(Header(None, now, 'grad'), 'None', Vector3(4, 5, 0),
Quaternion(), Vector3(), 1, 4.0, 1.0, 1.0, 0, None,
False, [])
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(SoMessage(Header(None, now, 'grad'), 'None',
Vector3(4.5, 5, 0), Quaternion(),
Vector3(), 1, 4.0, 1.0, 1.0, 0, None,
False, []))
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_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_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_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_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_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_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_store_data_max_aggregation_distance(self):
"""
test store_data method aggregation option = max,
aggregation distance = 0.0
"""
bffr = SoBuffer(aggregation={'DEFAULT': AGGREGATION.MAX},
aggregation_distance=0.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(2, 2, 0), Quaternion(), Vector3(),
1, 5.0, 1.0, 1.0, 0, None, False, [])
testlist.append(deepcopy(msg))
bffr.store_data(msg, now)
# store max value within aggregation distance!
msg = SoMessage(None, None, Vector3(2, 1, 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})
