48 lines
1.4 KiB
C++
48 lines
1.4 KiB
C++
#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 ¶ms)
|
|
: 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");
|