From fe5bbc96e5859ec168b8d43b4e38f6f7ff61dd29 Mon Sep 17 00:00:00 2001
From: Sebastian Castro <sebas.a.castro@gmail.com>
Date: Sun, 23 Jul 2023 13:54:51 -0400
Subject: [PATCH 1/7] Add robot class tests

---
 pyrobosim/pyrobosim/core/robot.py             |  32 ++-
 pyrobosim/pyrobosim/navigation/execution.py   |   2 +
 .../pyrobosim/navigation/path_planner.py      |   3 +-
 .../test_get_entities.py                      |   0
 test/{world_model => core}/test_hallway.py    |   0
 test/{world_model => core}/test_locations.py  |   0
 test/{world_model => core}/test_objects.py    |   0
 test/core/test_robot.py                       | 223 ++++++++++++++++++
 test/{world_model => core}/test_room.py       |   0
 .../examples => test/core}/test_world.py      |   0
 10 files changed, 250 insertions(+), 10 deletions(-)
 rename test/{world_model => core}/test_get_entities.py (100%)
 rename test/{world_model => core}/test_hallway.py (100%)
 rename test/{world_model => core}/test_locations.py (100%)
 rename test/{world_model => core}/test_objects.py (100%)
 create mode 100644 test/core/test_robot.py
 rename test/{world_model => core}/test_room.py (100%)
 rename {pyrobosim/examples => test/core}/test_world.py (100%)

diff --git a/pyrobosim/pyrobosim/core/robot.py b/pyrobosim/pyrobosim/core/robot.py
index 42b2d5c5..2cdd07ea 100644
--- a/pyrobosim/pyrobosim/core/robot.py
+++ b/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):
diff --git a/pyrobosim/pyrobosim/navigation/execution.py b/pyrobosim/pyrobosim/navigation/execution.py
index 78d2fe05..6b83e202 100644
--- a/pyrobosim/pyrobosim/navigation/execution.py
+++ b/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
diff --git a/pyrobosim/pyrobosim/navigation/path_planner.py b/pyrobosim/pyrobosim/navigation/path_planner.py
index 6ec9da44..5c1b90bd 100644
--- a/pyrobosim/pyrobosim/navigation/path_planner.py
+++ b/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
diff --git a/test/world_model/test_get_entities.py b/test/core/test_get_entities.py
similarity index 100%
rename from test/world_model/test_get_entities.py
rename to test/core/test_get_entities.py
diff --git a/test/world_model/test_hallway.py b/test/core/test_hallway.py
similarity index 100%
rename from test/world_model/test_hallway.py
rename to test/core/test_hallway.py
diff --git a/test/world_model/test_locations.py b/test/core/test_locations.py
similarity index 100%
rename from test/world_model/test_locations.py
rename to test/core/test_locations.py
diff --git a/test/world_model/test_objects.py b/test/core/test_objects.py
similarity index 100%
rename from test/world_model/test_objects.py
rename to test/core/test_objects.py
diff --git a/test/core/test_robot.py b/test/core/test_robot.py
new file mode 100644
index 00000000..8d49c1a3
--- /dev/null
+++ b/test/core/test_robot.py
@@ -0,0 +1,223 @@
+#!/usr/bin/env python3
+
+"""
+Test script for robot model.
+"""
+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
diff --git a/test/world_model/test_room.py b/test/core/test_room.py
similarity index 100%
rename from test/world_model/test_room.py
rename to test/core/test_room.py
diff --git a/pyrobosim/examples/test_world.py b/test/core/test_world.py
similarity index 100%
rename from pyrobosim/examples/test_world.py
rename to test/core/test_world.py

From c7a9ca2a659faf6fdbe572eaaf7d25d901fb3cbe Mon Sep 17 00:00:00 2001
From: Sebastian Castro <sebas.a.castro@gmail.com>
Date: Sun, 23 Jul 2023 14:06:59 -0400
Subject: [PATCH 2/7] Tag expected warnings and fix docstrings

