Add Robot class tests

pyrobosim/pyrobosim/core/robot.py

@@ -22,7 +22,7 @@ def __init__(
         pose=Pose(),
         radius=0.0,
         height=0.0,
-        color=(0.8, 0, 0.8),
+        color=(0.8, 0.0, 0.8),
         path_planner=None,
         path_executor=None,
         grasp_generator=None,
@@ -160,7 +160,19 @@ def follow_path(
             )
             self.nav_thread.start()
             if blocking:
-                success = self.nav_thread.join()
+                self.nav_thread.join()
+
+                # Validate that the robot made it to its goal pose
+                if path.num_poses == 0:
+                    success = True
+                else:
+                    # TODO: Implement pose equality check
+                    goal_pose = path.poses[-1]
+                    success = (
+                        self.pose.x == goal_pose.x
+                        and self.pose.y == goal_pose.y
+                        and self.pose.get_yaw() == goal_pose.get_yaw()
+                    )
             else:
                 success = True
         else:
@@ -392,8 +404,9 @@ def execute_action(self, action, blocking=False):
         if self.world.has_gui:
             self.world.gui.set_buttons_during_action(True)
         print(f"[{self.name}] Action completed with success: {success}")
-        self.current_action = None
-        self.executing_action = False
+        if blocking:
+            self.current_action = None
+            self.executing_action = False
         return success
 
     def execute_plan(self, plan, blocking=False, delay=0.5):
@@ -414,8 +427,10 @@ def execute_plan(self, plan, blocking=False, delay=0.5):
             print(f"[{self.name}] Plan is None. Returning.")
             return False
 
-        self.executing_plan = True
-        self.current_plan = plan
+        if blocking:
+            self.executing_plan = True
+            self.current_plan = plan
+
         print(f"[{self.name}] Executing task plan...")
         if self.world.has_gui:
             self.world.gui.set_buttons_during_action(False)
@@ -433,8 +448,9 @@ def execute_plan(self, plan, blocking=False, delay=0.5):
         if self.world.has_gui:
             self.world.gui.set_buttons_during_action(True)
         print(f"[{self.name}] Task plan completed with success: {success}")
-        self.current_plan = None
-        self.executing_plan = False
+        if blocking:
+            self.executing_plan = False
+            self.current_plan = None
         return success
 
     def __repr__(self):

pyrobosim/pyrobosim/navigation/execution.py

@@ -72,4 +72,6 @@ def execute(self, path, realtime_factor=1.0):
         # Finalize path execution
         time.sleep(0.1)  # To ensure background threads get the end of the path.
         self.robot.executing_nav = False
+        self.robot.executing_action = False
+        self.robot.current_action = None
         return True

pyrobosim/pyrobosim/navigation/path_planner.py

@@ -34,8 +34,7 @@ def __init__(self, planner_type, **planner_config):
             )
             return None
         if not planner_config:
-            warnings.warn(f"No planner configuration provided", UserWarning)
-            return None
+            planner_config = {}
 
         self.planner_type = planner_type
         self.planner_config = planner_config

test/world_model/test_get_entities.py → test/core/test_get_entities.py

@@ -1,7 +1,7 @@
 #!/usr/bin/env python3
 
 """
-Test script for entity getting with world models.
+Unit tests for entity getting with world models.
 """
 import os
 import pytest
@@ -102,18 +102,21 @@ def test_valid_room(self):
 
     def test_invalid_room(self):
         """Checks for existence of invalid room."""
-        result = self.world.get_room_by_name("living room")
-        assert result is None
+        with pytest.warns(UserWarning):
+            result = self.world.get_room_by_name("living room")
+            assert result is None
 
     def test_add_remove_room(self):
         """Checks adding a room and removing it cleanly."""
         room_name = "test_room"
         coords = [(9, 9), (11, 9), (11, 11), (9, 11)]
         result = self.world.add_room(name=room_name, footprint=coords, color=[0, 0, 0])
         assert result is not None
-        self.world.remove_room(room_name)
-        result = self.world.get_room_by_name(room_name)
-        assert result is None
+
+        with pytest.warns(UserWarning):
+            self.world.remove_room(room_name)
+            result = self.world.get_room_by_name(room_name)
+            assert result is None
 
     def test_valid_location(self):
         """Checks for existence of valid location."""
@@ -122,8 +125,9 @@ def test_valid_location(self):
 
     def test_invalid_location(self):
         """Checks for existence of invalid location."""
-        result = self.world.get_location_by_name("table42")
-        assert result is None
+        with pytest.warns(UserWarning):
+            result = self.world.get_location_by_name("table42")
+            assert result is None
 
     def test_valid_spawn(self):
         """Checks for existence of valid object spawn."""
@@ -152,10 +156,12 @@ def test_add_remove_location(self):
         )
         assert isinstance(new_desk, Location)
         self.world.remove_location(loc_name)
