46 lines
1.5 KiB
C++
46 lines
1.5 KiB
C++
#include "behavior_tree/BtService.hpp"
|
|
#include "behaviortree_cpp_v3/bt_factory.h"
|
|
#include "rbs_skill_interfaces/srv/add_planning_scene_object.hpp"
|
|
|
|
using namespace BT;
|
|
using AddPlanningSceneObjectClient =
|
|
rbs_skill_interfaces::srv::AddPlanningSceneObject;
|
|
|
|
class GetPickPlacePose : public BtService<AddPlanningSceneObjectClient> {
|
|
public:
|
|
GetPickPlacePose(const std::string &name, const BT::NodeConfiguration &config)
|
|
: BtService<AddPlanningSceneObjectClient>(name, config) {
|
|
RCLCPP_INFO(_node->get_logger(), "Start the node");
|
|
}
|
|
|
|
AddPlanningSceneObjectClient::Request::SharedPtr populate_request() override {
|
|
auto request = std::make_shared<AddPlanningSceneObjectClient::Request>();
|
|
RCLCPP_INFO(_node->get_logger(), "Start populate_request()");
|
|
object_name_ = getInput<std::string>("ObjectName").value();
|
|
auto place_pose_ = getInput<std::vector<double>>("pose").value();
|
|
request->object_id = object_name_;
|
|
request->object_pose = place_pose_;
|
|
|
|
return request;
|
|
}
|
|
|
|
BT::NodeStatus handle_response(
|
|
AddPlanningSceneObjectClient::Response::SharedPtr response) override {
|
|
RCLCPP_INFO(_node->get_logger(), "Response %d", response->success);
|
|
return BT::NodeStatus::SUCCESS;
|
|
}
|
|
|
|
static PortsList providedPorts() {
|
|
return providedBasicPorts({
|
|
InputPort<std::string>("ObjectName"),
|
|
InputPort<std::vector<double>>("pose"),
|
|
});
|
|
}
|
|
|
|
private:
|
|
std::string object_name_{};
|
|
};
|
|
|
|
BT_REGISTER_NODES(factory) {
|
|
factory.registerNodeType<GetPickPlacePose>("AddPlanningSceneObject");
|
|
}
|