add workspace inspector skill

rbs_bt_executor/src/MoveToPoseArray.cpp

@@ -0,0 +1,56 @@
+#include "behavior_tree/BtAction.hpp"
+#include "behaviortree_cpp_v3/behavior_tree.h"
+#include "geometry_msgs/msg/pose_array.hpp"
+#include "rbs_skill_interfaces/action/moveit_send_pose.hpp"
+#include "rbs_utils/rbs_utils.hpp"
+#include <behaviortree_cpp_v3/bt_factory.h>
+#include <geometry_msgs/msg/detail/pose_array__struct.hpp>
+#include <rclcpp/logging.hpp>
+#include <rclcpp/node.hpp>
+
+using MoveToPoseArrayAction = rbs_skill_interfaces::action::MoveitSendPose;
+
+class MoveToPoseArray : public BtAction<MoveToPoseArrayAction> {
+public:
+  MoveToPoseArray(const std::string &name, const BT::NodeConfiguration &config)
+      : BtAction<MoveToPoseArrayAction>(name, config) {
+    RCLCPP_INFO_STREAM(_node->get_logger(), "Start node.");
+  }
+
+  MoveToPoseArrayAction::Goal populate_goal() override {
+    auto goal = MoveToPoseArrayAction::Goal();
+    getInput<std::string>("robot_name", robot_name_);
+    getInput<geometry_msgs::msg::PoseArray>("pose_vec_in", target_pose_vec_);
+    for (auto &point : target_pose_vec_.poses) {
+      RCLCPP_INFO(_node->get_logger(), "Pose array: \n %f \n %f \n %f",
+                  point.position.x, point.position.y, point.position.z);
+    }
+    if (!target_pose_vec_.poses.empty()) {
+      goal.robot_name = robot_name_;
+      goal.target_pose = target_pose_vec_.poses.at(0);
+
+      target_pose_vec_.poses.erase(target_pose_vec_.poses.begin());
+      setOutput<geometry_msgs::msg::PoseArray>("pose_vec_out", target_pose_vec_);
+    } else {
+      RCLCPP_WARN(_node->get_logger(), "Target pose vector empty");
+    }
+
+    return goal;
+  }
+
+  static BT::PortsList providedPorts() {
+    return providedBasicPorts({
+        BT::InputPort<std::string>("robot_name"),
+        BT::InputPort<geometry_msgs::msg::PoseArray>("pose_vec_in"),
+        BT::OutputPort<geometry_msgs::msg::PoseArray>("pose_vec_out")
+    });
+  }
+
+private:
+  std::string robot_name_;
+  geometry_msgs::msg::PoseArray target_pose_vec_;
+};
+
+BT_REGISTER_NODES(factory) {
+  factory.registerNodeType<MoveToPoseArray>("MoveToPoseArray");
+}