Skip to content

Commit

Permalink
Enable setting of initial state in HW components (backport #1046) (#1064
Browse files Browse the repository at this point in the history 
)

(cherry picked from commit cf4448d)


---------
Co-authored-by: Dr. Denis <denis@stoglrobotics.de>
Co-authored-by: Bence Magyar <bence.magyar.robotics@gmail.com>
Co-authored-by: Christoph Froehlich <christoph.froehlich@ait.ac.at>
  • Loading branch information
@mergify
mergify[bot] authored Jan 22, 2024
1 parent 3b592a6 commit 2d31b1b
Show file tree
Hide file tree
Showing 5 changed files with 194 additions and 101 deletions.
28 changes: 17 additions & 11 deletions controller_manager/doc/userdoc.rst
View file
Original file line number Diff line number Diff line change
Expand Up @@ -39,21 +39,27 @@ The limits will be applied after you log out and in again.
Parameters
-----------

activate_components_on_start (optional; list<string>; default: empty)
Define which hardware components should be activated when controller manager is started.
hardware_components_initial_state
Map of parameters for controlled lifecycle management of hardware components.
The names of the components are defined as attribute of ``<ros2_control>``-tag in ``robot_description``.
All other components will stay ``UNCONFIGURED``.
If this and ``configure_components_on_start`` are empty, all available components will be activated.
If this or ``configure_components_on_start`` are not empty, any component not in either list will be in unconfigured state.
Hardware components found in ``robot_description``, but without explicit state definition will be immediately activated.
Detailed explanation of each parameter is given below.
The full structure of the map is given in the following example:

.. code-block:: yaml
configure_components_on_start (optional; list<string>; default: empty)
Define which hardware components should be configured when controller manager is started.
The names of the components are defined as attribute of ``<ros2_control>``-tag in ``robot_description``.
All other components will stay ``UNCONFIGURED``.
If this and ``activate_components_on_start`` are empty, all available components will be activated.
If this or ``activate_components_on_start`` are not empty, any component not in either list will be in unconfigured state.
hardware_components_initial_state:
unconfigured:
- "arm1"
- "arm2"
inactive:
- "base3"
hardware_components_initial_state.unconfigured (optional; list<string>; default: empty)
Defines which hardware components will be only loaded immediately when controller manager is started.

hardware_components_initial_state.inactive (optional; list<string>; default: empty)
Defines which hardware components will be configured immediately when controller manager is started.

robot_description (mandatory; string)
String with the URDF string as robot description.
Expand Down
98 changes: 76 additions & 22 deletions controller_manager/src/controller_manager.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -367,46 +367,100 @@ 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)
{
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);
}
}
}

Expand Down
132 changes: 98 additions & 34 deletions controller_manager/test/test_hardware_management_srvs.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -35,6 +35,7 @@ using hardware_interface::lifecycle_state_names::ACTIVE;
using hardware_interface::lifecycle_state_names::FINALIZED;
using hardware_interface::lifecycle_state_names::INACTIVE;
using hardware_interface::lifecycle_state_names::UNCONFIGURED;
using hardware_interface::lifecycle_state_names::UNKNOWN;

using ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_CLASS_TYPE;
using ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES;
Expand Down Expand Up @@ -69,9 +70,11 @@ class TestControllerManagerHWManagementSrvs : public TestControllerManagerSrvs
cm_->set_parameter(
rclcpp::Parameter("robot_description", ros2_control_test_assets::minimal_robot_urdf));
cm_->set_parameter(rclcpp::Parameter(
"activate_components_on_start", std::vector<std::string>({TEST_ACTUATOR_HARDWARE_NAME})));
"hardware_components_initial_state.unconfigured",
std::vector<std::string>({TEST_SYSTEM_HARDWARE_NAME})));
cm_->set_parameter(rclcpp::Parameter(
"configure_components_on_start", std::vector<std::string>({TEST_SENSOR_HARDWARE_NAME})));
"hardware_components_initial_state.inactive",
std::vector<std::string>({TEST_SENSOR_HARDWARE_NAME})));

