add mtp with orientation constraint

rbs_bt_executor/src/MoveToPoseOrientationConstraint.cpp

@@ -0,0 +1,62 @@
+#include "behaviortree_ros2/bt_action_node.hpp"
+#include "rbs_skill_interfaces/action/moveit_send_pose_with_constraints.hpp"
+#include <behaviortree_cpp/basic_types.h>
+#include <behaviortree_cpp/tree_node.h>
+#include <behaviortree_ros2/plugins.hpp>
+#include <behaviortree_ros2/ros_node_params.hpp>
+
+using namespace BT;
+using MoveitSendPoseWithConstraintAction =
+    rbs_skill_interfaces::action::MoveitSendPoseWithConstraints;
+
+class MoveToPoseOrientationConstraint
+    : public RosActionNode<MoveitSendPoseWithConstraintAction> {
+public:
+  MoveToPoseOrientationConstraint(const std::string &name,
+                                  const NodeConfig &conf,
+                                  const RosNodeParams &params)
+      : RosActionNode<MoveitSendPoseWithConstraintAction>(name, conf, params) {
+
+    RCLCPP_INFO(this->logger(), "Starting MoveToPose");
+  }
+
+  static BT::PortsList providedPorts() {
+    return providedBasicPorts(
+        {BT::InputPort<std::string>("robot_name"),
+         BT::InputPort<std::shared_ptr<geometry_msgs::msg::Pose>>("pose"),
+         BT::InputPort<double>("x_tol"), BT::InputPort<double>("y_tol"),
+         BT::InputPort<double>("z_tol"), BT::InputPort<double>("weight"),
+         BT::InputPort<double>("duration")});
+  }
+
+  bool setGoal(RosActionNode::Goal &goal) override {
+    RCLCPP_INFO(this->logger(), "[%s] Starting send goal [%s]", name().c_str(),
+                this->action_name_.c_str());
+
+    getInput("x_tol", goal.orientation_constraint.absolute_x_axis_tolerance);
+    getInput("y_tol", goal.orientation_constraint.absolute_y_axis_tolerance);
+    getInput("z_tol", goal.orientation_constraint.absolute_z_axis_tolerance);
+    getInput("weight", goal.orientation_constraint.weight);
+    getInput("robot_name", goal.robot_name);
+    getInput("pose", m_target_pose);
+    getInput("duration", goal.duration);
+    goal.target_pose = *m_target_pose;
+    return true;
+  }
+
+  NodeStatus onResultReceived(const WrappedResult &wr) override {
+
+    RCLCPP_INFO(this->logger(), "[%s] Starting get response %s with status %b",
+                name().c_str(), this->action_name_.c_str(), wr.result->success);
+    if (!wr.result->success) {
+      return NodeStatus::FAILURE;
+    }
+    return NodeStatus::SUCCESS;
+  }
+
+private:
+  std::shared_ptr<geometry_msgs::msg::Pose> m_target_pose;
+};
+
+CreateRosNodePlugin(MoveToPoseOrientationConstraint,
+                    "MtpOrientationConstraint");