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})