questions about hall doors, planning, and failures

[dcconner]

I'm being lazy and don't want to dig into pyrobosim code details, so I will post a few questions here that might be of general interest.

RobotState has "last_visited". For most locations, I can look up my current room corresponding to a location. Except hallways, as it could be one of two rooms for given door name (e.g. 'hall_dining_closet').

1) a) Is there any "easy" way to find out which room you are in without doing some polygon inclusion test based on pose?
b) Can we add current_room to state? (preferred)

2) If FollowPath fails, can I try to execute the same path and have it pick up from the middle, or should I replan? Either works for me, just need to clarify whether replanning is necessary.

3) Suppose the robot is at a location and I request a plan to that same location. Does the action succeed with with empty path [ ], single point [location], or [location location] with 0 travel distance? Or does the action fail?

Ideally, I'd like a sub-behavior to travel to door and open. If any action fails, I'd like to invoke the sub-behavior again and if I'm already at the door just have plan-follow part trivially succeed.

3b) related: Can I follow an empty path or [start start] and have it succeed?

[dcconner]

Based on my testing so far, I thought the answer to 3 is that both plan and follow "succeeds" if you are at the goal location, which is what I hoped for. But on another test, I got repeated failures to plan to hall_dining_kitchen even though I was at the door already (after simulated open failure). So I'm not sure if it was difference of hallway door vs. charger dock.

[dcconner]

Follow on question. charger_dock does not appear as location in world file. Are there other constructed location names that are not listed as explicit location ?

[sea-bass]

1. a) Is there any "easy" way to find out which room you are in without doing some polygon inclusion test based on pose? b) Can we add current_room to state? (preferred)

Without doing some polygon inclusion test based on pose, in a generic sense? No. But there already are things running in the background doing this for us, so we can definitely take advantage of that and move up the semantic ladder, so to speak.

Basically, if you have a robot's location, you have a few options:

• A room, in which case you already have the information.
• An object spawn, in which case you can back out the room name structurally with get_room_name()
• A hallway, in which case you would need to figure out which of the 2 adjacent rooms you're standing at (if at all). This third part would be the missing piece to fill out this functionality, and I believe I can factor this out from whatever is here.

---

2. If FollowPath fails, can I try to execute the same path and have it pick up from the middle, or should I replan? Either works for me, just need to clarify whether replanning is necessary.

The way path following works is just by incrementally "teleporting" the robot along the path from start to finish at a specified speed/interpolation distance. So I think replanning would be necessary at the moment.

---

3. Suppose the robot is at a location and I request a plan to that same location. Does the action succeed with with empty path [ ], single point [location], or [location location] with 0 travel distance? Or does the action fail?

Ideally, I'd like a sub-behavior to travel to door and open. If any action fails, I'd like to invoke the sub-behavior again and if I'm already at the door just have plan-follow part trivially succeed.

3b) related: Can I follow an empty path or [start start] and have it succeed?

These cases should all succeed as a trivial case.

I just pulled up PyRoboSim and tried to run the /robot/plan_path action on a hallway the robot was already at, and it still succeeded with the trivial [start, start] path you specified. I even tried staging an open door failure first and it succeeded, so curious about your exact steps so I can chase down any issues?

---

Follow on question. charger_dock does not appear as location in world file. Are there other constructed location names that are not listed as explicit location ?

The locations are explicit, but in a more subtle way. If you look at the location metadata for the charger type, you'll see it has a dock sub-location defined.

This means that if you instantiate various types of chargers, e.g. charger0 and charger1, then they will have charger0_dock and charger1_dock sub-locations (or object spawns), respectively.

However, the key thing here is that PyRoboSim is "smart" enough to resolve just charger to charger_dock because it's the only child sub-location in this case, unlike for example counter0_left/counter0_right in the default world, in which you would have had to specify one or the other.

So in the case of all our workshop locations, which have single object spawns, you should be able to specify all nav goals simply as charger without the extra _dock. Same applies to the other locations dumpster_disposal, table_tabletop, fridge_storage, etc.

[dcconner]

A hallway, in which case you would need to figure out which of the 2 adjacent rooms you're standing at (if at all). This third part would be the missing piece to fill out this functionality, and I believe I can factor this out from whatever is here.

That looks to be it, so if we could store that and report as current_room in RobotState it would cut down on the amount of semantic calculations we need to do in behaviors. I think that would be simpler for our attendees (and me!)