---
 test/core/test_get_entities.py         | 36 ++++++++++++++++----------
 test/core/test_robot.py                |  2 +-
 test/core/test_room.py                 |  7 ++---
 test/core/test_world.py                |  6 ++---
 test/manipulation/test_grasping.py     |  2 +-
 test/planning/test_pddlstream_manip.py |  2 +-
 test/planning/test_pddlstream_nav.py   |  2 +-
 test/system/test_system.py             |  2 +-
 test/utils/test_polygon_utils.py       |  2 +-
 9 files changed, 35 insertions(+), 26 deletions(-)

diff --git a/test/core/test_get_entities.py b/test/core/test_get_entities.py
index b678d90c..ad142d67 100644
--- a/test/core/test_get_entities.py
+++ b/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,8 +102,9 @@ 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."""
@@ -111,9 +112,11 @@ def test_add_remove_room(self):
         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"
 
diff --git a/test/core/test_robot.py b/test/core/test_robot.py
index 8d49c1a3..6516cfb6 100644
--- a/test/core/test_robot.py
+++ b/test/core/test_robot.py
@@ -1,7 +1,7 @@
 #!/usr/bin/env python3
 
 """
-Test script for robot model.
+Unit tests for robot class.
 """
 import os
 import numpy as np
diff --git a/test/core/test_room.py b/test/core/test_room.py
index 9cd759fb..b85cc4f2 100644
--- a/test/core/test_room.py
+++ b/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__":
diff --git a/test/core/test_world.py b/test/core/test_world.py
index 14017b97..a779d4be 100644
--- a/test/core/test_world.py
+++ b/test/core/test_world.py
@@ -140,9 +140,9 @@ def test_remove_location():
 
         assert TestWorldModeling.world.remove_location("study_desk") is True
         assert len(TestWorldModeling.world.locations) == 1
-        assert (
-            TestWorldModeling.world.get_location_by_name("study_desk") is None
-        )  # Raises a warning
+        with pytest.warns(UserWarning):
+            assert TestWorldModeling.world.get_location_by_name("study_desk") is None
+
 
     @staticmethod
     @pytest.mark.dependency(depends=["TestWorldModeling::test_create_hallway"])
diff --git a/test/manipulation/test_grasping.py b/test/manipulation/test_grasping.py
index 264327ed..bc066341 100644
--- a/test/manipulation/test_grasping.py
+++ b/test/manipulation/test_grasping.py
@@ -1,7 +1,7 @@
 #!/usr/bin/env python3
 
 """
-Test script for grasp generation.
+Unit tests for grasp generation.
 """
 
 from pyrobosim.manipulation.grasping import (
diff --git a/test/planning/test_pddlstream_manip.py b/test/planning/test_pddlstream_manip.py
index 8fdffb1d..a53ac491 100644
--- a/test/planning/test_pddlstream_manip.py
+++ b/test/planning/test_pddlstream_manip.py
@@ -1,7 +1,7 @@
 #!/usr/bin/env python3
 
 """
-Test script for PDDLStream planning with manipulation streams.
+Tests for PDDLStream planning with manipulation streams.
 """
 import numpy as np
 import os
diff --git a/test/planning/test_pddlstream_nav.py b/test/planning/test_pddlstream_nav.py
index 5ed782f0..386c1729 100644
--- a/test/planning/test_pddlstream_nav.py
+++ b/test/planning/test_pddlstream_nav.py
@@ -1,7 +1,7 @@
 #!/usr/bin/env python3
 
 """
-Test script for PDDLStream planning with navigation streams.
+Tests for PDDLStream planning with navigation streams.
 """
 import os
 import threading
diff --git a/test/system/test_system.py b/test/system/test_system.py
index 146f2299..8be293ee 100644
--- a/test/system/test_system.py
+++ b/test/system/test_system.py
@@ -85,7 +85,7 @@ def test_pick_place(self):
         Test pick and place UI actions.
         """
         pick_place_queries = [
-            ("table", "apple2", "table"),  # Pick and place in same location
+            ("table", "gala", "table"),  # Pick and place in same location
             ("counter0_left", "water", "desk"),  # Pick and place in different location
         ]
 
