def test_gossip(self): """ test class gossip max """ bffr = SoBuffer() gossip = GossipMax(bffr, 'gossip', 'max') bffr._moving = { 'gossip': { 'robot1': [ SoMessage(None, None, Vector3(2, 2, 0), Quaternion(), Vector3(), 1, 1.0, 1.0, 1.0, 0, None, True, {'max': 1.0}) ], 'robot2': [ SoMessage(None, None, Vector3(1, 2, 0), Quaternion(), Vector3(), 1, 2.0, 1.0, 1.0, 0, None, True, {'max': 4.0}) ], 'robot4': [ SoMessage(None, None, Vector3(5, 6, 0), Quaternion(), Vector3(), 1, 2.0, 1.0, 1.0, 0, None, True, {'max': 8.9}) ] } } bffr._own_pos = [ SoMessage(None, None, Vector3(1, 1, 0), Quaternion(), Vector3(), 1, 1.0, 1.0, 1.0, 0, None, False, {'max': '3.0'}) ] self.assertEqual(gossip.calc_value()[0], 4.0)
def test_morphogenesis(self): """ test class morphogenesis barycenter """ bffr = SoBuffer() morph = MorphogenesisBarycenter(bffr, 'morphogenesis', 'dist') bffr._moving = { 'morphogenesis': { 'robot1': [ SoMessage(None, None, Vector3(2, 2, 0), Quaternion(), Vector3(), 1, 1.0, 1.0, 1.0, 0, None, True, {'dist': 7.0}) ], 'robto2': [ SoMessage(None, None, Vector3(1, 2, 0), Quaternion(), Vector3(), 1, 2.0, 1.0, 1.0, 0, None, True, {'dist': 4.0}) ] } } bffr._own_pos = [ SoMessage(None, None, Vector3(1, 1, 0), Quaternion(), Vector3(), 1, 1.0, 1.0, 1.0, 0, None, False, {'dist': 5.0}) ] self.assertEqual(morph.state, 'None') self.assertEqual(morph.calc_value()[0], np.sqrt(2) + 1) self.assertEqual(morph.calc_value()[1], 'Center')
def test_gradients(self): """ test gradients method """ bffr = SoBuffer(view_distance=2.0) # 2 neighbors within view, one outside view bffr._moving = { 'robot': { 'robot1': [SoMessage(), SoMessage(None, None, Vector3(2, 2, 0), Quaternion(), Vector3(), 1, 1.0, 1.0, 1.0, 0, None, True, [])], 'robot2': [SoMessage(), SoMessage(None, None, Vector3(5, 6, 0), Quaternion(), Vector3(), 1, 2.0, 1.0, 1.0, 0, None, True, [])], 'robot3': [SoMessage(), SoMessage(), SoMessage(None, None, Vector3(1, 2, 0), Quaternion(), Vector3(), -1, 4.0, 1.0, 1.0, 0, None, True, [])], 'robot4': [] } } bffr._static = { 'None': [SoMessage(None, None, Vector3(2, 1, 0), Quaternion(), Vector3(), 1, 1.0, 1.0, 1.0, 0, None, False, []), SoMessage()], } bffr._own_pos = [ SoMessage(None, None, Vector3(1, 1, 1), Quaternion(), Vector3(), 1, 1.0, 1.0, 1.0, 0, None, False, [])] result = [ SoMessage(None, None, Vector3(1, 2, 0), Quaternion(), Vector3(), -1, 4.0, 1.0, 1.0, 0, None, True, []), SoMessage(None, None, Vector3(2, 2, 0), Quaternion(), Vector3(), 1, 1.0, 1.0, 1.0, 0, None, True, []) ] self.assertEqual(bffr.gradients(static=False, time=self.now), result) result = [SoMessage(None, None, Vector3(2, 1, 0), Quaternion(), Vector3(), 1, 1.0, 1.0, 1.0, 0, None, False, []), SoMessage(None, None, Vector3(1, 2, 0), Quaternion(), Vector3(), -1, 4.0, 1.0, 1.0, 0, None, True, []), SoMessage(None, None, Vector3(2, 2, 0), Quaternion(), Vector3(), 1, 1.0, 1.0, 1.0, 0, None, True, [])] self.assertEqual(bffr.gradients(time=self.now), result)
def test_quorum(self): """ test quorum sensing class :return: """ bffr = SoBuffer() quorum = Quorum(bffr, 2) self.assertEqual(quorum.calc_value()[1], False) # 2 neighbors within view, one outside view bffr._moving = { 'robot': { 'robot1': [ SoMessage(), SoMessage(None, None, Vector3(2, 2, 0), Quaternion(), Vector3(), 1, 1.0, 1.0, 1.0, 0, None, True, []) ], 'robot2': [ SoMessage(), SoMessage(None, None, Vector3(5, 6, 0), Quaternion(), Vector3(), 1, 2.0, 1.0, 1.0, 0, None, True, []) ], 'robot3': [ SoMessage(), SoMessage(), SoMessage(None, None, Vector3(1, 2, 0), Quaternion(), Vector3(), 1, 4.0, 1.0, 1.0, 0, None, True, []) ], 'robot4': [] } } bffr._static = { 'None': [ SoMessage(None, None, Vector3(5, 6, 5), Quaternion(), Vector3(), 1, 1.0, 1.0, 1.0, 0, None, False, []) ] } bffr._own_pos = [ SoMessage(None, None, Vector3(1, 1, 1), Quaternion(), Vector3(), 1, 1.0, 1.0, 1.0, 0, None, False, []) ] self.assertEqual(quorum.calc_value()[1], True) quorum.threshold = 3 self.assertEqual(quorum.calc_value()[1], False)
def test_agent_set(self): """ test agent set function :return: """ bffr = SoBuffer(view_distance=2.0) self.assertEqual(bffr.agent_set([bffr.pose_frame], time=self.now), []) # 2 neighbors within view, one outside view bffr._moving = { 'robot': { 'robot1': [SoMessage(None, None, Vector3(0, 2, 0), Quaternion(), Vector3(), 1, 1.0, 1.0, 1.0, 0, None, True, []), SoMessage(None, None, Vector3(2, 2, 0), Quaternion(), Vector3(), 1, 1.0, 1.0, 1.0, 0, None, True, [])], 'robot2': [SoMessage(), SoMessage(None, None, Vector3(5, 6, 0), Quaternion(), Vector3(), 1, 2.0, 1.0, 1.0, 0, None, True, [])], 'robot3': [SoMessage(), SoMessage(), SoMessage(None, None, Vector3(1, 2, 0), Quaternion(), Vector3(), 1, 4.0, 1.0, 1.0, 0, None, True, [])], 'robot4': [] } } bffr._own_pos = [ SoMessage(None, None, Vector3(1, 1, 1), Quaternion(), Vector3(), 1, 1.0, 1.0, 1.0, 0, None, False, [])] result = [ SoMessage(None, None, Vector3(1, 2, 0), Quaternion(), Vector3(), 1, 4.0, 1.0, 1.0, 0, None, True, []), SoMessage(None, None, Vector3(0, 2, 0), Quaternion(), Vector3(), 1, 1.0, 1.0, 1.0, 0, None, True, []), SoMessage(None, None, Vector3(2, 2, 0), Quaternion(), Vector3(), 1, 1.0, 1.0, 1.0, 0, None, True, []) ] agent_set = bffr.agent_set([bffr.pose_frame], time=self.now) self.assertEqual(agent_set, result)
def test_repulsion_fernandez(self): """ test gradient repulsion method based on Fernandez-Marquez et al. :return: """ # Mechanism bffr = SoBuffer(id='robot1', view_distance=2.0) repulsion = RepulsionFernandez(bffr) # no own position specified self.assertEqual(repulsion.move(), None) bffr._own_pos = [ SoMessage(Header(None, rospy.Time.now(), 'None'), None, Vector3(2, 4, 0), Quaternion(), Vector3(), -1, 4.0, 1.0, 1.0, 0, rospy.Time.now(), False, []), SoMessage(Header(None, rospy.Time.now(), 'None'), None, Vector3(2, 2, 0), Quaternion(), Vector3(), -1, 1.0, 0.0, 1.0, 0, rospy.Time.now(), False, []) ] # no neighbors specified self.assertEqual(repulsion.move(), None) bffr._moving = { 'robot': { 'robot2': [ SoMessage(Header(None, rospy.Time.now(), 'None'), 'None', Vector3(1, 3, 0), Quaternion(), Vector3(), -1, 1.0, 1.0, 1.0, 0, rospy.Time.now(), False, []) ], 'robot3': [ SoMessage(Header(None, rospy.Time.now(), 'None'), 'None', Vector3(2, 2, 0), Quaternion(), Vector3(), -1, 4.0, 1.0, 1.0, 0, rospy.Time.now(), False, []), SoMessage(Header(None, rospy.Time.now(), 'None'), 'None', Vector3(3, 2, 0), Quaternion(), Vector3(), -1, 1.0, 0.8, 1.0, 0, rospy.Time.now(), False, []) ], 'robot4': [] } } # calculate resulting vector result = repulsion.move() result.x = round(result.x, 2) result.y = round(result.y, 2) result.z = round(result.z, 2) # calculate vector self.assertEqual(result, Vector3(-0.88, -0.48, 0)) # neighbor within goal_radius - returns vector with # ||vector|| = repulsion_radius bffr._moving = { 'robot': { 'robot2': [ SoMessage(Header(None, rospy.Time.now(), 'None'), None, Vector3(2, 2, 0), Quaternion(), Vector3(), -1, 2.0, 1.0, 1.0, 0, rospy.Time.now(), False, []) ] } } d = round(so_data.calc.vector_length(repulsion.move()), 0) # calculate vector self.assertEqual(d, 1.0)
def test_gradient_repulsion(self): """ test gradient repulsion method based on gradients :return: """ bffr = SoBuffer(id='robot1') repulsion = RepulsionGradient(bffr) # no own position specified self.assertEqual(repulsion.move(), None) bffr._own_pos = [ SoMessage(Header(None, rospy.Time.now(), 'None'), None, Vector3(2, 4, 0), Quaternion(), Vector3(), 1, 4.0, 0.0, 1.0, 0, None, False, []), SoMessage(Header(None, rospy.Time.now(), 'None'), None, Vector3(2, 2, 0), Quaternion(), Vector3(), 1, 2.0, 0.0, 1.0, 0, None, False, []) ] # no neighbors specified self.assertEqual(repulsion.move(), None) bffr._moving = { 'robot': { 'robot2': [ SoMessage(Header(None, rospy.Time.now(), 'robot'), 'robot2', Vector3(1, 3, 0), Quaternion(), Vector3(), -1, 1.0, 1.0, 1.0, 0, None, False, []) ], 'robot3': [ SoMessage(Header(None, rospy.Time.now(), 'robot'), 'robot3', Vector3(2, 2, 0), Quaternion(), Vector3(), -1, 4.0, 1.0, 1.0, 0, None, False, []), SoMessage(Header(None, rospy.Time.now(), 'robot'), 'robot3', Vector3(3, 2, 0), Quaternion(), Vector3(), -1, 1.0, 0.8, 1.0, 0, None, False, []) ] } } # calculate resulting vector result = repulsion.move() result.x = round(result.x, 2) result.y = round(result.y, 2) result.z = round(result.z, 2) # calculate vector self.assertEqual(result, Vector3(-0.39, -0.41, 0.0)) # neighbor within goal_radius - returns vector with # ||vector|| = repulsion_radius bffr._moving = { 'robot': { 'robot2': [ SoMessage(Header(None, rospy.Time.now(), 'robot'), 'robot2', Vector3(2, 1.5, 0), Quaternion(), Vector3(), -1, 2.0, 1.0, 1.0, 0, None, False, []) ] } } d = round(so_data.calc.vector_length(repulsion.move()), 0) # calculate vector self.assertEqual(d, 2.0)
def test_follow_max_reach(self): """ test FollowMin Mechanism :return: """ bffr = SoBuffer() chem = FollowMaxReach(bffr, moving=False, static=True, maxvel=np.inf) bffr._own_pos = [ SoMessage(None, None, Vector3(1, 2, 3), Quaternion(), Vector3(), -1, 3.0, 0.0, 1.0, 0, None, False, []) ] bffr._moving = { 'robot1': { 'None': [ SoMessage(None, None, Vector3(1, 2, 3), Quaternion(), Vector3(), -1, 3.0, 0.0, 1.0, 0, None, False, []) ] }, 'robot2': {} } bffr._static = { 'gradient': [ SoMessage(None, None, Vector3(2, 3, 1), Quaternion(), Vector3(), -1, 3.0, 1.0, 1.0, 0, None, False, []), SoMessage(None, None, Vector3(2, 2, 2), Quaternion(), Vector3(), 1, 1.0, 1.0, 1.0, 0, None, False, []) ], 'None': [ SoMessage(None, None, Vector3(7, 3, 2), Quaternion(), Vector3(), -1, 3.0, 1.0, 1.0, 0, None, False, []), SoMessage(None, None, Vector3(5, 6, 3), Quaternion(), Vector3(), 1, 2.0, 1.0, 1.0, 0, None, False, []) ], 'test': [ SoMessage(None, None, Vector3(5, 3, 2), Quaternion(), Vector3(), -1, 3.0, 1.0, 1.0, 0, None, False, []), SoMessage(None, None, Vector3(7, 2, 3), Quaternion(), Vector3(), -1, 3.0, 1.0, 1.0, 0, None, False, []), SoMessage(None, None, Vector3(1, 2, 6), Quaternion(), Vector3(), 1, 4.0, 1.0, 1.0, 0, None, False, []) ] } # with all frameIDs result = chem.move() result.x = round(result.x, 2) result.y = round(result.y, 2) result.z = round(result.z, 2) self.assertEqual(result, Vector3(0.0, 0.0, 0.5)) # only one frameID considered - no gradient within view chem.frames = ['None'] result = chem.move() self.assertEqual(result, None) # two frameIDs chem.frames = ['gradient'] result = chem.move() result.x = round(result.x, 2) result.y = round(result.y, 2) result.z = round(result.z, 2) self.assertEqual(result, Vector3(0.29, 0.0, -0.29))
def test_chemotaxis_ge(self): """ test chemotaxis ge mechanism :return: """ bffr = SoBuffer() chem = ChemotaxisGe(bffr, moving=False, static=True, maxvel=np.inf) bffr._own_pos = [ SoMessage(None, None, Vector3(1, -2, 0), Quaternion(), Vector3(), -1, 3.0, 0.0, 1.0, 0, None, False, []) ] bffr._static = { 'gradient': [ SoMessage(None, None, Vector3(2, 3, 1), Quaternion(), Vector3(), -1, 4.0, 1.0, 1.0, 0, None, False, []), SoMessage(None, None, Vector3(1, 0, 0), Quaternion(), Vector3(), 1, 3.0, 1.0, 1.0, 0, None, False, []) ], 'None': [ SoMessage(None, None, Vector3(4, 2, 0), Quaternion(), Vector3(), -1, 4.0, 2.0, 1.0, 0, None, False, []), SoMessage(None, None, Vector3(5, 6, 3), Quaternion(), Vector3(), 1, 2.0, 1.0, 1.0, 0, None, False, []) ] } bffr._moving = { 'robot1': { 'None': [ SoMessage(None, None, Vector3(1, -2, 1), Quaternion(), Vector3(), -1, 4.0, 0.0, 1.0, 0, None, False, []) ] }, 'robot2': {} } # only one frameID + repulsive gradient is not considered chem.frames = ['gradient'] result = chem.move() result.x = round(result.x, 2) result.y = round(result.y, 2) result.z = round(result.z, 2) self.assertEqual(result, Vector3(0.0, 1.0, 0.0)) # all frameIDs chem.frames = [] result = chem.move() result.x = round(result.x, 2) result.y = round(result.y, 2) result.z = round(result.z, 2) self.assertEqual(result, Vector3(-0.02, 0.98, 0.0)) # no gradient within view chem.frames = ['test'] result = chem.move() self.assertEqual(result, None) bffr._static = { 'test': [ SoMessage(None, None, Vector3(0, -1, 2), Quaternion(), Vector3(), -1, 1.0, 1.0, 1.0, 0, None, False, []) ] } chem.frames = ['test'] result = chem.move() self.assertEqual(result, Vector3())