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 1 commit
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
5 changes: 3 additions & 2 deletions 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,8 +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 rclcpp::NodeOptions & node_options = rclcpp::NodeOptions());
const std::string & controller_name, const std::string & urdf,
const std::string & namespace_ = "", const rclcpp::NodeOptions & node_options = rclcpp::NodeOptions());

/// Custom configure method to read additional parameters for controller-nodes
/*
Expand Down Expand Up @@ -223,6 +223,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
5 changes: 3 additions & 2 deletions 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 rclcpp::NodeOptions & node_options)
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
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
Loading