diff --git a/test/utils/test_polygon_utils.py b/test/utils/test_polygon_utils.py
index 930c3ee2..2728f37d 100644
--- a/test/utils/test_polygon_utils.py
+++ b/test/utils/test_polygon_utils.py
@@ -1,7 +1,7 @@
 #!/usr/bin/env python3
 
 """
-Test script for polygon utilities
+Unit tests for polygon utilities.
 """
 
 import pytest

From da04babaf3e01d9c3c1c76fd92774756136c9ee3 Mon Sep 17 00:00:00 2001
From: Sebastian Castro <sebas.a.castro@gmail.com>
Date: Sun, 23 Jul 2023 14:11:35 -0400
Subject: [PATCH 3/7] Missed formatting

---
 test/core/test_world.py | 1 -
 1 file changed, 1 deletion(-)

diff --git a/test/core/test_world.py b/test/core/test_world.py
index a779d4be..b23ff04a 100644
--- a/test/core/test_world.py
+++ b/test/core/test_world.py
@@ -143,7 +143,6 @@ def test_remove_location():
         with pytest.warns(UserWarning):
             assert TestWorldModeling.world.get_location_by_name("study_desk") is None
 
-
     @staticmethod
     @pytest.mark.dependency(depends=["TestWorldModeling::test_create_hallway"])
     def test_remove_hallway():

From 31df5bece36efea632f0dbb01c4591bd9673d82d Mon Sep 17 00:00:00 2001
From: Sebastian Castro <sebas.a.castro@gmail.com>
Date: Sun, 23 Jul 2023 14:16:08 -0400
Subject: [PATCH 4/7] Small fix now that blocking nav returns success flag

---
 test/core/test_robot.py | 5 +----
 1 file changed, 1 insertion(+), 4 deletions(-)

diff --git a/test/core/test_robot.py b/test/core/test_robot.py
index 6516cfb6..59492728 100644
--- a/test/core/test_robot.py
+++ b/test/core/test_robot.py
@@ -107,12 +107,9 @@ def test_robot_path_executor(self):
         # Threaded option with blocking
         robot.set_pose(init_pose)
         path = robot.plan_path(goal=goal_pose)
-        robot.follow_path(
+        result = 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)

From 2c06a8cd073eb679ef9a8fd3594eb4615c3ef629 Mon Sep 17 00:00:00 2001
From: Sebastian Castro <sebas.a.castro@gmail.com>
Date: Tue, 25 Jul 2023 09:52:41 -0400
Subject: [PATCH 5/7] Reinstate path planner warning with more descriptive
 message

---
 pyrobosim/pyrobosim/navigation/path_planner.py | 6 +++++-
 1 file changed, 5 insertions(+), 1 deletion(-)

diff --git a/pyrobosim/pyrobosim/navigation/path_planner.py b/pyrobosim/pyrobosim/navigation/path_planner.py
index 5c1b90bd..0e0168f8 100644
--- a/pyrobosim/pyrobosim/navigation/path_planner.py
+++ b/pyrobosim/pyrobosim/navigation/path_planner.py
@@ -34,7 +34,11 @@ def __init__(self, planner_type, **planner_config):
             )
             return None
         if not planner_config:
-            planner_config = {}
+            warnings.warn(
+                f"No planner configuration provided. Must provide either a World or OccupancyGrid object.",
+                UserWarning,
+            )
+            return None
 
         self.planner_type = planner_type
         self.planner_config = planner_config

From 875036e3e8cbab8c97cf6dc88bf70410ac313ba8 Mon Sep 17 00:00:00 2001
From: Sebastian Castro <sebas.a.castro@gmail.com>
Date: Wed, 26 Jul 2023 07:50:47 -0400
Subject: [PATCH 6/7] Use new Pose equivalency checks (and fix bugs in them)