[dcconner]

I just pulled up PyRoboSim and tried to run the /robot/plan_path action on a hallway the robot was already at, and it still succeeded with the trivial [start, start] path you specified. I even tried staging an open door failure first and it succeeded, so curious about your exact steps so I can chase down any issues?

I need to do more testing and tracking on my end as well. It was late, and I seemed to be getting the issue where the UI did not reflect the world state again so I was getting mixed info from pyrobosim UI and my behavior. So I don't have anything repeatable at present to pass along to you.

[dcconner]

Copying this from discussion in #20

In this example, the robot has already opened the dumpster, retrieved the waste, and on its way back to the dumpster. But UI shows state before I opened the initial door.

I'm not able to recreate this when running this example... I ran it a bunch of times to verify. Wonder what it might be...

in problems/delib_ws_python/launch/run.launch.py could you add more verbose output to the world to see if anything else crops up?

problem_node = Node(
    package="delib_ws_worlds",
    executable="run",
    name="run_world",
    parameters=[{"problem_number": LaunchConfiguration("problem_number")}],
    output="screen",    # add this
    emulate_tty=True,    # add this
)

[dcconner]

@sea-bass Another issue with UI not updating. Moved to charger_dock, RobotState reports 100% but UI still shows 65.%.

The UI did not update until after I had planned a new path. I don't think it updated until the robot actually moved, but I can't say that for certain.

Update: Confirmed and repeatable. The UI does not update the battery level until after the robot starts moving again. And the robot arrives at kitchen still showing battery level at 100% instead of 94% like robot state message

[sea-bass]

I think I know what that one is about. If you use the full "navigate" abstract action, there is a specific step that updates the UI after nav is complete.

That is not the case with the individual "follow path" action, which you are using... so I will add that in when I can.

[dcconner]

@sea-bass Interesting sequence.

start_behavior-1] [WARN] [1725895560.421339847] [behavior]: PlanPath : 'robot/plan_path' - target location='dining` planning failure (2) 'Path planning failed.'
[start_behavior-1] [WARN] [1725895560.422429513] [behavior]: Failed to plan path.
[start_behavior-1] [INFO] [1725895560.524948630] [behavior]: PlanDoor - planning to target location='hall_dining_kitchen` successful with 3 waypoints
[start_behavior-1] [INFO] [1725895560.525481443] [behavior]: Successfully planned path.
[start_behavior-1] [INFO] [1725895560.526350170] [behavior]: Send follow path goal with 3 waypoints ...
[start_behavior-1] [INFO] [1725895562.633562708] [behavior]:   'GoDoor' : 'robot/follow_path' returned 4 pyrobosim_msgs.action.FollowPath_Result(execution_result=pyrobosim_msgs.msg.ExecutionResult(status=0, message=''))
[start_behavior-1] [INFO] [1725895562.634100134] [behavior]: Successfully followed path.
[start_behavior-1] [INFO] [1725895562.735725700] [behavior]: OpenDoor - successfully opened!
[start_behavior-1] [INFO] [1725895562.736259996] [behavior]: Successfully completed door action.

Above is as expected, can't go to next room, so plan to door and open door.

Try to plan to room again, and it fails

[start_behavior-1] [WARN] [1725895565.249891136] [behavior]: PlanPath : 'robot/plan_path' - target location='dining` planning failure (2) 'Path planning failed.'
[start_behavior-1] [WARN] [1725895565.250411363] [behavior]: Failed to plan path.

Did it fail because of random planning failure, or was something not reset in state with door being opened?

At any rate, with planning failure, I do the door again.

[start_behavior-1] [INFO] [1725895565.352244366] [behavior]: PlanDoor - planning to target location='hall_dining_kitchen` successful with 2 waypoints
[start_behavior-1] [INFO] [1725895565.353233686] [behavior]: Successfully planned path.
[start_behavior-1] [INFO] [1725895565.354382390] [behavior]: Send follow path goal with 2 waypoints ...
[start_behavior-1] [INFO] [1725895565.958743631] [behavior]:   'GoDoor' : 'robot/follow_path' returned 4 pyrobosim_msgs.action.FollowPath_Result(execution_result=pyrobosim_msgs.msg.ExecutionResult(status=0, message=''))
[start_behavior-1] [INFO] [1725895565.959284097] [behavior]: Successfully followed path.
[start_behavior-1] [WARN] [1725895566.061235194] [behavior]: OpenDoor : '/execute_action' - failed to 'open' command - '[robot] Simulated opening failure.'
[start_behavior-1] [WARN] [1725895566.061761875] [behavior]: Failed to complete door action.

