Adapter is being sent duplicated waypoints when the path leads to a dock
b-Tomas opened this issue · comments
Bug report
Required information:
- Operating system and version:
osrf/ros:humble-desktopdocker image
- Open-RMF installation type:
- From binaries utilizing the latest version available in the Humble repositories
- Open-RMF version or commit hash
- Listing the installed rmf packages:
sudo apt search rmf | grep installed
WARNING: apt does not have a stable CLI interface. Use with caution in scripts.
ros-humble-rmf-api-msgs/jammy,now 0.0.1-1jammy.20230425.235013 amd64 [installed,automatic]
ros-humble-rmf-battery/jammy,now 0.1.3-2jammy.20230426.004634 amd64 [installed,automatic]
ros-humble-rmf-building-map-msgs/jammy,now 1.2.0-6jammy.20230426.024658 amd64 [installed,automatic]
ros-humble-rmf-building-map-tools/jammy,now 1.6.0-1jammy.20230426.051346 amd64 [installed]
ros-humble-rmf-dispenser-msgs/jammy,now 3.0.2-1jammy.20230426.024711 amd64 [installed,automatic]
ros-humble-rmf-door-msgs/jammy,now 3.0.2-1jammy.20230426.012842 amd64 [installed,automatic]
ros-humble-rmf-fleet-adapter/jammy,now 2.1.2-1jammy.20230426.081144 amd64 [installed,automatic]
ros-humble-rmf-fleet-adapter-python/jammy,now 2.1.2-1jammy.20230426.100114 amd64 [installed]
ros-humble-rmf-fleet-msgs/jammy,now 3.0.2-1jammy.20230426.012847 amd64 [installed,automatic]
ros-humble-rmf-ingestor-msgs/jammy,now 3.0.2-1jammy.20230426.030829 amd64 [installed,automatic]
ros-humble-rmf-lift-msgs/jammy,now 3.0.2-1jammy.20230426.013038 amd64 [installed,automatic]
ros-humble-rmf-obstacle-msgs/jammy,now 3.0.2-1jammy.20230426.024941 amd64 [installed,automatic]
ros-humble-rmf-site-map-msgs/jammy,now 3.0.2-1jammy.20230426.013041 amd64 [installed,automatic]
ros-humble-rmf-task/jammy,now 2.1.1-1jammy.20230426.005408 amd64 [installed,automatic]
ros-humble-rmf-task-msgs/jammy,now 3.0.2-1jammy.20230426.030831 amd64 [installed]
ros-humble-rmf-task-ros2/jammy,now 2.1.2-1jammy.20230426.075148 amd64 [installed]
ros-humble-rmf-task-sequence/jammy,now 2.1.1-1jammy.20230426.011306 amd64 [installed,automatic]
ros-humble-rmf-traffic/jammy,now 3.0.0-1jammy.20230426.000839 amd64 [installed,automatic]
ros-humble-rmf-traffic-editor/jammy,now 1.6.0-1jammy.20230426.001005 amd64 [installed]
ros-humble-rmf-traffic-msgs/jammy,now 3.0.2-1jammy.20230426.024954 amd64 [installed,automatic]
ros-humble-rmf-traffic-ros2/jammy,now 2.1.2-1jammy.20230426.070352 amd64 [installed]
ros-humble-rmf-utils/jammy,now 1.4.0-2jammy.20230426.000131 amd64 [installed,automatic]
ros-humble-rmf-visualization/jammy,now 2.0.0-1jammy.20230426.114525 amd64 [installed]
ros-humble-rmf-visualization-building-systems/jammy,now 2.0.0-1jammy.20230426.034631 amd64 [installed,automatic]
ros-humble-rmf-visualization-fleet-states/jammy,now 2.0.0-1jammy.20230426.073715 amd64 [installed,automatic]
ros-humble-rmf-visualization-floorplans/jammy,now 2.0.0-1jammy.20230426.073802 amd64 [installed,automatic]
ros-humble-rmf-visualization-msgs/jammy,now 1.2.0-4jammy.20230426.013040 amd64 [installed,automatic]
ros-humble-rmf-visualization-navgraphs/jammy,now 2.0.0-1jammy.20230426.073303 amd64 [installed,automatic]
ros-humble-rmf-visualization-obstacles/jammy,now 2.0.0-1jammy.20230426.073720 amd64 [installed,automatic]
ros-humble-rmf-visualization-rviz2-plugins/jammy,now 2.0.0-1jammy.20230426.110404 amd64 [installed,automatic]
ros-humble-rmf-visualization-schedule/jammy,now 2.0.0-1jammy.20230426.073319 amd64 [installed,automatic]
ros-humble-rmf-websocket/jammy,now 2.1.2-1jammy.20230426.070332 amd64 [installed,automatic]
- ROS distribution and version:
- ROS2 Humble Hawksbill from at the time of writing latest version of
osrf/ros:humble-desktop
- ROS2 Humble Hawksbill from at the time of writing latest version of
- ROS installation type:
osrf/ros:humble-desktop
- Package or library, if applicable:
- N/A
Description of the bug
Every time the method rmf_adapter.RobotCommandHandle.follow_new_path() is called by rmf core, a list of waypoints of type rmf_adapter.plan.Waypoint for the robot to follow is sent. When the path ends at a dock vertex, the vertex right before it is duplicated (exact same timestamp and pose). For example, in the path A -> B -> C -> Dock, the list of waypoints received has C duplicated. Such thing doesn't happen with dock to vertex paths or vertex to vertex paths.
Steps to reproduce the bug
- Create an RMF full control fleet adapter from https://github.com/open-rmf/fleet_adapter_template and log waypoint list and their data. In our case, we are using rmf_inorbit_fleet_adapter.
- Add a robot to the fleet
- Send a loop task starting anywhere and ending at a dock
Expected behavior
Get a list of waypoints with no consecutive dupicates
Actual behavior
The RobotCommandHandle receives a list of waypoints with the waypoint previous to the dock being duplicated.
Hi there,
Every time the method rmf_adapter.RobotCommandHandle.follow_new_path() is called by rmf core, a list of waypoints of type rmf_adapter.plan.Waypoint for the robot to follow is sent. In this list, the first two elements (or at least this is the pattern we've found) have the same x, y, theta values (but are different objects).
This is expected behavior and part of the Responsive Wait feature which you can disable. See this discussion for more information. Another approach is to filter them out from your fleet adapter as you mentioned.
I'll clarify that the two elements in the list are unique as the time component of each waypoint is different. The second waypoint's time will be 30s after that of the first one, indicating that the robot is expected to remain at this waypoint for the next 30s. It's way to have the robot's current position synced with the traffic schedule system so that other agents are aware of this robot's presence. It's a temporary workaround until we have a "Reservation" system integrated with Open-RMF which will keep track of and assign resources such as waypoints to agents.
Thank you @Yadunund for you quick response. You are right, I was not looking at the time component of the waypoints. The waypoints I found were delayed by 30 seconds.
However, when comparing position and time between waypoints, I still found one occasion when RMF sends duplicated waypoints. When the path ends at a dock vertex, the vertex right before it is duplicated (exact same time and position). For example, in the path A -> B -> C -> Dock, the list of waypoints received has C duplicated. Such thing doesn't happen with dock to vertex paths or vertex to vertex paths.
Is that a design decision as well? If so, I am curious to learn about it.
I didn't get a chance to look at your RobotCommandHandle::dock() implementation in detail but any chance your start_docking() is returning false which preempts the docking? That would result in the same behavior as explained in this issue: open-rmf/fleet_adapter_template#16 (comment)
Thanks for your suggestion @Yadunund, but I don't think it is related to the dock method. What I am referring to is the list of waypoints received by one single call of RobotCommandHandle::follow_new_path(), before any docking logic is executed. In this list of waypoints, the first one is the start vertex of the path and the last one a dock. The vertex right before the dock, which would be the start of the docking lane, seems to be duplicated in this list (timestamp included). I can't tell if that is intentional or not.
This is a different issue from the one mentioned in the root comment of this issue, which I was wrong about. But I think it is strange.
What I am referring to is the list of waypoints received by one single call of RobotCommandHandle::follow_new_path(), before any docking logic is executed. In this list of waypoints, the first one is the start vertex of the path and the last one a dock. The vertex right before the dock, which would be the start of the docking lane, seems to be duplicated in this list (timestamp included). I can't tell if that is intentional or not.
Oh I see what you mean. You're suggesting that the waypoints in the follow_new_path() call contains the dock waypoint and a duplicate of the waypoint at the start of the docking lane. That does not sound intentional and we might be accidentally appending the entry waypoint (for the second time) and the dock waypoint to the list. Will look into it.
Great. If that's ok, I will change the bug description (because the initial issue was explained in your first comment by the Responsive Wait feature).
I think we should open a separate ticket for this issue since the original one was related to the responsive wait