[RM] Execute error callback of component on returning ERROR or with exception (ros-controls#1730)

Author: saikishor

hardware_interface/src/actuator.cpp

@@ -183,7 +183,9 @@ const rclcpp_lifecycle::State & Actuator::deactivate()
 const rclcpp_lifecycle::State & Actuator::error()
 {
   std::unique_lock<std::recursive_mutex> lock(actuators_mutex_);
-  if (impl_->get_lifecycle_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN)
+  if (
+    impl_->get_lifecycle_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN &&
+    impl_->get_lifecycle_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED)
   {
     switch (impl_->on_error(impl_->get_lifecycle_state()))
     {

hardware_interface/src/resource_manager.cpp

@@ -1808,6 +1808,7 @@ HardwareReadWriteStatus ResourceManager::read(
       }
       if (ret_val == return_type::ERROR)
       {
+        component.error();
         read_write_status.ok = false;
         read_write_status.failed_hardware_names.push_back(component.get_name());
         resource_storage_->remove_all_hardware_interfaces_from_available_list(component.get_name());
@@ -1868,6 +1869,7 @@ HardwareReadWriteStatus ResourceManager::write(
       }
       if (ret_val == return_type::ERROR)
       {
+        component.error();
         read_write_status.ok = false;
         read_write_status.failed_hardware_names.push_back(component.get_name());
         resource_storage_->remove_all_hardware_interfaces_from_available_list(component.get_name());

hardware_interface/src/sensor.cpp

@@ -182,7 +182,9 @@ const rclcpp_lifecycle::State & Sensor::deactivate()
 const rclcpp_lifecycle::State & Sensor::error()
 {
   std::unique_lock<std::recursive_mutex> lock(sensors_mutex_);
-  if (impl_->get_lifecycle_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN)
+  if (
+    impl_->get_lifecycle_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN &&
+    impl_->get_lifecycle_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED)
   {
     switch (impl_->on_error(impl_->get_lifecycle_state()))
     {

hardware_interface/src/system.cpp

@@ -181,7 +181,9 @@ const rclcpp_lifecycle::State & System::deactivate()
 const rclcpp_lifecycle::State & System::error()
 {
   std::unique_lock<std::recursive_mutex> lock(system_mutex_);
-  if (impl_->get_lifecycle_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN)
+  if (
+    impl_->get_lifecycle_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN &&
+    impl_->get_lifecycle_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED)
   {
     switch (impl_->on_error(impl_->get_lifecycle_state()))
     {