This said "Simulated open failure", so it retries the door again.

[start_behavior-1] [INFO] [1725895566.062519900] [behavior]: Behavior Hint: Retry the door
[start_behavior-1] [INFO] [1725895566.063998010] [behavior]: <-- Want result: '/Through Door/RetryDoor' > 'door'
[start_behavior-1] [INFO] [1725895574.536016244] [behavior]: --> Request autonomy changed to 2 on 'Traverse'
[start_behavior-1] [INFO] [1725895574.714639083] [behavior]: PlanDoor - planning to target location='hall_dining_kitchen` successful with 2 waypoints
[start_behavior-1] [INFO] [1725895574.715175734] [behavior]: Successfully planned path.
[start_behavior-1] [INFO] [1725895574.716020279] [behavior]: Send follow path goal with 2 waypoints ...
[start_behavior-1] [INFO] [1725895575.319713512] [behavior]:   'GoDoor' : 'robot/follow_path' returned 4 pyrobosim_msgs.action.FollowPath_Result(execution_result=pyrobosim_msgs.msg.ExecutionResult(status=0, message=''))
[start_behavior-1] [INFO] [1725895575.320812546] [behavior]: Successfully followed path.
[start_behavior-1] [WARN] [1725895575.423164403] [behavior]: OpenDoor : '/execute_action' - failed to 'open' command - 'Hallway: hall_dining_kitchen is already open.'
[start_behavior-1] [WARN] [1725895575.423679421] [behavior]: Failed to complete door action.

Now it fails because the door is already open, and it gets stuck here with the current logic I had of immediately retrying door and not trying to plan through the door again. Maybe poor design on my choice.

So .... 1) Do we have a way of detecting "is door open" other than remembering it ourselves in case of repeated planning failures? 2) Should "Open Door" fail if already open? (same for "Close"/closed) My code requires "SUCCESS". It treats any of these action returns as "failed"

                elif result.status in (ExecutionResult.PRECONDITION_FAILURE,
                                       ExecutionResult.PLANNING_FAILURE,
                                       ExecutionResult.EXECUTION_FAILURE):
                    Logger.logwarn(f"{self} : '{self._topic}' - failed to '{self._action_type}' command - '{result.message}'")
                    self._return = 'failed'

If door is already open, should "Open" door trivially succeed?

[sea-bass]

