Skip to content

Commit

Permalink
Merge branch 'master' into add-simple-joint-limiter
Browse files Browse the repository at this point in the history
  • Loading branch information
@destogl
destogl authored Jun 27, 2023
2 parents d6db2a1 + cc1667b commit 1e393f3
Show file tree
Hide file tree
Showing 31 changed files with 346 additions and 114 deletions.
9 changes: 9 additions & 0 deletions controller_interface/CHANGELOG.rst
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,6 +2,15 @@
Changelog for package controller_interface
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^

3.15.0 (2023-06-23)
-------------------

3.14.0 (2023-06-14)
-------------------
* Add -Wconversion flag to protect future developments (`#1053 <https://github.com/ros-controls/ros2_control/issues/1053>`_)
* enable ReflowComments to also use ColumnLimit on comments (`#1037 <https://github.com/ros-controls/ros2_control/issues/1037>`_)
* Contributors: Sai Kishor Kothakota, gwalck

3.13.0 (2023-05-18)
-------------------

Expand Down
2 changes: 1 addition & 1 deletion controller_interface/package.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,7 +2,7 @@
<?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="2">
<name>controller_interface</name>
<version>3.13.0</version>
<version>3.15.0</version>
<description>Description of controller_interface</description>
<maintainer email="bence.magyar.robotics@gmail.com">Bence Magyar</maintainer>
<maintainer email="denis@stoglrobotics.de">Denis Štogl</maintainer>
Expand Down
15 changes: 15 additions & 0 deletions controller_manager/CHANGELOG.rst
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,6 +2,21 @@
Changelog for package controller_manager
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^

3.15.0 (2023-06-23)
-------------------
* Enable setting of initial state in HW compoments (`#1046 <https://github.com/ros-controls/ros2_control/issues/1046>`_)
* [CM] Improve output when using robot description topic and give output about correct topic even remapped. (`#1059 <https://github.com/ros-controls/ros2_control/issues/1059>`_)
* Contributors: Dr. Denis

3.14.0 (2023-06-14)
-------------------
* Add -Wconversion flag to protect future developments (`#1053 <https://github.com/ros-controls/ros2_control/issues/1053>`_)
* [CM] Use `robot_description` topic instead of parameter and don't crash on empty URDF 🦿 (`#940 <https://github.com/ros-controls/ros2_control/issues/940>`_)
* enable ReflowComments to also use ColumnLimit on comments (`#1037 <https://github.com/ros-controls/ros2_control/issues/1037>`_)
* Docs: Use branch name substitution for all links (`#1031 <https://github.com/ros-controls/ros2_control/issues/1031>`_)
* Add text to assertions references (`#1023 <https://github.com/ros-controls/ros2_control/issues/1023>`_)
* Contributors: Christoph Fröhlich, Felix Exner (fexner), Manuel Muth, Sai Kishor Kothakota, gwalck

3.13.0 (2023-05-18)
-------------------
* Add class for thread management of async hw interfaces (`#981 <https://github.com/ros-controls/ros2_control/issues/981>`_)
Expand Down
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
2 changes: 1 addition & 1 deletion controller_manager/package.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,7 +2,7 @@
<?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="2">
<name>controller_manager</name>
<version>3.13.0</version>
<version>3.15.0</version>
<description>Description of controller_manager</description>
<maintainer email="bence.magyar.robotics@gmail.com">Bence Magyar</maintainer>
<maintainer email="denis@stoglrobotics.de">Denis Štogl</maintainer>
Expand Down
105 changes: 81 additions & 24 deletions controller_manager/src/controller_manager.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -203,16 +203,17 @@ void ControllerManager::subscribe_to_robot_description_topic()
{
// set QoS to transient local to get messages that have already been published
// (if robot state publisher starts before controller manager)
RCLCPP_INFO(
get_logger(), "Subscribing to '~/robot_description' topic for robot description file.");
robot_description_subscription_ = create_subscription<std_msgs::msg::String>(
"~/robot_description", rclcpp::QoS(1).transient_local(),
std::bind(&ControllerManager::robot_description_callback, this, std::placeholders::_1));
RCLCPP_INFO(
get_logger(), "Subscribing to '%s' topic for robot description.",
robot_description_subscription_->get_topic_name());
}

void ControllerManager::robot_description_callback(const std_msgs::msg::String & robot_description)
{
RCLCPP_INFO(get_logger(), "Received robot description file.");
RCLCPP_INFO(get_logger(), "Received robot description from topic.");
RCLCPP_DEBUG(
get_logger(), "'Content of robot description file: %s", robot_description.data.c_str());
// TODO(Manuel): errors should probably be caught since we don't want controller_manager node
Expand Down Expand Up @@ -244,46 +245,102 @@ 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;

auto set_components_to_state =
[&](const std::string & parameter_name, rclcpp_lifecycle::State state)
{
std::vector<std::string> components_to_set = std::vector<std::string>({});
if (get_parameter(parameter_name, components_to_set))
{
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 (Remove at the end of 2023)
std::vector<std::string> configure_components_on_start = std::vector<std::string>({});
if (get_parameter("configure_components_on_start", configure_components_on_start))
get_parameter("configure_components_on_start", configure_components_on_start);
if (!configure_components_on_start.empty())
{
RCLCPP_WARN(
get_logger(),
"[Deprecated]: Usage of parameter \"configure_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)
{
resource_manager_->set_component_state(component, inactive_state);
}
"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 (Remove at the end of 2023)
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);
rclcpp_lifecycle::State active_state(
State::PRIMARY_STATE_ACTIVE, hardware_interface::lifecycle_state_names::ACTIVE);
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.");
"Parameter 'activate_components_on_start' is deprecated. "
"Components are activated per default. Don't use this parameters in combination with the new "
"'hardware_interface_state_after_start' 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
126 changes: 94 additions & 32 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_COMMAND_INTERFACES;
using ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_NAME;
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 @@ -199,36 +202,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.");
}

cm_->init_resource_manager(robot_description);

SetUpSrvsCMExecutor();
}
};

TEST_F(TestControllerManagerHWManagementSrvs, list_hardware_components)
{
// Default status after start:
Expand Down Expand Up @@ -386,6 +359,36 @@ 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.");
}

cm_->init_resource_manager(robot_description);

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 @@ -409,3 +412,62 @@ 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

0 comments on commit 1e393f3

Please sign in to comment.