Add Non-Blocking try_get_params Function for Real-Time Control Systems (

#205)
PickNikRobotics · Jun 24, 2024 · 138e501 · 138e501
1 parent 4ad23ba
commit 138e501
Show file tree

Hide file tree

Showing 2 changed files with 24 additions and 4 deletions.
diff --git a/example/test/example_test_gtest.cpp b/example/test/example_test_gtest.cpp
@@ -40,17 +40,17 @@ class ExampleTest : public ::testing::Test {
   void SetUp() {
     example_test_node_ = std::make_shared<rclcpp::Node>("example_test_node");
 
-    std::shared_ptr<admittance_controller::ParamListener> param_listener =
-        std::make_shared<admittance_controller::ParamListener>(
-            example_test_node_->get_node_parameters_interface());
-    params_ = param_listener->get_params();
+    param_listener_ = std::make_shared<admittance_controller::ParamListener>(
+        example_test_node_->get_node_parameters_interface());
+    params_ = param_listener_->get_params();
   }
 
   void TearDown() { example_test_node_.reset(); }
 
  protected:
   std::shared_ptr<rclcpp::Node> example_test_node_;
   admittance_controller::Params params_;
+  std::shared_ptr<admittance_controller::ParamListener> param_listener_;
 };
 
 TEST_F(ExampleTest, check_parameters) {
@@ -64,6 +64,15 @@ TEST_F(ExampleTest, check_parameters) {
   ASSERT_EQ(params_.ft_sensor.filter_coefficient, 0.1);
 }
 
+TEST_F(ExampleTest, try_get_params) {
+  ASSERT_TRUE(param_listener_->try_get_params(params_));
+
+  const rclcpp ::Parameter new_param("interpolation_mode", "linear");
+  example_test_node_->set_parameter(new_param);
+  ASSERT_TRUE(param_listener_->try_get_params(params_));
+  ASSERT_EQ(params_.interpolation_mode, "linear");
+}
+
 int main(int argc, char** argv) {
   ::testing::InitGoogleTest(&argc, argv);
   rclcpp::init(argc, argv);

diff --git a/...ter_library_py/generate_parameter_library_py/jinja_templates/cpp/parameter_library_header b/...ter_library_py/generate_parameter_library_py/jinja_templates/cpp/parameter_library_header
@@ -123,6 +123,17 @@ struct StackParams {
       return params_;
     }
 
+    bool try_get_params(Params & params_in) const {
+      if (mutex_.try_lock()) {
+        if (const bool is_old = params_in.__stamp != params_.__stamp; is_old) {
+          params_in = params_;
+        }
+        mutex_.unlock();
+        return true;
+      }
+      return false;
+    }
+
     bool is_old(Params const& other) const {
       std::lock_guard<std::mutex> lock(mutex_);
       return params_.__stamp != other.__stamp;