[RM] Execute error callback of component on returning ERROR or with…

… exception (ros-controls#1730)
StoglRobotics-forks · Sep 11, 2024 · 111321c · 111321c
1 parent aefbad3
commit 111321c
Show file tree

Hide file tree

Showing 4 changed files with 11 additions and 3 deletions.
diff --git a/hardware_interface/src/actuator.cpp b/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()))
  {

diff --git a/hardware_interface/src/resource_manager.cpp b/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());

diff --git a/hardware_interface/src/sensor.cpp b/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()))
  {

diff --git a/hardware_interface/src/system.cpp b/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()))
  {