Easy way to determine what end of closed hallway robot is at

[analog-cbarber]

It would be nice if a robot is positioned at one end of a closed hallway that it would report which end.

You can figure this out nontrivially from the pose of course, but it would be nice if there was an easier way.

[sea-bass]

This should be doable since the 2 nodes added to the graph are exactly ordered as [start, end] -- are you just using the regular Python API?

[analog-cbarber]

Yes, I am using the regular python API.

I found that I could test for the hallway end using this:

        hallway_end: str | None = None
        if isinstance(robot.location, Hallway):
            # Add information about what end of hallway robot is at if still
            # in a room.
            hallway = robot.location
            pose = robot.get_pose()
            for room in [hallway.room_start, hallway.room_end]:
                if intersects_xy(room.buffered_polygon, pose.x, pose.y):
                    hallway_end = room.name
                    break

[sea-bass]

You could also do this without a polygon check, knowing that hallway.graph_nodes[0] and [1] correspond to the start and end room, respectively.

So something like:

hallway = robot.location
pose = robot.get_pose()
if pose.is_approx(hallway.graph_nodes[0]):
    hallway_end = hallway.room_start.name
elif pose.is_approx(hallway.graph_nodes[1]):
    hallway_end = hallway.room_end.name 
else:
    print("uh oh")

Or if you want to be somewhat more robust, you could also check the distance between the robot pose and each graph node, e.g.,

dist_start = pose.get_linear_distance(self.graph_nodes[0])
dist_end = pose.get_linear_distance(self.graph_nodes[1])
hallway_end = room_start.name if (dist_start < dist_end) else room_end.name