Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Enable non-loaded hardware components after start of CM. #1049

Open
wants to merge 18 commits into
base: master
Choose a base branch
from
Open
Changes from 1 commit
Commits
Show all changes
18 commits
Select commit Hold shift + click to select a range
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Prev Previous commit
Next Next commit
Remove not-loaded functionality that needs more changes.
destogl committed Jun 5, 2023

Verified

This commit was signed with the committer’s verified signature.
destogl Dr. Denis
commit 9322373577978fab7a157b314eeaf2310f8dd191
5 changes: 0 additions & 5 deletions controller_manager/doc/userdoc.rst
Original file line number Diff line number Diff line change
@@ -49,17 +49,12 @@ hardware_components_initial_state
.. code-block:: yaml

hardware_components_initial_state:
not_loaded:
- "gripper1"
unconfigured:
- "arm1"
- "arm2"
inactive:
- "base3"

hardware_components_initial_state.not_loaded (optional; list<string>; default: empty)
Defines which hardware components (pluings) should not be loaded activated when controller manager is started.

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

38 changes: 7 additions & 31 deletions controller_manager/src/controller_manager.cpp
Original file line number Diff line number Diff line change
@@ -209,46 +209,22 @@ void ControllerManager::init_resource_manager(const std::string & robot_descript
{
if (components_to_activate.find(component) == components_to_activate.end())
{
if (state.id() == State::PRIMARY_STATE_UNKNOWN)
{
RCLCPP_WARN(
get_logger(),
"Hardware component '%s' is unknown, but defined to not be initial loaded. "
"Please check the parameters of Controller Manager.",
component.c_str());
}
else
{
RCLCPP_WARN(
get_logger(), "Hardware component '%s' is unknown, therefore not set in '%s' state.",
component.c_str(), state.label().c_str());
}
RCLCPP_WARN(
get_logger(), "Hardware component '%s' is unknown, therefore not set in '%s' state.",
component.c_str(), state.label().c_str());
}
else
{
// if state is not set then component should not be loaded
if (state.id() == State::PRIMARY_STATE_UNKNOWN)
{
RCLCPP_INFO(
get_logger(), "Known component '%s' will not be loaded.", component.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);
}
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);
}
}
}
};

// not loaded
set_components_to_state(
"hardware_components_initial_state.not_loaded", rclcpp_lifecycle::State());

// unconfigured (loaded only)
set_components_to_state(
"hardware_components_initial_state.unconfigured",
60 changes: 0 additions & 60 deletions controller_manager/test/test_hardware_management_srvs.cpp
Original file line number Diff line number Diff line change
@@ -359,66 +359,6 @@ TEST_F(TestControllerManagerHWManagementSrvs, selective_activate_deactivate_comp
}));
}

class TestControllerManagerHWManagementSrvsNotLoaded : 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(
"hardware_components_initial_state.not_loaded",
std::vector<std::string>({TEST_SYSTEM_HARDWARE_NAME})));
cm_->set_parameter(rclcpp::Parameter(
"hardware_components_initial_state.inactive",
std::vector<std::string>({TEST_SENSOR_HARDWARE_NAME, TEST_ACTUATOR_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.");
}

cm_->init_resource_manager(robot_description);

SetUpSrvsCMExecutor();
}
};

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

// there is not system loaded therefore test should not break having only two members for checking
// results
list_hardware_components_and_check(
// actuator, sensor, system
std::vector<uint8_t>(
{LFC_STATE::PRIMARY_STATE_INACTIVE, LFC_STATE::PRIMARY_STATE_INACTIVE,
LFC_STATE::PRIMARY_STATE_UNKNOWN}),
std::vector<std::string>({INACTIVE, INACTIVE, UNKNOWN}),
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
}));
}

class TestControllerManagerHWManagementSrvsWithoutParams
: public TestControllerManagerHWManagementSrvs
{