Remove graph nodes from locked-off hallways when searching for paths (#…

…253)

pyrobosim/pyrobosim/core/world.py

@@ -1088,8 +1088,8 @@ def get_hallways_from_rooms(self, room1, room2):
         :type room1: :class:`pyrobosim.core.room.Room`/str
         :param room2: Instance or name of second room.
         :type room2: :class:`pyrobosim.core.room.Room`/str
-        :return: Hallway instance matching the inputs, or ``None`` if not valid.
-        :rtype: :class:`pyrobosim.core.hallway.Hallway`
+        :return: List of hallways.
+        :rtype: list[:class:`pyrobosim.core.hallway.Hallway`]
         """
         # Validate room input
         if isinstance(room1, str):
@@ -1113,6 +1113,30 @@ def get_hallways_from_rooms(self, room1, room2):
                 hallways.append(hall)
         return hallways
 
+    def get_hallways_attached_to_room(self, room):
+        """
+        Returns a list of hallways attached to a specific room.
+
+        :param room: Instance or name of room.
+        :type room: :class:`pyrobosim.core.room.Room`/str
+        :return: List of hallways.
+        :rtype: list[:class:`pyrobosim.core.hallway.Hallway`]
+        """
+        # Validate room input
+        if isinstance(room, str):
+            room = self.get_room_by_name(room)
+        if not isinstance(room, Room):
+            warnings.warn("Invalid room specified.")
+            return []
+
+        # Now search through the hallways and add any valid ones to the list
+        hallways = []
+        for hall in room.hallways:
+            is_valid_hallway = (hall.room_start == room) or (hall.room_end == room)
+            if is_valid_hallway:
+                hallways.append(hall)
+        return hallways
+
     def get_locations(self, category_list=None):
         """
         Gets all locations, optionally filtered by category.
@@ -1478,6 +1502,31 @@ def graph_node_from_entity(
             graph_nodes = entity.graph_nodes
         elif isinstance(entity, Hallway):
             graph_nodes = [entity.graph_nodes[0], entity.graph_nodes[-1]]
+
+            # Special rule: If all the hallways connected to the room are closed, and the robot is not in the room, remove the graph node from consideration.
+            if robot is not None:
+                robot_in_start_room = entity.room_start.is_collision_free(
+                    robot.get_pose()
+                )
+                if not robot_in_start_room:
+                    room_accessible = False
+                    for hall in self.get_hallways_attached_to_room(entity.room_start):
+                        if hall.is_open:
+                            room_accessible = True
+                            break
+                    if not room_accessible:
+                        graph_nodes.remove(entity.graph_nodes[0])
+
+                robot_in_end_room = entity.room_end.is_collision_free(robot.get_pose())
+                if not robot_in_end_room:
+                    room_accessible = False
+                    for hall in self.get_hallways_attached_to_room(entity.room_end):
+                        if hall.is_open:
+                            room_accessible = True
+                            break
+                    if not room_accessible:
+                        graph_nodes.remove(entity.graph_nodes[-1])
+
         elif isinstance(entity, Object):
             graph_nodes = entity.parent.graph_nodes
         elif isinstance(entity, Location):