Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

[IMUSensorBroadcaster] Create ParamListener and get parameters on configure #795

Closed
wants to merge 1 commit into from

Conversation

YueErro
Copy link

@YueErro YueErro commented Oct 10, 2023

Hi,

It seems there is a very similar issue as in #698. The IMUSensorBroadcaster stopped working for me.
After looking for the issue, I've found that there is a validation of the parameter sensor_name (added here), that is failing always because the parameters have not been loaded into the node yet and then, the parameter is empty at that point.

I have the parameter declared in the yaml file that I'm loading in the node but the error I'm having is the following:

[spawner-4] [INFO] [1696928866.880159937] [spawner_imu_sensor_broadcaster]: Set controller type to "imu_sensor_broadcaster/IMUSensorBroadcaster" for imu_sensor_broadcaster
[gzserver-1] [INFO] [1696928866.881161921] [controller_manager]: Loading controller 'imu_sensor_broadcaster'
[gzserver-1] [ERROR] [1696928866.897606928] [imu_sensor_broadcaster]: Exception thrown during init stage with message: Invalid value set during initialization for parameter 'sensor_name': Parameter 'sensor_name' cannot be empty 
[gzserver-1] 
[gzserver-1] [ERROR] [1696928866.897674246] [controller_manager]: Could not initialize the controller named 'imu_sensor_broadcaster'
[spawner-4] [FATAL] [1696928866.904203561] [spawner_imu_sensor_broadcaster]: Failed loading controller imu_sensor_broadcaster
[ERROR] [spawner-4]: process has died [pid 61163, exit code 1, cmd '/opt/pal/alum/lib/controller_manager/spawner imu_sensor_broadcaster --controller-manager controller_manager --controller-type imu_sensor_broadcaster/IMUSensorBroadcaster --param-file /home/yueerro/git/my_dockers/shared/alum/staging/pmb3_ws/install/pmb3_controller_configuration/share/pmb3_controller_configuration/config/imu_sensor_broadcaster.yaml --ros-args'].

I guess the same happens with the validation of the parameter frame_id (added here).

By creating the ParamListener and reading the parameters on configure instead of on init is working for me:

