diff --git a/rbs_mill_assist/bt/plugins/get_grasp_place_pose.cpp b/rbs_mill_assist/bt/plugins/get_grasp_place_pose.cpp deleted file mode 100644 index f8a2a95..0000000 --- a/rbs_mill_assist/bt/plugins/get_grasp_place_pose.cpp +++ /dev/null @@ -1,119 +0,0 @@ -#include "behaviortree_ros2/bt_service_node.hpp" - -#include "behaviortree_ros2/plugins.hpp" -#include -// #include -#include "rbs_skill_interfaces/srv/get_pick_place_poses.hpp" -#include -#include -#include -#include -// #include -#include -#include - -using GraspingService = rbs_skill_interfaces::srv::GetPickPlacePoses; -using namespace BT; - -class Grasping : public RosServiceNode { -public: - Grasping(const std::string &name, const NodeConfig &conf, - const RosNodeParams ¶ms) - : RosServiceNode(name, conf, params) { - - RCLCPP_INFO(this->logger(), "Starting GetGraspPose"); - } - - static PortsList providedPorts() { - return providedBasicPorts( - {InputPort("object_name"), - InputPort("action_name"), - OutputPort>("pose"), - OutputPort>("pre_pose"), - OutputPort>("post_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("action_name", request->action_name)) { - RCLCPP_ERROR(this->node_.lock()->get_logger(), - "[%s] Failed to get object_name from input port", - name().c_str()); - return false; - } - return true; - } - - NodeStatus onResponseReceived(const Response::SharedPtr &response) override { - if (!response->ok) { - RCLCPP_ERROR(this->node_.lock()->get_logger(), - "[%s] Response indicates failure.", name().c_str()); - return NodeStatus::FAILURE; - } - - RCLCPP_INFO(this->node_.lock()->get_logger(), - "[%s] Response received successfully.", name().c_str()); - - auto logPose = [this](const std::string &pose_name, - const geometry_msgs::msg::Pose &pose) { - RCLCPP_INFO(this->node_.lock()->get_logger(), - "[%s] %s:\n" - " Position: x = %.4f, y = %.4f, z = %.4f\n" - " Orientation: x = %.4f, y = %.4f, z = %.4f, w = %.4f", - name().c_str(), 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(), "[%s] Got Pick Response", name().c_str()); - std::vector 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(); - auto pregrasp_pose = std::make_shared(); - auto postgrasp_pose = std::make_shared(); - - *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(), "[%s] Got Place Response", name().c_str()); - std::vector 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(); - auto preplace_pose = std::make_shared(); - auto postplace_pose = std::make_shared(); - - *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(), "[%s] Could not response grasp pose", name().c_str()); - return NodeStatus::FAILURE; - } - // virtual NodeStatus onFailure(ServiceNodeErrorCode error) override {} -}; - -CreateRosNodePlugin(Grasping, "GetGraspPlacePose"); diff --git a/rbs_mill_assist/bt/plugins/get_grasp_pose.cpp b/rbs_mill_assist/bt/plugins/get_grasp_pose.cpp new file mode 100644 index 0000000..9e0451a --- /dev/null +++ b/rbs_mill_assist/bt/plugins/get_grasp_pose.cpp @@ -0,0 +1,89 @@ +#include "behaviortree_ros2/bt_service_node.hpp" + +#include "behaviortree_ros2/plugins.hpp" +#include +// #include +#include "rbs_skill_interfaces/srv/get_pick_place_poses.hpp" +// #include "rbs_mill_interfaces" +// #include "rbs_mill_interfaces/srv/get_slot_grasping_pose.hpp" +#include "rbs_mill_interfaces/srv/get_grasping_pose.hpp" +#include +#include +#include +#include +// #include +#include +#include + +using GraspingService = rbs_mill_interfaces::srv::GetGraspingPose; +using namespace BT; + +class GetGraspPose : public RosServiceNode { +public: + GetGraspPose(const std::string &name, const NodeConfig &conf, + const RosNodeParams ¶ms) + : RosServiceNode(name, conf, params) { + + RCLCPP_INFO(this->logger(), "Starting GetSlotGraspingPose"); + } + + static PortsList providedPorts() { + return providedBasicPorts( + {InputPort("from_pose"), + InputPort("relative_to"), + OutputPort>("grasp"), + OutputPort>("pregrasp"), + OutputPort>("postgrasp")}); + } + + 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("from_pose", request->pose_name)) { + RCLCPP_ERROR(this->node_.lock()->get_logger(), + "[%s] Failed to get slot_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 relative_to from input port", + name().c_str()); + return false; + } + request->action_type = "pick"; + return true; + } + + NodeStatus onResponseReceived(const Response::SharedPtr &response) override { + if (!response->ok) { + RCLCPP_ERROR(this->node_.lock()->get_logger(), + "[%s] Response indicates failure.", name().c_str()); + return NodeStatus::FAILURE; + } + RCLCPP_INFO(this->node_.lock()->get_logger(), + "[%s] Response received successfully.", name().c_str()); + + auto grasp = + std::make_shared(response->grasp_poses.pose); + auto pregrasp = std::make_shared( + response->grasp_poses.pre_pose); + auto postgrasp = std::make_shared( + response->grasp_poses.post_pose); + + if (!grasp || !pregrasp || !postgrasp) { + RCLCPP_ERROR(this->logger(), "[%s] Response poses are empty", + name().c_str()); + return NodeStatus::FAILURE; + } + + setOutput("grasp", grasp); + setOutput("pregrasp", pregrasp); + setOutput("postgrasp", postgrasp); + + return NodeStatus::SUCCESS; + } + // virtual NodeStatus onFailure(ServiceNodeErrorCode error) override {} +}; + +CreateRosNodePlugin(GetGraspPose, "GetSlotGraspPoses"); diff --git a/rbs_mill_assist/bt/plugins/get_named_pose.cpp b/rbs_mill_assist/bt/plugins/get_named_pose.cpp index e14ad5d..518782c 100644 --- a/rbs_mill_assist/bt/plugins/get_named_pose.cpp +++ b/rbs_mill_assist/bt/plugins/get_named_pose.cpp @@ -4,7 +4,7 @@ #include // #include #include "rbs_skill_interfaces/srv/get_pick_place_poses.hpp" -#include "rbs_utils_interfaces/srv/get_named_pose.hpp" +#include "rbs_utils_interfaces/srv/get_relative_named_pose.hpp" #include #include #include @@ -12,21 +12,22 @@ #include #include -using GetNamedPoseService = rbs_utils_interfaces::srv::GetNamedPose; +using GetRelativeNamedPose = rbs_utils_interfaces::srv::GetRelativeNamedPose; using namespace BT; -class GetNamedPose : public RosServiceNode { +class GetNamedPose : public RosServiceNode { public: GetNamedPose(const std::string &name, const NodeConfig &conf, const RosNodeParams ¶ms) - : RosServiceNode(name, conf, params) { + : RosServiceNode(name, conf, params) { - RCLCPP_INFO(this->logger(), "Starting GetGraspPose"); + RCLCPP_INFO(this->logger(), "Starting GetNamedPose"); } static PortsList providedPorts() { return providedBasicPorts( {InputPort("pose_name"), + InputPort("relative_to"), OutputPort>("output_pose")}); } @@ -39,6 +40,19 @@ public: 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; } @@ -59,7 +73,6 @@ public: return NodeStatus::SUCCESS; } - // virtual NodeStatus onFailure(ServiceNodeErrorCode error) override {} }; CreateRosNodePlugin(GetNamedPose, "GetNamedPose"); diff --git a/rbs_mill_assist/bt/plugins/get_place_pose.cpp b/rbs_mill_assist/bt/plugins/get_place_pose.cpp new file mode 100644 index 0000000..777e201 --- /dev/null +++ b/rbs_mill_assist/bt/plugins/get_place_pose.cpp @@ -0,0 +1,90 @@ +#include "behaviortree_ros2/bt_service_node.hpp" + +#include "behaviortree_ros2/plugins.hpp" +#include +// #include +#include "rbs_skill_interfaces/srv/get_pick_place_poses.hpp" +// #include "rbs_mill_interfaces" +// #include "rbs_mill_interfaces/srv/get_slot_grasping_pose.hpp" +#include "rbs_mill_interfaces/srv/get_grasping_pose.hpp" +#include +#include +#include +#include +// #include +#include +#include + +using GraspingService = rbs_mill_interfaces::srv::GetGraspingPose; +using namespace BT; + +class GetPlacePoses : public RosServiceNode { +public: + GetPlacePoses(const std::string &name, const NodeConfig &conf, + const RosNodeParams ¶ms) + : RosServiceNode(name, conf, params) { + + RCLCPP_INFO(this->logger(), "Starting GetPlacePose"); + } + + static PortsList providedPorts() { + return providedBasicPorts( + {InputPort("to_pose"), + InputPort("relative_to"), + OutputPort>("place"), + OutputPort>("preplace"), + OutputPort>("postplace")}); + } + + 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("to_pose", request->pose_name)) { + RCLCPP_ERROR(this->node_.lock()->get_logger(), + "[%s] Failed to get slot_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 relative_to from input port", + name().c_str()); + return false; + } + request->action_type = "place"; + return true; + } + + NodeStatus onResponseReceived(const Response::SharedPtr &response) override { + if (!response->ok) { + RCLCPP_ERROR(this->node_.lock()->get_logger(), + "[%s] Response indicates failure.", name().c_str()); + return NodeStatus::FAILURE; + } + + RCLCPP_INFO(this->node_.lock()->get_logger(), + "[%s] Response received successfully.", name().c_str()); + + auto place = + std::make_shared(response->place_poses.pose); + auto preplace = std::make_shared( + response->place_poses.pre_pose); + auto postplace = std::make_shared( + response->place_poses.post_pose); + + if (!place || !preplace || !postplace) { + RCLCPP_ERROR(this->logger(), "[%s] Response poses are empty", + name().c_str()); + return NodeStatus::FAILURE; + } + + setOutput("place", place); + setOutput("preplace", preplace); + setOutput("postplace", postplace); + + return NodeStatus::SUCCESS; + } + // virtual NodeStatus onFailure(ServiceNodeErrorCode error) override {} +}; + +CreateRosNodePlugin(GetPlacePoses, "GetPlacePoses");