cnc-graver-assist/rbs_mill_assist/bt/plugins/get_named_pose.cpp

#include "behaviortree_ros2/bt_service_node.hpp"

#include "behaviortree_ros2/plugins.hpp"
#include <behaviortree_cpp/basic_types.h>
// #include <geometry_msgs/msg/detail/point__struct.hpp>
#include "rbs_skill_interfaces/srv/get_pick_place_poses.hpp"
#include "rbs_utils_interfaces/srv/get_relative_named_pose.hpp"
#include <geometry_msgs/msg/detail/pose__struct.hpp>
#include <geometry_msgs/msg/detail/pose_array__struct.hpp>
#include <geometry_msgs/msg/detail/quaternion__struct.hpp>
#include <memory>
#include <rclcpp/logging.hpp>
#include <string>

using GetRelativeNamedPose = rbs_utils_interfaces::srv::GetRelativeNamedPose;
using namespace BT;

class GetNamedPose : public RosServiceNode<GetRelativeNamedPose> {
public:
  GetNamedPose(const std::string &name, const NodeConfig &conf,
               const RosNodeParams &params)
      : RosServiceNode<GetRelativeNamedPose>(name, conf, params) {

    RCLCPP_INFO(this->logger(), "Starting GetNamedPose");
  }

  static PortsList providedPorts() {
    return providedBasicPorts(
        {InputPort<std::string>("pose_name"),
         InputPort<std::string>("relative_to"),
         OutputPort<std::shared_ptr<geometry_msgs::msg::Pose>>("output_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());
    if (!getInput("pose_name", request->pose_name)) {
      RCLCPP_ERROR(this->node_.lock()->get_logger(),
                   "[%s] Failed to get pose_name from input port",
                   name().c_str());
      return false;
    }
    if (!getInput("pose_name", request->pose_name)) {
      RCLCPP_ERROR(this->node_.lock()->get_logger(),
                   "[%s] Failed to get pose_name from input port",
                   name().c_str());
      return false;
    }

    if (!getInput("relative_to", request->relative_to)) {
      RCLCPP_ERROR(this->node_.lock()->get_logger(),
                   "[%s] Failed to get pose_name from input port",
                   name().c_str());
      return false;
    }
    return true;
  }

  NodeStatus onResponseReceived(const Response::SharedPtr &response) override {
    if (!response->ok) {
      RCLCPP_ERROR(this->logger(), "[%s] Response indicates failure.",
                   name().c_str());
      return NodeStatus::FAILURE;
    }

    RCLCPP_INFO(this->logger(),
                "[%s] Response received successfully with pose name [%s]",
                name().c_str(), response->named_pose.name.c_str());

    auto pose = std::make_shared<geometry_msgs::msg::Pose>();
    *pose = response->named_pose.pose;
    setOutput("output_pose", pose);

    return NodeStatus::SUCCESS;
  }
};

CreateRosNodePlugin(GetNamedPose, "GetNamedPose");