48 lines
1.6 KiB
C++
48 lines
1.6 KiB
C++
|
#include "rbs_skill_interfaces/srv/add_planning_scene_object.hpp"
|
||
|
|
#include "behaviortree_cpp_v3/bt_factory.h"
|
||
|
|
#include "behavior_tree/BtService.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");
|
||
|
|
}
|