Posted by @ejalaa12:
Hello,
I have a mocked robot simulated in Python.
That fake robot simulates the movement of a robot, i.e everytime it receives a PathRequest, the robot executes the path in thread, and publish regular RobotState containing its position, heading, and remaining waypoints.
I’ve runned a robot_state_aggregator node and full_control node.
I have a custom nav_graph with 5 waypoints, start0
is a charger and is where the robot must be spawned
Those are the issues I’m encountering:
- even though the mocked robot is currently located at the first waypoint (
start0
) of the nav_graph, the full_control
node still receives a follow_new_path
request with two waypoints: start0
AND start0
. I’m not really sure why this happens. I’m thinking this is to make sure the robot really goes to the spawned waypoint of the graph, but then why twice?
- As soon as my mocked robot publishes a state that contains an empty path list → meaning that my robot has finished traveling the path. As expected, the full_control node calls the
path_finished_callback()
method.
- Immediately after that, the full_control node receives a new
follow_new_path
with the same two waypoints as in step1. And the loop continues forever.
Is this behavior normal? I would assume step2 should be the end of the interaction between full_control_node
and my mocked_robot
, until something like a task_dispatcher node request a new task.
Thank you in advance for your answer.
PS: Note that I know that the full_control
fleet_adapter is just an example, I’m actually writing my own fleet_adapter, however I got the exact same behavior with both.
Posted by @Yadunund:
Hi @ejalaa12
The behaviour you are seeing is the expected consequence of the ResponsiveWait
feature introduced to the fleet adapters in these PRs Introducing ResponsiveWait task phase by mxgrey · Pull Request #308 · osrf/rmf_core · GitHub, Give a floating tail prediction to the Responsive Wait phase by mxgrey · Pull Request #30 · open-rmf/rmf_ros2 · GitHub
When there are no tasks to perform, the robots in the fleet are put into a “ResponsiveWait” mode where they “wait” at the nearest waypoint on the graph (in your case this must be start0
). The robot is given an itinerary with two waypoints (two is the min number of waypoints needed to define a trajectory in the rmf schedule), both at the same location but with the second waypoint having a time component that is 30s after the first. For these waypoints in follow_new_path
, the fleet adapter should ensure that the robot will stay on this waypoint until the specified time in the time component has passed. So sending two waypoints at the same location but 30s apart is essentially an instruction for the robot to wait at that location for 30s. During these 30s the robot remains active in the rmf schedule and can negotiate with other robots to give way if required. In this regard, it is “responsive”. After the 30s elapses, if the robot still doesn’t have any tasks to perform, it will receive another similar itinerary.
Hopefully that clarifies what you observed.
Glad to hear that you’re writing your own fleet adapter 
Posted by @ejalaa12:
Thanks a lot for the quick response, that’s very helpful.
So the TaskManager linked to that mocked robot, default’s behavior is to execute the ResponsiveWait
phase.
What is not clear to me is what makes the robot still “active” in the rmf schedule. In other words, if the ResponsiveWait was not implemented, why does the robot becomes “inactive” in the rmf schedule? Which part of the code actually send a kind-of “keep-alive” message to the rmf_schedule_node.
Is the ResponsiveWait mode a way to force the update_state
cb to get called?
The issue I see with that “ResponsiveWait” mode is that the robot has to handle all those PathRequest even though it is not required to move. I guess what I could do on the fleet_adapter side is to interpret those two waypoints as a wait and not send them to the mocked robot. That way I can reduce the number of exchanges, while my fleet_adapter can “keep alive” the robot with rmf_schedule.
Thank you again 
Posted by @mxgrey:
if the ResponsiveWait was not implemented, why does the robot becomes “inactive” in the rmf schedule?
The RobotUpdateHandle
and RobotCommandHandle
work together to set expectations and communicate deviations between the RMF schedule and the robot fleet. The ResponsiveWait
phase is the current (admittedly not yet very elegant) approach to communicate to the robot fleet that the schedule is expecting the robot to be waiting at a specific location. That information is provided to the robot through the RobotCommandHandle
. Then it’s up to the RobotUpdateHandle
and RobotCommandHandle
callbacks to communicate back to RMF about any deviations from the commanded behavior. That way RMF can trigger a negotiation with other robots if your robot is not actually waiting in its expected location, or if your robot is blocking other robots by being parked there.
Before the responsive wait behavior was implemented, users would frequently get themselves into trouble when their robots would park themselves in the middle of an active traffic lane. Without responsive waiting, a parked robot would permanently block traffic. One option we considered was to simply treat a parked robot as a permanent obstacle, but many of our testing spaces have tight constraints and narrow corridors, so that would likely cause permanent traffic deadlock in many cases.
For a more elegant solution we’re currently developing a parking spot reservation system. When that feature is finished, all idle robots will be automatically assigned a designated parking spot and be commanded to travel there. Once there, the robot would be taken off the schedule, and no other robots would be allowed to use the reserved parking spot. The responsive wait mode for idle robots would no longer be the default, but it could still be used for situations like a queuing system.
I guess what I could do on the fleet_adapter side is to interpret those two waypoints as a wait and not send them to the mocked robot.
Yes, this is how I would recommend approaching it to minimize network noise. It can be very tricky to communicate these edge case behaviors through an API that is both narrow and general, so sometimes the fleet adapter may need to do some heavy lifting in its logic to make the downstream information flow efficient. We’re open to suggestions for improving the API, as long as the API changes wouldn’t unfairly favor one platform over others.
Edited by @mxgrey at 2021-07-12T11:08:19Z
Posted by @zhangyuran-gg:
In ResponsiveWait phase,sent path_requests,x and y are the same but yaw is different between the two points,why?
Looking forward to your reply
Posted by @zhangyuran-gg:
This sometimes leads to unnecessary rotation of the robot
Posted by @mxgrey:
That shouldn’t happen. Are you able to reproduce this issue in simulation or do you only see it in your own fleet adapter?
The most likely cause is you may have an error in how you’re converting to or from your robot’s yaw coordinate.
Posted by @zhangyuran-gg:
This always appears in my real and simulated scenes.The ResponsiveWait does not cause rotation during initial starup(Before assigning tasks).After the robot is assigned a task and completes the task,the ResponsiveWait cause rotation.
1.In simulation,I created building.yaml
file and generated .world
models
and /nav_graphs/0.yaml
file. Afterwards,I created test.launch.xml
based on rmf_demos_gz_classic/office.launch.xml
to start my simulation.
2.In my real scenes,I am using free fleet for integration.
The following figure,tinyRobot3 has not been assigned a task and tinyRobot2 is assigned a task and execution ends.After that,Each ResponsiveWait of tinyRobot2 causes rotation and sometimes tinyRobot3 also rotates.
What should I do to solve this problem.
Looking forward to receiving your reply.
Posted by @zhangyuran-gg:
There is no such situation in office.launch.xml
,which step of mine has a problem?
Posted by @mxgrey:
Are you able to share the .building.yaml
and launch files so I can reproduce the problem easily? That would make the debugging much faster.
If the files cannot be shared publicly you are welcome to email them to mxgrey@intrinsic.ai
Posted by @mxgrey:
Thanks for providing the reproducible example.
I’ve figured out what the problem is. The default value that we’ve chosen for the max_merge_waypoint_distance
is smaller than the precision that the simulated robot tries to achieve when it moves towards a goal. As a result, the planner believes the robot needs to keep adjusting itself to get closer to the goal.
To get rid of the unnecessary spinning, you just need to add something like
max_merge_waypoint_distance: 0.05 # 5cm threshold
to your fleet config under rmf_fleet:
in your fleet configuration file.
Posted by @zhangyuran-gg:
Thank you for your help! I have tested in both simulated and actual scenarios, and this issue has indeed been resolved.
I encountered another problem when I was designing multi-layer scenes: When the robot arrives at the elevator door,it waits for the elevator to open.After the elevator opens,the robot walks towards the elevator,but gets stuck halfway.
![Uploading Screenshot from 2023-11-14 13-58-37.png…]()
Posted by @zhangyuran-gg:
Posted by @mxgrey:
It looks like the name of the lift is LF1
. Unfortunately the simulation is a little too simplistic right now and requires the lift’s name to have the word lift
in it (any mix of lower or uppercase). Otherwise the simulated robot will think the lift itself is an obstacle and stop to avoid colliding with it.
We’re developing a new simulation pipeline that won’t have that limitation, but for now we don’t have a workaround.
Edited by @mxgrey at 2023-11-17T02:55:15Z
Posted by @zhangyuran-gg:
Thank you for your reply! The problem has been resolved