Loading robot_description from topic does not work as expected

[pac48]

Describe the bug
There are two ways to load the robot description in the controller manager 1) via a parameter 2) via a topic. If I try to load the description via a topic, I get an error:

 [ros2_control_node-1] [WARN] [1703336900.604603840] [resource_manager]: (hardware 'PandaFakeSystem'): 'panda_joint1/position' command interface not in available list. This should not happen (hint: multiple cleanup calls).

The reason this happens is because ControllerManager::init_resource_manager does not get called before the first call to ControllerManager::read. This is because it takes some time to receive robot description from the topic.

To Reproduce
It should be reproducible in most configurations as long as the robot description is published to a topic instead of passed in as a parameter.

Expected behavior
I expect the controller manager to either wait for the resource manager to be initialized before entering the main loop or maybe wait to load controllers until the resource manager is initialized.

Environment (please complete the following information):

• OS: Ubuntu 22.04
• Version Humble

[mhubii]

Just wondering whether you re-mapped the robot description?

The controller manager loads the description from ~/robot_description, which expands to /controller_manager/robot_description in most cases. The robot state publisher on the other hand publishes to robot_description, which in most cases expands to /robot_description.

You can check this example https://github.com/lbr-stack/lbr_fri_ros2_stack/blob/1e200b5b6d675d1d023be6a1114aa6c27e17a2f9/lbr_ros2_control/lbr_ros2_control/launch_mixin.py#L74

I do also think that this is rather inconvenient but maybe there is a motivation for this choice? It works fine however.

[pac48]

@mhubii Yes, I made sure to add the remapping. I am seeing this issue when running the servo demos in Moveit. I made a simple fix on my fork here. This solves the problem. However, I am not sure if there is a better way to handle this, so I am holding off on opening a PR at the moment.

[dyackzan]

Attempted to implement loading robot_description from a topic instead of a param in moveit studio and never see the robot render.
Found this in the logs:

moveit_studio-drivers-1       | [ros2_control_node-2] [ERROR] [1704303580.172077753] [joint_state_broadcaster]: None of requested interfaces exist. Controller will not run.
moveit_studio-drivers-1       | [ros2_control_node-2] [WARN] [1704303580.172122053] [joint_state_broadcaster]: Error occurred while doing error handling.
moveit_studio-drivers-1       | [ros2_control_node-2] [ERROR] [1704303580.172147027] [controller_manager]: After activation, controller 'joint_state_broadcaster' is in state 'unconfigured' (1), expected 'active' (3).
moveit_studio-drivers-1       | [ros2 run controller_manager spawner --controller-manager-timeout 180 joint_state_broadcaster-5] [INFO] [1704303580.174731442] [spawner_joint_state_broadcaster]: Configured and activated joint_state_broadcaster

And when listing the controllers I see the joint_state_broadcaster controller is unconfigured:

dwyackzan@dwy-workstation:~/user_ws$ ros2 control list_controllers
joint_trajectory_controller[joint_trajectory_controller/JointTrajectoryController] inactive  
io_and_status_controller[ur_controllers/GPIOController] inactive  
robotiq_gripper_controller[position_controllers/GripperActionController] inactive  
force_torque_sensor_broadcaster[force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster] inactive  
robotiq_activation_controller[robotiq_controllers/RobotiqActivationController] inactive  
streaming_controller[position_controllers/JointGroupPositionController] inactive  
joint_trajectory_controller_chained_open_door[joint_trajectory_controller/JointTrajectoryController] inactive  
joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] unconfigured

@destogl could you help us figure out a solution and provide some feedback on Paul's fix

[saikishor]

Hello @dyackzan and @pac48,

Can you test it with the changes in this PR: #1218, and let us know if it helps?

Thank you,

[mhubii]

maybe it is a matter of launching order then? Maybe launch the robot state publisher prior and add an event handler? Sorry just guessing. Not saying this should be necessary :D

Refer https://github.com/lbr-stack/lbr_fri_ros2_stack/blob/96c1994b9a5c8ea6bae542c6d72588c1c92c8cb8/lbr_bringup/launch/real.launch.py#L25

[saikishor]

Hello @pac48 @dyackzan

I've added a small change in the CM services setup in this PR: #1271. This should ideally solve your issue. The changes are simple, it won't expose any controller manager service until the resource manager is set up. I think this way the spawners can wait for the service availability and then trigger the controller switching.

I would appreciate it if you could test it and let us know.

Thank you,

[dyackzan]

@saikishor we're using humble in our moveit studio setup where I saw the errors I mentioned above so I cherry-picked your #1271 commits onto the humble branch of ros2_control and tested on our setup. Looks like the robot_description gets loaded from the topic without issue and I no longer see those joint_state_broadcaster errors pop up in the logs with your changes added so that's all looking good to me

[dyackzan]

I opened a PR for humble changes: #1272

[saikishor]

@dyackzan I'm glad it worked for you :)