Resolve "Добавить параметры в конфиг pose estimation узла"

rbs_bt_executor/CMakeLists.txt

@@ -19,6 +19,8 @@ find_package(rbs_skill_interfaces REQUIRED)
 find_package(behavior_tree REQUIRED)
 find_package(control_msgs REQUIRED)
 find_package(component_interfaces REQUIRED)
+find_package(lifecycle_msgs REQUIRED)
+find_package(rcl_interfaces REQUIRED)
 
 if (NOT CMAKE_CXX_STANDARD)
     set(CMAKE_CXX_STANDARD 17)
@@ -39,6 +41,8 @@ set(dependencies
     std_msgs
     control_msgs
     component_interfaces
+    lifecycle_msgs
+    rcl_interfaces
 )
 
 include_directories(include)
@@ -61,8 +65,8 @@ list(APPEND plugin_libs rbs_add_planning_scene_object)
 add_library(rbs_assemble_process_state SHARED src/AssembleProcessState.cpp)
 list(APPEND plugin_libs rbs_assemble_process_state)
 
-add_library(rbs_detect_object SHARED src/DetectObject.cpp)
-list(APPEND plugin_libs rbs_detect_object)
+add_library(rbs_pose_estimation SHARED src/PoseEstimation.cpp)
+list(APPEND plugin_libs rbs_pose_estimation)
 
 foreach(bt_plugin ${plugin_libs})
     ament_target_dependencies(${bt_plugin} ${dependencies})

rbs_bt_executor/src/DetectObject.cpp

@@ -1,51 +0,0 @@
-#include <string>
-#include <behavior_tree/BtService.hpp>
-#include <rclcpp/rclcpp.hpp>
-
-#include <component_interfaces/srv/detect_object.hpp>
-
-using DetectObjectSrv = component_interfaces::srv::DetectObject;
-class DetectObject: public BtService<DetectObjectSrv>
-{
-public:
-  DetectObject(const std::string &name, const BT::NodeConfiguration &config)
-  : BtService<DetectObjectSrv>(name, config)
-  {
-    RCLCPP_INFO_STREAM(_node->get_logger(), "Start node.");
-  }
-
-  DetectObjectSrv::Request::SharedPtr populate_request()
-  {
-    auto request = std::make_shared<DetectObjectSrv::Request>();
-    auto req_kind = getInput<std::string>("ReqKind").value();
-    if (req_kind == "DETECT")
-      request->req_kind = 0;
-    else if (req_kind == "FOLLOW")
-      request->req_kind = 1;
-    else
-      request->req_kind = 2;
-    request->object_name = getInput<std::string>("ObjectName").value();
-    return request;
-  }
-
-  BT::NodeStatus handle_response(DetectObjectSrv::Response::SharedPtr response)
-  {
-    return response->call_status? BT::NodeStatus::SUCCESS: BT::NodeStatus::FAILURE;
-  }
-
-  static BT::PortsList providedPorts()
-  {
-    return providedBasicPorts({
-      BT::InputPort<std::string>("ReqKind"),
-      BT::InputPort<std::string>("ObjectName"),
-    });
-  }
-};
-
-#include "behaviortree_cpp_v3/bt_factory.h"
-
-BT_REGISTER_NODES(factory)
-{
-  factory.registerNodeType<DetectObject>("DetectObject");
-}
-

rbs_bt_executor/src/PoseEstimation.cpp

@@ -0,0 +1,132 @@
+#include <ament_index_cpp/get_package_share_directory.hpp>
+#include <behavior_tree/BtService.hpp>
+
+#include <behaviortree_cpp_v3/basic_types.h>
+#include <string>
+
+#include "rcl_interfaces/msg/parameter.hpp"
+#include "rcl_interfaces/srv/set_parameters.hpp"
+
+#include "lifecycle_msgs/msg/transition.hpp"
+#include "lifecycle_msgs/srv/change_state.hpp"
+// #include <component_interfaces/srv/detect_object.hpp>
+
+using PoseEstimationSrv = lifecycle_msgs::srv::ChangeState;
+class PoseEstimation : public BtService<PoseEstimationSrv> {
+public:
+  PoseEstimation(const std::string &name, const BT::NodeConfiguration &config)
+      : 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");
+
+    _model_name = getInput<std::string>("ObjectName").value();
+  }
+
+  PoseEstimationSrv::Request::SharedPtr populate_request() {
+    auto request = std::make_shared<PoseEstimationSrv::Request>();
+    _req_type = getInput<std::string>("ReqKind").value();
+    request->set__transition(transition_event(_req_type));
+    return request;
+  }
+
+  BT::NodeStatus
+  handle_response(const PoseEstimationSrv::Response::SharedPtr response) {
+    if (response->success) {
+      return BT::NodeStatus::SUCCESS;
+    }
+    return BT::NodeStatus::FAILURE;
+  }
+
+  static BT::PortsList providedPorts() {
+    return providedBasicPorts({
+        BT::InputPort<std::string>("ReqKind"),
+        BT::InputPort<std::string>("ObjectName"),
+        BT::InputPort<std::string>("ObjectPath"),
+    });
+  }
+
+private:
+  uint8_t transition_id_{};
+  std::string _model_name{};
+  std::string _model_path{};
+  std::string _req_type;
+  rclcpp::Client<rcl_interfaces::srv::SetParameters>::SharedPtr
+      _set_params_client;
+  std::vector<rcl_interfaces::msg::Parameter> _params;
+  rcl_interfaces::msg::Parameter _param;
+
+  lifecycle_msgs::msg::Transition
+  transition_event(const std::string &req_type) {
+    lifecycle_msgs::msg::Transition translation{};
+    if (req_type == "configure") {
+      set_mesh_param();
+      transition_id_ = lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE;
+    } else if (req_type == "activate") {
+      transition_id_ = lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE;
+    } else if (req_type == "deactivate") {
+      transition_id_ = lifecycle_msgs::msg::Transition::TRANSITION_DEACTIVATE;
+    } else if (req_type == "cleanup") {
+      transition_id_ = lifecycle_msgs::msg::Transition::TRANSITION_CLEANUP;
+    }
+    translation.set__id(transition_id_);
+    return translation;
+  }
+
+  inline std::string build_model_path(const std::string &model_name,
+                                      const std::string &package_path) {
+    return package_path + "/config/" + model_name + ".ply";
+  }
+
+  inline std::string build_model_path(const std::string &model_path) {
+    return model_path;
+  }
+
+  void set_mesh_param() {
+    auto _package_name =
+        ament_index_cpp::get_package_share_directory("rbs_perception");
+    _model_path = build_model_path(_model_name, _package_name);
+
+    auto param_request =
+        std::make_shared<rcl_interfaces::srv::SetParameters::Request>();
+    rcl_interfaces::msg::ParameterValue val;
+    val.set__string_value(_model_path);
+    _param.name = "mesh_path";
+    _param.value = val;
+    _params.push_back(_param);
+    param_request->set__parameters(_params);
+
+    while (!_set_params_client->wait_for_service(1s)) {
+      if (!rclcpp::ok()) {
+        RCLCPP_ERROR(_node->get_logger(),
+                     "Interrupted while waiting for the service. Exiting.");
+        break;
+      }
+      RCLCPP_INFO(_node->get_logger(),
+                  "service not available, waiting again...");
+    }
+    auto response = send_request(_node, _set_params_client, param_request);
+  }
+
+  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);
+    // Wait for the result.
+    if (rclcpp::spin_until_future_complete(node, result) ==
+        rclcpp::FutureReturnCode::SUCCESS) {
+      return result.get();
+    } else {
+      return NULL;
+    }
+  }
+};
+
+#include "behaviortree_cpp_v3/bt_factory.h"
+
+BT_REGISTER_NODES(factory) {
+  factory.registerNodeType<PoseEstimation>("PoseEstimation");
+}