From c5f37c587fcaa7bb99c7b05e23bc50771e178804 Mon Sep 17 00:00:00 2001
From: shalenikol <shaniks77s@gmail.com>
Date: Wed, 15 Nov 2023 21:07:32 +0300
Subject: [PATCH] set 'mesh_path' from BT node

---
 rbs_bt_executor/src/PoseEstimation.cpp        | 41 +++----------------
 .../scripts/pose_estimation_lifecycle.py      |  5 ++-
 2 files changed, 10 insertions(+), 36 deletions(-)

diff --git a/rbs_bt_executor/src/PoseEstimation.cpp b/rbs_bt_executor/src/PoseEstimation.cpp
index fee2fe2..0af9ddc 100644
--- a/rbs_bt_executor/src/PoseEstimation.cpp
+++ b/rbs_bt_executor/src/PoseEstimation.cpp
@@ -2,6 +2,7 @@
 #include <behavior_tree/BtService.hpp>
 
 #include <behaviortree_cpp_v3/basic_types.h>
+#include <rclcpp/logging.hpp>
 #include <string>
 
 #include "rcl_interfaces/msg/parameter.hpp"
@@ -17,9 +18,7 @@ public:
       : BtService<PoseEstimationSrv>(name, config) {
     RCLCPP_INFO_STREAM(_node->get_logger(), "Start node.");
 
-    _set_params_client =
-        this->_node->create_client<rcl_interfaces::srv::SetParameters>(
-            "/image_sub2/set_parameters");
+    _set_params_client = std::make_shared<rclcpp::AsyncParametersClient>(_node, "/image_sub2");
 
     while (!_set_params_client->wait_for_service(1s)) {
       if (!rclcpp::ok()) {
@@ -62,8 +61,7 @@ private:
   std::string _model_name{};
   std::string _model_path{};
   std::string _req_type{};
-  rclcpp::Client<rcl_interfaces::srv::SetParameters>::SharedPtr
-      _set_params_client;
+  std::shared_ptr<rclcpp::AsyncParametersClient> _set_params_client;
   std::vector<rcl_interfaces::msg::Parameter> _params;
   rcl_interfaces::msg::Parameter _param;
 
@@ -94,39 +92,12 @@ private:
   }
 
   void set_mesh_param() {
-    auto _package_name =
-        ament_index_cpp::get_package_share_directory("rbs_perception");
+    auto _package_name = ament_index_cpp::get_package_share_directory("rbs_perception");
     _model_path = build_model_path(_model_name, _package_name);
     RCLCPP_INFO_STREAM(_node->get_logger(), _model_path);
 
-    auto param_request =
-        std::make_shared<rcl_interfaces::srv::SetParameters::Request>();
-    rcl_interfaces::msg::ParameterValue model_path_param_value;
-    model_path_param_value.set__string_value(_model_path);
-    _param.name = "mesh_path";
-    _param.value = model_path_param_value;
-    _params.push_back(_param);
-    param_request->set__parameters(_params);
-
-    auto response = send_request(_node, _set_params_client, param_request);
-    RCLCPP_INFO(_node->get_logger(), "Response");
-
-    for (auto &tt : response->results) {
-      RCLCPP_INFO(_node->get_logger(), "%d \n", tt.successful);
-    }
-  }
-
-  rcl_interfaces::srv::SetParameters::Response::SharedPtr send_request(
-      rclcpp::Node::SharedPtr node,
-      rclcpp::Client<rcl_interfaces::srv::SetParameters>::SharedPtr client,
-      rcl_interfaces::srv::SetParameters::Request::SharedPtr request) {
-    auto result = client->async_send_request(request);
-    if (rclcpp::spin_until_future_complete(node, result) ==
-        rclcpp::FutureReturnCode::SUCCESS) {
-      return result.get();
-    } else {
-      return nullptr;
-    }
+    std::vector<rclcpp::Parameter> _parameters{rclcpp::Parameter("mesh_path", _model_path)};
+    _set_params_client->set_parameters(_parameters);
   }
 
 };
diff --git a/rbs_perception/scripts/pose_estimation_lifecycle.py b/rbs_perception/scripts/pose_estimation_lifecycle.py
index 6a5042c..f368b6e 100755
--- a/rbs_perception/scripts/pose_estimation_lifecycle.py
+++ b/rbs_perception/scripts/pose_estimation_lifecycle.py
@@ -82,8 +82,11 @@ class PoseEstimator(Node):
         self.mytemppath = Path(self.tmpdir) / "rbs_per"
         self.mytemppath.mkdir(exist_ok=True)
 
+        # for other nodes
+        kwargs["allow_undeclared_parameters"] = True
+        kwargs["automatically_declare_parameters_from_overrides"] = True
         super().__init__(self.nodeName, **kwargs)
-        self.declare_parameter("mesh_path", "")
+        self.declare_parameter("mesh_path", rclpy.Parameter.Type.STRING)
 
         # Initialize the transform broadcaster
         self.tf_broadcaster = TransformBroadcaster(self)