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

Pass URDF to controllers on init #1088

Merged
merged 9 commits into from
Nov 6, 2023
Merged

Pass URDF to controllers on init #1088

Show file tree
Hide file tree
Changes from 7 commits
Commits
Show all changes
9 commits
Select commit Hold shift + click to select a range
c80c1e6
Pass URDF to controllers on init
bmagyar Jul 26, 2023
67a9f6c
Merge branch 'master' into pass-urdf-to-controllers
bmagyar Sep 26, 2023
132ef99
store the robot_description in the controller_manager class variable
saikishor Oct 21, 2023
1271a92
added minimal test to verify the update of robot description in contr…
saikishor Oct 22, 2023
56c1ef6
fix code formatting
saikishor Oct 22, 2023
4705785
added test with loading and unloading the controller
saikishor Oct 22, 2023
3495866
Merge branch 'master' into pass-urdf-to-controllers
saikishor Oct 22, 2023
5f40b28
added changes as per bence's review comments
saikishor Oct 23, 2023
caef34d
added get_robot_description method to the base class
saikishor Oct 23, 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
4 changes: 3 additions & 1 deletion controller_interface/include/controller_interface/controller_interface_base.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -114,7 +114,8 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy

CONTROLLER_INTERFACE_PUBLIC
virtual return_type init(
const std::string & controller_name, const std::string & namespace_ = "",
const std::string & controller_name, const std::string & urdf,
const std::string & namespace_ = "",
const rclcpp::NodeOptions & node_options = rclcpp::NodeOptions().enable_logger_service(true));

/// Custom configure method to read additional parameters for controller-nodes
Expand Down Expand Up @@ -223,6 +224,7 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy
std::vector<hardware_interface::LoanedStateInterface> state_interfaces_;
unsigned int update_rate_ = 0;
bool is_async_ = false;
std::string urdf_ = "";
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Should this be a std::optional<std::string> urdf_ = std::nullopt instead?

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I don't want to communicate the option of not being set as anything close to valid. The empty string initial value is only there so we can test if it was properly set...

I also thought about how we could do this better, a const string would be nice but that can only be set once which doesn't work well with the longer term idea of getting URDF updates from topics.


