Loopback Simulator Package

[SteveMacenski]

Offer a simplistic Gazebo replacement which sends robot velocity commands to TF as the robot's odometry to completely remove the need for complex simulation for testing higher level behavioral attributes not related to perception or control.

This would be unable to generate sensor data, only robot motion, so in using it the documentation should mention that a map is required and have the static map in the local costmap to avoid obstacles. AMCL will probably need to be disabled in this mode and the simulator just publish the robot's map-odom pose since we know perfectly where it is from initial pose + perfect odometry.

A nice thing about this is that you can spawn the robot anywhere for some deterministic tests repeatedly

This would be a nice medium sized contribution from an outside contributor looking to make an impact!

[smitdumore]

Hi @SteveMacenski,

I'm excited about the prospect of contributing to the project, and I'm reaching out to express my interest in tackling this issue.
This would mark my first venture into the realm of open source contributions. While I boast intermediate proficiency in ROS1, I'm in the early stages of acquainting myself with ROS2, with a commitment to learning and growing along the way.

For a bit of background, you can explore my portfolio here: smitdumore.github.io

I'm wondering if I could take up the aforementioned issue ?

Thank you.

[SteveMacenski]

Absolutely! You happen to have found something I'm intentionally not going to build with the goal of it being a good first issue for a new contributor that's interested in giving back could take on -- and hopefully grow your skills to continue to contribute well afterward :-)

I think a good place to start is to review this topic and come up with a proposal of a plan of attack. It might be worth getting the basic TB3 demo working from our docs to have a base-case and see what kinds of things you might need to fake out by looking at topics, TF tree, and the role that odometry + AMCL play in the success of the robot. We can review that with you to make sure we have a good plan in place before you get too deep into the implementation -- but I'd never argue with you experimenting yourself a bit as well.

I believe due to the nature of this package in testing (e.g. not hardware or production use) and not requiring high performance, you could use Python3 to implement this if you so choose. It would not be my preference that you did that, but that option does exist for you uniquely for this project over others -- if you happen to be more comfortable with python than C++. Future projects though would be primarily C++.

P.S. Your docking experience might be something we can work on too, its on my roadmap with a similar holding off for the right time.

[smitdumore]

Thank you, @SteveMacenski! I'm thrilled to tackle this topic and lay out a plan of attack. Given the complexity, I anticipate needing about a week to formulate a solid plan and get up to speed on this topic. C++ is my forte, so the package is in good hands there.

With my previous experience in robot docking projects, I'm eager to contribute that expertise to any future endeavors. Let's make it happen!

[smitdumore]

@SteveMacenski following is the plan of action, and some questions that I have regarding the project.

Plan :

1. Disable Gazebo and AMCL:
Modify "tb3_simulation_launch.py" to disable Gazebo and AMCL.
2. Launch Loopback Simulator Node:
Include a launch configuration to start the Loopback simulator node.
3. Declare Robot Start Pose Parameter:
Add a parameter declaration in the Simulator node for the robot's start pose.
4. Publish Initial TF Between map and base_link:
Utilize the declared parameter to publish the initial TF between "map" and "base_link" in the Simulator node.
Similarly, publish the initial TF between "map" and "odom" based on the specified parameter value.
5. Listen to Robot Velocity Commands:
Create a module to subscribe to robot velocity commands in the Simulator node.
6. Start Listening on Navigation Goal:
Begin listening to robot velocity commands when the robot receives a navigation goal.
Implement Velocity Command Subscription.
7. Simulate Robot Unicycle Model:
Implement the robot's unicycle model within the velocity callback function.
Simulate perfect odometry based on the received velocity commands.
8. Update Robot Pose and TF:
Update the robot's pose based on the simulated odometry.
Adjust the TF tree between "odom" and "base_link" accordingly.
9. Check Robot Pose in Static Map:
Integrate a check to ensure the updated robot pose is within the static map boundaries.
Verify that the robot is not colliding with occupied map cells in the local cost map.
10. Write Deterministic Tests:
Develop deterministic tests to spawn the robot and provide navigation goals.
Verify that the robot's behavior aligns with expectations in a controlled testing environment.

Questions :

1. What's the purpose of this package? What are higher-level behavioral attributes?
2. Is this package standalone, or is it integrated into nav2?
3. How does it adapt if the robot isn't a differential drive system?
4. How can I expect to receive feedback during the project?
5. Besides publishing fake odometry and tf, what other aspects should be simulated or faked?

[SteveMacenski]

Modify "tb3_simulation_launch.py" to disable Gazebo and AMCL.

Something important to note on the outset is that the default system needs to stay, this is not a replacement for the current system, but an additional capability. For now, it should be limited to its own package, but it may make sense after we have something working to have an analog system to tb3_simulation_launch.py for the loopback simulator. But I think that's TBD on the details, but I think that would be nice to have an entry-level example. I just don't know if it makes more sense in nav2_bringup or in the nav2_loopback_sim package.