---
 pyrobosim/pyrobosim/core/robot.py          |  8 +----
 pyrobosim/pyrobosim/utils/pose.py          | 38 ++++++++++------------
 pyrobosim_ros/test/test_ros_conversions.py | 10 ++----
 3 files changed, 21 insertions(+), 35 deletions(-)

diff --git a/pyrobosim/pyrobosim/core/robot.py b/pyrobosim/pyrobosim/core/robot.py
index 2cdd07ea..aa56ac1d 100644
--- a/pyrobosim/pyrobosim/core/robot.py
+++ b/pyrobosim/pyrobosim/core/robot.py
@@ -166,13 +166,7 @@ def follow_path(
                 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()
-                    )
+                    success = self.pose.is_approx(path.poses[-1])
             else:
                 success = True
         else:
diff --git a/pyrobosim/pyrobosim/utils/pose.py b/pyrobosim/pyrobosim/utils/pose.py
index eff93af2..6ba07a6e 100644
--- a/pyrobosim/pyrobosim/utils/pose.py
+++ b/pyrobosim/pyrobosim/utils/pose.py
@@ -194,21 +194,7 @@ def get_translation(self):
         """
         return np.array([self.x, self.y, self.z])
 
-    def __repr__(self):
-        """
-        Representation for printing a Pose object.
-
-        :return: Printable string.
-        :rtype: str
-        """
-        pos_str = f"x={self.x:.2f}, y={self.y:.2f}, z={self.z:.2f}"
-        quat_str = (
-            f"qw={self.q[0]:.3f}, qx={self.q[1]:.3f}, "
-            + f"qy={self.q[2]:.3f}, qz={self.q[3]:.3f}"
-        )
-        return f"Pose: [{pos_str}, {quat_str}]"
-
-    def is_approx(self, other, rel_tol=1e-09, abs_tol=0.0):
+    def is_approx(self, other, rel_tol=1e-06, abs_tol=1e-06):
         """
         Check if two poses are approximately equal with a tolerance.
 
@@ -218,7 +204,6 @@ def is_approx(self, other, rel_tol=1e-09, abs_tol=0.0):
         :type rel_tol: float
         :param abs_tol: Absolute tolerance
         :type abs_tol: float
-
         :return: True if the Poses are approximately equal, else False
         :rtype: bool
         """
@@ -226,26 +211,39 @@ def is_approx(self, other, rel_tol=1e-09, abs_tol=0.0):
             raise TypeError("Expected a Pose")
 
         return np.allclose(
-            self.get_translation, other.get_translation, rel_tol, abs_tol
+            self.get_translation(), other.get_translation(), rel_tol, abs_tol
         ) and nearly_equivalent(self.q, other.q, rel_tol, abs_tol)
 
     def __eq__(self, other):
         """
-        Check if two poses are equal
+        Check if two poses are exactly equal.
 
         :param other: Pose with which to check equality.
         :type other: :class:`pyrobosim.utils.pose.Pose`
-
         :return: True if the poses are equal, else False
         :rtype: bool
         """
         if not (isinstance(other, Pose)):
             raise TypeError("Expected a Pose")
 
-        return np.all(self.get_translation == other.get_translation) and np.all(
+        return np.all(self.get_translation() == other.get_translation()) and np.all(
             self.q == other.q
         )
 
+    def __repr__(self):
+        """
+        Representation for printing a Pose object.
+
+        :return: Printable string.
+        :rtype: str
+        """
+        pos_str = f"x={self.x:.2f}, y={self.y:.2f}, z={self.z:.2f}"
+        quat_str = (
+            f"qw={self.q[0]:.3f}, qx={self.q[1]:.3f}, "
+            + f"qy={self.q[2]:.3f}, qz={self.q[3]:.3f}"
+        )
+        return f"Pose: [{pos_str}, {quat_str}]"
+
 
 def get_angle(p1, p2):
     """
