Fix hardware component unconfiguration

ros-controls · Sep 16, 2024 · a9363ba · a9363ba
1 parent eb4c19d
commit a9363ba
Show file tree

Hide file tree

Showing 3 changed files with 10 additions and 10 deletions.
diff --git a/hardware_interface/src/actuator.cpp b/hardware_interface/src/actuator.cpp
@@ -96,12 +96,12 @@ const rclcpp_lifecycle::State & Actuator::cleanup()
   std::unique_lock<std::recursive_mutex> lock(actuators_mutex_);
   if (impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE)
   {
+    impl_->set_lifecycle_state(rclcpp_lifecycle::State(
+      lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED,
+      lifecycle_state_names::UNCONFIGURED));
     switch (impl_->on_cleanup(impl_->get_lifecycle_state()))
     {
       case CallbackReturn::SUCCESS:
-        impl_->set_lifecycle_state(rclcpp_lifecycle::State(
-          lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED,
-          lifecycle_state_names::UNCONFIGURED));
         break;
       case CallbackReturn::FAILURE:
       case CallbackReturn::ERROR:

diff --git a/hardware_interface/src/sensor.cpp b/hardware_interface/src/sensor.cpp
@@ -95,12 +95,12 @@ const rclcpp_lifecycle::State & Sensor::cleanup()
   std::unique_lock<std::recursive_mutex> lock(sensors_mutex_);
   if (impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE)
   {
+    impl_->set_lifecycle_state(rclcpp_lifecycle::State(
+      lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED,
+      lifecycle_state_names::UNCONFIGURED));
     switch (impl_->on_cleanup(impl_->get_lifecycle_state()))
     {
       case CallbackReturn::SUCCESS:
-        impl_->set_lifecycle_state(rclcpp_lifecycle::State(
-          lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED,
-          lifecycle_state_names::UNCONFIGURED));
         break;
       case CallbackReturn::FAILURE:
       case CallbackReturn::ERROR:

diff --git a/hardware_interface/src/system.cpp b/hardware_interface/src/system.cpp
@@ -73,7 +73,7 @@ const rclcpp_lifecycle::State & System::configure()
     switch (impl_->on_configure(impl_->get_lifecycle_state()))
     {
       case CallbackReturn::SUCCESS:
-        impl_->set_lifecycle_state(rclcpp_lifecycle::State(
+              impl_->set_lifecycle_state(rclcpp_lifecycle::State(
           lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, lifecycle_state_names::INACTIVE));
         break;
       case CallbackReturn::FAILURE:
@@ -94,12 +94,12 @@ const rclcpp_lifecycle::State & System::cleanup()
   std::unique_lock<std::recursive_mutex> lock(system_mutex_);
   if (impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE)
   {
+    impl_->set_lifecycle_state(rclcpp_lifecycle::State(
+      lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED,
+      lifecycle_state_names::UNCONFIGURED));
     switch (impl_->on_cleanup(impl_->get_lifecycle_state()))
     {
       case CallbackReturn::SUCCESS:
-        impl_->set_lifecycle_state(rclcpp_lifecycle::State(
-          lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED,
-          lifecycle_state_names::UNCONFIGURED));
         break;
       case CallbackReturn::FAILURE:
       case CallbackReturn::ERROR: