From a86ce63d2a40dabd88bfae6ecc712a796434099f Mon Sep 17 00:00:00 2001
From: shalenikol <shaniks77s@gmail.com>
Date: Wed, 9 Apr 2025 12:41:05 +0300
Subject: [PATCH] updating BT for queue work

---
 rbs_mill_assist/bt/CMakeLists.txt             |  6 +++
 rbs_mill_assist/bt/plugins/takeon_task.cpp    | 54 +++++++++++++++++++
 rbs_mill_assist/bt/plugins/task_completed.cpp | 40 ++++++++++++++
 rbs_mill_assist/bt/xmls/MainTree.xml          | 24 ++++++---
 4 files changed, 118 insertions(+), 6 deletions(-)
 create mode 100644 rbs_mill_assist/bt/plugins/takeon_task.cpp
 create mode 100644 rbs_mill_assist/bt/plugins/task_completed.cpp

diff --git a/rbs_mill_assist/bt/CMakeLists.txt b/rbs_mill_assist/bt/CMakeLists.txt
index e038ae5..6b5fd57 100644
--- a/rbs_mill_assist/bt/CMakeLists.txt
+++ b/rbs_mill_assist/bt/CMakeLists.txt
@@ -36,6 +36,12 @@ list(APPEND plugin_libs is_in_pose)
 add_library(twist_command_with_condition SHARED plugins/twist_cmd.cpp)
 list(APPEND plugin_libs twist_command_with_condition)
 
+add_library(takeon_task SHARED plugins/takeon_task.cpp)
+list(APPEND plugin_libs takeon_task)
+
+add_library(task_completed SHARED plugins/task_completed.cpp)
+list(APPEND plugin_libs task_completed)
+
 foreach(bt_plugin ${plugin_libs})
     ament_target_dependencies(${bt_plugin} ${dependencies})
     target_compile_definitions(${bt_plugin} PRIVATE BT_PLUGIN_EXPORT)