-        result = self.world.get_location_by_name(loc_name)
-        assert result is None
-        result = self.world.get_entity_by_name(loc_name + "_desktop")
-        assert result is None
+
+        with pytest.warns(UserWarning):
+            result = self.world.get_location_by_name(loc_name)
+            assert result is None
+            result = self.world.get_entity_by_name(loc_name + "_desktop")
+            assert result is None
 
     def test_valid_object(self):
         """Checks for existence of valid object."""
@@ -172,8 +178,10 @@ def test_invalid_object_valid_name(self):
         Checks for existence of invalid object, but whose name is a valid name
         for another entity type.
         """
-        result = self.world.get_object_by_name("counter0")
-        assert result is None
+        with pytest.warns(UserWarning):
+            result = self.world.get_object_by_name("counter0")
+            assert result is None
+
         result = self.world.get_entity_by_name("counter0")
         assert isinstance(result, Location) and result.name == "counter0"
 

test/core/test_robot.py

@@ -0,0 +1,223 @@
+#!/usr/bin/env python3
+
+"""
+Unit tests for robot class.
+"""
+import os
+import numpy as np
+import pytest
+import time
+
+from pyrobosim.core import Pose, Robot, WorldYamlLoader
+from pyrobosim.navigation import ConstantVelocityExecutor, PathPlanner
+from pyrobosim.planning.actions import TaskAction, TaskPlan
+from pyrobosim.utils.general import get_data_folder
+from pyrobosim.utils.motion import Path
+
+
+class TestRobot:
+    @pytest.fixture(autouse=True)
+    def create_test_world(self):
+        self.test_world = WorldYamlLoader().from_yaml(
+            os.path.join(get_data_folder(), "test_world.yaml")
+        )
+
+    def test_create_robot_default_args(self):
+        """Check that a robot can be created with all default arguments."""
+        robot = Robot()
+
+        assert robot.name == "robot"
+        assert robot.pose.x == pytest.approx(0.0)
+        assert robot.pose.y == pytest.approx(0.0)
+        assert robot.pose.z == pytest.approx(0.0)
+        assert robot.pose.eul == pytest.approx([0.0, 0.0, 0.0])
+        assert robot.radius == pytest.approx(0.0)
+        assert robot.height == pytest.approx(0.0)
+        assert robot.color == pytest.approx((0.8, 0.0, 0.8))
+        assert robot.path_planner == None
+        assert robot.path_executor == None
+        assert robot.grasp_generator == None
+        assert robot.world == None
+
+    def test_create_robot_nondefault_args(self):
+        """Check that a robot can be created with nondefault arguments."""
+        robot = Robot(
+            name="test_robot",
+            pose=Pose(x=1.0, y=2.0, yaw=np.pi / 2.0),
+            radius=0.1,
+            height=0.15,
+            color=(0.5, 0.5, 0.5),
+        )
+
+        assert robot.name == "test_robot"
+        assert robot.pose.x == pytest.approx(1.0)
+        assert robot.pose.y == pytest.approx(2.0)
+        assert robot.pose.z == pytest.approx(0.0)
+        assert robot.pose.eul == pytest.approx([0.0, 0.0, np.pi / 2.0])
+        assert robot.radius == pytest.approx(0.1)
+        assert robot.height == pytest.approx(0.15)
+        assert robot.color == pytest.approx((0.5, 0.5, 0.5))
+
+    def test_robot_path_planner(self):
+        """Check that path planners can be used from a robot."""
+        init_pose = Pose(x=1.0, y=0.5, yaw=0.0)
+        goal_pose = Pose(x=2.5, y=3.0, yaw=np.pi / 2.0)
+
+        robot = Robot(
+            pose=init_pose,
+            path_planner=PathPlanner("world_graph", world=self.test_world),
+        )
+        robot.world = self.test_world
+        robot.location = self.test_world.get_entity_by_name("kitchen")
+
+        # Explicitly provide start pose
+        path = robot.plan_path(start=init_pose, goal=goal_pose)
+        assert isinstance(path, Path)
+        assert path.poses[0] == init_pose
+        assert path.poses[-1] == goal_pose
+
+        # Assumes start pose is already the robot pose
+        path = robot.plan_path(goal=goal_pose)
+        assert isinstance(path, Path)
+        assert path.poses[0] == init_pose
+        assert path.poses[-1] == goal_pose
+
+    def test_robot_path_executor(self):
+        """Check that path executors can be used from a robot."""
+        init_pose = Pose(x=1.0, y=0.5, yaw=0.0)
+        goal_pose = Pose(x=2.5, y=3.0, yaw=np.pi / 2.0)
+        target_location = self.test_world.get_entity_by_name("bedroom")
+
+        robot = Robot(
+            pose=init_pose,
+            path_planner=PathPlanner("world_graph", world=self.test_world),
+            path_executor=ConstantVelocityExecutor(linear_velocity=5.0, dt=0.1),
+        )
+        robot.world = self.test_world
+        robot.location = self.test_world.get_entity_by_name("kitchen")
+
+        # Non-threaded option -- blocks
+        robot.set_pose(init_pose)
+        path = robot.plan_path(goal=goal_pose)
+        result = robot.follow_path(
+            path, target_location=target_location, use_thread=False
+        )
+        assert result
+
+        # Threaded option with blocking
+        robot.set_pose(init_pose)
+        path = robot.plan_path(goal=goal_pose)
+        robot.follow_path(
+            path, target_location=target_location, use_thread=True, blocking=True
+        )
+        assert robot.pose.x == pytest.approx(goal_pose.x)
+        assert robot.pose.y == pytest.approx(goal_pose.y)
+        assert robot.pose.q == pytest.approx(goal_pose.q)
+
+        # Threaded option without blocking -- must check result
+        robot.set_pose(init_pose)
+        path = robot.plan_path(goal=goal_pose)
+        robot.follow_path(
+            path, target_location=target_location, use_thread=True, blocking=False
+        )
+        assert robot.executing_nav
+        while robot.executing_nav:
+            time.sleep(0.1)
+        assert not robot.executing_nav
+        assert robot.pose.x == pytest.approx(goal_pose.x)
+        assert robot.pose.y == pytest.approx(goal_pose.y)
+        assert robot.pose.q == pytest.approx(goal_pose.q)
+
+    def test_robot_manipulation(self):
+        """Check that the robot can manipulate objects."""
+        # Spawn the robot near the kitchen table
+        robot = Robot(
+            pose=Pose(x=1.0, y=0.5, yaw=0.0),
+        )
+        robot.world = self.test_world
+        robot.location = self.test_world.get_entity_by_name("table0_tabletop")
+
+        # Pick up the apple on the kitchen table (named "gala")
+        result = robot.pick_object("apple")
+        assert result
+        assert robot.manipulated_object == self.test_world.get_entity_by_name("gala")
+
+        # Try pick up another object, which should fail since the robot is holding something.
+        with pytest.warns(UserWarning):
+            result = robot.pick_object("banana")
+            assert not result
+
+        # Try place the object
+        result = robot.place_object()
+        assert result
+        assert robot.manipulated_object is None
+
+        # Try place an object, which should fail since the robot is holding nothing.
+        with pytest.warns(UserWarning):
+            result = robot.place_object()
+            assert not result
+
+    def test_execute_action(self):
+        """Tests execution of a single action."""
+        init_pose = Pose(x=1.0, y=0.5, yaw=0.0)
+        robot = Robot(
+            pose=init_pose,
+            path_planner=PathPlanner("world_graph", world=self.test_world),
+            path_executor=ConstantVelocityExecutor(linear_velocity=5.0, dt=0.1),
+        )
+        robot.location = "kitchen"
+        robot.world = self.test_world
+        action = TaskAction(
+            "navigate",
+            source_location="kitchen",
+            target_location="my_desk",
+        )
+
+        # Blocking action
+        result = robot.execute_action(action, blocking=True)
+        assert result
+
+        # Non-blocking action
+        robot.set_pose(init_pose)
+        result = robot.execute_action(action, blocking=False)
+        assert result
+        assert robot.executing_action
+        assert robot.current_action == action
+        while robot.executing_action:
+            time.sleep(0.1)
+        assert not robot.executing_action
+        assert robot.current_action is None
+
+    def test_execute_plan(self):
+        """Tests execution of a plan consisting of multiple actions."""
+        init_pose = Pose(x=1.0, y=0.5, yaw=0.0)
+        robot = Robot(
+            pose=init_pose,
+            path_planner=PathPlanner("world_graph", world=self.test_world),
+            path_executor=ConstantVelocityExecutor(linear_velocity=5.0, dt=0.1),
+        )
+        robot.location = "kitchen"
+        robot.world = self.test_world
+
+        actions = [
+            TaskAction(
+                "navigate",
+                source_location="kitchen",
+                target_location="my_desk",
+            ),
+            TaskAction("pick", object="apple"),
+            TaskAction("place", "object", "apple"),
+        ]
+        plan = TaskPlan(actions)
+
+        # Blocking plan
+        result = robot.execute_plan(plan, blocking=True)
+        assert result
+
+        # Non-blocking plan
+        robot.set_pose(init_pose)
+        result = robot.execute_plan(plan, blocking=False)
+        assert result
+        while robot.executing_action:
+            time.sleep(0.1)
+        assert not robot.executing_action

test/world_model/test_room.py → test/core/test_room.py

@@ -53,9 +53,10 @@ def test_add_room_to_world_in_collision(self):
 
         # This new room should fail to add since it's in the collision with the first room.
         new_coords = [(0.0, 0.0), (2.0, 0.0), (2.0, 2.0), (0.0, 2.0)]
-        result = world.add_room(footprint=new_coords)
-        assert result is None
-        assert world.num_rooms == 1
+        with pytest.warns(UserWarning):
+            result = world.add_room(footprint=new_coords)
+            assert result is None
+            assert world.num_rooms == 1
 
 
 if __name__ == "__main__":