feat: Enhance pose handling and add place poses functionality
- Modified GetPose service node to accept relative positioning - Added new GetPlacePoses service node for retrieving place, preplace, and postplace poses
This commit is contained in:
parent
7f01666528
commit
c0842b0da8
4 changed files with 198 additions and 125 deletions
|
@ -1,119 +0,0 @@
|
|||
#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(), "[%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<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(), "[%s] Got Place Response", name().c_str());
|
||||
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(), "[%s] Could not response grasp pose", name().c_str());
|
||||
return NodeStatus::FAILURE;
|
||||
}
|
||||
// virtual NodeStatus onFailure(ServiceNodeErrorCode error) override {}
|
||||
};
|
||||
|
||||
CreateRosNodePlugin(Grasping, "GetGraspPlacePose");
|
89
rbs_mill_assist/bt/plugins/get_grasp_pose.cpp
Normal file
89
rbs_mill_assist/bt/plugins/get_grasp_pose.cpp
Normal file
|
@ -0,0 +1,89 @@
|
|||
#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 "rbs_mill_interfaces"
|
||||
// #include "rbs_mill_interfaces/srv/get_slot_grasping_pose.hpp"
|
||||
#include "rbs_mill_interfaces/srv/get_grasping_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 <rbs_utils_interfaces/srv/detail/get_grasp_pose__struct.hpp>
|
||||
#include <rclcpp/logging.hpp>
|
||||
#include <string>
|
||||
|
||||
using GraspingService = rbs_mill_interfaces::srv::GetGraspingPose;
|
||||
using namespace BT;
|
||||
|
||||
class GetGraspPose : public RosServiceNode<GraspingService> {
|
||||
public:
|
||||
GetGraspPose(const std::string &name, const NodeConfig &conf,
|
||||
const RosNodeParams ¶ms)
|
||||
: RosServiceNode<GraspingService>(name, conf, params) {
|
||||
|
||||
RCLCPP_INFO(this->logger(), "Starting GetSlotGraspingPose");
|
||||
}
|
||||
|
||||
static PortsList providedPorts() {
|
||||
return providedBasicPorts(
|
||||
{InputPort<std::string>("from_pose"),
|
||||
InputPort<std::string>("relative_to"),
|
||||
OutputPort<std::shared_ptr<geometry_msgs::msg::Pose>>("grasp"),
|
||||
OutputPort<std::shared_ptr<geometry_msgs::msg::Pose>>("pregrasp"),
|
||||
OutputPort<std::shared_ptr<geometry_msgs::msg::Pose>>("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<geometry_msgs::msg::Pose>(response->grasp_poses.pose);
|
||||
auto pregrasp = std::make_shared<geometry_msgs::msg::Pose>(
|
||||
response->grasp_poses.pre_pose);
|
||||
auto postgrasp = std::make_shared<geometry_msgs::msg::Pose>(
|
||||
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");
|
|
@ -4,7 +4,7 @@
|
|||
#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 "rbs_utils_interfaces/srv/get_relative_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>
|
||||
|
@ -12,21 +12,22 @@
|
|||
#include <rclcpp/logging.hpp>
|
||||
#include <string>
|
||||
|
||||
using GetNamedPoseService = rbs_utils_interfaces::srv::GetNamedPose;
|
||||
using GetRelativeNamedPose = rbs_utils_interfaces::srv::GetRelativeNamedPose;
|
||||
using namespace BT;
|
||||
|
||||
class GetNamedPose : public RosServiceNode<GetNamedPoseService> {
|
||||
class GetNamedPose : public RosServiceNode<GetRelativeNamedPose> {
|
||||
public:
|
||||
GetNamedPose(const std::string &name, const NodeConfig &conf,
|
||||
const RosNodeParams ¶ms)
|
||||
: RosServiceNode<GetNamedPoseService>(name, conf, params) {
|
||||
: RosServiceNode<GetRelativeNamedPose>(name, conf, params) {
|
||||
|
||||
RCLCPP_INFO(this->logger(), "Starting GetGraspPose");
|
||||
RCLCPP_INFO(this->logger(), "Starting GetNamedPose");
|
||||
}
|
||||
|
||||
static PortsList providedPorts() {
|
||||
return providedBasicPorts(
|
||||
{InputPort<std::string>("pose_name"),
|
||||
InputPort<std::string>("relative_to"),
|
||||
OutputPort<std::shared_ptr<geometry_msgs::msg::Pose>>("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");
|
||||
|
|
90
rbs_mill_assist/bt/plugins/get_place_pose.cpp
Normal file
90
rbs_mill_assist/bt/plugins/get_place_pose.cpp
Normal file
|
@ -0,0 +1,90 @@
|
|||
#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 "rbs_mill_interfaces"
|
||||
// #include "rbs_mill_interfaces/srv/get_slot_grasping_pose.hpp"
|
||||
#include "rbs_mill_interfaces/srv/get_grasping_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 <rbs_utils_interfaces/srv/detail/get_grasp_pose__struct.hpp>
|
||||
#include <rclcpp/logging.hpp>
|
||||
#include <string>
|
||||
|
||||
using GraspingService = rbs_mill_interfaces::srv::GetGraspingPose;
|
||||
using namespace BT;
|
||||
|
||||
class GetPlacePoses : public RosServiceNode<GraspingService> {
|
||||
public:
|
||||
GetPlacePoses(const std::string &name, const NodeConfig &conf,
|
||||
const RosNodeParams ¶ms)
|
||||
: RosServiceNode<GraspingService>(name, conf, params) {
|
||||
|
||||
RCLCPP_INFO(this->logger(), "Starting GetPlacePose");
|
||||
}
|
||||
|
||||
static PortsList providedPorts() {
|
||||
return providedBasicPorts(
|
||||
{InputPort<std::string>("to_pose"),
|
||||
InputPort<std::string>("relative_to"),
|
||||
OutputPort<std::shared_ptr<geometry_msgs::msg::Pose>>("place"),
|
||||
OutputPort<std::shared_ptr<geometry_msgs::msg::Pose>>("preplace"),
|
||||
OutputPort<std::shared_ptr<geometry_msgs::msg::Pose>>("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<geometry_msgs::msg::Pose>(response->place_poses.pose);
|
||||
auto preplace = std::make_shared<geometry_msgs::msg::Pose>(
|
||||
response->place_poses.pre_pose);
|
||||
auto postplace = std::make_shared<geometry_msgs::msg::Pose>(
|
||||
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");
|
Loading…
Add table
Add a link
Reference in a new issue