Additional features for the Python3 Simple Navigator Commander

[SteveMacenski]

• calling the controller
• Add default arguments for full action API to functions (that don't necessarily need to be provided, but if they are, use them. example: behavior tree xmls for nav requests)
• Allowing the wait for navigation to bring up to have a configurable set of nodes to wait for (and default to the ones currently present)
• Call common recoveries
• Constructor to take in optional namespaces/topics to use for creating the clients and subscribers
• setting parameters

[jacobwaller]

To add on to

Allowing the wait for navigation to bring up to have a configurable set of nodes to wait

as far as I can see, simple navigator does not work if SLAM-Toolbox is doing the localization because of its reliance on the nav2_amcl node

[SteveMacenski]

Yeah, that's part of what that should do, have options for the nodes to wait on and not assume which are in place if the user can provide better estimates.

[oldtopos]

Do not understand "calling the controller" and "Call common recoveries"

wait for navigation refers to function "waitUntilNav2Active" ?

ComputePathToPose and ComputePathThroughPoses take planner_id and use_start, should these be given overrides similar to bt override ?

[SteveMacenski]

Do not understand "calling the controller" and "Call common recoveries"

Its the analog to getPlan by calling the planner server, but for local trajectory planning via the controller server. See the FollowPath.action in nav2_msgs: https://github.com/ros-planning/navigation2/tree/main/nav2_msgs/action

Call common recoveries is the same, have wait() spin() backup() APIs calling their appropriate action servers (see the nav2_msgs link above).

With planning/control/recoveries, a Python application can have essentially full control over Nav2 without the Behavior Tree to create a simple application if they like or call servers to compute particular values for debug / application needs.

ComputePathToPose and ComputePathThroughPoses take planner_id and use_start, should these be given overrides similar to bt override ?

They can, yes, I added a new bullet around having the Python3 APIs be able to specify the full action request API like algorithm IDs, but not have them be required if the action doesn't require them.

wait for navigation refers to function "waitUntilNav2Active" ?

Yeah, so by default we wait for BT Nav and AMCL. Instead, we should have those be the default arguments, but allow them to be overrided to use some others instead in case they have other servers. If not using AMCL for SLAM, then the wait for initial pose bit doesn't make sense either, that's a special case for localization (e.g. non-mapping)

[tonynajjar]

I'm working on adding wait, spin and backup. Spin and backup make sense to me but I'm not sure how wait is expected to work in a python script. As far as I understand in a BT, the wait node would stall the BT. In a python script, it would behave like a time.sleep()?

[SteveMacenski]

Essentially, yes. I agree it might not make too much sense actually to include in its own right. You could simply sleep in the python script. I think its OK to skip unless you feel it would be good to add to be complete

[SteveMacenski]

For setting parameters, I adapted this class to handle it. This can be massaged into the simple commander

class SetExternalParam(Node):

    def __init__(self, server_name):
        super().__init__('nav2_simple_commander_parameter_setter')

        self.cli = self.create_client(SetParameters, '/' + server_name + '/set_parameters')
        while not self.cli.wait_for_service(timeout_sec=1.0):
            self.get_logger().info('service not available, waiting again...')
        self.req = SetParameters.Request()

    def send_request(self, param_name, param_value):
        if isinstance(param_value, float):
            val = ParameterValue(double_value=param_value, type=ParameterType.PARAMETER_DOUBLE)
        elif isinstance(param_value, int):
            val = ParameterValue(integer_value=param_value, type=ParameterType.PARAMETER_INTEGER)
        elif isinstance(param_value, str):
            val = ParameterValue(string_value=param_value, type=ParameterType.PARAMETER_STRING)
        elif isinstance(param_value, bool):
            val = ParameterValue(bool_value=param_value, type=ParameterType.PARAMETER_BOOL)
        self.req.parameters = [Parameter(name=param_name, value=val)]
        self.future = self.cli.call_async(self.req)
        while rclpy.ok():
            rclpy.spin_once(self)
            if self.future.done():
                try:
                    response = self.future.result()
                    if response[0].successful:
                        return True
                except Exception as e:
                    pass
                return False

[tonynajjar]

Did you add the SetExternalParam class somewhere or that's just a draft? Can't find it

[SteveMacenski]

No, that's just a draft I put together while working on another project and needed the capability in short order.

[VineetTambe]

Has the development on the following issue item been completed?

• Constructor to take in optional namespaces/topics to use for creating the clients and subscribers

I am working on multi-robot stuff; having the nav2 simple commander API use namespaces would be really helpful.
If it is yet to be done, I was planning to make the changes and raise a pull request.

[SteveMacenski]

I do not believe it is done (yet)

[VineetTambe]

Cool! I'll raise a PR in a few days for this then!

[VineetTambe]

Update: I have the initial name space thing working on a single robot with namespace argument set as "" (This is for sanity check to ensure the normal functionality of simple commander is not affected).

But I am having issues testing the multirobot functionality of nav2.

I am checking out to the humble branch and then running:
ros2 launch nav2_bringup multi_tb3_simulation_launch.py use_sim_time:=True

Then in another terminal I am running the following test-script. The DDS that I am running is cycloneDDS.

However, when I run the above script I get data on the /robot1/initialpose topic but then the scipt gets stuck at this line with the error: Result of get_state: unconfigured

Can someone help me figure out where I am going wrong?? Am I missing a step?