diff --git a/templates/ros2_control/controller/controller.cpp b/templates/ros2_control/controller/controller.cpp index 0a5e8588..ff534ad3 100644 --- a/templates/ros2_control/controller/controller.cpp +++ b/templates/ros2_control/controller/controller.cpp @@ -1,33 +1,29 @@ $LICENSE$ +#include "$package_name$/$file_name$.hpp" + #include #include #include #include -#include "$package_name$/$file_name$.hpp" #include "controller_interface/helpers.hpp" namespace $package_name$ { $ClassName$::$ClassName$() : controller_interface::ControllerInterface() {} -controller_interface::return_type $ClassName$::init(const std::string & controller_name) +CallbackReturn $ClassName$::on_init() { - auto ret = ControllerInterface::init(controller_name); - if (ret != controller_interface::return_type::OK) { - return ret; - } - try { - get_node()->declare_parameter>("joints", {}); + get_node()->declare_parameter>("joints", std::vector({})); get_node()->declare_parameter("interface_name", ""); } catch (const std::exception & e) { fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); - return controller_interface::return_type::ERROR; + return CallbackReturn::ERROR; } - return controller_interface::return_type::OK; + return CallbackReturn::SUCCESS; } CallbackReturn $ClassName$::on_configure(const rclcpp_lifecycle::State & /*previous_state*/) @@ -150,7 +146,8 @@ CallbackReturn $ClassName$::on_deactivate(const rclcpp_lifecycle::State & /*prev return CallbackReturn::SUCCESS; } -controller_interface::return_type $ClassName$::update() +controller_interface::return_type $ClassName$::update( + const rclcpp::Time & time, const rclcpp::Duration & /*period*/) { auto current_command = input_command_.readFromRT(); @@ -161,7 +158,7 @@ controller_interface::return_type $ClassName$::update() } if (state_publisher_ && state_publisher_->trylock()) { - state_publisher_->msg_.header.stamp = get_node()->now(); + state_publisher_->msg_.header.stamp = time; state_publisher_->msg_.set_point = command_interfaces_[0].get_value(); state_publisher_->unlockAndPublish(); diff --git a/templates/ros2_control/controller/controller.hpp b/templates/ros2_control/controller/controller.hpp index 153b548e..77104b19 100644 --- a/templates/ros2_control/controller/controller.hpp +++ b/templates/ros2_control/controller/controller.hpp @@ -29,7 +29,7 @@ class $ClassName$ : public controller_interface::ControllerInterface $ClassName$(); $PACKAGE_NAME$_PUBLIC - controller_interface::return_type init(const std::string & controller_name) override; + CallbackReturn on_init() override; $PACKAGE_NAME$_PUBLIC controller_interface::InterfaceConfiguration command_interface_configuration() const override; @@ -47,7 +47,8 @@ class $ClassName$ : public controller_interface::ControllerInterface CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override; $PACKAGE_NAME$_PUBLIC - controller_interface::return_type update() override; + controller_interface::return_type update( + const rclcpp::Time & time, const rclcpp::Duration & period) override; protected: std::vector joint_names_; diff --git a/templates/ros2_control/controller/test_controller.cpp b/templates/ros2_control/controller/test_controller.cpp index 9b45e196..468487f5 100644 --- a/templates/ros2_control/controller/test_controller.cpp +++ b/templates/ros2_control/controller/test_controller.cpp @@ -17,10 +17,10 @@ TEST_P($ClassName$TestParameterizedParameters, one_parameter_is_missing) } // TODO(anyone): the new gtest version after 1.8.0 uses INSTANTIATE_TEST_SUITE_P -INSTANTIATE_TEST_CASE_P( +INSTANTIATE_TEST_SUITE_P( MissingMandatoryParameterDuringConfiguration, $ClassName$TestParameterizedParameters, ::testing::Values( - std::make_tuple(std::string("joints"), rclcpp::ParameterValue(std::vector())), + std::make_tuple(std::string("joints"), rclcpp::ParameterValue(std::vector({}))), std::make_tuple(std::string("interface_name"), rclcpp::ParameterValue("")))); TEST_F($ClassName$Test, joint_names_parameter_not_set) @@ -96,7 +96,10 @@ TEST_F($ClassName$Test, update_success) ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); - ASSERT_EQ(controller_->update(), controller_interface::return_type::OK); + + rclcpp::Time node_time = controller_->get_node()->now(); + rclcpp::Duration duration = rclcpp::Duration::from_nanoseconds(1000000); // 1ms + ASSERT_EQ(controller_->update(node_time, duration), controller_interface::return_type::OK); } TEST_F($ClassName$Test, deactivate_success) @@ -116,7 +119,10 @@ TEST_F($ClassName$Test, reactivate_success) ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_EQ(controller_->on_deactivate(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); - ASSERT_EQ(controller_->update(), controller_interface::return_type::OK); + + rclcpp::Time node_time = controller_->get_node()->now(); + rclcpp::Duration duration = rclcpp::Duration::from_nanoseconds(1000000); // 1ms + ASSERT_EQ(controller_->update(node_time, duration), controller_interface::return_type::OK); } TEST_F($ClassName$Test, publish_status_success) @@ -125,7 +131,10 @@ TEST_F($ClassName$Test, publish_status_success) ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); - ASSERT_EQ(controller_->update(), controller_interface::return_type::OK); + + rclcpp::Time node_time = controller_->get_node()->now(); + rclcpp::Duration duration = rclcpp::Duration::from_nanoseconds(1000000); // 1ms + ASSERT_EQ(controller_->update(node_time, duration), controller_interface::return_type::OK); ControllerStateMsg msg; subscribe_and_get_messages(msg); @@ -141,7 +150,10 @@ TEST_F($ClassName$Test, receive_message_and_publish_updated_status) ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); - ASSERT_EQ(controller_->update(), controller_interface::return_type::OK); + + rclcpp::Time node_time = controller_->get_node()->now(); + rclcpp::Duration duration = rclcpp::Duration::from_nanoseconds(1000000); // 1ms + ASSERT_EQ(controller_->update(node_time, duration), controller_interface::return_type::OK); ControllerStateMsg msg; subscribe_and_get_messages(msg); @@ -151,7 +163,8 @@ TEST_F($ClassName$Test, receive_message_and_publish_updated_status) publish_commands(); ASSERT_TRUE(controller_->wait_for_commands(executor)); - ASSERT_EQ(controller_->update(), controller_interface::return_type::OK); + node_time = controller_->get_node()->now(); + ASSERT_EQ(controller_->update(node_time, duration), controller_interface::return_type::OK); EXPECT_EQ(joint_command_values_[0], 0.45); diff --git a/templates/ros2_control/controller/test_controller.hpp b/templates/ros2_control/controller/test_controller.hpp index b2390543..db825894 100644 --- a/templates/ros2_control/controller/test_controller.hpp +++ b/templates/ros2_control/controller/test_controller.hpp @@ -16,6 +16,7 @@ #include "hardware_interface/loaned_state_interface.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" #include "rclcpp/parameter_value.hpp" +#include "rclcpp/time.hpp" #include "rclcpp/utilities.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" @@ -149,7 +150,9 @@ class $ClassName$Test : public ::testing::Test "/test_$package_name$/state", 10, subs_callback); // call update to publish the test value - ASSERT_EQ(controller_->update(), controller_interface::return_type::OK); + rclcpp::Time node_time = controller_->get_node()->now(); + rclcpp::Duration duration = rclcpp::Duration::from_nanoseconds(1000000); // 1ms + ASSERT_EQ(controller_->update(node_time, duration), controller_interface::return_type::OK); // wait for message to be passed ASSERT_EQ(wait_for(subscription), rclcpp::WaitResultKind::Ready); @@ -189,8 +192,8 @@ class $ClassName$Test : public ::testing::Test // TODO(anyone): adjust the members as needed // Controller-related parameters - const std::vector joint_names_ = {"joint1"}; - const std::string interface_name_ = "my_interface"; + std::vector joint_names_ = {"joint1"}; + std::string interface_name_ = "my_interface"; std::array joint_state_values_ = {1.1}; std::array joint_command_values_ = {101.101}; @@ -217,8 +220,7 @@ class $ClassName$TestParameterizedParameters void SetUpController(bool set_parameters = true) { $ClassName$Test::SetUpController(set_parameters); - controller_->get_node()->undeclare_parameter(std::get<0>(GetParam())); - controller_->get_node()->declare_parameter(std::get<0>(GetParam()), std::get<1>(GetParam())); + controller_->get_node()->set_parameter({std::get<0>(GetParam()), std::get<1>(GetParam())}); } };