united rbs_interface with rbs_bt_executor

rbs_bt_executor/src/BTExec.cpp

@@ -0,0 +1,201 @@
+#include <behaviortree_cpp/tree_node.h>
+#include <behaviortree_ros2/plugins.hpp>
+#include <behaviortree_ros2/ros_node_params.hpp>
+#include <string>
+#include <fstream>
+#include "behaviortree_ros2/bt_service_node.hpp"
+#include "behaviortree_ros2/bt_action_node.hpp"
+#include "rbs_skill_interfaces/srv/rbs_bt.hpp"
+#include "rbs_skill_interfaces/action/rbs_bt.hpp"
+
+#define STR_ACTION   "do"
+#define STR_SID      "sid"
+#define STR_COMMAND  "command"
+#define NODE_NAME    "rbs_interface"
+#define SERVICE_NAME "rbs_interface_s"
+#define SERVER_NAME  "rbs_interface_a"
+#define FILE_BT      "bt.xml"
+
+template<typename T> std::string to_string(const T &t)
+{
+    std::stringstream ss;
+    ss << t;
+    return ss.str();
+}
+
+using namespace BT;
+using namespace std::chrono_literals;
+using RbsConditionSrv = rbs_skill_interfaces::srv::RbsBt;
+
+using RbsActionSrv = rbs_skill_interfaces::action::RbsBt;
+
+class RbsAction : public RosActionNode<RbsActionSrv> {
+public:
+  RbsAction(const std::string &name, const NodeConfig &conf, const RosNodeParams &params)
+      : RosActionNode<RbsActionSrv>(name, conf, params) {
+
+    m_action = getInput<std::string>(STR_ACTION).value();
+    m_command = getInput<std::string>(STR_COMMAND).value();
+    RCLCPP_INFO_STREAM(logger(), "Start node RbsAction: " + m_action + "/" + m_command);
+
+    m_client = std::make_shared<rclcpp::AsyncParametersClient>(node_.lock(), NODE_NAME);
+
+    while (!m_client->wait_for_service(1s)) {
+      if (!rclcpp::ok()) {
+        RCLCPP_ERROR(logger(), "Interrupted while waiting for the service. Exiting.");
+        break;
+      }
+      RCLCPP_WARN(logger(), NODE_NAME " service not available, waiting again...");
+    }
+
+  }
+
+  static BT::PortsList providedPorts() {
+    return providedBasicPorts({
+      BT::InputPort<std::string>(STR_SID),
+      BT::InputPort<std::string>(STR_ACTION),
+      BT::InputPort<std::string>(STR_COMMAND)
+      });
+  }
+
+  bool setGoal(RosActionNode::Goal& goal) override {
+    getInput(STR_ACTION, goal.action);
+    getInput(STR_COMMAND, goal.command);
+    getInput(STR_SID, goal.sid);
+    m_action = goal.action;
+    m_command = goal.command;
+
+    m_i++;
+    RCLCPP_INFO_STREAM(logger(), "setGoal: " + to_string(m_i) + ") " + goal.action + "/" + goal.command + " goal.sid = " + to_string(goal.sid));
+    return true;
+  }
+
+  NodeStatus onResultReceived(const WrappedResult& wr) override {
+    m_i++;
+    RCLCPP_INFO_STREAM(logger(), "onResultReceived: " + to_string(m_i) + ") " + m_action + "/" + m_command + " wr.result->ok - " + to_string(wr.result->ok));
+
+    if (!wr.result->ok) {
+      return NodeStatus::FAILURE;
+    }
+    return NodeStatus::SUCCESS;
+  }
+
+private:
+  int m_i{0};
+  std::string m_action{};
+  std::string m_command{};
+  std::shared_ptr<rclcpp::AsyncParametersClient> m_client;
+};
+
+// **********************************************************
+class RbsCondition : public RosServiceNode<RbsConditionSrv> {
+public:
+  RbsCondition(const std::string &name, const NodeConfig& conf, const RosNodeParams& params)
+      : RosServiceNode<RbsConditionSrv>(name, conf, params) {
+
+    m_action_name = getInput<std::string>(STR_ACTION).value();
+    m_command = getInput<std::string>(STR_COMMAND).value();
+    RCLCPP_INFO_STREAM(logger(), "Start node RbsCondition: " + m_action_name + "/" + m_command);
+
+    m_params_client = std::make_shared<rclcpp::AsyncParametersClient>(node_.lock(), NODE_NAME);
+
+    while (!m_params_client->wait_for_service(1s)) {
+      if (!rclcpp::ok()) {
+        RCLCPP_ERROR(logger(), "Interrupted while waiting for the service. Exiting.");
+        break;
+      }
+      RCLCPP_WARN(logger(), NODE_NAME " service not available, waiting again...");
+    }
+  }
+
+  bool setRequest(Request::SharedPtr &request) override {
+    request->action = m_action_name; 
+    request->command = m_command;
+    getInput(STR_SID, request->sid);
+
+    m_i++;
+    RCLCPP_INFO_STREAM(logger(), "setRequest: " + to_string(m_i) + ") " + m_action_name + "/" + m_command + " request->sid = " + to_string(request->sid));
+    return true;
+  }
+
+  NodeStatus onResponseReceived(const Response::SharedPtr &response) override {
+    m_i++;
+    RCLCPP_INFO_STREAM(logger(), "onResponseReceived: " + to_string(m_i) + ") " + m_action_name + "/" + m_command + " response->ok - " + to_string(response->ok));
+
+    if (!response->ok) {
+      return NodeStatus::FAILURE;
+    }
+    return NodeStatus::SUCCESS;
+  }
+
+  static PortsList providedPorts() {
+    return providedBasicPorts({
+        InputPort<std::string>(STR_SID),
+        InputPort<std::string>(STR_ACTION),
+        InputPort<std::string>(STR_COMMAND)
+    });
+  }
+
+private:
+  int m_i{0};
+  std::string m_action_name{};
+  std::string m_command{};
+  std::shared_ptr<rclcpp::AsyncParametersClient> m_params_client;
+};
+
+// class RbsCondition : public RosTopicSubNode {
+// }
+// Simple function that return a NodeStatus
+// BT::NodeStatus TestIf()
+// {
+//   std::string m_if = "if"; //getInput<std::string>("if").value();
+//   // std::cout << "[ Test: OK ]" << std::endl;
+  
+//   std::stringstream ss;
+//   ss << "[ Test: OK ] " << m_if;
+//   std::string str = ss.str();
+//   std::cout << str << std::endl;
+//   return BT::NodeStatus::SUCCESS;
+// }
+
+int main(int argc, char** argv)
+{
+    // filename Behavior Tree
+    std::string bt = std::string(argv[1]) + "/" FILE_BT;
+    std::ifstream fh(bt, std::ios::in);
+    // reading xml
+    std::string xml{std::istreambuf_iterator<char>(fh), std::istreambuf_iterator<char>()}; 
+
+  rclcpp::init(argc, argv);
+
+  // RCLCPP_INFO(rclcpp::get_logger("rclcpp"), xml.c_str());
+  auto nh = std::make_shared<rclcpp::Node>("bt_exec"); //"_action");
+  // auto nh2 = std::make_shared<rclcpp::Node>("bt_exe_condition");
+
+  RosNodeParams params; //,params2;
+  params.nh = nh;
+  params.default_port_value = SERVICE_NAME;
+  params.server_timeout = 3s;
+
+  BehaviorTreeFactory factory;
+  factory.registerNodeType<RbsCondition>("Is", params);
+
+  // params.nh = nh;
+  params.default_port_value = SERVER_NAME;
+  // params.server_timeout = 3s;
+  factory.registerNodeType<RbsAction>("RbsAction", params);
+  // factory.registerSimpleCondition("Is", [&](TreeNode&) { return TestIf(); });
+  // factory.registerRosAction("Is", params);
+
+  auto tree = factory.createTreeFromText(xml);
+
+  // while(rclcpp::ok())
+  if(rclcpp::ok())
+  {
+    tree.tickWhileRunning();
+  }
+
+  rclcpp::shutdown();
+
+  return 0;
+}