diff --git a/pyrobosim_ros/test/test_ros_conversions.py b/pyrobosim_ros/test/test_ros_conversions.py
index 30fb2036..cd31c7a1 100644
--- a/pyrobosim_ros/test/test_ros_conversions.py
+++ b/pyrobosim_ros/test/test_ros_conversions.py
@@ -35,10 +35,7 @@ def test_pose_conversion():
 
     # Convert back to a pyrobosim Pose
     new_pose = pose_from_ros(ros_pose)
-    assert new_pose.x == pytest.approx(orig_pose.x)
-    assert new_pose.y == pytest.approx(orig_pose.y)
-    assert new_pose.z == pytest.approx(orig_pose.z)
-    assert new_pose.q == pytest.approx(orig_pose.q)
+    assert new_pose.is_approx(orig_pose)
 
 
 def test_path_conversion():
@@ -68,10 +65,7 @@ def test_path_conversion():
     new_path = path_from_ros(ros_path)
     assert new_path.num_poses == orig_path.num_poses
     for orig_pose, new_pose in zip(orig_path.poses, new_path.poses):
-        assert orig_pose.x == pytest.approx(new_pose.x)
-        assert orig_pose.y == pytest.approx(new_pose.y)
-        assert orig_pose.z == pytest.approx(new_pose.z)
-        assert orig_pose.q == pytest.approx(new_pose.q)
+        assert orig_pose.is_approx(new_pose)
 
 
 def test_task_action_conversion():

From 82dc6345aeff93c25b193f621b1ef9e3a5714343 Mon Sep 17 00:00:00 2001
From: Sebastian Castro <sebas.a.castro@gmail.com>
Date: Wed, 26 Jul 2023 07:59:41 -0400
Subject: [PATCH 7/7] Fix escape sequence warnings in docstrings

---
 pyrobosim/pyrobosim/core/world.py                  | 8 ++++----
 pyrobosim/pyrobosim/planning/pddlstream/planner.py | 2 +-
 2 files changed, 5 insertions(+), 5 deletions(-)

diff --git a/pyrobosim/pyrobosim/core/world.py b/pyrobosim/pyrobosim/core/world.py
index be036488..5e4fe81d 100644
--- a/pyrobosim/pyrobosim/core/world.py
+++ b/pyrobosim/pyrobosim/core/world.py
@@ -108,7 +108,7 @@ def set_inflation_radius(self, inflation_radius=0.0):
     # World Building Methods #
     ##########################
     def add_room(self, **room_config):
-        """
+        r"""
         Adds a room to the world.
 
         If the room does not have a specified name, it will be given an automatic
@@ -195,7 +195,7 @@ def remove_room(self, room_name):
         return True
 
     def add_hallway(self, **hallway_config):
-        """
+        r"""
         Adds a hallway from with specified parameters related to the :class:`pyrobosim.core.hallway.Hallway` class.
 
         :param \*\*hallway_config: Keyword arguments describing the hallway.
@@ -279,7 +279,7 @@ def remove_hallway(self, hallway):
         return True
 
     def add_location(self, **location_config):
-        """
+        r"""
         Adds a location at the specified parent entity, usually a room.
 
         If the location does not have a specified name, it will be given an
@@ -428,7 +428,7 @@ def remove_location(self, loc):
         return False
 
     def add_object(self, **object_config):
-        """
+        r"""
         Adds an object to a specific location.
 
         If the object does not have a specified name, it will be given an
diff --git a/pyrobosim/pyrobosim/planning/pddlstream/planner.py b/pyrobosim/pyrobosim/planning/pddlstream/planner.py
index 979ce0cd..294b886a 100644
--- a/pyrobosim/pyrobosim/planning/pddlstream/planner.py
+++ b/pyrobosim/pyrobosim/planning/pddlstream/planner.py
@@ -59,7 +59,7 @@ def plan(
         verbose=False,
         **planner_config,
     ):
-        """
+        r"""
         Searches for a set of actions that completes a goal specification
         given an initial state of the world.
         This uses the "adaptive" planner in PDDLStream, which demonstrates the best performance