59 lines
2 KiB
C++
59 lines
2 KiB
C++
#include <behavior_tree/BtService.hpp>
|
|
|
|
#include <behaviortree_cpp_v3/basic_types.h>
|
|
#include <env_manager_interfaces/srv/detail/start_env__struct.hpp>
|
|
#include <geometry_msgs/msg/detail/pose_array__struct.hpp>
|
|
#include <memory>
|
|
#include <rclcpp/logging.hpp>
|
|
#include <string>
|
|
#include "rbs_utils/rbs_utils.hpp"
|
|
#include "env_manager_interfaces/srv/start_env.hpp"
|
|
#include "env_manager_interfaces/srv/unload_env.hpp"
|
|
|
|
using EnvStarterService = env_manager_interfaces::srv::StartEnv;
|
|
|
|
class EnvManagerStarter : public BtService<EnvStarterService> {
|
|
public:
|
|
EnvManagerStarter(const std::string &name,
|
|
const BT::NodeConfiguration &config)
|
|
: BtService<EnvStarterService>(name, config) {
|
|
RCLCPP_INFO_STREAM(_node->get_logger(), "Start node.");
|
|
}
|
|
|
|
EnvStarterService::Request::SharedPtr populate_request() override {
|
|
auto request = std::make_shared<EnvStarterService::Request>();
|
|
std::string env_name = getInput<std::string>("env_name").value();
|
|
std::string env_class = getInput<std::string>("env_class").value();
|
|
request->name = env_name;
|
|
request->type = env_class;
|
|
|
|
return request;
|
|
}
|
|
|
|
BT::NodeStatus handle_response(
|
|
const EnvStarterService::Response::SharedPtr response) override {
|
|
if (response->ok) {
|
|
// TODO We need better perfomance for call it in another place for all BT nodes
|
|
// - Make it via shared_ptr forward throught blackboard
|
|
auto t = std::make_shared<rbs_utils::AssemblyConfigLoader>("asp-example2", _node);
|
|
auto p = t->getWorkspaceInspectorTrajectory();
|
|
setOutput("workspace", p);
|
|
return BT::NodeStatus::SUCCESS;
|
|
}
|
|
return BT::NodeStatus::FAILURE;
|
|
}
|
|
|
|
static BT::PortsList providedPorts() {
|
|
return providedBasicPorts({
|
|
BT::InputPort<std::string>("env_name"),
|
|
BT::InputPort<std::string>("env_class"),
|
|
BT::OutputPort<geometry_msgs::msg::PoseArray>("workspace"),
|
|
});
|
|
}
|
|
};
|
|
|
|
#include "behaviortree_cpp_v3/bt_factory.h"
|
|
|
|
BT_REGISTER_NODES(factory) {
|
|
factory.registerNodeType<EnvManagerStarter>("EnvStarter");
|
|
}
|