std::string robot_description = "";
cm_->get_parameter("robot_description", robot_description);
Expand Down Expand Up @@ -201,38 +204,6 @@ class TestControllerManagerHWManagementSrvs : public TestControllerManagerSrvs
}
};

class TestControllerManagerHWManagementSrvsWithoutParams
: public TestControllerManagerHWManagementSrvs
{
public:
void SetUp() override
{
executor_ = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
cm_ = std::make_shared<controller_manager::ControllerManager>(
std::make_unique<hardware_interface::ResourceManager>(), executor_, TEST_CM_NAME);
run_updater_ = false;

// TODO(destogl): separate this to init_tests method where parameter can be set for each test
// separately
cm_->set_parameter(
rclcpp::Parameter("robot_description", ros2_control_test_assets::minimal_robot_urdf));

std::string robot_description = "";
cm_->get_parameter("robot_description", robot_description);
if (robot_description.empty())
{
throw std::runtime_error(
"Unable to initialize resource manager, no robot description found.");
}

auto msg = std_msgs::msg::String();
msg.data = robot_description;
cm_->robot_description_callback(msg);

SetUpSrvsCMExecutor();
}
};

TEST_F(TestControllerManagerHWManagementSrvs, list_hardware_components)
{
// Default status after start:
Expand Down Expand Up @@ -390,6 +361,38 @@ TEST_F(TestControllerManagerHWManagementSrvs, selective_activate_deactivate_comp
}));
}

class TestControllerManagerHWManagementSrvsWithoutParams
: public TestControllerManagerHWManagementSrvs
{
public:
void SetUp() override
{
executor_ = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
cm_ = std::make_shared<controller_manager::ControllerManager>(
std::make_unique<hardware_interface::ResourceManager>(), executor_, TEST_CM_NAME);
run_updater_ = false;

// TODO(destogl): separate this to init_tests method where parameter can be set for each test
// separately
cm_->set_parameter(
rclcpp::Parameter("robot_description", ros2_control_test_assets::minimal_robot_urdf));

std::string robot_description = "";
cm_->get_parameter("robot_description", robot_description);
if (robot_description.empty())
{
throw std::runtime_error(
"Unable to initialize resource manager, no robot description found.");
}

auto msg = std_msgs::msg::String();
msg.data = robot_description;
cm_->robot_description_callback(msg);

SetUpSrvsCMExecutor();
}
};

TEST_F(TestControllerManagerHWManagementSrvsWithoutParams, test_default_activation_of_all_hardware)
{
// "configure_components_on_start" and "activate_components_on_start" are not set (empty)
Expand All @@ -413,3 +416,64 @@ TEST_F(TestControllerManagerHWManagementSrvsWithoutParams, test_default_activati
{{false, false, false, false}, {false, false, false, false, false, false, false}}, // system
}));
}

// BEGIN: Deprecated parameters
class TestControllerManagerHWManagementSrvsOldParameters
: public TestControllerManagerHWManagementSrvs
{
public:
void SetUp() override
{
executor_ = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
cm_ = std::make_shared<controller_manager::ControllerManager>(
std::make_unique<hardware_interface::ResourceManager>(), executor_, TEST_CM_NAME);
run_updater_ = false;

cm_->set_parameter(
rclcpp::Parameter("robot_description", ros2_control_test_assets::minimal_robot_urdf));
cm_->set_parameter(rclcpp::Parameter(
"activate_components_on_start", std::vector<std::string>({TEST_ACTUATOR_HARDWARE_NAME})));
cm_->set_parameter(rclcpp::Parameter(
"configure_components_on_start", std::vector<std::string>({TEST_SENSOR_HARDWARE_NAME})));

std::string robot_description = "";
cm_->get_parameter("robot_description", robot_description);
if (robot_description.empty())
{
throw std::runtime_error(
"Unable to initialize resource manager, no robot description found.");
}

auto msg = std_msgs::msg::String();
msg.data = robot_description;
cm_->robot_description_callback(msg);

SetUpSrvsCMExecutor();
}
};

