robossembler/runtime
7
0
Fork 0
Code Issues 3 Pull requests Projects Releases Packages Wiki Activity Actions

BT interface node works with web-service

Browse source
This commit is contained in:
shalenikol 2024-05-27 08:35:27 +00:00 • committed by Igor Brylyov
parent 47c5e89913
commit 49e036af5e
15 changed files with 536 additions and 124 deletions
Download patch file Download diff file Expand all files Collapse all files

46
rbs_bt_executor/src/rbsBTAction.cpp
View file

@ -1,9 +1,11 @@
// #include "behavior_tree/BtAction.hpp"
#include "behavior_tree/BtService.hpp"
// #include "rbs_skill_interfaces/action/rbs_bt.hpp"
#include "rbs_skill_interfaces/srv/rbs_bt.hpp"
// using RbsBtActionSrv = rbs_skill_interfaces::action::RbsBt;
#define STR_ACTION "do"
#define STR_SID "sid"
#define STR_COMMAND "command"
#define NODE_NAME "rbs_interface"
using RbsBtActionSrv = rbs_skill_interfaces::srv::RbsBt;
class RbsBtAction : public BtService<RbsBtActionSrv> {
@ -11,45 +13,30 @@ public:
RbsBtAction(const std::string &name, const BT::NodeConfiguration &config)
: BtService<RbsBtActionSrv>(name, config) {
_action_name = getInput<std::string>("do").value();
_action_name = getInput<std::string>(STR_ACTION).value();
RCLCPP_INFO_STREAM(_node->get_logger(), "Start node RbsBtAction: " + _action_name);
_set_params_client = std::make_shared<rclcpp::AsyncParametersClient>(_node, "rbs_interface");
_set_params_client = std::make_shared<rclcpp::AsyncParametersClient>(_node, NODE_NAME);
while (!_set_params_client->wait_for_service(1s)) {
if (!rclcpp::ok()) {
RCLCPP_ERROR(_node->get_logger(), "Interrupted while waiting for the service. Exiting.");
break;
}
RCLCPP_WARN(_node->get_logger(), "'rbs_interface' service not available, waiting again...");
RCLCPP_WARN(_node->get_logger(), NODE_NAME " service not available, waiting again...");
}
// set_str_param();
}
// RbsBtActionSrv::Goal populate_goal() override {
// auto goal = RbsBtActionSrv::Goal();
// goal.action = _action_name;
// goal.command = getInput<std::string>("command").value();
// RCLCPP_INFO_STREAM(_node->get_logger(), "Goal send " + _action_name);
// return goal;
// }
RbsBtActionSrv::Request::SharedPtr populate_request() override {
auto request = std::make_shared<RbsBtActionSrv::Request>();
request->action = _action_name;
request->command = getInput<std::string>("command").value();
// RCLCPP_INFO_STREAM(_node->get_logger(), "RbsBtAction:populate_request");
request->sid = getInput<std::string>(STR_SID).value();
request->command = getInput<std::string>(STR_COMMAND).value();
return request;
}
BT::NodeStatus handle_response(const RbsBtActionSrv::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);
// RCLCPP_INFO_STREAM(_node->get_logger(), "RbsBtAction:handle_response");
return BT::NodeStatus::SUCCESS;
}
return BT::NodeStatus::FAILURE;
@ -57,22 +44,15 @@ public:
static BT::PortsList providedPorts() {
return providedBasicPorts({
BT::InputPort<std::string>("do"),
BT::InputPort<std::string>("command")
BT::InputPort<std::string>(STR_SID),
BT::InputPort<std::string>(STR_ACTION),
BT::InputPort<std::string>(STR_COMMAND)
});
}
private:
std::string _action_name{};
std::shared_ptr<rclcpp::AsyncParametersClient> _set_params_client;
// void set_str_param() {
// RCLCPP_INFO_STREAM(_node->get_logger(),"Set string parameter: <" + _action_name + ">");
// std::vector<rclcpp::Parameter> _parameters{rclcpp::Parameter("action_name", _action_name)};
// _set_params_client->set_parameters(_parameters);
// }
// auto _package_name = ament_index_cpp::get_package_share_directory("rbs_perception");
};
#include "behaviortree_cpp_v3/bt_factory.h"