add workspace inspector skill

rbs_bt_executor/src/EnvManager.cpp

@@ -2,9 +2,11 @@
 
 #include <behaviortree_cpp_v3/basic_types.h>
 #include <env_manager_interfaces/srv/detail/start_env__struct.hpp>
+#include <geometry_msgs/msg/detail/pose_array__struct.hpp>
+#include <memory>
 #include <rclcpp/logging.hpp>
 #include <string>
-
+#include "rbs_utils/rbs_utils.hpp"
 #include "env_manager_interfaces/srv/start_env.hpp"
 #include "env_manager_interfaces/srv/unload_env.hpp"
 
@@ -31,6 +33,11 @@ public:
   BT::NodeStatus handle_response(
       const EnvStarterService::Response::SharedPtr response) override {
     if (response->ok) {
+      // TODO We need better perfomance for call it in another place for all BT nodes
+      // - Make it via shared_ptr forward throught blackboard
+      auto t = std::make_shared<rbs_utils::AssemblyConfigLoader>("asp-example2", _node);
+      auto p = t->getWorkspaceInspectorTrajectory();
+      setOutput("workspace", p);
       return BT::NodeStatus::SUCCESS;
     }
     return BT::NodeStatus::FAILURE;
@@ -40,6 +47,7 @@ public:
     return providedBasicPorts({
         BT::InputPort<std::string>("env_name"),
         BT::InputPort<std::string>("env_class"),
+        BT::OutputPort<geometry_msgs::msg::PoseArray>("workspace"),
     });
   }
 };

rbs_bt_executor/src/MoveToPose.cpp

@@ -1,19 +1,14 @@
+// Copyright [2023] bill-finger
 #include "rbs_skill_interfaces/action/moveit_send_pose.hpp"
 
 #include "behavior_tree/BtAction.hpp"
 #include "behaviortree_cpp_v3/bt_factory.h"
-#include "geometry_msgs/msg/pose_stamped.hpp"
-#include "moveit_msgs/action/move_group.h"
 // #include "Eigen/Geometry"
-#include "rclcpp/parameter.hpp"
 
-using namespace BT;
 using MoveToPoseAction = rbs_skill_interfaces::action::MoveitSendPose;
-static const rclcpp::Logger LOGGER =
-    rclcpp::get_logger("MoveToPoseActionClient");
 
 class MoveToPose : public BtAction<MoveToPoseAction> {
-public:
+ public:
   MoveToPose(const std::string &name, const BT::NodeConfiguration &config)
       : BtAction<MoveToPoseAction>(name, config) {}
 
@@ -42,23 +37,17 @@ public:
     return goal;
   }
 
-  static PortsList providedPorts() {
-    return providedBasicPorts({InputPort<std::string>("robot_name"),
-                               InputPort<geometry_msgs::msg::Pose>("pose")});
+  static BT::PortsList providedPorts() {
+    return providedBasicPorts(
+        {BT::InputPort<std::string>("robot_name"),
+         BT::InputPort<geometry_msgs::msg::Pose>("pose")});
   }
 
 private:
   std::string robot_name_;
-  // std::vector<double> pose_;
   geometry_msgs::msg::Pose pose_des;
-  // std::map<std::string, geometry_msgs::msg::Pose> Poses;
-
-  // geometry_msgs::msg::Pose array_to_pose(std::vector<double> pose_array){
-
-  // }
-
-}; // class MoveToPose
+};  // class MoveToPose
 
 BT_REGISTER_NODES(factory) {
   factory.registerNodeType<MoveToPose>("MoveToPose");
-}
+}

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");
+}