implementation of own event in rmf core

[Marcusttk]

Continuation of this closed issue:

#271

@mxgrey

I have done as you advised in the previous comment. Attached is the link to my RequestLift code.

https://drive.google.com/file/d/1EflBWRnEwYIi4QWZ3_ZIjpg9ZWkn7fgx/view?usp=sharing

I am able to "pause" the lift sequence with my while loop, causing the robot to wait for my signal before it ends the lift.

However I face the current warning: [gzserver-10] [WARN] [1612544473.928771614] [rcl.logging_rosout]: Publisher already registered for provided node name. If this is due to multiple nodes with the same name then all logs for that logger name will go out over the existing publisher. As soon as any node with that name is destructed it will unregister the publisher, preventing any further logs for that name from being published on the rosout topic.

And even though the "go" signal is received, the robot is unable to continue its journey, and is stuck at its position.

I have tried placing the line that creates the subscriber: rclcpp::spin(std::make_shared());

in many different places with no success. I can only pause the sequence, but I am unable to resume the sequence.

Any advice? Thanks

[mxgrey]

In general when requesting code reviews, I strongly recommend following the standard collaboration procedure of forking the repo and then committing and pushing your changes. That way we can see a nice clean diff of what changes you've made. Some basic instructions on forking and pushing can be found here.

Specifically to facilitate a code review, you'll want to commit and push your changes to a branch other than master and then open a pull request from that branch to the master branch of your own GitHub repository. Then you can link us to that pull request and we can provide in-line comments on any issues that we identify.

For this matter in particular, there are some fundamental problems with the approach you're trying to take. Tomorrow I'll spend some time making the necessary changes and provide an experimental API that you can plug into.

[Marcusttk]

Marcusttk#1

Forked and pushed. The above link is the pull request for the master branch of my own github repo.

[mxgrey]

There are a number of fundamental issues with the way that PR tries to implement this feature.

It looks to me that you'd like an arrangement where you can simply create a minimal node to provide the decision about whether or not the robot can enter the lift. I've gone ahead and added an experimental feature to support that use case. The PRs for the feature can be found here and here. When I say that these features are experimental I mean that we will not be officially supporting the feature. The way we solve your use case will most likely change significantly in the future, including a significant change to the API. But I've gone ahead and added this feature prototype to unblock you so you can continue your own experiments with RMF.

The detail that you'll want to focus on is rmf_fleet_adapter/src/experimental_lift_watchdog/main.cpp. You should replace this block with your own logic for deciding whether or not a robot can enter the lift.

To run an experiment with this feature, the latest master branch of rmf_demos provides these launch files:

• ros2 launch demos experimental_hotel.launch.xml: Start up a simulation that includes an experimental lift watchdog
• ros2 launch demos experimental_hotel_loop.launch.xml: Tell a robot to start looping between floors (run this in a separate terminal from the simulation launch file)
• ros2 launch demos experimental_crowded_lift.launch.xml: Tell the watchdog that the lifts are all crowded
• ros2 launch demos experimental_clear_lift.launch.xml: Tell the watchdog that the lifts are all clear

Use the last two launch files to toggle between telling the watchdog that the lifts are clear or crowded. When you tell the watchdog that the lifts are crowded, you will see the lift doors open (per the fleet adapter's request) and then immediately close again (because the fleet adapter ends the session). Eventually the doors will open again when the fleet adapter tries again. If you switch the watchdog from crowded to clear at any time in between the lift request attempts, the robot will be able to enter.

[Marcusttk]

@mxgrey Thanks for your help, it means a great deal to my team & I. I have looked through your code and learnt a lot on the way you implemented it.

[Marcusttk]

@mxgrey regarding your experimental lift watchdog implementation, for my use case I had set the permission to false at the start of the program, making sure that the tinyrobot will always have to check that the lift is empty before going in.

In its current implementation, once the tinyrobot hears the go ahead signal, the permission boolean will change to true, and remain true if the watchdog does not hear any crowded/clear calls. What is the best way to implement a feature which will automatically turn the permission boolean back to false after the lift sequence or part of it has been completed?

One quick solution for this would be to send another "crowded" lift signal before the start of the second lift sequence to change the permission boolean back to false. However, for our use case, we also need the robot to enter the lift and check whether the lift is clear/crowded again before it proceeds. So, the sequence will be robot waits for lift, hears clear signal, enters lift, waits for clear/crowded signal again, and then proceed with the rest of the lift sequence after the 2nd clear signal is sent. The above quick solution will not work for this case.

Please advise. Thank you.

[mxgrey]

If the robot needs to run a multi-stage procedure where it looks, enters, looks again, and backs off if needed, then that whole procedure is what you should have the robot perform when the watchdog callback is triggered.

If the procedure determines that the lift is too crowded, then the robot should return to its original waypoint outside of the lift doors and then the service should return the Crowded flag. If the procedure determines that the lift is clear, then the robot can remain inside the lift and send the Clear flag.

If you want to implement it using the ROS2 service example node that I provided, then you need to make sure you're okay with the service call blocking while the robot goes through this routine. If you use a single-threaded executor, that means only one robot in your whole deployment could inspect a lift at a time. Using a multi-threaded executor may allow multiple service calls to run simultaneously, although you might have to use callback groups... truthfully I haven't done much with multi-threaded execution in ROS2 yet, so I'm not clear on the details for that.

Alternatively, if you're doing this on a real robot, you'll need to create a fleet adapter for it using this API, so at that point you can just set up the watchdog callback to trigger your inspection routine. You would simply save the inner callback somewhere until you finish inspecting, and then you trigger that inner callback with the result.

To be clear, the full control fleet adapter API will not be able to command the whole routine of entering/inspecting/exiting the lift for you. That is very custom logic which is specific to your application, so you'll need to program that logic yourself. But the full control fleet adapter can accommodate that logic as I've described above.