4. Publish Initial TF Between map and base_link:

Keep in mind the tree is map->odom->base_link. You should keep it in that order. My personal recommendation to start (which might be wrong, so take it with a grain of salt) is to set the robot's pose w.r.t. the initial pose in map frame. The map frame stays static from that point on to where-ever it is initially located based on the map.yaml (e.g. the normal thing). When the robot moves, that is with respect to the odom frame. So the map->odom part of it just stays static from the initial settings or when someone uses the set initial pose tool to move the robot around. That way you only need to update the odom->base_link transform as the robot moves as part of the simulation mechanics. That would best mimick what the full-physics simulation and the real-world robot does.

Begin listening to robot velocity commands when the robot receives a navigation goal.

Why not just always listen? I'm not sure you need to be aware of navigation goals, but perhaps there's something you're thinking of that I'm not thinking about.

8. Update Robot Pose and TF:

I believe that you just need to update TF, though AMCL / SLAM Toolbox do have some pose topic, but I forget specifically what the topic name / type is off of the top of my head. Periodically updating that topic is a 'nice to have' but not a 'need to have'. Its not used internally to Nav2 and I strongly suggest users not actually use it either for anything other than debugging / visualization. But with that said, some users don't have a rudimentary understanding of TF, so there are some that legit use it.

9. Check Robot Pose in Static Map:

Please elaborate. I didn't think this being necessary but maybe there are some nice to have things here? Perhaps once in collision stop listening to commands (e.g. robot is stuck) that don't take the robot out of collision? I'd put this as a 'nice to have' that should be added after the initial design / implementation just to not overcomplicate things up front. But something like this (or other ideas you have?) are welcome and great.

10. Write Deterministic Tests:

I'd start with a tb3_simulation_launch.py analog in the package so you can do testing. But yes, some unit /simulation testing using this work would be absolutely fantastic and necessary to be merged in.

---

The only thing that I see missing from a first look is that you should also listen to the initial pose topic that AMCL has for resetting the pose. That way a user can move the robot and just continue with another experiment. Also would make unit testing much easier 😉

I'd just check AMCL and Gazebo's topics (both pub and sub) just to make sure we didn't miss any other things of that nature.

---

Is this package standalone, or is it integrated into nav2?

In Nav2 😄 -- how about a new package nav2_loopback_sim that'll live here?

How does it adapt if the robot isn't a differential drive system?

If you just take a Twist command and project it, that'll handle all the different types of robot platforms. You can assume the Twist you receive is already describing the motion capabilities possible of the platform.

How can I expect to receive feedback during the project?

Through here, a draft PR once you have something worth showing for bigger questions. I'd recommend joining the Nav2 Slack for any day-to-day nuts and bolts questions, you can PM me there and we can chat. I'm pretty responsive on weekdays.

Besides publishing fake odometry and tf, what other aspects should be simulated or faked?

Odom, TF, initial pose, I think that is it. Basically everything the full simulator does minus the sensor data.

[smitdumore]

Something important to note on the outset is that the default system needs to stay, this is not a replacement for the current system, but an additional capability

So the map->odom part of it just stays static from the initial settings or when someone uses the set initial pose tool to move the robot around. That way you only need to update the odom->base_link transform as the robot moves as part of the simulation mechanics.

I believe that you just need to update TF,

The only thing that I see missing from a first look is that you should also listen to the initial pose topic that AMCL has for resetting the pose. That way a user can move the robot and just continue with another experiment. Also would make unit testing much easier 😉

perfect, Got it

Why not just always listen?

I proposed limiting the acceptance of velocity commands to when the robot receives a navigation goal. This restriction aims to prevent changes to the robot pose caused by velocity commands originating from sources other than nav2.

I'd put this (static map collision checking) as a 'nice to have' that should be added after the initial design / implementation just to not overcomplicate things up front

In Nav2 😄 -- how about a new package nav2_loopback_sim that'll live here?

Sure, I agree

Thank you for the detailed answers. I'll begin building and implementing the package. If I have any minor clarifications needed, I'll reach out to you on Slack. I'm excited!

[SteveMacenski]

This restriction aims to prevent changes to the robot pose caused by velocity commands originating from sources other than nav2

Lets not do that. Even if you were to listen to a navigation goal, how would you determine when to "stop" listening reliably?

Great!

[SteveMacenski]

@smitdumore any updates? I’d really love this in for jazzy and I think you were just working on minor details

[smitdumore]

I have sent a new PR, please review it.
Thank you

[SteveMacenski]

Working on this now, should have something by the end of the week.

[SteveMacenski]

Working on the final launch file details today and should have a PR this afternoon

[SteveMacenski]

#4614