TEST_F(TestControllerManagerHWManagementSrvsOldParameters, list_hardware_components)
{
// Default status after start:
// checks if "configure_components_on_start" and "activate_components_on_start" are correctly read

list_hardware_components_and_check(
// actuator, sensor, system
std::vector<uint8_t>(
{LFC_STATE::PRIMARY_STATE_ACTIVE, LFC_STATE::PRIMARY_STATE_INACTIVE,
LFC_STATE::PRIMARY_STATE_UNCONFIGURED}),
std::vector<std::string>({ACTIVE, INACTIVE, UNCONFIGURED}),
std::vector<std::vector<std::vector<bool>>>({
// is available
{{true, true}, {true, true, true}}, // actuator
{{}, {true}}, // sensor
{{false, false, false, false}, {false, false, false, false, false, false, false}}, // system
}),
std::vector<std::vector<std::vector<bool>>>({
// is claimed
{{false, false}, {false, false, false}}, // actuator
{{}, {false}}, // sensor
{{false, false, false, false}, {false, false, false, false, false, false, false}}, // system
}));
}
// END: Deprecated parameters
15 changes: 3 additions & 12 deletions hardware_interface/include/hardware_interface/resource_manager.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -61,8 +61,7 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager
* \param[in] validate_interfaces boolean argument indicating whether the exported
* interfaces ought to be validated. Defaults to true.
* \param[in] activate_all boolean argument indicating if all resources should be immediately
* activated. Currently used only in tests. In typical applications use parameters
* "autostart_components" and "autoconfigure_components" instead.
* activated. Currently used only in tests.
*/
explicit ResourceManager(
const std::string & urdf, bool validate_interfaces = true, bool activate_all = false);
Expand Down Expand Up @@ -375,7 +374,7 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager
* Reads from all active hardware components.
*
* Part of the real-time critical update loop.
* It is realtime-safe if used hadware interfaces are implemented adequately.
* It is realtime-safe if used hardware interfaces are implemented adequately.
*/
HardwareReadWriteStatus read(const rclcpp::Time & time, const rclcpp::Duration & period);

Expand All @@ -384,18 +383,10 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager
* Writes to all active hardware components.
*
* Part of the real-time critical update loop.
* It is realtime-safe if used hadware interfaces are implemented adequately.
* It is realtime-safe if used hardware interfaces are implemented adequately.
*/
HardwareReadWriteStatus write(const rclcpp::Time & time, const rclcpp::Duration & period);

/// Activates all available hardware components in the system.
/**
* All available hardware components int the ros2_control framework are activated.
* This is used to preserve default behavior from previous versions where all hardware components
* are activated per default.
*/
void activate_all_components();

/// Checks whether a command interface is registered under the given key.
/**
* \param[in] key string identifying the interface to check.
Expand Down
22 changes: 0 additions & 22 deletions hardware_interface/src/resource_manager.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1387,28 +1387,6 @@ void ResourceManager::validate_storage(
throw std::runtime_error(err_msg);
}
}

// END: private methods

// Temporary method to keep old interface and reduce framework changes in the PRs
void ResourceManager::activate_all_components()
{
using lifecycle_msgs::msg::State;
rclcpp_lifecycle::State active_state(
State::PRIMARY_STATE_ACTIVE, hardware_interface::lifecycle_state_names::ACTIVE);

for (auto & component : resource_storage_->actuators_)
{
set_component_state(component.get_name(), active_state);
}
for (auto & component : resource_storage_->sensors_)
{
set_component_state(component.get_name(), active_state);
}
for (auto & component : resource_storage_->systems_)
{
set_component_state(component.get_name(), active_state);
}
}

} // namespace hardware_interface

0 comments on commit 2d31b1b

Please sign in to comment.