runtime/rbs_bt_executor/src/AddPlanningSceneObject.cpp
2023-11-22 14:43:30 +03:00

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");
}