def test_no_look(self): # parameters x_coordinate = 2.1 y_coordinate = 3.7 z_coordinate = 0 frame_id = "/map" robot = Mockbot() arm = ArmDesignator(robot, {}, name="arm_designator") frame = Designator(frame_stamped(frame_id, x_coordinate, y_coordinate, z_coordinate), name="frame_designator") pc, oc = arms_reach_constraint(frame, arm, look=False) # verify positionconstraint self.assertEqual(pc.frame, frame_id) publicarm = arm.resolve() radius = math.hypot(publicarm.base_offset.x(), publicarm.base_offset.y()) margin = 0.075 ro = "(x-{})^2+(y-{})^2 < {}^2".format(x_coordinate, y_coordinate, radius + margin) ri = "(x-{})^2+(y-{})^2 > {}^2".format(x_coordinate, y_coordinate, radius - margin) verification_string = ri + " and " + ro equal, msg = constraint_strings_equal(pc.constraint, verification_string) self.assertTrue(equal, msg) # verify orientationconstraint self.assertIsNone(oc)
def setUp(self): self.robot = Mockbot() self.entity = Entity( "12345", "dummy", "/map", kdl.Frame(kdl.Rotation.RPY(1, 0, 0), kdl.Vector(3, 3, 3)), None, {}, None, 0)
def setUpClass(cls): # simple rooms setup, all rooms are 5x5x3m # 5 ------- --------- # | kitchen | bedroom | # 0---------5---------10 box1 = BoxVolume(kdl.Vector(0, 0, 0), kdl.Vector(5, 5, 3)) box2 = BoxVolume(kdl.Vector(0, 0, 0), kdl.Vector(1, 1, 0.5)) cls.robot = Mockbot() cls._kitchen = Entity( "kitchen", "room", "/map", kdl.Frame(kdl.Rotation.RPY(0, 0, 0), kdl.Vector(0, 0, 0)), None, {"in": box1}, ["room"], 0) cls._bedroom = Entity( "bedroom", "room", "/map", kdl.Frame(kdl.Rotation.RPY(0, 0, 0), kdl.Vector(5, 0, 0)), None, {"in": box1}, ["room"], 0) cls._cabinet = Entity( "cabinet", "furniture", "/map", kdl.Frame(kdl.Rotation.RPY(0, 0, 0), kdl.Vector(4, 4, 0)), None, {"on_top_off": box2}, ["furniture"], 0) cls._bookcase = Entity( "bookcase", "furniture", "/map", kdl.Frame(kdl.Rotation.RPY(0, 0, 0), kdl.Vector(8, 1, 0)), None, {"on_top_off": box2}, ["furniture"], 0) cls.robot.ed._static_entities = { e.id: e for e in [cls._kitchen, cls._bedroom, cls._cabinet, cls._bookcase] } cls.tour_guide = TourGuide(cls.robot)
def test_construction(self): """ If no exception is raised, this test will succeed """ os.environ["ROBOT_ENV"] = "robotics_testlabs" from robot_skills.mockbot import Mockbot from challenge_set_the_table.set_the_table import setup_statemachine robot = Mockbot() setup_statemachine(robot)
def test_construction(self): """ If no exception is raised, this test will succeed """ os.environ["ROBOT_ENV"] = "robotics_testlabs" from robot_skills.mockbot import Mockbot from challenge_restaurant.restaurant import Restaurant robot = Mockbot() Restaurant(robot)
def test_construction(self): """ If no exception is raised, this test will succeed """ os.environ["ROBOT_ENV"] = "robotics_testlabs" from robot_skills.mockbot import Mockbot from challenge_serving_drinks.serving_drinks import ServingDrinks robot = Mockbot() ServingDrinks(robot)
def test_construction(self): """ If no exception is raised, this test will succeed """ os.environ["ROBOT_ENV"] = "robotics_testlabs" from robot_skills.mockbot import Mockbot from challenge_take_out_the_garbage.take_out_the_garbage import TakeOutGarbage robot = Mockbot() TakeOutGarbage(robot)
def test_construction(self): """ If no exception is raised, this test will succeed """ os.environ["ROBOT_ENV"] = "robotics_testlabs" from robot_skills.mockbot import Mockbot from challenge_where_is_this.where_is_this import WhereIsThis robot = Mockbot() WhereIsThis(robot)
def setUp(self): self.robot = Mockbot() box = BoxVolume(kdl.Vector(0, 0, 0), kdl.Vector(1, 1, 1)) self.entity = Entity( "12345", "dummy", "/map", kdl.Frame(kdl.Rotation.RPY(1, 0, 0), kdl.Vector(3, 3, 3)), None, {"dummy_volume": box}, None, 0) self.area = "dummy_volume"
def setUp(self): self.robot = Mockbot() hull = RightPrism( None, # No actual convex hull z_min=0, z_max=1) self.entity = Entity( "12345", "dummy", "/map", kdl.Frame(kdl.Rotation.RPY(1, 0, 0), kdl.Vector(3, 3, 3)), hull, {}, None, 0)
def test_show_image(self): robot = Mockbot() # Test if state returns succeeded if file exists with open("/tmp/foo", "w") as f: f.write("bar") state = ShowImageState(robot, "/tmp/foo") self.assertEqual(state.execute(), "succeeded") os.remove("/tmp/foo") # Test if state returns failed if file does not exist state = ShowImageState(robot, "/tmp/bar") self.assertEqual(state.execute(), "failed")
def test_grammar(): # Export a (default) robot env. This is necessary because the action server # loads knowledge on construction of actions. # It is desirable to improve this in the future. os.environ["ROBOT_ENV"] = "robotics_testlabs" knowledge = load_knowledge('challenge_demo') # Construct a Mockbot object and add a number of static entities robot = Mockbot() robot.ed._static_entities = { "couch_table": from_entity_info(EntityInfo(id="couch_table")), "operator": from_entity_info(EntityInfo(id="operator")), } robot.ed._dynamic_entities = dict() test_grammar(robot, knowledge.grammar, knowledge.grammar_target)
def setUpClass(cls): cls.robot = Mockbot()
def setUp(self): self.robot = Mockbot()