Plugin crashes if controller manager throw exception during init

[mateusmenezes95]

Description

I was setting up a simulation environment for my robot, and accidentally set the update_rate as a double instead of an integer, causing the simulation to crash. The log output is provided below.

[ruby $(which gz) sim-1] [INFO 1724887713.540788480] [gz_ros_control]: Loading controller_manager
[ruby $(which gz) sim-1] terminate called after throwing an instance of 'rclcpp::exceptions::InvalidParameterTypeException'
[ruby $(which gz) sim-1]   what():  parameter 'update_rate' has invalid type: expected [integer] got [double]
[ruby $(which gz) sim-1] Stack trace (most recent call last):
[ruby $(which gz) sim-1] #31   Object "/usr/lib/x86_64-linux-gnu/libruby-3.2.so.3.2", at 0x7f2ec38155db, in 
[ruby $(which gz) sim-1] #30   Object "/usr/lib/x86_64-linux-gnu/libruby-3.2.so.3.2", at 0x7f2ec38110fe, in 
[ruby $(which gz) sim-1] #29   Object "/usr/lib/x86_64-linux-gnu/libruby-3.2.so.3.2", at 0x7f2ec380e8ef, in 
[ruby $(which gz) sim-1] #28   Object "/usr/lib/x86_64-linux-gnu/ruby/3.2.0/fiddle.so", at 0x7f2ebe447b13, in 
[ruby $(which gz) sim-1] #27   Object "/usr/lib/x86_64-linux-gnu/libruby-3.2.so.3.2", at 0x7f2ec37d73bb, in rb_nogvl
[ruby $(which gz) sim-1] #26   Object "/usr/lib/x86_64-linux-gnu/ruby/3.2.0/fiddle.so", at 0x7f2ebe44743b, in 
[ruby $(which gz) sim-1] #25   Object "/usr/lib/x86_64-linux-gnu/libffi.so.8", at 0x7f2ebe43b0bd, in ffi_call
[ruby $(which gz) sim-1] #24   Object "/usr/lib/x86_64-linux-gnu/libffi.so.8", at 0x7f2ebe4383ee, in 
[ruby $(which gz) sim-1] #23   Object "/usr/lib/x86_64-linux-gnu/libffi.so.8", at 0x7f2ebe43bb15, in 
[ruby $(which gz) sim-1] #22   Object "/usr/lib/x86_64-linux-gnu/libgz-sim8-gz.so.8.6.0", at 0x7f2ebd91a4af, in runServer
[ruby $(which gz) sim-1] #21   Object "/usr/lib/x86_64-linux-gnu/libgz-sim8.so.8", at 0x7f2ebd618d17, in 
[ruby $(which gz) sim-1] #20   Object "/usr/lib/x86_64-linux-gnu/libgz-sim8.so.8", at 0x7f2ebd626396, in gz::sim::v8::SimulationRunner::Run(unsigned long)
[ruby $(which gz) sim-1] #19   Object "/usr/lib/x86_64-linux-gnu/libgz-sim8.so.8", at 0x7f2ebd625a11, in gz::sim::v8::SimulationRunner::Step(gz::sim::v8::UpdateInfo const&)
[ruby $(which gz) sim-1] #18   Object "/usr/lib/x86_64-linux-gnu/libgz-sim8.so.8", at 0x7f2ebd6220e1, in gz::sim::v8::SimulationRunner::UpdateSystems()
[ruby $(which gz) sim-1] #17   Object "/usr/lib/x86_64-linux-gnu/gz-sim-8/plugins/libgz-sim-user-commands-system.so", at 0x7f2e8b95dca8, in gz::sim::v8::systems::UserCommands::PreUpdate(gz::sim::v8::UpdateInfo const&, gz::sim::v8::EntityComponentManager&)
[ruby $(which gz) sim-1] #16   Object "/usr/lib/x86_64-linux-gnu/gz-sim-8/plugins/libgz-sim-user-commands-system.so", at 0x7f2e8b962bc0, in gz::sim::v8::systems::CreateCommand::Execute()
[ruby $(which gz) sim-1] #15   Object "/usr/lib/x86_64-linux-gnu/libgz-sim8.so.8", at 0x7f2ebd5f7988, in gz::sim::v8::SdfEntityCreator::CreateEntities(sdf::v14::Model const*)
[ruby $(which gz) sim-1] #14   Object "/usr/lib/x86_64-linux-gnu/libgz-sim8.so.8", at 0x7f2ebd5cc4fe, in 
[ruby $(which gz) sim-1] #13   Object "/usr/lib/x86_64-linux-gnu/libgz-sim8.so.8", at 0x7f2ebd62911c, in gz::sim::v8::SimulationRunner::LoadPlugins(unsigned long, std::vector<sdf::v14::Plugin, std::allocator<sdf::v14::Plugin> > const&)
[ruby $(which gz) sim-1] #12   Object "/usr/lib/x86_64-linux-gnu/libgz-sim8.so.8", at 0x7f2ebd634ad2, in gz::sim::v8::SystemManager::LoadPlugin(unsigned long, sdf::v14::Plugin const&)
[ruby $(which gz) sim-1] #11   Object "/usr/lib/x86_64-linux-gnu/libgz-sim8.so.8", at 0x7f2ebd634540, in gz::sim::v8::SystemManager::AddSystemImpl(gz::sim::v8::SystemInternal, std::shared_ptr<sdf::v14::Element const>)
[ruby $(which gz) sim-1] #10   Object "/opt/ros/jazzy/lib/libgz_ros2_control-system.so", at 0x7f2e8a7c786e, in gz_ros2_control::GazeboSimROS2ControlPlugin::Configure(unsigned long const&, std::shared_ptr<sdf::v14::Element const> const&, gz::sim::v8::EntityComponentManager&, gz::sim::v8::EventManager&)
[ruby $(which gz) sim-1] #9    Object "/opt/ros/jazzy/lib/libcontroller_manager.so", at 0x7f2e8a6dfd52, in controller_manager::ControllerManager::ControllerManager(std::unique_ptr<hardware_interface::ResourceManager, std::default_delete<hardware_interface::ResourceManager> >, std::shared_ptr<rclcpp::Executor>, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, rclcpp::NodeOptions const&)
[ruby $(which gz) sim-1] #8    Object "/opt/ros/jazzy/lib/libcontroller_manager.so", at 0x7f2e8a6dc64a, in controller_manager::ControllerManager::init_controller_manager()
[ruby $(which gz) sim-1] #7    Object "/opt/ros/jazzy/lib/libcontroller_manager.so", at 0x7f2e8a6c7e2c, in 
[ruby $(which gz) sim-1] #6    Object "/usr/lib/x86_64-linux-gnu/libstdc++.so.6", at 0x7f2ebe266127, in __cxa_throw
[ruby $(which gz) sim-1] #5    Object "/usr/lib/x86_64-linux-gnu/libstdc++.so.6", at 0x7f2ebe250a48, in std::terminate()
[ruby $(which gz) sim-1] #4    Object "/usr/lib/x86_64-linux-gnu/libstdc++.so.6", at 0x7f2ebe265e9b, in 
[ruby $(which gz) sim-1] #3    Object "/usr/lib/x86_64-linux-gnu/libstdc++.so.6", at 0x7f2ebe250ffd, in 
[ruby $(which gz) sim-1] #2    Object "/usr/lib/x86_64-linux-gnu/libc.so.6", at 0x7f2ec32128fe, in abort
[ruby $(which gz) sim-1] #1    Object "/usr/lib/x86_64-linux-gnu/libc.so.6", at 0x7f2ec322f26d, in gsignal
[ruby $(which gz) sim-1] #0    Object "/usr/lib/x86_64-linux-gnu/libc.so.6", at 0x7f2ec3288b1c, in pthread_kill
[ruby $(which gz) sim-1] Aborted (Signal sent by tkill() 33053 1001)
[INFO] [ruby $(which gz) sim-1]: process has finished cleanly [pid 33013]
[INFO] [launch]: process[ruby $(which gz) sim-1] was required: shutting down launched system
[INFO] [spawner-4]: sending signal 'SIGINT' to process[spawner-4]
[INFO] [robot_state_publisher-2]: sending signal 'SIGINT' to process[robot_state_publisher-2]
[robot_state_publisher-2] [INFO 1724887713.813616208] [rclcpp]: signal_handler(signum=2)
[spawner-4] Traceback (most recent call last):
[spawner-4]   File "/opt/ros/jazzy/lib/controller_manager/spawner", line 33, in <module>
[spawner-4]     sys.exit(load_entry_point('controller-manager==4.15.0', 'console_scripts', 'spawner')())
[spawner-4]              ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
[spawner-4]   File "/opt/ros/jazzy/lib/python3.12/site-packages/controller_manager/spawner.py", line 261, in main
[spawner-4]     if not wait_for_controller_manager(
[spawner-4]            ^^^^^^^^^^^^^^^^^^^^^^^^^^^^
[spawner-4]   File "/opt/ros/jazzy/lib/python3.12/site-packages/controller_manager/spawner.py", line 123, in wait_for_controller_manager
[spawner-4]     return wait_for_value_or(
[spawner-4]            ^^^^^^^^^^^^^^^^^^
[spawner-4]   File "/opt/ros/jazzy/lib/python3.12/site-packages/controller_manager/spawner.py", line 72, in wait_for_value_or
[spawner-4]     time.sleep(0.2)
[spawner-4] KeyboardInterrupt
[ERROR] [spawner-4]: process has died [pid 33016, exit code -2, cmd '/opt/ros/jazzy/lib/controller_manager/spawner standard_effort_controllers --controller-manager controller_manager --load-only --controller-manager-timeout 60 --ros-args'].
[INFO] [robot_state_publisher-2]: process has finished cleanly [pid 33014]

It seems that the plugin is not handling the exceptions that the controller manager may throw during initialization, as in the invalid parameter example above. Thus, a possible solution is to put try/catch during the controller manager initialization. If you agree, I can submit a PR with the solution.

[christophfroehlich]

What happens if the plugin itself can't be initialized? Will gz crash anyways? If no, a PR is welcome ;)
The second traceback from the spawner is a different story, I don't know if that can be stopped more gracefully.

[mateusmenezes95]

@christophfroehlich, I didn't understand it very well. Could you explain more about the questions?

[christophfroehlich]

Sorry, I'll try once again ;)
The error may have happened in GazeboSimROS2ControlPlugin::Configure here

gz_ros2_control/gz_ros2_control/src/gz_ros2_control_plugin.cpp

Lines 441 to 446 in dc6375d

	this->dataPtr->controller_manager_.reset(
	new controller_manager::ControllerManager(
	std::move(resource_manager_),
	this->dataPtr->executor_,
	controllerManagerNodeName,
	this->dataPtr->node_->get_namespace(), options));

A try catch here is fine, but what will be the consequence? The configure method does not have a return type to tell gazebo that it could not be loaded -> you only can rethrow the error and let gazebo crash anyways.