Controller Manager recovery from invalid URDF errors #2775
Conversation
github-actions
bot
requested review from
LukasMacha97,
bijoua29,
bmagyar,
destogl,
fmauch,
homalozoa and
mhubii
|
@VitezGabriela, all pull requests must be targeted towards the |
|
This pull request is in conflict. Could you fix it @VitezGabriela? |
…tion, robot description callback and resource manager constructor
VitezGabriela
force-pushed
the
feature/invalid-urdf-cm-recovery
branch
from
194ed06 to
f5d9c81
Compare
| find_package(ament_cmake_gmock REQUIRED) | ||
| find_package(ros2_control_test_assets REQUIRED) | ||
| find_package(example_interfaces REQUIRED) | ||
| find_package(ament_cmake_gtest REQUIRED) |
There was a problem hiding this comment.
We already have ament_cmake_gmock included a few lines above.
The general recommendation is to use gmock whenever possible instead of gtest even if you don't want to use the more advanced (and indeed, more complex) notations because the prints you get with existing gtest assertions are already much more informative than the gtest ones.
| find_package(ament_cmake_gtest REQUIRED) |
| ${controller_manager_msgs_TARGETS} | ||
| ) | ||
|
|
||
| ament_add_gtest(test_controller_manager_with_resource_manager |
There was a problem hiding this comment.
| ament_add_gtest(test_controller_manager_with_resource_manager | |
| ament_add_gmock(test_controller_manager_with_resource_manager |
| * | ||
| * @return rclcpp::TimerBase::SharedPtr if the timer exists, nullptr otherwise | ||
| */ | ||
| rclcpp::TimerBase::SharedPtr get_robot_description_timer() const { |
There was a problem hiding this comment.
A getter is smart way of doing this but why expose to the outside that this API can be used for this?
I suggest being more direct with this and implementing a has_valid_robot_description function instead where you could use the timer check trick internally.
| <exec_depend>python3-filelock</exec_depend> | ||
|
|
||
| <test_depend>ament_cmake_gmock</test_depend> | ||
| <test_depend>ament_cmake_gtest</test_depend> |
There was a problem hiding this comment.
| <test_depend>ament_cmake_gtest</test_depend> |
|
|
||
| // Get parameters needed for RT "update" loop to work | ||
| if (is_resource_manager_initialized()) | ||
| if (!is_resource_manager_initialized()) |
There was a problem hiding this comment.
a little bit of documentation about the logic happening here would be nice
There was a problem hiding this comment.
@VitezGabriela I think we shouldn't change here the logic. But add this to "else" statement as we might lose handling of limits here.
| "Resource Manager has been successfully initialized. Starting Controller Manager " | ||
| "services..."); |
There was a problem hiding this comment.
| "Resource Manager has been successfully initialized. Starting Controller Manager " | |
| "services..."); | |
| "Resource Manager successfully initialized. Starting Controller Manager services..."); |
I know this is just being picky. I saved one line! :D
| // The RM failed to init AFTER we received the description - a critical error. | ||
| // don't finalize controller manager, instead keep waiting for robot description - fallback state |
There was a problem hiding this comment.
I think this is the documentation I asked for above. This would sit better in the palce where you set up the timer
| } | ||
| } | ||
|
|
||
|
|
| if (load && !load_and_initialize_components(params)) | ||
| { | ||
| RCLCPP_WARN( | ||
| get_logger(), | ||
| "Could not load and initialize hardware. Please check previous output for more details. " | ||
| "After you have corrected your URDF, try to publish robot description again."); | ||
| return; |
There was a problem hiding this comment.
why catch and do an early return here though? wouldn't it be fine to just throw instead? you already handle that case outside in the CM.
In general, the RAII concept is pretty common in C++, meaning that if you have an object, people will expect it to be usable. To this end, it is recommended to only either crash out of a constructor, or return a fully set up object.
3 participants
Goal:
Update the Controller Manager to handle errors when the ResourceManager fails to initialize due to an invalid URDF or faulty hardware plugin.
Behavior:
Observations:
The Controller Manager now no longer crashes on invalid URDFs. Instead, it safely recovers by initializing the resource_manager_ pointer to a minimal state, allowing the controller manager to continue operating and respond to future valid robot descriptions.
Testing:
Unit tests were created with multiple invalid URDF scenarios. All tests passed successfully. Testing can be done by executing:
./build/controller_manager/test_controller_manager_with_resource_manager