118 lines
4.5 KiB
C++
118 lines
4.5 KiB
C++
#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 <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 <rbs_utils_interfaces/srv/detail/get_grasp_pose__struct.hpp>
|
|
#include <rclcpp/logging.hpp>
|
|
#include <string>
|
|
|
|
using GraspingService = rbs_skill_interfaces::srv::GetPickPlacePoses;
|
|
using namespace BT;
|
|
|
|
class Grasping : public RosServiceNode<GraspingService> {
|
|
public:
|
|
Grasping(const std::string &name, const NodeConfig &conf,
|
|
const RosNodeParams ¶ms)
|
|
: RosServiceNode<GraspingService>(name, conf, params) {
|
|
|
|
RCLCPP_INFO(this->logger(), "Starting GetGraspPose");
|
|
}
|
|
|
|
static PortsList providedPorts() {
|
|
return providedBasicPorts(
|
|
{InputPort<std::string>("object_name"),
|
|
InputPort<std::string>("action_name"),
|
|
OutputPort<std::shared_ptr<geometry_msgs::msg::Pose>>("pose"),
|
|
OutputPort<std::shared_ptr<geometry_msgs::msg::Pose>>("pre_pose"),
|
|
OutputPort<std::shared_ptr<geometry_msgs::msg::Pose>>("post_pose")});
|
|
}
|
|
|
|
bool setRequest(Request::SharedPtr &request) override {
|
|
RCLCPP_INFO(this->logger(), "Starting send request for: [%s]",
|
|
this->service_name_.c_str());
|
|
if (!getInput("action_name", request->action_name)) {
|
|
RCLCPP_ERROR(this->node_.lock()->get_logger(),
|
|
"Failed to get object_name from input port");
|
|
return false;
|
|
}
|
|
return true;
|
|
}
|
|
|
|
NodeStatus onResponseReceived(const Response::SharedPtr &response) override {
|
|
if (!response->ok) {
|
|
RCLCPP_ERROR(this->node_.lock()->get_logger(),
|
|
"Response indicates failure.");
|
|
return NodeStatus::FAILURE;
|
|
}
|
|
|
|
RCLCPP_INFO(this->node_.lock()->get_logger(),
|
|
"Response received successfully.");
|
|
|
|
auto logPose = [this](const std::string &pose_name,
|
|
const geometry_msgs::msg::Pose &pose) {
|
|
RCLCPP_INFO(this->node_.lock()->get_logger(),
|
|
"%s:\n"
|
|
" Position: x = %.4f, y = %.4f, z = %.4f\n"
|
|
" Orientation: x = %.4f, y = %.4f, z = %.4f, w = %.4f",
|
|
pose_name.c_str(), pose.position.x, pose.position.y,
|
|
pose.position.z, pose.orientation.x, pose.orientation.y,
|
|
pose.orientation.z, pose.orientation.w);
|
|
};
|
|
if (!response->grasp.empty()) {
|
|
RCLCPP_INFO(this->logger(), "Got Pick Response");
|
|
std::vector<std::string> poses = {"Pregrasp Pose", "Grasp Pose",
|
|
"Postgrasp Pose"};
|
|
|
|
for (size_t n = 0; n < poses.size() && n < response->grasp.size(); ++n) {
|
|
logPose(poses[n], response->grasp.at(n));
|
|
}
|
|
|
|
auto grasp_pose = std::make_shared<geometry_msgs::msg::Pose>();
|
|
auto pregrasp_pose = std::make_shared<geometry_msgs::msg::Pose>();
|
|
auto postgrasp_pose = std::make_shared<geometry_msgs::msg::Pose>();
|
|
|
|
*pregrasp_pose = response->grasp.at(0);
|
|
*grasp_pose = response->grasp.at(1);
|
|
*postgrasp_pose = response->grasp.at(2);
|
|
|
|
setOutput("pre_pose", pregrasp_pose);
|
|
setOutput("pose", grasp_pose);
|
|
setOutput("post_pose", postgrasp_pose);
|
|
return NodeStatus::SUCCESS;
|
|
}
|
|
if (!response->place.empty()) {
|
|
RCLCPP_INFO(this->logger(), "Got Place Response");
|
|
std::vector<std::string> poses = {"Preplace Pose", "Place Pose",
|
|
"Postplace Pose"};
|
|
|
|
for (size_t n = 0; n < poses.size() && n < response->place.size(); ++n) {
|
|
logPose(poses[n], response->place.at(n));
|
|
}
|
|
|
|
auto place_pose = std::make_shared<geometry_msgs::msg::Pose>();
|
|
auto preplace_pose = std::make_shared<geometry_msgs::msg::Pose>();
|
|
auto postplace_pose = std::make_shared<geometry_msgs::msg::Pose>();
|
|
|
|
*preplace_pose = response->place.at(0);
|
|
*place_pose = response->place.at(1);
|
|
*postplace_pose = response->place.at(2);
|
|
|
|
setOutput("pre_pose", preplace_pose);
|
|
setOutput("pose", place_pose);
|
|
setOutput("post_pose", postplace_pose);
|
|
return NodeStatus::SUCCESS;
|
|
}
|
|
|
|
RCLCPP_FATAL(this->logger(), "Could not response grasp pose");
|
|
return NodeStatus::FAILURE;
|
|
}
|
|
// virtual NodeStatus onFailure(ServiceNodeErrorCode error) override {}
|
|
};
|
|
|
|
CreateRosNodePlugin(Grasping, "GetGraspPlacePose");
|