diff --git a/rbs_mill_assist/bt/plugins/takeon_task.cpp b/rbs_mill_assist/bt/plugins/takeon_task.cpp
new file mode 100644
index 0000000..844ee92
--- /dev/null
+++ b/rbs_mill_assist/bt/plugins/takeon_task.cpp
@@ -0,0 +1,54 @@
+#include "nlohmann/json.hpp"
+#include "behaviortree_ros2/bt_service_node.hpp"
+#include "behaviortree_ros2/plugins.hpp"
+#include "rbs_utils_interfaces/srv/task_from_queue.hpp"
+#include <string>
+#include <sstream>
+#include <iostream>
+
+using TaskFromQueue = rbs_utils_interfaces::srv::TaskFromQueue;
+using namespace BT;
+
+class TakeonTask : public RosServiceNode<TaskFromQueue> {
+public:
+  TakeonTask(const std::string &name, const NodeConfig &conf, const RosNodeParams &params)
+      : RosServiceNode<TaskFromQueue>(name, conf, params) {
+
+    RCLCPP_INFO(this->logger(), "Starting TakeonTask");
+  }
+
+  static PortsList providedPorts() {
+    return providedBasicPorts(
+        {OutputPort<std::string>("from_pose")});
+  }
+
+  bool setRequest(Request::SharedPtr &request) override {
+    RCLCPP_INFO(this->logger(), "[%s] Starting send request for: [%s]",
+                name().c_str(), this->service_name_.c_str());
+    request->mode = "takeon";
+    return true;
+  }
+
+  NodeStatus onResponseReceived(const Response::SharedPtr &response) override {
+    if (!response->ok) {
+      RCLCPP_WARN(this->node_.lock()->get_logger(),
+                   "[%s] No tasks in queue", name().c_str());
+      return NodeStatus::FAILURE;
+    }
+    auto json_task = nlohmann::json::parse(response->task);
+    int TaskId = json_task["TaskId"];
+    int slot = json_task["obj_id"];
+
+    std::ostringstream oss;
+    oss << "slot_" << slot;
+    const char *x = oss.str().c_str();
+
+    setOutput("from_pose", x);
+
+    RCLCPP_INFO(this->node_.lock()->get_logger(),
+                "[%s] Task takeon: Id = %d, slot: %d", name().c_str(), TaskId, slot);
+    return NodeStatus::SUCCESS;
+  }
+};
+
+CreateRosNodePlugin(TakeonTask, "TakeonTask");
diff --git a/rbs_mill_assist/bt/plugins/task_completed.cpp b/rbs_mill_assist/bt/plugins/task_completed.cpp
new file mode 100644
index 0000000..d94c7b6
--- /dev/null
+++ b/rbs_mill_assist/bt/plugins/task_completed.cpp
@@ -0,0 +1,40 @@
+#include "behaviortree_ros2/bt_service_node.hpp"
+#include "behaviortree_ros2/plugins.hpp"
+#include "rbs_utils_interfaces/srv/task_from_queue.hpp"
+
+using TaskFromQueue = rbs_utils_interfaces::srv::TaskFromQueue;
+using namespace BT;
+
+class TaskCompleted : public RosServiceNode<TaskFromQueue> {
+public:
+TaskCompleted(const std::string &name, const NodeConfig &conf, const RosNodeParams &params)
+      : RosServiceNode<TaskFromQueue>(name, conf, params) {
+
+    // RCLCPP_INFO(this->logger(), "Starting TakeonTask");
+  }
+
+  static PortsList providedPorts() {
+    return providedBasicPorts({});
+  }
+
+  bool setRequest(Request::SharedPtr &request) override {
+    RCLCPP_INFO(this->logger(), "[%s] Starting send request for: [%s]",
+                name().c_str(), this->service_name_.c_str());
+    request->mode = "completed";
+    return true;
+  }
+
+  NodeStatus onResponseReceived(const Response::SharedPtr &response) override {
+    if (!response->ok) {
+      RCLCPP_WARN(this->node_.lock()->get_logger(),
+                   "[%s] Not completed", name().c_str());
+      return NodeStatus::FAILURE;
+    }
+
+    RCLCPP_INFO(this->node_.lock()->get_logger(),
+                "[%s] Task completed", name().c_str());
+    return NodeStatus::SUCCESS;
+  }
+};
+
+CreateRosNodePlugin(TaskCompleted, "TaskCompleted");
diff --git a/rbs_mill_assist/bt/xmls/MainTree.xml b/rbs_mill_assist/bt/xmls/MainTree.xml
index bf9987b..e3f7fd9 100644
--- a/rbs_mill_assist/bt/xmls/MainTree.xml
+++ b/rbs_mill_assist/bt/xmls/MainTree.xml
@@ -3,14 +3,21 @@
   <BehaviorTree ID="Main">
 
     <Sequence>
-      <!-- <Action ID="GetTask" name="" data="{task}" num_cycles="{num_cycles}"/> -->
+      <!-- <Action ID="AddTask" tasks_csv="..." service_name="queue/add_tasks"/> -->
 
       <Fallback>
         <Repeat num_cycles="1">
           <Sequence>
             <!-- Preparing -->
 
-            <Action ID="GetSlotGraspPoses" from_pose="slot_1" 
+            <Action ID="TakeonTask" from_pose="{from_pose}" service_name="queue/takeon_task"/>
+            <!-- <ReactiveFallback>
+              <Action ID="TakeonTask" service_name="queue/takeon_task" from_pose="{from_pose}"/>
+              <Delay delay_msec=1000>
+              </Delay>
+            </ReactiveFallback> -->
+
+            <Action ID="GetSlotGraspPoses" from_pose="{from_pose}" 
               relative_to="base_link"
               pregrasp="{pregrasp}"
               grasp="{grasp}"
@@ -73,6 +80,8 @@
               postgrasp="{postgrasp}"
             />
 
+            <Action ID="TaskCompleted" service_name="queue/task_completed"/>
+
             <!-- Ending -->
           </Sequence>
         </Repeat>
@@ -103,10 +112,13 @@
 
     <Action ID="NotifyOperator" editable="true" />
 
-    <Action ID="GetTask" editable="true">
-      <input_port name="name"/> 
-      <input_port name="data"/>
-      <input_port name="num_cycles"/> 
+    <Action ID="TakeonTask" editable="true">
+      <input_port name="from_pose"/> 
+      <input_port name="service_name"/>
+    </Action>
+
+    <Action ID="TaskCompleted" editable="true">
+      <input_port name="service_name"/>
     </Action>
 
     <Action ID="SendTask" editable="true" />