1. If you call the /request_world_state service, that should contain info on whether a hallway is open or closed. So that's a ROSsy way to check?
2. Good question. I made these conditions fail in both the open/close case (as in, you can't open what is already open), but this could be switched to be a trivial success as well. I don't have a strong inclination one way or another.

[dcconner]

I can go either way. Making service calls to check let's me demonstrate service calls, where letting Open trivially succeed if already open simplifies the logic, which may help the students. What do others @matthias-mayr @ct2034 think?

[sea-bass]

@dcconner for the issue with robot state (battery/location) display in the GUI, could you verify this PR? https://github.com/sea-bass/pyrobosim/pull/260

[sea-bass]

I can go either way. Making service calls to check let's me demonstrate service calls, where letting Open trivially succeed if already open simplifies the logic, which may help the students. What do others @matthias-mayr @ct2034 think?

If we are okay with making this change, https://github.com/sea-bass/pyrobosim/pull/261 is ready to go as well.

[dcconner]

@sea-bass I'll try to look at these later tonight, if not early tomorrow. Regarding #261 I think others need to chime in on that one; it looks good to me.

[dcconner]

Both your # 260 and 261 look good to me, and I now have the service check functional so I'm good to go and will close this issue later today.

[dcconner]

@sea-bass I spoke to soon. I just had the following result: planned path to dining/close door, got successful follow, then open door simulated failure. Thereafter, the request to plan to hall_dining_closet failed - Nothing is shown in the pyrobosim terminal.

start_behavior-1] [INFO] [1725996761.013040509] [behavior]: PlanDoor - planning to target location='hall_dining_closet' successful with 4 waypoints
[start_behavior-1] [INFO] [1725996761.013563555] [behavior]: Successfully planned path.
[start_behavior-1] [INFO] [1725996761.014431486] [behavior]: Send follow path goal with 4 waypoints ...
[start_behavior-1] [INFO] [1725996764.125874159] [behavior]:   'GoDoor' : 'robot/follow_path' returned 4 pyrobosim_msgs.action.FollowPath_Result(execution_result=pyrobosim_msgs.msg.ExecutionResult(status=0, message=''))
[start_behavior-1] [INFO] [1725996764.127024959] [behavior]: Successfully followed path.
[start_behavior-1] [INFO] [1725996764.128593951] [behavior]: CheckOpen: Calling service request_world_state ...
[start_behavior-1] [WARN] [1725996764.332232308] [behavior]: OpenDoor : '/execute_action' - failed to 'open' command - '[robot] Simulated opening failure.'
[start_behavior-1] [WARN] [1725996764.332749213] [behavior]: Failed to complete door action.
[start_behavior-1] [INFO] [1725996764.333450564] [behavior]: Behavior Hint: Retry the door
[start_behavior-1] [WARN] [1725996764.435186816] [behavior]: PlanDoor : 'robot/plan_path' - target location='hall_dining_closet' planning failure (2) 'Path planning failed.'
[start_behavior-1] [WARN] [1725996764.435729506] [behavior]: Failed to plan path.
[start_behavior-1] [INFO] [1725996764.436486239] [behavior]: Behavior Hint: Retry the door
[start_behavior-1] [WARN] [1725996764.636361644] [behavior]: PlanDoor : 'robot/plan_path' - target location='hall_dining_closet' planning failure (2) 'Path planning failed.'
[start_behavior-1] [WARN] [1725996764.636874339] [behavior]: Failed to plan path.
[start_behavior-1] [INFO] [1725996764.637635104] [behavior]: Behavior Hint: Retry the door
[start_behavior-1] [WARN] [1725996764.837700907] [behavior]: PlanDoor : 'robot/plan_path' - target location='hall_dining_closet' planning failure (2) 'Path planning failed.'
[start_behavior-1] [WARN] [1725996764.838207800] [behavior]: Failed to plan path.
[start_behavior-1] [INFO] [1725996764.838958574] [behavior]: Behavior Hint: Retry the door
[start_behavior-1] [WARN] [1725996765.038959487] [behavior]: PlanDoor : 'robot/plan_path' - target location='hall_dining_closet' planning failure (2) 'Path planning failed.'
[start_behavior-1] [WARN] [1725996765.039468134] [behavior]: Failed to plan path.
[start_behavior-1] [INFO] [1725996765.040201241] [behavior]: Behavior Hint: Retry the door
[start_behavior-1] [WARN] [1725996765.240243785] [behavior]: PlanDoor : 'robot/plan_path' - target location='hall_dining_closet' planning failure (2) 'Path planning failed.'
[start_behavior-1] [WARN] [1725996765.240755533] [behavior]: Failed to plan path.
[start_behavior-1] [INFO] [1725996765.241502227] [behavior]: Behavior Hint: Retry the door
[start_behavior-1] [WARN] [1725996765.441655733] [behavior]: PlanDoor : 'robot/plan_path' - target location='hall_dining_closet' planning failure (2) 'Path planning failed.'
[start_behavior-1] [WARN] [1725996765.442172848] [behavior]: Failed to plan path.

Can we make this change (submitted along with another change as PR # 262)

--- a/pyrobosim/pyrobosim/core/robot.py
+++ b/pyrobosim/pyrobosim/core/robot.py
@@ -12,6 +12,7 @@ from .objects import Object
 from ..manipulation.grasping import Grasp
 from ..planning.actions import ExecutionResult, ExecutionStatus
 from ..utils.knowledge import query_to_entity
+from ..utils.motion import Path
 from ..utils.polygon import sample_from_polygon, transform_polygon
 from ..utils.pose import Pose

@@ -281,7 +282,10 @@ class Robot:
                 return None
             goal = goal_node.pose

-        path = self.path_planner.plan(start, goal)
+        if start.is_approx(goal):
+            path = Path([start, goal])
+        else:
+            path = self.path_planner.plan(start, goal)
         if self.world and self.world.has_gui:
             show_graphs = True
             self.world.gui.canvas.show_planner_and_path_signal.emit(

[dcconner]

closing this as the remaining issues are being discussed in https://github.com/sea-bass/pyrobosim/pull/262 and https://github.com/sea-bass/pyrobosim/pull/264