finishing the incomplete test when_ref_timeout_zero_for_reference_cal…

…lback_expect_reference_msg_being_used_only_once and extending its relavant code in controllers
StoglRobotics · Mar 29, 2023 · 9692b17 · 9692b17
1 parent 74b4d11
commit 9692b17
Show file tree

Hide file tree

Showing 4 changed files with 25 additions and 0 deletions.
diff --git a/templates/ros2_control/controller/dummy_chainable_controller.cpp b/templates/ros2_control/controller/dummy_chainable_controller.cpp
@@ -252,13 +252,17 @@ controller_interface::return_type DummyClassName::update_reference_from_subscrib
         reference_interfaces_[i] = (*current_ref)->displacements[i];
         if (ref_timeout_ == rclcpp::Duration::from_seconds(0)) {
           (*current_ref)->displacements[i] = std::numeric_limits<double>::quiet_NaN();
+          (*current_ref)->velocities[i] = std::numeric_limits<double>::quiet_NaN();
+          (*current_ref)->duration = std::numeric_limits<double>::quiet_NaN();
         }
       }
     } else {
       if (!std::isnan((*current_ref)->displacements[i])) {
         reference_interfaces_[i] = 0.0;
 
         (*current_ref)->displacements[i] = std::numeric_limits<double>::quiet_NaN();
+        (*current_ref)->velocities[i] = std::numeric_limits<double>::quiet_NaN();
+        (*current_ref)->duration = std::numeric_limits<double>::quiet_NaN();
       }
     }
   }
@@ -268,6 +272,7 @@ controller_interface::return_type DummyClassName::update_reference_from_subscrib
 controller_interface::return_type DummyClassName::update_and_write_commands(
   const rclcpp::Time & time, const rclcpp::Duration & /*period*/)
 {
+  auto current_ref = input_ref_.readFromRT();
   // TODO(anyone): depending on number of interfaces, use definitions, e.g., `CMD_MY_ITFS`,
   // instead of a loop
   for (size_t i = 0; i < command_interfaces_.size(); ++i) {
@@ -279,6 +284,9 @@ controller_interface::return_type DummyClassName::update_and_write_commands(
       command_interfaces_[i].set_value(reference_interfaces_[i]);
       if (ref_timeout_ == rclcpp::Duration::from_seconds(0)) {
         reference_interfaces_[i] = std::numeric_limits<double>::quiet_NaN();
+        (*current_ref)->displacements[i] = std::numeric_limits<double>::quiet_NaN();
+        (*current_ref)->velocities[i] = std::numeric_limits<double>::quiet_NaN();
+        (*current_ref)->duration = std::numeric_limits<double>::quiet_NaN();
       }
     }
   }

diff --git a/templates/ros2_control/controller/dummy_controller.cpp b/templates/ros2_control/controller/dummy_controller.cpp
@@ -235,11 +235,15 @@ controller_interface::return_type DummyClassName::update(
         command_interfaces_[i].set_value((*current_ref)->displacements[i]);
         if (ref_timeout_ == rclcpp::Duration::from_seconds(0)) {
           (*current_ref)->displacements[i] = std::numeric_limits<double>::quiet_NaN();
+          (*current_ref)->velocities[i] = std::numeric_limits<double>::quiet_NaN();
+          (*current_ref)->duration = std::numeric_limits<double>::quiet_NaN();
         }
       }
     } else {
       command_interfaces_[i].set_value(0.0);
       (*current_ref)->displacements[i] = std::numeric_limits<double>::quiet_NaN();
+      (*current_ref)->velocities[i] = std::numeric_limits<double>::quiet_NaN();
+      (*current_ref)->duration = std::numeric_limits<double>::quiet_NaN();
     }
   }
 

diff --git a/templates/ros2_control/controller/test_dummy_chainable_controller.cpp b/templates/ros2_control/controller/test_dummy_chainable_controller.cpp
@@ -530,6 +530,13 @@ TEST_F(
   EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->displacements[0], 0.45);
   EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->velocities[0], 0.0);
   EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->duration, 1.25);
+  ASSERT_EQ(
+    controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)),
+    controller_interface::return_type::OK);
+  EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->displacements[0]));
+  EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->velocities[0]));
+  EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->duration));
+
 }
 
 int main(int argc, char ** argv)

diff --git a/templates/ros2_control/controller/test_dummy_controller.cpp b/templates/ros2_control/controller/test_dummy_controller.cpp
@@ -408,6 +408,12 @@ TEST_F(
   EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->displacements[0], 0.45);
   EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->velocities[0], 0.0);
   EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->duration, 1.25);
+  ASSERT_EQ(
+    controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)),
+    controller_interface::return_type::OK);
+  EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->displacements[0]));
+  EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->velocities[0]));
+  EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->duration));
 }
 
 int main(int argc, char ** argv)