Update last visited in set pose and simplify replanning is start and goal are approximately the same

[dcconner]

changes from origin/update-viz-on-follow-path plus:

• update last_visited_location when setting pose
• add start ~= goal check in planner

[dcconner]

Hmmm. After running several times successfully, I received the following exception.

[run-1]   warnings.warn("Could not find a path from start to goal.")
[run-1] Exception in thread Thread-1 (<lambda>):
[run-1] Traceback (most recent call last):
[run-1]   File "/usr/lib/python3.10/threading.py", line 1016, in _bootstrap_inner
[run-1]     self.run()
[run-1]   File "/usr/lib/python3.10/threading.py", line 953, in run
[run-1]     self._target(*self._args, **self._kwargs)
[run-1]   File "/home/conner/delib_ws/build/delib_ws_worlds/delib_ws_worlds/run.py", line 41, in <lambda>
[run-1]     ros_thread = threading.Thread(target=lambda: node.start(wait_for_gui=True))
[run-1]   File "/home/conner/delib_ws/install/pyrobosim_ros/lib/python3.10/site-packages/pyrobosim_ros/ros_interface.py", line 205, in start
[run-1]     executor.spin()
[run-1]   File "/opt/ros/iron/lib/python3.10/site-packages/rclpy/executors.py", line 295, in spin
[run-1]     self.spin_once()
[run-1]   File "/opt/ros/iron/lib/python3.10/site-packages/rclpy/executors.py", line 852, in spin_once
[run-1]     self._spin_once_impl(timeout_sec)
[run-1]   File "/opt/ros/iron/lib/python3.10/site-packages/rclpy/executors.py", line 849, in _spin_once_impl
[run-1]     future.result()
[run-1]   File "/opt/ros/iron/lib/python3.10/site-packages/rclpy/task.py", line 94, in result
[run-1]     raise self.exception()
[run-1]   File "/opt/ros/iron/lib/python3.10/site-packages/rclpy/task.py", line 239, in __call__
[run-1]     self._handler.send(None)
[run-1]   File "/opt/ros/iron/lib/python3.10/site-packages/rclpy/executors.py", line 489, in handler
[run-1]     await call_coroutine()
[run-1]   File "/opt/ros/iron/lib/python3.10/site-packages/rclpy/executors.py", line 353, in _execute
[run-1]     await await_or_execute(tmr.callback)
[run-1]   File "/opt/ros/iron/lib/python3.10/site-packages/rclpy/executors.py", line 108, in await_or_execute
[run-1]     return callback(*args)
[run-1]   File "/home/conner/delib_ws/install/pyrobosim_ros/lib/python3.10/site-packages/pyrobosim_ros/ros_interface.py", line 672, in publish_robot_state
[run-1]     pub.publish(self.package_robot_state(robot))
[run-1]   File "/home/conner/delib_ws/install/pyrobosim_ros/lib/python3.10/site-packages/pyrobosim_ros/ros_interface.py", line 660, in package_robot_state
[run-1]     state_msg.last_visited_location = robot.location.name
[run-1] AttributeError: 'NoneType' object has no attribute 'name'
[run-1] [ERROR] [1726000629.657840152] [run_world.action_server]: Error raised in execute callback: Failed get goal status array: feedback publisher is invalid, at ./src/rcl_action/action_server.c:940
[run-1] Traceback (most recent call last):
[run-1]   File "/opt/ros/iron/lib/python3.10/site-packages/rclpy/action/server.py", line 333, in _execute_goal
[run-1]     execute_result = await await_or_execute(execute_callback, goal_handle)
[run-1]   File "/opt/ros/iron/lib/python3.10/site-packages/rclpy/executors.py", line 108, in await_or_execute
[run-1]     return callback(*args)
[run-1]   File "/home/conner/delib_ws/install/pyrobosim_ros/lib/python3.10/site-packages/pyrobosim_ros/ros_interface.py", line 552, in robot_path_follow_callback
[run-1]     goal_handle.succeed()
[run-1]   File "/opt/ros/iron/lib/python3.10/site-packages/rclpy/action/server.py", line 150, in succeed
[run-1]     self._update_state(_rclpy.GoalEvent.SUCCEED)
[run-1]   File "/opt/ros/iron/lib/python3.10/site-packages/rclpy/action/server.py", line 117, in _update_state
[run-1]     self._action_server._handle.publish_status()
[run-1] rclpy._rclpy_pybind11.RCLError: Failed get goal status array: feedback publisher is invalid, at ./src/rcl_action/action_server.c:940

This occurred with robot near hall_dining_kitchen (on kitchen side). I'm not sure if this is related to my changes or not. As None is valid return from the get location, we may need to protect this. I'll make that change and re-push.

[dcconner]

I relaxed tolerances because I think the RRT planner may be failing for close poses, but that is more of a hunch than based on conclusive tests.

[dcconner]

@sea-bass Your change in #264 fixes most of the issues, but I'm still seeing issues with planning.

I plan and execute path to hall_dining_kitchen, after deliberately skipping open door, and redoing plan to hall_dining_kitchen, if fails repeatedly with code 2.

The issue is that if the robot is already at the hall_dining_kitchen location then it fails the robot_in_end_room = entity.room_end.is_collision_free(robot.get_pose()) test in World.graph_node_from_entity and the node is removed.

It seems that the position of door entry needs to be adjusted to place within the containing room, or we need to tweak the checks. I can tweak and test, but would like a pointer in the direction you'd like to go to fix.

[sea-bass]

Brilliant, thanks for the investigation @dcconner -- indeed, by recreating what you said and turning on the "Show collision polygons" checkbox in the GUI, that confirms it.

The modification was actually fairly simple, and I've amended #264 -- could you give that a test?

[dcconner]

I'll try to test later today. I'm going to close this PR as irrelevant now, and continue any discussion in #264