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.

Sign up for GitHub

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

Jump to bottom

Remove deprecated parameters and warning about getting robot description from parameter. #1210

Closed
wants to merge 6 commits into from
Closed

Remove deprecated parameters and warning about getting robot description from parameter. #1210

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
6 commits
Select commit Hold shift + click to select a range
8d3e598
Remove deprecated parameters and warning about getting robot descritp…
destogl Dec 11, 2023
58d8791
Remove old parameter from tests.
destogl Dec 11, 2023
2c6c84f
Update controller_manager.cpp
destogl Dec 11, 2023
2f2192d
Recover defintion of 'active_state' variable.
destogl Dec 12, 2023
42f76eb
Remove loading from robot description from parameter.
destogl Dec 12, 2023
35ad40e
Move 'manage_switch' method and 'do_switch' flag to more appropriate …
destogl Dec 12, 2023
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
134 changes: 40 additions & 94 deletions controller_manager/src/controller_manager.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -272,21 +272,7 @@ ControllerManager::ControllerManager(
RCLCPP_WARN(get_logger(), "'update_rate' parameter not set, using default value.");
}

robot_description_ = "";
// TODO(destogl): remove support at the end of 2023
get_parameter("robot_description", robot_description_);
if (robot_description_.empty())
{
subscribe_to_robot_description_topic();
}
else
{
RCLCPP_WARN(
get_logger(),
"[Deprecated] Passing the robot description parameter directly to the control_manager node "
"is deprecated. Use '~/robot_description' topic from 'robot_state_publisher' instead.");
init_resource_manager(robot_description_);
}
subscribe_to_robot_description_topic();

diagnostics_updater_.setHardwareID("ros2_control");
diagnostics_updater_.add(
Expand Down Expand Up @@ -410,56 +396,17 @@ void ControllerManager::init_resource_manager(const std::string & robot_descript
State::PRIMARY_STATE_UNCONFIGURED, hardware_interface::lifecycle_state_names::UNCONFIGURED));

// inactive (configured)
// BEGIN: Keep old functionality on for backwards compatibility (Remove at the end of 2023)
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(),
"Parameter 'configure_components_on_start' is deprecated. "
"Use 'hardware_components_initial_state.inactive' instead, to set component's initial "
"state to 'inactive'. Don't use this parameters in combination with the new "
"'hardware_components_initial_state' 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));
}
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 (Remove at the end of 2023)
std::vector<std::string> activate_components_on_start = std::vector<std::string>({});
get_parameter("activate_components_on_start", activate_components_on_start);
// activate all other components
rclcpp_lifecycle::State active_state(
State::PRIMARY_STATE_ACTIVE, hardware_interface::lifecycle_state_names::ACTIVE);
if (!activate_components_on_start.empty())
for (const auto & [component, state] : components_to_activate)
{
RCLCPP_WARN(
get_logger(),
"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.");
for (const auto & component : activate_components_on_start)
{
resource_manager_->set_component_state(component, active_state);
}
}
// END: Keep old functionality on humble for backwards compatibility (Remove at the end of 2023)
else
{
// activate all other components
for (const auto & [component, state] : components_to_activate)
{
resource_manager_->set_component_state(component, active_state);
}
resource_manager_->set_component_state(component, active_state);
}
}

Expand Down Expand Up @@ -1332,37 +1279,6 @@ controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::add_co
return to.back().c;
}

void ControllerManager::manage_switch()
{
// Ask hardware interfaces to change mode
if (!resource_manager_->perform_command_mode_switch(
activate_command_interface_request_, deactivate_command_interface_request_))
{
RCLCPP_ERROR(get_logger(), "Error while performing mode switch.");
}

std::vector<ControllerSpec> & rt_controller_list =
rt_controllers_wrapper_.update_and_get_used_by_rt_list();

deactivate_controllers(rt_controller_list, deactivate_request_);

switch_chained_mode(to_chained_mode_request_, true);
switch_chained_mode(from_chained_mode_request_, false);

// activate controllers once the switch is fully complete
if (!switch_params_.activate_asap)
{
activate_controllers(rt_controller_list, activate_request_);
}
else
{
// activate controllers as soon as their required joints are done switching
activate_controllers_asap(rt_controller_list, activate_request_);
}

// TODO(destogl): move here "do_switch = false"
}

void ControllerManager::deactivate_controllers(
const std::vector<ControllerSpec> & rt_controller_list,
const std::vector<std::string> controllers_to_deactivate)
Expand Down Expand Up @@ -1572,8 +1488,6 @@ void ControllerManager::activate_controllers(
async_controller_threads_.at(controller_name)->activate();
}
}
// All controllers activated, switching done
switch_params_.do_switch = false;
}

void ControllerManager::activate_controllers_asap(
Expand Down Expand Up @@ -2030,6 +1944,38 @@ void ControllerManager::read(const rclcpp::Time & time, const rclcpp::Duration &
}
}

void ControllerManager::manage_switch()
{
// Ask hardware interfaces to change mode
if (!resource_manager_->perform_command_mode_switch(
activate_command_interface_request_, deactivate_command_interface_request_))
{
RCLCPP_ERROR(get_logger(), "Error while performing mode switch.");
}

std::vector<ControllerSpec> & rt_controller_list =
rt_controllers_wrapper_.update_and_get_used_by_rt_list();

deactivate_controllers(rt_controller_list, deactivate_request_);

switch_chained_mode(to_chained_mode_request_, true);
switch_chained_mode(from_chained_mode_request_, false);

// activate controllers once the switch is fully complete
if (!switch_params_.activate_asap)
{
activate_controllers(rt_controller_list, activate_request_);
}
else
{
// activate controllers as soon as their required joints are done switching
activate_controllers_asap(rt_controller_list, activate_request_);
}

// All controllers switched --> switching done
switch_params_.do_switch = false;
}

controller_interface::return_type ControllerManager::update(
const rclcpp::Time & time, const rclcpp::Duration & period)
{
Expand Down
66 changes: 4 additions & 62 deletions controller_manager/test/test_hardware_management_srvs.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -205,7 +205,8 @@ class TestControllerManagerHWManagementSrvs : public TestControllerManagerSrvs
TEST_F(TestControllerManagerHWManagementSrvs, list_hardware_components)
{
// Default status after start:
// checks if "configure_components_on_start" and "activate_components_on_start" are correctly read
// checks if "hardware_components_initial_state.unconfigured" and
// "hardware_components_initial_state.inactive" are correctly read

list_hardware_components_and_check(
// actuator, sensor, system
Expand Down Expand Up @@ -391,8 +392,8 @@ class TestControllerManagerHWManagementSrvsWithoutParams

TEST_F(TestControllerManagerHWManagementSrvsWithoutParams, test_default_activation_of_all_hardware)
{
// "configure_components_on_start" and "activate_components_on_start" are not set (empty)
// therefore all hardware components are active
// "hardware_components_initial_state.unconfigured" and "hardware_components_initial_state.inactive"
// are not set (empty). Therefore, all hardware components are active.
list_hardware_components_and_check(
// actuator, sensor, system
std::vector<uint8_t>(
Expand All @@ -412,62 +413,3 @@ TEST_F(TestControllerManagerHWManagementSrvsWithoutParams, test_default_activati
{{false, false, false, false}, {false, false, false, false, false, false, false}}, // system
}));
}

// BEGIN: Remove at the end of 2023
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.");
}

cm_->init_resource_manager(robot_description);

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: Remove at the end of 2023
Loading