#include "behaviortree_ros2/bt_topic_sub_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_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 MSG = geometry_msgs::msg::Pose;
using namespace BT;

class IsInPose : public RosTopicSubNode<MSG> {
public:
  IsInPose(const std::string &name, const NodeConfig &conf,
           const RosNodeParams &params)
      : RosTopicSubNode<MSG>(name, conf, params) {

    RCLCPP_INFO(this->logger(), "Starting IsInPose");
  }
  //
  // static PortsList providedPorts() {
  //   return {};
  // }

  NodeStatus onTick(const std::shared_ptr<MSG> &t_last_msg) override {
    if (t_last_msg) {
      RCLCPP_INFO(this->logger(), "[%s] Got message Pose", name().c_str());
      m_last_msg = t_last_msg;
      return NodeStatus::SUCCESS;
    }
    RCLCPP_INFO(this->logger(), "[%s] NOT message received", name().c_str());
    return NodeStatus::SUCCESS;
  }
  bool latchLastMessage() const override {
    return true;
  }

private:
  MSG::SharedPtr m_last_msg;
};

CreateRosNodePlugin(IsInPose, "IsInPose");