runtime/robossembler/include/robot_bt/behavior_tree_nodes/GetGraspPoses.hpp
2022-04-01 01:36:48 +08:00

38 lines
No EOL
1.1 KiB
C++

#pragma once
#include <string>
#include <vector>
#include <behavior_tree/BtService.hpp>
#include <scene_monitor_interfaces/srv/get_grasp_poses.hpp>
#include <behaviortree_cpp_v3/bt_factory.h>
#include <geometry_msgs/msg/pose.hpp>
#include <rclcpp/rclcpp.hpp>
class GetGraspPoses : public BtService<scene_monitor_interfaces::srv::GetGraspPoses>
{
public:
GetGraspPoses(const std::string &xml_tag_name,
const BT::NodeConfiguration &conf);
scene_monitor_interfaces::srv::GetGraspPoses::Request::SharedPtr populate_request() override;
BT::NodeStatus handle_response(scene_monitor_interfaces::srv::GetGraspPoses::Response::SharedPtr response) override;
static BT::PortsList providedPorts()
{
return providedBasicPorts({
BT::InputPort<std::string>("part_id"),
BT::OutputPort<geometry_msgs::msg::Pose>("grasp_pose"),
BT::OutputPort<geometry_msgs::msg::Pose>("pre_grasp_pose"),
BT::OutputPort<double>("pre_gap_distance"),
BT::OutputPort<double>("gap_distance")
});
}
private:
std::string frame_;
scene_monitor_interfaces::msg::GraspPose grasp_pose_;
};