private:
std::shared_ptr<rclcpp_lifecycle::LifecycleNode> node_;
Expand Down
3 changes: 2 additions & 1 deletion controller_interface/src/controller_interface_base.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -25,11 +25,12 @@
namespace controller_interface
{
return_type ControllerInterfaceBase::init(
const std::string & controller_name, const std::string & namespace_,
const std::string & controller_name, const std::string & urdf, const std::string & namespace_,
const rclcpp::NodeOptions & node_options)
{
node_ = std::make_shared<rclcpp_lifecycle::LifecycleNode>(
controller_name, namespace_, node_options, false); // disable LifecycleNode service interfaces
urdf_ = urdf;

try
{
Expand Down
12 changes: 6 additions & 6 deletions controller_interface/test/test_chainable_controller_interface.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -21,7 +21,7 @@ TEST_F(ChainableControllerInterfaceTest, default_returns)
TestableChainableControllerInterface controller;

// initialize, create node
ASSERT_EQ(controller.init(TEST_CONTROLLER_NAME), controller_interface::return_type::OK);
ASSERT_EQ(controller.init(TEST_CONTROLLER_NAME, ""), controller_interface::return_type::OK);
ASSERT_NO_THROW(controller.get_node());

EXPECT_TRUE(controller.is_chainable());
Expand All @@ -33,7 +33,7 @@ TEST_F(ChainableControllerInterfaceTest, export_reference_interfaces)
TestableChainableControllerInterface controller;

// initialize, create node
ASSERT_EQ(controller.init(TEST_CONTROLLER_NAME), controller_interface::return_type::OK);
ASSERT_EQ(controller.init(TEST_CONTROLLER_NAME, ""), controller_interface::return_type::OK);
ASSERT_NO_THROW(controller.get_node());

auto reference_interfaces = controller.export_reference_interfaces();
Expand All @@ -50,7 +50,7 @@ TEST_F(ChainableControllerInterfaceTest, reference_interfaces_storage_not_correc
TestableChainableControllerInterface controller;

// initialize, create node
ASSERT_EQ(controller.init(TEST_CONTROLLER_NAME), controller_interface::return_type::OK);
ASSERT_EQ(controller.init(TEST_CONTROLLER_NAME, ""), controller_interface::return_type::OK);
ASSERT_NO_THROW(controller.get_node());

// expect empty return because storage is not resized
Expand All @@ -64,7 +64,7 @@ TEST_F(ChainableControllerInterfaceTest, reference_interfaces_prefix_is_not_node
TestableChainableControllerInterface controller;

// initialize, create node
ASSERT_EQ(controller.init(TEST_CONTROLLER_NAME), controller_interface::return_type::OK);
ASSERT_EQ(controller.init(TEST_CONTROLLER_NAME, ""), controller_interface::return_type::OK);
ASSERT_NO_THROW(controller.get_node());

controller.set_name_prefix_of_reference_interfaces("some_not_correct_interface_prefix");
Expand All @@ -79,7 +79,7 @@ TEST_F(ChainableControllerInterfaceTest, setting_chained_mode)
TestableChainableControllerInterface controller;

// initialize, create node
ASSERT_EQ(controller.init(TEST_CONTROLLER_NAME), controller_interface::return_type::OK);
ASSERT_EQ(controller.init(TEST_CONTROLLER_NAME, ""), controller_interface::return_type::OK);
ASSERT_NO_THROW(controller.get_node());

auto reference_interfaces = controller.export_reference_interfaces();
Expand Down Expand Up @@ -126,7 +126,7 @@ TEST_F(ChainableControllerInterfaceTest, test_update_logic)
TestableChainableControllerInterface controller;

// initialize, create node
ASSERT_EQ(controller.init(TEST_CONTROLLER_NAME), controller_interface::return_type::OK);
ASSERT_EQ(controller.init(TEST_CONTROLLER_NAME, ""), controller_interface::return_type::OK);
ASSERT_NO_THROW(controller.get_node());

EXPECT_FALSE(controller.is_in_chained_mode());
Expand Down
8 changes: 4 additions & 4 deletions controller_interface/test/test_controller_interface.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -38,7 +38,7 @@ TEST(TestableControllerInterface, init)
ASSERT_THROW(controller.get_node(), std::runtime_error);

// initialize, create node
ASSERT_EQ(controller.init(TEST_CONTROLLER_NAME), controller_interface::return_type::OK);
ASSERT_EQ(controller.init(TEST_CONTROLLER_NAME, ""), controller_interface::return_type::OK);
ASSERT_NO_THROW(controller.get_node());

// update_rate is set to default 0
Expand All @@ -60,7 +60,7 @@ TEST(TestableControllerInterface, setting_update_rate_in_configure)

TestableControllerInterface controller;
// initialize, create node
ASSERT_EQ(controller.init(TEST_CONTROLLER_NAME), controller_interface::return_type::OK);
ASSERT_EQ(controller.init(TEST_CONTROLLER_NAME, ""), controller_interface::return_type::OK);

// initialize executor to be able to get parameter update
auto executor =
Expand Down Expand Up @@ -122,7 +122,7 @@ TEST(TestableControllerInterfaceInitError, init_with_error)
TestableControllerInterfaceInitError controller;

// initialize, create node
ASSERT_EQ(controller.init(TEST_CONTROLLER_NAME), controller_interface::return_type::ERROR);
ASSERT_EQ(controller.init(TEST_CONTROLLER_NAME, ""), controller_interface::return_type::ERROR);

rclcpp::shutdown();
}
Expand All @@ -136,7 +136,7 @@ TEST(TestableControllerInterfaceInitFailure, init_with_failure)
TestableControllerInterfaceInitFailure controller;

// initialize, create node
ASSERT_EQ(controller.init(TEST_CONTROLLER_NAME), controller_interface::return_type::ERROR);
ASSERT_EQ(controller.init(TEST_CONTROLLER_NAME, ""), controller_interface::return_type::ERROR);

rclcpp::shutdown();
}
Expand Down
4 changes: 2 additions & 2 deletions controller_interface/test/test_controller_with_options.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -42,7 +42,7 @@ TEST(ControllerWithOption, init_with_overrides)
rclcpp::init(argc, argv);
// creates the controller
FriendControllerWithOptions controller;
EXPECT_EQ(controller.init("controller_name"), controller_interface::return_type::OK);
EXPECT_EQ(controller.init("controller_name", ""), controller_interface::return_type::OK);
// checks that the node options have been updated
const auto & node_options = controller.get_node()->get_node_options();
EXPECT_TRUE(node_options.allow_undeclared_parameters());
Expand All @@ -63,7 +63,7 @@ TEST(ControllerWithOption, init_without_overrides)
rclcpp::init(argc, argv);
// creates the controller
FriendControllerWithOptions controller;
EXPECT_EQ(controller.init("controller_name"), controller_interface::return_type::ERROR);
EXPECT_EQ(controller.init("controller_name", ""), controller_interface::return_type::ERROR);
// checks that the node options have been updated
const auto & node_options = controller.get_node()->get_node_options();
EXPECT_TRUE(node_options.allow_undeclared_parameters());
Expand Down
5 changes: 3 additions & 2 deletions controller_interface/test/test_controller_with_options.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -40,13 +40,14 @@ class ControllerWithOptions : public controller_interface::ControllerInterface
}

controller_interface::return_type init(
const std::string & controller_name, const std::string & namespace_ = "",
const std::string & controller_name, const std::string & urdf,
const std::string & namespace_ = "",
const rclcpp::NodeOptions & node_options =
rclcpp::NodeOptions()
.allow_undeclared_parameters(true)
.automatically_declare_parameters_from_overrides(true)) override
{
ControllerInterface::init(controller_name, namespace_, node_options);
ControllerInterface::init(controller_name, urdf, namespace_, node_options);

switch (on_init())
{
Expand Down
1 change: 1 addition & 0 deletions controller_manager/include/controller_manager/controller_manager.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -534,6 +534,7 @@ class ControllerManager : public rclcpp::Node
std::vector<std::string> activate_command_interface_request_,
deactivate_command_interface_request_;

std::string robot_description_;
rclcpp::Subscription<std_msgs::msg::String>::SharedPtr robot_description_subscription_;

struct SwitchParams
Expand Down
13 changes: 7 additions & 6 deletions controller_manager/src/controller_manager.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -267,10 +267,10 @@ ControllerManager::ControllerManager(
RCLCPP_WARN(get_logger(), "'update_rate' parameter not set, using default value.");
}

std::string robot_description = "";
robot_description_ = "";
// TODO(destogl): remove support at the end of 2023
get_parameter("robot_description", robot_description);
if (robot_description.empty())
get_parameter("robot_description", robot_description_);
if (robot_description_.empty())
{
subscribe_to_robot_description_topic();
}
Expand All @@ -280,7 +280,7 @@ ControllerManager::ControllerManager(
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);
init_resource_manager(robot_description_);
}

diagnostics_updater_.setHardwareID("ros2_control");
Expand Down Expand Up @@ -337,6 +337,7 @@ void ControllerManager::robot_description_callback(const std_msgs::msg::String &
// to die if a non valid urdf is passed. However, should maybe be fine tuned.
try
{
robot_description_ = robot_description.data;
if (resource_manager_->is_urdf_already_loaded())
{
RCLCPP_WARN(
Expand All @@ -345,7 +346,7 @@ void ControllerManager::robot_description_callback(const std_msgs::msg::String &
"description file.");
return;
}
init_resource_manager(robot_description.data.c_str());
init_resource_manager(robot_description_);
}
catch (std::runtime_error & e)
{
Expand Down Expand Up @@ -1283,7 +1284,7 @@ controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::add_co
}

if (
controller.c->init(controller.info.name, get_namespace()) ==
controller.c->init(controller.info.name, robot_description_, get_namespace()) ==
controller_interface::return_type::ERROR)
{
to.clear();
Expand Down
2 changes: 2 additions & 0 deletions controller_manager/test/test_controller/test_controller.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -119,6 +119,8 @@ void TestController::set_state_interface_configuration(
state_iface_cfg_ = cfg;
}

const std::string & TestController::getRobotDescription() const { return urdf_; }

} // namespace test_controller

#include "pluginlib/class_list_macros.hpp"
Expand Down
2 changes: 2 additions & 0 deletions controller_manager/test/test_controller/test_controller.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -65,6 +65,8 @@ class TestController : public controller_interface::ControllerInterface
CONTROLLER_MANAGER_PUBLIC
void set_state_interface_configuration(const controller_interface::InterfaceConfiguration & cfg);

const std::string & getRobotDescription() const;

unsigned int internal_counter = 0;
bool simulate_cleanup_failure = false;
// Variable where we store when cleanup was called, pointer because the controller
Expand Down
4 changes: 2 additions & 2 deletions controller_manager/test/test_controller_failed_init/test_controller_failed_init.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -32,8 +32,8 @@ TestControllerFailedInit::on_init()
}

controller_interface::return_type TestControllerFailedInit::init(
const std::string & /* controller_name */, const std::string & /*namespace_*/,
const rclcpp::NodeOptions & /*node_options*/)
const std::string & /* controller_name */, const std::string & /* urdf */,
const std::string & /*namespace_*/, const rclcpp::NodeOptions & /*node_options*/)
{
return controller_interface::return_type::ERROR;
}
Expand Down
3 changes: 2 additions & 1 deletion controller_manager/test/test_controller_failed_init/test_controller_failed_init.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -40,7 +40,8 @@ class TestControllerFailedInit : public controller_interface::ControllerInterfac

CONTROLLER_INTERFACE_PUBLIC
controller_interface::return_type init(
const std::string & controller_name, const std::string & namespace_ = "",
const std::string & controller_name, const std::string & urdf,
const std::string & namespace_ = "",
const rclcpp::NodeOptions & node_options =
rclcpp::NodeOptions()
.allow_undeclared_parameters(true)
Expand Down
28 changes: 28 additions & 0 deletions controller_manager/test/test_controller_manager.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -33,6 +33,34 @@ class TestControllerManagerWithStrictness
{
};

class TestControllerManagerRobotDescription
: public ControllerManagerFixture<controller_manager::ControllerManager>
{
};

TEST_F(TestControllerManagerRobotDescription, controller_robot_description_update)
{
auto test_controller = std::make_shared<test_controller::TestController>();
auto test_controller2 = std::make_shared<test_controller::TestController>();
constexpr char TEST_CONTROLLER2_NAME[] = "test_controller2_name";
bmagyar marked this conversation as resolved.
Show resolved Hide resolved
cm_->add_controller(
test_controller, test_controller::TEST_CONTROLLER_NAME,
test_controller::TEST_CONTROLLER_CLASS_NAME);
ASSERT_EQ(ros2_control_test_assets::minimal_robot_urdf, test_controller->getRobotDescription());

// Now change the robot description and then load a new controller and see if the new controller
// gets the new description and the old controller still maintains the configuration
auto msg = std_msgs::msg::String();
msg.data = ros2_control_test_assets::minimal_robot_missing_state_keys_urdf;
cm_->robot_description_callback(msg);
cm_->add_controller(
test_controller2, TEST_CONTROLLER2_NAME, test_controller::TEST_CONTROLLER_CLASS_NAME);
ASSERT_EQ(ros2_control_test_assets::minimal_robot_urdf, test_controller->getRobotDescription());
bmagyar marked this conversation as resolved.
Show resolved Hide resolved
ASSERT_EQ(
ros2_control_test_assets::minimal_robot_missing_state_keys_urdf,
test_controller2->getRobotDescription());
}

TEST_P(TestControllerManagerWithStrictness, controller_lifecycle)
{
const auto test_param = GetParam();
Expand Down
41 changes: 41 additions & 0 deletions controller_manager/test/test_controller_manager_srvs.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -456,6 +456,47 @@ TEST_F(TestControllerManagerSrvs, unload_controller_srv)
EXPECT_EQ(0u, cm_->get_loaded_controllers().size());
}

TEST_F(TestControllerManagerSrvs, robot_description_on_load_and_unload_controller)
{
rclcpp::executors::SingleThreadedExecutor srv_executor;
rclcpp::Node::SharedPtr srv_node = std::make_shared<rclcpp::Node>("srv_client");
srv_executor.add_node(srv_node);
rclcpp::Client<controller_manager_msgs::srv::UnloadController>::SharedPtr unload_client =
srv_node->create_client<controller_manager_msgs::srv::UnloadController>(
"test_controller_manager/unload_controller");

auto test_controller = std::make_shared<TestController>();
auto abstract_test_controller = cm_->add_controller(
test_controller, test_controller::TEST_CONTROLLER_NAME,
test_controller::TEST_CONTROLLER_CLASS_NAME);
EXPECT_EQ(1u, cm_->get_loaded_controllers().size());

// check the robot description
ASSERT_EQ(ros2_control_test_assets::minimal_robot_urdf, test_controller->getRobotDescription());

// Now change the robot description and then load a new controller and see if the new controller
// gets the new description and the old controller still maintains the configuration
auto msg = std_msgs::msg::String();
msg.data = ros2_control_test_assets::minimal_robot_missing_state_keys_urdf;
cm_->robot_description_callback(msg);
ASSERT_EQ(ros2_control_test_assets::minimal_robot_urdf, test_controller->getRobotDescription());
bmagyar marked this conversation as resolved.
Show resolved Hide resolved

// now unload and load the controller and see if the controller gets the new robot description
auto unload_request = std::make_shared<controller_manager_msgs::srv::UnloadController::Request>();
unload_request->name = test_controller::TEST_CONTROLLER_NAME;
auto result = call_service_and_wait(*unload_client, unload_request, srv_executor, true);
EXPECT_EQ(0u, cm_->get_loaded_controllers().size());

// now load it and check if it got the new robot description
cm_->add_controller(
test_controller, test_controller::TEST_CONTROLLER_NAME,
test_controller::TEST_CONTROLLER_CLASS_NAME);
EXPECT_EQ(1u, cm_->get_loaded_controllers().size());
ASSERT_EQ(
ros2_control_test_assets::minimal_robot_missing_state_keys_urdf,
test_controller->getRobotDescription());
}

TEST_F(TestControllerManagerSrvs, configure_controller_srv)
{
rclcpp::executors::SingleThreadedExecutor srv_executor;
Expand Down
Loading