[Servo] Smoothing plugin does not update with planning group.

[ibrahiminfinite]

Description

Current behaviour

The new Servo implementation supports changing the planning group, however the smoothing plugin implementation is not yet updated to support this.
The plugins initialize with a num_joints (ref) parameter, but this this can now change after the initialization of the plugin, depending on the planning group.

Expected behaviour

Smoothing plugins should have the ability to re-initialize the filter size when the planning group changes.

Your environment

• ROS Distro: [Rolling]
• OS Version: Ubuntu 22.04
• Source build
• Branch: main

NOTE : This issue is currently being worked on.

[ibrahiminfinite]

Approach 1: Add a check for planning group change here and re-initialize the plugin.

Currently the planning group is set using a parameter, and this parameter is used to fetch the planning group on every iteration of Servo. This ensures that the planning group can be changed easily using the intended functionality of the ROS parameter.
But since we want to re-initialize the smoothing plugin when planning group changes, the loop time would take a significant hit for the single iteration when the re-initialization happens. This is the easiest way to do it if the one time hit to loop time is not a big problem.

Approach 2: Add a separate service to update the planning group

This requires adding some easy but considerable amount of changes to Servo.
The plugin would be re-initialized from the service.
However functionality of the ROS parameter becomes redundant here, this parameter will also need to be updated from the service.

[ibrahiminfinite]

@AndyZe ,@sea-bass, @henningkayser Which of the approaches would be preferable ?

[AndyZe]

I would make the planning group param static. It doesn't seem like a good idea to update which joints are being controlled in a realtime control scenario, anyway.

[sea-bass]

We already are working with users who want to switch the planning group (arm + linear rail vs. arm only), so I do not think we should have a static planning group.

Between the proposed approaches, I'd choose Approach 1 because it's easier on the user (no need to explicitly create a service, but rather just use the one already exposed by the parameter value itself).

[ibrahiminfinite]

I'd choose Approach 1 because it's easier on the user

Yeah, it is easier.
But there is the issue of time overhead in the iteration right after the parameter update from re-initializing the smoothing plugin This will happen even if the parameter was changed with Servo paused, since the plugin can only be re-initialized when checking for parameter updates which only happens when the getNextJointState API is called. However this probably wont be an issue unless Servo is running at a high rate.

Approach 2 (The one implemented in the linked PR ) is much more safter and clearer, but at the cost of some extra infrastructure.

[ibrahiminfinite]

parameter updates which only happens when the getNextJointState API is called.

Aha !
There is third option which is cleaner and requires much less additional infrastructure.

We make the UpdateParams() public and call it in servo loop in ServoNode even when Servo is paused.
So the re-initialization happens with Servo paused and there is no overhead when resuming the control.

[AndyZe]

So it can only update when Servo is paused? That seems acceptable from a safety standpoint, too.

[sea-bass]

This seems good to me as well!

[github-actions]

This issue is being labeled as stale because it has been open 45 days with no activity. It will be automatically closed after another 45 days without follow-ups.

[ibrahiminfinite]

The required use case is currently satisfied by #2396.
Closing this issue.