def test_add_instance(self, mock_initialize_kb_service, mock_services): kb.initialize() def _update_knowledge_base(kb_operation, kb_item): return KnowledgeUpdateServiceResponse(True) services = {"update_knowledge_base": _update_knowledge_base} mock_services.__getitem__.side_effect = lambda key: services[key] self.assertTrue(kb.add_instance('v1', 'videotape')) self.assertTrue(kb.add_instance('v2', 'videotape')) self.assertTrue(kb.add_instance('c', 'container')) self.assertTrue(kb.add_instance('b1', 'box')) self.assertTrue(kb.add_instance('b2', 'box')) def _get_current_instances(type_name): return GetInstanceServiceResponse(['v1', 'v2', 'c', 'b1', 'b2']) services = {"get_current_instances": _get_current_instances} mock_services.__getitem__.side_effect = lambda key: services[key] instances = kb.list_instances() success = True for instance in instances: if instance not in ['v1', 'v2', 'c', 'b1', 'b2']: success = False self.assertTrue(success, 'Returned not correct instances')
def prepare_instances(): p = Pose(Point(0.25, .0, .0), Quaternion(0, 0, 0, 1)) kb.add_instance("p1", "place") sdb.add_element("p1", sdb.Element(p, "place")) p = Pose(Point(0.50, .0, .0), Quaternion(0, 0, 0, 1)) kb.add_instance("p2", "place") sdb.add_element("p2", sdb.Element(p, "place")) p = Pose(Point(0.75, .0, .0), Quaternion(0, 0, 0, 1)) kb.add_instance("p3", "place") sdb.add_element("p3", sdb.Element(p, "place")) p = Pose(Point(1.00, .0, .0), Quaternion(0, 0, 0, 1)) kb.add_instance("p4", "place") sdb.add_element("p4", sdb.Element(p, "place"))
import rosplan_pytools import rosplan_pytools.controller.knowledge_base as kb import rosplan_pytools.controller.scene_database as sdb import rosplan_pytools.controller.planning_system as ps rospy.init_node("teste") #rosplan_pytools.init() kb.initialize("/rosplan_knowledge_base") print("OLAR") kb.reset() # Using the KB kb.add_instance("rover1", "rover") kb.list_instances() # You can store stuff into the scene database with a third arg # kb.add_instance("msg1", "msg_type") # sdb.add_element("msg1", sdb.Element(std_msgs.msg.String("Be sure to drink your ovaltine"), "msg_type")) # kb.add_goal("robot-at", loc="loc1") # kb.add_goal("has-received-message", msg="msg1", loc="loc1") # # Then, plan and execute! using PS # ps.plan() # # Now, let's try stopping it # time.sleep(2) # ps.cancel()
def test_add_instance_incorrect_arguments(self): self.assertRaises(TypeError, lambda: kb.add_instance(10.0, '')) self.assertRaises(TypeError, lambda: kb.add_instance('', 10.0))