Bill Finger
c0842b0da8
- Modified GetPose service node to accept relative positioning - Added new GetPlacePoses service node for retrieving place, preplace, and postplace poses
90 lines
3.3 KiB
C++
90 lines
3.3 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 "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");
|