-
-
Notifications
You must be signed in to change notification settings - Fork 39
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Create ros-humble-controller-manager.patch
- Loading branch information
1 parent
f214fc9
commit 6019f0e
Showing
1 changed file
with
127 additions
and
0 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,127 @@ | ||
| diff --git a/src/controller_manager.cpp b/src/controller_manager.cpp | ||
| index 779ed76a5c..d4f0b8a15b 100644 | ||
| --- a/src/controller_manager.cpp | ||
| +++ b/src/controller_manager.cpp | ||
| @@ -367,30 +367,83 @@ void ControllerManager::init_resource_manager(const std::string & robot_descript | ||
| // TODO(destogl): manage this when there is an error - CM should not die because URDF is wrong... | ||
| resource_manager_->load_urdf(robot_description); | ||
|
|
||
| + // Get all components and if they are not defined in parameters activate them automatically | ||
| + auto components_to_activate = resource_manager_->get_components_status(); | ||
| + | ||
| using lifecycle_msgs::msg::State; | ||
|
|
||
| - std::vector<std::string> configure_components_on_start = std::vector<std::string>({}); | ||
| - if (get_parameter("configure_components_on_start", configure_components_on_start)) | ||
| + auto set_components_to_state = | ||
| + [&](const std::string & parameter_name, rclcpp_lifecycle::State state) | ||
| { | ||
| - RCLCPP_WARN_STREAM( | ||
| - get_logger(), | ||
| - "[Deprecated]: Usage of parameter \"activate_components_on_start\" is deprecated. Use " | ||
| - "hardware_spawner instead."); | ||
| - rclcpp_lifecycle::State inactive_state( | ||
| - State::PRIMARY_STATE_INACTIVE, hardware_interface::lifecycle_state_names::INACTIVE); | ||
| - for (const auto & component : configure_components_on_start) | ||
| + std::vector<std::string> components_to_set = std::vector<std::string>({}); | ||
| + if (get_parameter(parameter_name, components_to_set)) | ||
| { | ||
| - resource_manager_->set_component_state(component, inactive_state); | ||
| + for (const auto & component : components_to_set) | ||
| + { | ||
| + if (component.empty()) | ||
| + { | ||
| + continue; | ||
| + } | ||
| + if (components_to_activate.find(component) == components_to_activate.end()) | ||
| + { | ||
| + RCLCPP_WARN( | ||
| + get_logger(), "Hardware component '%s' is unknown, therefore not set in '%s' state.", | ||
| + component.c_str(), state.label().c_str()); | ||
| + } | ||
| + else | ||
| + { | ||
| + RCLCPP_INFO( | ||
| + get_logger(), "Setting component '%s' to '%s' state.", component.c_str(), | ||
| + state.label().c_str()); | ||
| + resource_manager_->set_component_state(component, state); | ||
| + components_to_activate.erase(component); | ||
| + } | ||
| + } | ||
| } | ||
| + }; | ||
| + | ||
| + // unconfigured (loaded only) | ||
| + set_components_to_state( | ||
| + "hardware_components_initial_state.unconfigured", | ||
| + rclcpp_lifecycle::State( | ||
| + State::PRIMARY_STATE_UNCONFIGURED, hardware_interface::lifecycle_state_names::UNCONFIGURED)); | ||
| + | ||
| + // inactive (configured) | ||
| + // BEGIN: Keep old functionality on for backwards compatibility | ||
| + std::vector<std::string> configure_components_on_start = std::vector<std::string>({}); | ||
| + get_parameter("configure_components_on_start", configure_components_on_start); | ||
| + if (!configure_components_on_start.empty()) | ||
| + { | ||
| + RCLCPP_WARN( | ||
| + get_logger(), | ||
| + "[Deprecated]: Parameter 'configure_components_on_start' is deprecated. " | ||
| + "Use 'hardware_interface_state_after_start.inactive' instead, to set component's initial " | ||
| + "state to 'inactive'. Don't use this parameters in combination with the new " | ||
| + "'hardware_interface_state_after_start' parameter structure."); | ||
| + set_components_to_state( | ||
| + "configure_components_on_start", | ||
| + rclcpp_lifecycle::State( | ||
| + State::PRIMARY_STATE_INACTIVE, hardware_interface::lifecycle_state_names::INACTIVE)); | ||
| + } | ||
| + // END: Keep old functionality on humble backwards compatibility (Remove at the end of 2023) | ||
| + else | ||
| + { | ||
| + set_components_to_state( | ||
| + "hardware_components_initial_state.inactive", | ||
| + rclcpp_lifecycle::State( | ||
| + State::PRIMARY_STATE_INACTIVE, hardware_interface::lifecycle_state_names::INACTIVE)); | ||
| } | ||
|
|
||
| + // BEGIN: Keep old functionality on for backwards compatibility | ||
| std::vector<std::string> activate_components_on_start = std::vector<std::string>({}); | ||
| - if (get_parameter("activate_components_on_start", activate_components_on_start)) | ||
| + get_parameter("activate_components_on_start", activate_components_on_start); | ||
| + if (!activate_components_on_start.empty()) | ||
| { | ||
| - RCLCPP_WARN_STREAM( | ||
| + RCLCPP_WARN( | ||
| get_logger(), | ||
| - "[Deprecated]: Usage of parameter \"activate_components_on_start\" is deprecated. Use " | ||
| - "hardware_spawner instead."); | ||
| + "[Deprecated]: Parameter 'activate_components_on_start' is deprecated. " | ||
| + "Components are activated per default. Don't use this parameters in combination with the new " | ||
| + "'hardware_components_initial_state' parameter structure."); | ||
| rclcpp_lifecycle::State active_state( | ||
| State::PRIMARY_STATE_ACTIVE, hardware_interface::lifecycle_state_names::ACTIVE); | ||
| for (const auto & component : activate_components_on_start) | ||
| @@ -398,15 +451,16 @@ void ControllerManager::init_resource_manager(const std::string & robot_descript | ||
| resource_manager_->set_component_state(component, active_state); | ||
| } | ||
| } | ||
| - // if both parameter are empty or non-existing preserve behavior where all components are | ||
| - // activated per default | ||
| - if (configure_components_on_start.empty() && activate_components_on_start.empty()) | ||
| + // END: Keep old functionality on humble for backwards compatibility (Remove at the end of 2023) | ||
| + else | ||
| { | ||
| - RCLCPP_WARN_STREAM( | ||
| - get_logger(), | ||
| - "[Deprecated]: Automatic activation of all hardware components will not be supported in the " | ||
| - "future anymore. Use hardware_spawner instead."); | ||
| - resource_manager_->activate_all_components(); | ||
| + // activate all other components | ||
| + for (const auto & [component, state] : components_to_activate) | ||
| + { | ||
| + rclcpp_lifecycle::State active_state( | ||
| + State::PRIMARY_STATE_ACTIVE, hardware_interface::lifecycle_state_names::ACTIVE); | ||
| + resource_manager_->set_component_state(component, active_state); | ||
| + } | ||
| } | ||
| } | ||
|
|