[spawner-4] [INFO] [1696930457.282406584] [spawner_imu_sensor_broadcaster]: Set controller type to "imu_sensor_broadcaster/IMUSensorBroadcaster" for imu_sensor_broadcaster
[gzserver-1] [INFO] [1696930457.283297464] [controller_manager]: Loading controller 'imu_sensor_broadcaster'
[gzserver-1] [INFO] [1696930457.295975680] [controller_manager]: Setting use_sim_time=True for imu_sensor_broadcaster to match controller manager (see ros2_control#325 for details)
[spawner-4] [INFO] [1696930457.302856674] [spawner_imu_sensor_broadcaster]: Loaded imu_sensor_broadcaster
[spawner-4] [INFO] [1696930457.305764498] [spawner_imu_sensor_broadcaster]: Loaded parameters file "/home/yueerro/git/my_dockers/shared/alum/staging/pmb3_ws/install/pmb3_controller_configuration/share/pmb3_controller_configuration/config/imu_sensor_broadcaster.yaml" for imu_sensor_broadcaster
[spawner-4] [INFO] [1696930457.306153768] [spawner_imu_sensor_broadcaster]: Loaded /home/yueerro/git/my_dockers/shared/alum/staging/pmb3_ws/install/pmb3_controller_configuration/share/pmb3_controller_configuration/config/imu_sensor_broadcaster.yaml into imu_sensor_broadcaster
[gzserver-1] [INFO] [1696930457.306952999] [controller_manager]: Configuring controller 'imu_sensor_broadcaster'
[spawner-4] [INFO] [1696930457.333095661] [spawner_imu_sensor_broadcaster]: Configured and activated imu_sensor_broadcaster
[spawner-4] Set parameter sensor_name successful
[spawner-4] Set parameter frame_id successful
[spawner-4] Set parameter publish_rate successful
[INFO] [spawner-4]: process has finished cleanly [pid 66228]

I would like to have also a backport for humble.

Thank you in advance!

Copy link
Contributor

@christophfroehlich christophfroehlich left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Thanks for pointing this out, it seems that I introduced the issue.

But I'm not sure why this fails, having a look at JTC

controller_interface::CallbackReturn JointTrajectoryController::on_init()
{
try
{
// Create the parameter listener and get the parameters
param_listener_ = std::make_shared<ParamListener>(get_node());
params_ = param_listener_->get_params();

the param_listener is created and get_params() called also in the on_init method.

@bmagyar @destogl where is the correct place doing this? Is this documented somewhere?

3. Implement the ``on_init`` method. The first line usually calls the parent ``on_init`` method.
Here is the best place to initialize the variables, reserve memory, and most importantly, declare node parameters used by the controller. If everything works fine return ``controller_interface::return_type::OK`` or ``controller_interface::return_type::ERROR`` otherwise.
4. Write the ``on_configure`` method. Parameters are usually read here, and everything is prepared so that the controller can be started.

vs
https://github.com/ros-controls/ros2_control/blob/e07736011bf28895596cf10dcb7cbcdb40d86daa/hardware_interface/doc/writing_new_hardware_component.rst?plain=1#L48-L52

@bmagyar
Copy link
Member

bmagyar commented Oct 20, 2023

I think you are onto something!

Even recent code such as the toy controller I created for the roscon workshop does the same in the on_init() function. That being said, those parameters had a default value and no validator.

I think we need to take a closer look at generate_parameter_library and find where validation happens.

On first read I don't see anything wrong with moving those two lines into on_configure() and it'd probably be good to future-proof all our controllers if this is the right solution.

@christophfroehlich
Copy link
Contributor

Maybe @tylerjw can help to answer how to use generate_parameter_library properly here?

@tylerjw
Copy link
Contributor

tylerjw commented Oct 23, 2023

I don't think there is any harm in moving this into on_configure. The constructor for the parameter listener sets up callbacks to ROS that will validate when a parameter is set. This includes if parameters have been set during launch it will then call the validation function and might fail. The reading of the parameters from ros happens when you call the get function.

This behavior of validating in a callback might be a bit confusing for some users but it is the interface we have from ROS.

@christophfroehlich
Copy link
Contributor

thanks @tylerjw, best-practice would be then doing it in on_configure of all lifecycle nodes?

@tylerjw
Copy link
Contributor

tylerjw commented Oct 23, 2023

Maybe, I'm not confident on the best way to use a lifecycle node.

@saikishor
Copy link
Member

Hello!

As far as I have checked, It seems like we can declare params in the init method, however, we can only retrieve them in the configure phase as we tend to register the callbacks of the node post on_init method.

https://github.com/ros-controls/ros2_control/blob/bd6911d663abb687dc956bf19a104d03648f8985/controller_interface/src/controller_interface_base.cpp#L34-L55

I think for the same reason, we get the parameters of the update_rate and is_async in the on_configure method
https://github.com/ros-controls/ros2_control/blob/bd6911d663abb687dc956bf19a104d03648f8985/controller_interface/src/controller_interface_base.cpp#L85-L89

@christophfroehlich
Copy link
Contributor

christophfroehlich commented Oct 24, 2023

I discussed this with @saikishor

  • In general, parameters should be declared in on_init to be able to set parameters (e.g. via CLI) before the controller is configured. This is also the issue with the failing tests of this PR -> they are not declared and, e.g.,
    imu_broadcaster_->get_node()->set_parameter({"sensor_name", sensor_name_}); fails,.
  • If generate_parameter_library is used, this is done by the constructor of the ParamListener by calling its declare_params method.

But I still don't know why get_params is failing for @YueErro here. Could you check if the creation of ParamListener in the on_init() but leaving get_params() in the on_configure() works for you? Then the CI should be happy here too.

@saikishor
Copy link
Member

saikishor commented Oct 30, 2023

Hello @YueErro!

Can you please check if the fix in the PR : ros-controls/ros2_control#1145, address your issue directly with the master or humble branches?

@bmagyar
Copy link
Member

bmagyar commented Oct 30, 2023

Fyi I'm happy to merge this PR if you get a green CI

@destogl
Copy link
Member

destogl commented Nov 6, 2023

@tylerjw and @christophfroehlich will your fix of those issues in Generate Parameter Library be beckported to humble? If yes, then there is not need for fixing this.

@tylerjw
Copy link
Contributor

tylerjw commented Nov 6, 2023

Yes, I will backport to humble.

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

Successfully merging this pull request may close these issues.

7 participants