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 controller manager update rate on the init of the controller interface #1141

Merged
merged 8 commits into from
Nov 20, 2023
Merged

Pass controller manager update rate on the init of the controller interface #1141

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
8 commits
Select commit Hold shift + click to select a range
cc5a3a2
added minimal test to verify the update of robot description in contr…
saikishor Oct 22, 2023
5766b83
added get_robot_description method to the base class
saikishor Oct 23, 2023
1010cce
added controller_manager update_rate to the controller interface
saikishor Oct 22, 2023
388dcf9
update tests with the cm_update_rate argument
saikishor Oct 22, 2023
3ca2a01
added tests for the cm_update_rate method
saikishor Oct 22, 2023
7c2c083
use controller manager update rate as default for the controller inte…
saikishor Nov 7, 2023
efe6359
fix controller interface tests
saikishor Nov 7, 2023
7d5a77a
remove the cm_update_rate API and only fix the controller update rate…
saikishor Nov 7, 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
2 changes: 1 addition & 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,7 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy

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

Expand Down
6 changes: 3 additions & 3 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,16 +25,16 @@
namespace controller_interface
{
return_type ControllerInterfaceBase::init(
const std::string & controller_name, const std::string & urdf, const std::string & namespace_,
const rclcpp::NodeOptions & node_options)
const std::string & controller_name, const std::string & urdf, unsigned int cm_update_rate,
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
{
auto_declare<int>("update_rate", 0);
auto_declare<int>("update_rate", cm_update_rate);
auto_declare<bool>("is_async", false);
}
catch (const std::exception & e)
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, "", 50.0), 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, "", 50.0), 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, "", 50.0), 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, "", 50.0), 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, "", 50.0), 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, "", 50.0), controller_interface::return_type::OK);
ASSERT_NO_THROW(controller.get_node());

EXPECT_FALSE(controller.is_in_chained_mode());
Expand Down
14 changes: 8 additions & 6 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,15 +38,15 @@ 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, "", 10.0), controller_interface::return_type::OK);
ASSERT_NO_THROW(controller.get_node());

// update_rate is set to default 0
ASSERT_EQ(controller.get_update_rate(), 0u);

// Even after configure is 0
// Even after configure is 10
controller.configure();
ASSERT_EQ(controller.get_update_rate(), 0u);
ASSERT_EQ(controller.get_update_rate(), 10u);

rclcpp::shutdown();
}
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, "", 1.0), controller_interface::return_type::OK);

// initialize executor to be able to get parameter update
auto executor =
Expand Down Expand Up @@ -122,7 +122,8 @@ 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, "", 100.0), controller_interface::return_type::ERROR);

rclcpp::shutdown();
}
Expand All @@ -136,7 +137,8 @@ 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, "", 50.0), 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", "", 50.0), 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", "", 50.0), 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
4 changes: 2 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,14 +40,14 @@ class ControllerWithOptions : public controller_interface::ControllerInterface
}

controller_interface::return_type init(
const std::string & controller_name, const std::string & urdf,
const std::string & controller_name, const std::string & urdf, unsigned int cm_update_rate,
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, urdf, namespace_, node_options);
ControllerInterface::init(controller_name, urdf, cm_update_rate, namespace_, node_options);

switch (on_init())
{
Expand Down
8 changes: 4 additions & 4 deletions controller_manager/src/controller_manager.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -736,7 +736,7 @@ controller_interface::return_type ControllerManager::configure_controller(
"update rate.",
controller_name.c_str(), controller_update_rate, cm_update_rate);
}
else if (controller_update_rate != 0 && cm_update_rate % controller_update_rate != 0)
else if (cm_update_rate % controller_update_rate != 0)
{
RCLCPP_WARN(
get_logger(),
Expand Down Expand Up @@ -1291,7 +1291,8 @@ controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::add_co
}

if (
controller.c->init(controller.info.name, robot_description_, get_namespace()) ==
controller.c->init(
controller.info.name, robot_description_, get_update_rate(), get_namespace()) ==
controller_interface::return_type::ERROR)
{
to.clear();
Expand Down Expand Up @@ -2040,8 +2041,7 @@ controller_interface::return_type ControllerManager::update(
if (!loaded_controller.c->is_async() && is_controller_active(*loaded_controller.c))
{
const auto controller_update_rate = loaded_controller.c->get_update_rate();
const bool run_controller_at_cm_rate =
(controller_update_rate == 0) || (controller_update_rate >= update_rate_);
const bool run_controller_at_cm_rate = (controller_update_rate >= update_rate_);
const auto controller_period =
run_controller_at_cm_rate ? period
: rclcpp::Duration::from_seconds((1.0 / controller_update_rate));
Expand Down
3 changes: 2 additions & 1 deletion 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 @@ -33,7 +33,8 @@ TestControllerFailedInit::on_init()

controller_interface::return_type TestControllerFailedInit::init(
const std::string & /* controller_name */, const std::string & /* urdf */,
const std::string & /*namespace_*/, const rclcpp::NodeOptions & /*node_options*/)
unsigned int cm_update_rate, const std::string & /*namespace_*/,
const rclcpp::NodeOptions & /*node_options*/)
{
return controller_interface::return_type::ERROR;
}
Expand Down
2 changes: 1 addition & 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,7 @@ class TestControllerFailedInit : public controller_interface::ControllerInterfac

CONTROLLER_INTERFACE_PUBLIC
controller_interface::return_type init(
const std::string & controller_name, const std::string & urdf,
const std::string & controller_name, const std::string & urdf, unsigned int cm_update_rate,
const std::string & namespace_ = "",
const rclcpp::NodeOptions & node_options =
rclcpp::NodeOptions()
Expand Down
Loading