Feature request: expose current robot pose in Simple Commander API

[Arseni1919]

🚀 Feature description

It would be awesome to add a current_pose method or property to Simple Commander API that returns the robot’s current (x, y, yaw) pose from /amcl_pose or localization source. Right now, you have to subscribe or parse topic output manually, which is a bit annoying.

Having a simple function like nav.current_pose would make scripting, debugging, and Jupyter notebooks way easier and cleaner! 🎉

💡 Implementation considerations

A straightforward way is to grab one message from /amcl_pose using something like:

import subprocess, yaml, math

def get_amcl_pose():
    output = subprocess.check_output(["ros2", "topic", "echo", "--once", "/amcl_pose"], text=True)
    msg = list(yaml.safe_load_all(output))[0]
    p = msg['pose']['pose']['position']
    o = msg['pose']['pose']['orientation']
    yaw = math.atan2(2*(o['w']*o['z'] + o['x']*o['y']), 1 - 2*(o['y']**2 + o['z']**2))
    return p['x'], p['y'], yaw

Pros:

Super useful for quick checks and interactive use.

Easy to add, low overhead.

Could optionally support config for topic or TF fallback.

Thanks for considering! 🙌
And thanks chatgpt for helping me writing this request with my broken English.

[SteveMacenski]

I think we should always use TF to check, not amcl_pose. That's there IMO as a debugging and state persistence topic and shouldn't be used as anything more than that. TF is for the time synchronized, accurate positioning data.

It should be easy enough to add a TF call for map->base_link (or have them set the global and local frames) so that users can check on the frame - sure!

[Arseni1919]

Oh, great! I will also change my function to look at the TF topic, as you suggested. Waiting for the update. Thank you in advance! 🙏

[SteveMacenski]

Sorry, what update are you waiting for? Not sure I understand that one.

Yes, I think using TF is better so that you can get the accurate pose information, like we do in the C++ code across the board. This should be a really easy contribution from Python TF tutorials

[Arseni1919]

Let me clarify. I was thinking it would be helpful to add a small utility method to the Nav2 Simple Commander API:

from nav2_simple_commander.robot_navigator import BasicNavigator

navigator = BasicNavigator()
curr_pos = navigator.get_current_pose()  # <- this would be the new method

This addition would be extremely useful for our team. I try to work in Python as much as possible, and the Simple Commander API has been a great tool—it keeps things clean and accessible.

[SteveMacenski]

Exactly, this would be an easy contribution for you to submit a PR for to add it to the simple commander :-)

[Arseni1919]

Got you! I will try my best so:)