Merge branch 'master' into tpo-add_semantic_cmd_interface

controller_interface/include/controller_interface/controller_interface_base.hpp

@@ -99,7 +99,7 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy
 public:
   ControllerInterfaceBase() = default;
 
-  virtual ~ControllerInterfaceBase() = default;
+  virtual ~ControllerInterfaceBase();
 
   /// Get configuration for controller's required command interfaces.
   /**

controller_interface/src/controller_interface_base.cpp

@@ -22,6 +22,22 @@
 
 namespace controller_interface
 {
+ControllerInterfaceBase::~ControllerInterfaceBase()
+{
+  // check if node is initialized and we still have a valid context
+  if (
+    node_.get() &&
+    get_lifecycle_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED &&
+    rclcpp::ok())
+  {
+    RCLCPP_DEBUG(
+      get_node()->get_logger(),
+      "Calling shutdown transition of controller node '%s' due to destruction.",
+      get_node()->get_name());
+    node_->shutdown();
+  }
+}
+
 return_type ControllerInterfaceBase::init(
   const std::string & controller_name, const std::string & urdf, unsigned int cm_update_rate,
   const std::string & node_namespace, const rclcpp::NodeOptions & node_options)
@@ -52,6 +68,11 @@ return_type ControllerInterfaceBase::init(
       break;
     case LifecycleNodeInterface::CallbackReturn::ERROR:
     case LifecycleNodeInterface::CallbackReturn::FAILURE:
+      RCLCPP_DEBUG(
+        get_node()->get_logger(),
+        "Calling shutdown transition of controller node '%s' due to init failure.",
+        get_node()->get_name());
+      node_->shutdown();
       return return_type::ERROR;
   }
 
@@ -158,6 +179,10 @@ void ControllerInterfaceBase::release_interfaces()
 
 const rclcpp_lifecycle::State & ControllerInterfaceBase::get_lifecycle_state() const
 {
+  if (!node_.get())
+  {
+    throw std::runtime_error("Lifecycle node hasn't been initialized yet!");
+  }
   return node_->get_current_state();
 }
 

controller_interface/test/test_controller_interface.cpp

@@ -36,16 +36,21 @@ TEST(TestableControllerInterface, init)
   rclcpp::init(argc, argv);
 
   TestableControllerInterface controller;
+  const TestableControllerInterface & const_controller = controller;
 
   // try to get node when not initialized
   ASSERT_THROW(controller.get_node(), std::runtime_error);
+  ASSERT_THROW(const_controller.get_node(), std::runtime_error);
+  ASSERT_THROW(controller.get_lifecycle_state(), std::runtime_error);
 
   // initialize, create node
   const auto node_options = controller.define_custom_node_options();
   ASSERT_EQ(
     controller.init(TEST_CONTROLLER_NAME, "", 10.0, "", node_options),
     controller_interface::return_type::OK);
   ASSERT_NO_THROW(controller.get_node());
+  ASSERT_NO_THROW(const_controller.get_node());
+  ASSERT_NO_THROW(controller.get_lifecycle_state());
 
   // update_rate is set to controller_manager's rate
   ASSERT_EQ(controller.get_update_rate(), 10u);
@@ -54,6 +59,7 @@ TEST(TestableControllerInterface, init)
   controller.configure();
   ASSERT_EQ(controller.get_update_rate(), 10u);
 
+  controller.get_node()->shutdown();
   rclcpp::shutdown();
 }
 
@@ -80,6 +86,8 @@ TEST(TestableControllerInterface, setting_negative_update_rate_in_configure)
   // The configure should fail and the update rate should stay the same
   ASSERT_EQ(controller.configure().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED);
   ASSERT_EQ(controller.get_update_rate(), 1000u);
+
+  controller.get_node()->shutdown();
   rclcpp::shutdown();
 }
 
@@ -149,6 +157,7 @@ TEST(TestableControllerInterface, setting_update_rate_in_configure)
   ASSERT_EQ(controller.get_update_rate(), 623u);
 
   executor->cancel();
+  controller.get_node()->shutdown();
   rclcpp::shutdown();
 }
 
@@ -166,6 +175,8 @@ TEST(TestableControllerInterfaceInitError, init_with_error)
     controller.init(TEST_CONTROLLER_NAME, "", 100.0, "", node_options),
     controller_interface::return_type::ERROR);
 
+  ASSERT_EQ(
+    controller.get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED);
   rclcpp::shutdown();
 }
 
@@ -183,6 +194,8 @@ TEST(TestableControllerInterfaceInitFailure, init_with_failure)
     controller.init(TEST_CONTROLLER_NAME, "", 50.0, "", node_options),
     controller_interface::return_type::ERROR);
 
+  ASSERT_EQ(
+    controller.get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED);
   rclcpp::shutdown();
 }
 

controller_interface/test/test_controller_with_options.cpp

@@ -41,7 +41,7 @@ TEST(ControllerWithOption, init_with_overrides)
   rclcpp::init(argc, argv);
   // creates the controller
   FriendControllerWithOptions controller;
-  EXPECT_EQ(
+  ASSERT_EQ(
     controller.init("controller_name", "", 50.0, "", controller.define_custom_node_options()),
     controller_interface::return_type::OK);
   // checks that the node options have been updated
@@ -54,6 +54,8 @@ TEST(ControllerWithOption, init_with_overrides)
   EXPECT_EQ(controller.params["parameter1"], 1.);
   EXPECT_EQ(controller.params["parameter2"], 2.);
   EXPECT_EQ(controller.params["parameter3"], 3.);
+
+  controller.get_node()->shutdown();
   rclcpp::shutdown();
 }
 
@@ -68,7 +70,7 @@ TEST(ControllerWithOption, init_with_node_options_arguments_parameters)
   controller_node_options.arguments(
     {"--ros-args", "-p", "parameter_list.parameter1:=1.", "-p", "parameter_list.parameter2:=2.",
      "-p", "parameter_list.parameter3:=3."});
-  EXPECT_EQ(
+  ASSERT_EQ(
     controller.init("controller_name", "", 50.0, "", controller_node_options),
     controller_interface::return_type::OK);
   // checks that the node options have been updated
@@ -81,6 +83,8 @@ TEST(ControllerWithOption, init_with_node_options_arguments_parameters)
   EXPECT_EQ(controller.params["parameter1"], 1.);
   EXPECT_EQ(controller.params["parameter2"], 2.);
   EXPECT_EQ(controller.params["parameter3"], 3.);
+
+  controller.get_node()->shutdown();
   rclcpp::shutdown();
 }
 
@@ -96,7 +100,7 @@ TEST(ControllerWithOption, init_with_node_options_arguments_parameters_file)
   std::cerr << params_file_path << std::endl;
   auto controller_node_options = controller.define_custom_node_options();
   controller_node_options.arguments({"--ros-args", "--params-file", params_file_path});
-  EXPECT_EQ(
+  ASSERT_EQ(
     controller.init("controller_name", "", 50.0, "", controller_node_options),
     controller_interface::return_type::OK);
   // checks that the node options have been updated
@@ -112,6 +116,8 @@ TEST(ControllerWithOption, init_with_node_options_arguments_parameters_file)
   bool use_sim_time(true);
   controller.get_node()->get_parameter_or("use_sim_time", use_sim_time, false);
   ASSERT_FALSE(use_sim_time);
+
+  controller.get_node()->shutdown();
   rclcpp::shutdown();
 }
 
@@ -130,7 +136,7 @@ TEST(
   controller_node_options.arguments(
     {"--ros-args", "--params-file", params_file_path, "-p", "parameter_list.parameter1:=562.785",
      "-p", "use_sim_time:=true"});
-  EXPECT_EQ(
+  ASSERT_EQ(
     controller.init("controller_name", "", 50.0, "", controller_node_options),
     controller_interface::return_type::OK);
   // checks that the node options have been updated
@@ -146,6 +152,8 @@ TEST(
   bool use_sim_time(false);
   controller.get_node()->get_parameter_or("use_sim_time", use_sim_time, false);
   ASSERT_TRUE(use_sim_time);
+
+  controller.get_node()->shutdown();
   rclcpp::shutdown();
 }
 
@@ -157,7 +165,7 @@ TEST(ControllerWithOption, init_without_overrides)
   rclcpp::init(argc, argv);
   // creates the controller
   FriendControllerWithOptions controller;
-  EXPECT_EQ(
+  ASSERT_EQ(
     controller.init("controller_name", "", 50.0, "", controller.define_custom_node_options()),
     controller_interface::return_type::ERROR);
   // checks that the node options have been updated
@@ -166,5 +174,6 @@ TEST(ControllerWithOption, init_without_overrides)
   EXPECT_TRUE(node_options.automatically_declare_parameters_from_overrides());
   // checks that no parameter has been declared from overrides
   EXPECT_EQ(controller.params.size(), 0u);
+
   rclcpp::shutdown();
 }

controller_manager/test/controller_manager_test_common.hpp

@@ -226,10 +226,4 @@ class ControllerManagerRunner
   ControllerManagerFixture<CtrlMgr> * cmf_;
 };
 
-class ControllerMock : public controller_interface::ControllerInterface
-{
-public:
-  MOCK_METHOD0(update, controller_interface::return_type(void));
-};
-
 #endif  // CONTROLLER_MANAGER_TEST_COMMON_HPP_

hardware_interface/CMakeLists.txt

@@ -54,6 +54,7 @@ target_include_directories(mock_components PUBLIC
   $<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>
   $<INSTALL_INTERFACE:include/hardware_interface>
 )
+target_link_libraries(mock_components PUBLIC hardware_interface)
 ament_target_dependencies(mock_components PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS})
 
 pluginlib_export_plugin_description_file(

joint_limits/test/joint_limits_rosparam_test.cpp

@@ -17,6 +17,7 @@
 #include <gtest/gtest.h>
 
 #include "joint_limits/joint_limits_rosparam.hpp"
+#include "lifecycle_msgs/msg/state.hpp"
 #include "rclcpp_lifecycle/lifecycle_node.hpp"
 
 class JointLimitsRosParamTest : public ::testing::Test
@@ -294,7 +295,17 @@ class JointLimitsLifecycleNodeUndeclaredRosParamTest : public ::testing::Test
     lifecycle_node_ = rclcpp_lifecycle::LifecycleNode::make_shared("JointLimitsRosparamTestNode");
   }
 
-  void TearDown() { lifecycle_node_.reset(); }
+  void TearDown()
+  {
+    if (
+      lifecycle_node_->get_current_state().id() !=
+        lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED &&
+      rclcpp::ok())
+    {
+      lifecycle_node_->shutdown();
+    }
+    lifecycle_node_.reset();
+  }
 
 protected:
   rclcpp_lifecycle::LifecycleNode::SharedPtr lifecycle_node_;