Skip to content

Commit

Permalink
Merge branch 'master' into master
Browse files Browse the repository at this point in the history
  • Loading branch information
@destogl
destogl authored Feb 28, 2024
2 parents b338d07 + 69e5e27 commit 17463f6
Show file tree
Hide file tree
Showing 3 changed files with 65 additions and 118 deletions.
53 changes: 49 additions & 4 deletions controller_manager/doc/userdoc.rst
View file
Original file line number Diff line number Diff line change
Expand Up @@ -70,10 +70,6 @@ hardware_components_initial_state.unconfigured (optional; list<string>; default:
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.
This is usually result of the parsed description files by ``xacro`` command.

update_rate (mandatory; integer)
The frequency of controller manager's real-time update loop.
This loop reads states from hardware, updates controller and writes commands to hardware.
Expand All @@ -83,6 +79,55 @@ update_rate (mandatory; integer)
Name of a plugin exported using ``pluginlib`` for a controller.
This is a class from which controller's instance with name "``controller_name``" is created.

Subscribers
-----------

robot_description (std_msgs/msg/String)
The URDF string as robot description.
This is usually published by the ``robot_state_publisher`` node.

Handling Multiple Controller Managers
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^

When dealing with multiple controller managers, you have two options for managing different robot descriptions:

1. **Using Namespaces:** You can place both the ``robot_state_publisher`` and the ``controller_manager`` nodes into the same namespace.

.. code-block:: python
control_node = Node(
package="controller_manager",
executable="ros2_control_node",
parameters=[robot_controllers],
output="both",
namespace="rrbot",
)
robot_state_pub_node = Node(
package="robot_state_publisher",
executable="robot_state_publisher",
output="both",
parameters=[robot_description],
namespace="rrbot",
)
2. **Using Remappings:** You can use remappings to handle different robot descriptions. This involves relaying topics using the ``remappings`` tag, allowing you to specify custom topics for each controller manager.

.. code-block:: python
control_node = Node(
package="controller_manager",
executable="ros2_control_node",
parameters=[robot_controllers],
output="both",
remappings=[('robot_description', '/rrbot/robot_description')]
)
robot_state_pub_node = Node(
package="robot_state_publisher",
executable="robot_state_publisher",
output="both",
parameters=[robot_description],
namespace="rrbot",
)
Helper scripts
--------------
Expand Down
61 changes: 11 additions & 50 deletions controller_manager/src/controller_manager.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -284,7 +284,7 @@ ControllerManager::ControllerManager(
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.");
"is deprecated. Use the 'robot_description' topic from 'robot_state_publisher' instead.");
init_resource_manager(robot_description_);
init_services();
}
Expand Down Expand Up @@ -329,7 +329,7 @@ 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)
robot_description_subscription_ = create_subscription<std_msgs::msg::String>(
"~/robot_description", rclcpp::QoS(1).transient_local(),
"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.",
Expand Down Expand Up @@ -414,56 +414,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);
rclcpp_lifecycle::State active_state(
State::PRIMARY_STATE_ACTIVE, hardware_interface::lifecycle_state_names::ACTIVE);
if (!activate_components_on_start.empty())
// activate all other components
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);
}
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
69 changes: 5 additions & 64 deletions controller_manager/test/test_hardware_management_srvs.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -207,7 +207,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 @@ -395,8 +396,9 @@ 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 @@ -416,64 +418,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.");
}

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

0 comments on commit 17463f6

Please sign in to comment.