Fix hardware component unconfiguration

Author: sahand-ghaffari-ocado

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)
   {
-    switch (impl_->on_cleanup(impl_->get_lifecycle_state()))
+    impl_->set_state(rclcpp_lifecycle::State(
+      lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED,
+      lifecycle_state_names::UNCONFIGURED));
+    switch (impl_->on_cleanup(impl_->get_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:

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)
   {
-    switch (impl_->on_cleanup(impl_->get_lifecycle_state()))
+    impl_->set_state(rclcpp_lifecycle::State(
+      lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED,
+      lifecycle_state_names::UNCONFIGURED));
+    switch (impl_->on_cleanup(impl_->get_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:

hardware_interface/src/system.cpp

@@ -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)
   {
-    switch (impl_->on_cleanup(impl_->get_lifecycle_state()))
+    impl_->set_state(rclcpp_lifecycle::State(
+      lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED,
+      lifecycle_state_names::UNCONFIGURED));
+    switch (impl_->on_cleanup(impl_->get_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: