Merge branch 'main' into 77-cartesian-controllers

rbs_bt_executor/CMakeLists.txt

@@ -71,6 +71,9 @@ list(APPEND plugin_libs rbs_assemble_process_state)
 add_library(rbs_pose_estimation SHARED src/PoseEstimation.cpp)
 list(APPEND plugin_libs rbs_pose_estimation)
 
+add_library(rbs_object_detection SHARED src/ObjectDetection.cpp)
+list(APPEND plugin_libs rbs_object_detection)
+
 add_library(rbs_env_manager_starter SHARED src/EnvManager.cpp)
 list(APPEND plugin_libs rbs_env_manager_starter)
 

rbs_bt_executor/bt_trees/test_objdet_cleanup.xml

@@ -0,0 +1,17 @@
+<?xml version="1.0"?>
+<root main_tree_to_execute="ObjDetection">
+    <BehaviorTree ID="ObjDetection">
+        <Sequence>
+            <Action ID="ObjectDetection" 
+            SettingPath=""
+            ReqKind = "deactivate"
+            server_name="/object_detection/change_state" 
+            server_timeout="1000"/>
+            <Action ID="ObjectDetection" 
+            SettingPath=""
+            ReqKind = "cleanup"
+            server_name="/object_detection/change_state" 
+            server_timeout="1000"/>
+        </Sequence>
+    </BehaviorTree>
+</root>

rbs_bt_executor/bt_trees/test_objdet_run.xml

@@ -0,0 +1,17 @@
+<?xml version="1.0"?>
+<root main_tree_to_execute="ObjDetection">
+    <BehaviorTree ID="ObjDetection">
+        <Sequence>
+            <Action ID="ObjectDetection" 
+            SettingPath="!/home/shalenikol/0yolov8/test.json"
+            ReqKind = "configure"
+            server_name="/object_detection/change_state" 
+            server_timeout="1000"/>
+            <Action ID="ObjectDetection" 
+            SettingPath=""
+            ReqKind = "activate"
+            server_name="/object_detection/change_state" 
+            server_timeout="1000"/>
+        </Sequence>
+    </BehaviorTree>
+</root>

rbs_bt_executor/config/params.yaml

@@ -7,6 +7,7 @@ bt_engine:
             "rbs_skill_move_joint_state",
             "rbs_add_planning_scene_object",
             "rbs_pose_estimation",
+            "rbs_object_detection",
             "rbs_env_manager_starter",
             "rbs_skill_move_topose_array_bt_action_client"
             ]

rbs_bt_executor/src/ObjectDetection.cpp

@@ -0,0 +1,119 @@
+#include <ament_index_cpp/get_package_share_directory.hpp>
+#include <behavior_tree/BtService.hpp>
+
+#include <behaviortree_cpp_v3/basic_types.h>
+#include <rclcpp/logging.hpp>
+#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"
+
+using ObjDetectionSrv = lifecycle_msgs::srv::ChangeState;
+class ObjectDetection : public BtService<ObjDetectionSrv> {
+public:
+  ObjectDetection(const std::string &name, const BT::NodeConfiguration &config)
+      : BtService<ObjDetectionSrv>(name, config) {
+    RCLCPP_INFO_STREAM(_node->get_logger(), "Start node.");
+
+    _set_params_client = std::make_shared<rclcpp::AsyncParametersClient>(
+        _node, "/object_detection");
+
+    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_WARN(_node->get_logger(),
+                  "service not available, waiting again...");
+    }
+
+    _setting_path = getInput<std::string>("SettingPath").value();
+  }
+
+  ObjDetectionSrv::Request::SharedPtr populate_request() {
+    auto request = std::make_shared<ObjDetectionSrv::Request>();
+    _req_type = getInput<std::string>("ReqKind").value();
+    request->set__transition(transition_event(_req_type));
+    return request;
+  }
+
+  BT::NodeStatus
+  handle_response(const ObjDetectionSrv::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>("SettingPath"),
+    });
+  }
+
+private:
+  uint8_t transition_id_{};
+  std::string _setting_path{};
+//   std::string _model_path{};
+  std::string _req_type{};
+  std::shared_ptr<rclcpp::AsyncParametersClient> _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_str_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_str_param() {
+    RCLCPP_INFO_STREAM(_node->get_logger(),
+                       "Set string parameter: <" + _setting_path + ">");
+
+    std::vector<rclcpp::Parameter> _parameters{
+        rclcpp::Parameter("setting_path", _setting_path)};
+    _set_params_client->set_parameters(_parameters);
+  }
+
+//   void set_all_param() {
+//     auto _package_name =
+//         ament_index_cpp::get_package_share_directory("rbs_perception");
+//     _model_path = build_model_path(_setting_path, _package_name);
+//     RCLCPP_INFO_STREAM(_node->get_logger(), _model_path);
+
+//     std::vector<rclcpp::Parameter> _parameters{
+//         rclcpp::Parameter("setting_path", _setting_path)};
+//     _set_params_client->set_parameters(_parameters);
+//   }
+};
+
+#include "behaviortree_cpp_v3/bt_factory.h"
+
+BT_REGISTER_NODES(factory) {
+  factory.registerNodeType<ObjectDetection>("ObjectDetection");
+}