fix multiple system rise cleanup

- fix system components - fix other components
ros-controls · Dec 12, 2023 · 63b725d · 63b725d
1 parent cf405bc
commit 63b725d
Show file tree

Hide file tree

Showing 4 changed files with 31 additions and 6 deletions.
diff --git a/hardware_interface/src/actuator.cpp b/hardware_interface/src/actuator.cpp
@@ -220,6 +220,11 @@ return_type Actuator::read(const rclcpp::Time & time, const rclcpp::Duration & p
 {
   // TODO(destogl): discuss what should be default return value, e.g., "NOT_EXECUTED"
   return_type result = return_type::ERROR;
+  if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED)
+  {
+    return return_type::OK;
+  }
+
   if (
     impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE ||
     impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)
@@ -237,6 +242,11 @@ return_type Actuator::write(const rclcpp::Time & time, const rclcpp::Duration &
 {
   // TODO(destogl): discuss what should be default return value, e.g., "NOT_EXECUTED"
   return_type result = return_type::ERROR;
+  if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED)
+  {
+    return return_type::OK;
+  }
+
   if (
     impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE ||
     impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)

diff --git a/hardware_interface/src/sensor.cpp b/hardware_interface/src/sensor.cpp
@@ -197,6 +197,11 @@ return_type Sensor::read(const rclcpp::Time & time, const rclcpp::Duration & per
 {
   // TODO(destogl): discuss what should be default return value, e.g., "NOT_EXECUTED"
   return_type result = return_type::ERROR;
+  if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED)
+  {
+    return return_type::OK;
+  }
+
   if (
     impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE ||
     impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)

diff --git a/hardware_interface/src/system.cpp b/hardware_interface/src/system.cpp
@@ -216,6 +216,11 @@ return_type System::read(const rclcpp::Time & time, const rclcpp::Duration & per
 {
   // TODO(destogl): discuss what should be default return value, e.g., "NOT_EXECUTED"
   return_type result = return_type::ERROR;
+  if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED)
+  {
+    return return_type::OK;
+  }
+
   if (
     impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE ||
     impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)
@@ -233,6 +238,11 @@ return_type System::write(const rclcpp::Time & time, const rclcpp::Duration & pe
 {
   // TODO(destogl): discuss what should be default return value, e.g., "NOT_EXECUTED"
   return_type result = return_type::ERROR;
+  if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED)
+  {
+    return return_type::OK;
+  }
+
   if (
     impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE ||
     impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)

diff --git a/hardware_interface/test/test_component_interfaces.cpp b/hardware_interface/test/test_component_interfaces.cpp
@@ -446,17 +446,17 @@ TEST(TestComponentInterfaces, dummy_actuator)
 
   double velocity_value = 1.0;
   command_interfaces[0].set_value(velocity_value);  // velocity
-  ASSERT_EQ(hardware_interface::return_type::ERROR, actuator_hw.write(TIME, PERIOD));
+  ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD));
 
   // Noting should change because it is UNCONFIGURED
   for (auto step = 0u; step < 10; ++step)
   {
-    ASSERT_EQ(hardware_interface::return_type::ERROR, actuator_hw.read(TIME, PERIOD));
+    ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD));
 
     ASSERT_TRUE(std::isnan(state_interfaces[0].get_value()));  // position value
     ASSERT_TRUE(std::isnan(state_interfaces[1].get_value()));  // velocity
 
-    ASSERT_EQ(hardware_interface::return_type::ERROR, actuator_hw.write(TIME, PERIOD));
+    ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD));
   }
 
   state = actuator_hw.configure();
@@ -587,12 +587,12 @@ TEST(TestComponentInterfaces, dummy_system)
   command_interfaces[0].set_value(velocity_value);  // velocity
   command_interfaces[1].set_value(velocity_value);  // velocity
   command_interfaces[2].set_value(velocity_value);  // velocity
-  ASSERT_EQ(hardware_interface::return_type::ERROR, system_hw.write(TIME, PERIOD));
+  ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD));
 
   // Noting should change because it is UNCONFIGURED
   for (auto step = 0u; step < 10; ++step)
   {
-    ASSERT_EQ(hardware_interface::return_type::ERROR, system_hw.read(TIME, PERIOD));
+    ASSERT_EQ(hardware_interface::return_type::OK, system_hw.read(TIME, PERIOD));
 
     ASSERT_TRUE(std::isnan(state_interfaces[0].get_value()));  // position value
     ASSERT_TRUE(std::isnan(state_interfaces[1].get_value()));  // velocity
@@ -601,7 +601,7 @@ TEST(TestComponentInterfaces, dummy_system)
     ASSERT_TRUE(std::isnan(state_interfaces[4].get_value()));  // position value
     ASSERT_TRUE(std::isnan(state_interfaces[5].get_value()));  // velocity
 
-    ASSERT_EQ(hardware_interface::return_type::ERROR, system_hw.write(TIME, PERIOD));
+    ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD));
   }
 
   state = system_hw.configure();