throw exception trying to access interfaces in unconfigured state

ros-controls · Nov 13, 2023 · 1d685a3 · 1d685a3
1 parent 0863acd
commit 1d685a3
Show file tree

Hide file tree

Showing 2 changed files with 44 additions and 4 deletions.
diff --git a/controller_manager/test/test_chainable_controller/test_chainable_controller.cpp b/controller_manager/test/test_chainable_controller/test_chainable_controller.cpp
@@ -32,13 +32,33 @@ TestChainableController::TestChainableController()
 controller_interface::InterfaceConfiguration
 TestChainableController::command_interface_configuration() const
 {
-  return cmd_iface_cfg_;
+  if (
+    get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE ||
+    get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)
+  {
+    return cmd_iface_cfg_;
+  }
+  else
+  {
+    throw std::runtime_error(
+      "Can not get command interface configuration until the controller is configured.");
+  }
 }
 
 controller_interface::InterfaceConfiguration
 TestChainableController::state_interface_configuration() const
 {
-  return state_iface_cfg_;
+  if (
+    get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE ||
+    get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)
+  {
+    return state_iface_cfg_;
+  }
+  else
+  {
+    throw std::runtime_error(
+      "Can not get state interface configuration until the controller is configured.");
+  }
 }
 
 controller_interface::return_type TestChainableController::update_reference_from_subscribers(

diff --git a/controller_manager/test/test_controller/test_controller.cpp b/controller_manager/test/test_controller/test_controller.cpp
@@ -31,12 +31,32 @@ TestController::TestController()
 
 controller_interface::InterfaceConfiguration TestController::command_interface_configuration() const
 {
-  return cmd_iface_cfg_;
+  if (
+    get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE ||
+    get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)
+  {
+    return cmd_iface_cfg_;
+  }
+  else
+  {
+    throw std::runtime_error(
+      "Can not get command interface configuration until the controller is configured.");
+  }
 }
 
 controller_interface::InterfaceConfiguration TestController::state_interface_configuration() const
 {
-  return state_iface_cfg_;
+  if (
+    get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE ||
+    get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)
+  {
+    return state_iface_cfg_;
+  }
+  else
+  {
+    throw std::runtime_error(
+      "Can not get state interface configuration until the controller is configured.");
+  }
 }
 
 controller_interface::return_type TestController::update(