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