updated template code for galacitc+ controller (StoglRobotics#28)

* updated template code for controllers to be compatible with galactic and (current) rolling

Co-authored-by: Denis Štogl <[email protected]>

templates/ros2_control/controller/controller.cpp

@@ -1,33 +1,29 @@
 $LICENSE$
 
+#include "$package_name$/$file_name$.hpp"
+
 #include <limits>
 #include <memory>
 #include <string>
 #include <vector>
 
-#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<std::vector<std::string>>("joints", {});
+    get_node()->declare_parameter<std::vector<std::string>>("joints", std::vector<std::string>({}));
     get_node()->declare_parameter<std::string>("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();

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<std::string> joint_names_;

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::string>())),
+    std::make_tuple(std::string("joints"), rclcpp::ParameterValue(std::vector<std::string>({}))),
     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);
 

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<std::string> joint_names_ = {"joint1"};
-  const std::string interface_name_ = "my_interface";
+  std::vector<std::string> joint_names_ = {"joint1"};
+  std::string interface_name_ = "my_interface";
   std::array<double, 1> joint_state_values_ = {1.1};
   std::array<double, 1> 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())});
   }
 };