81 lines
2.7 KiB
C++
81 lines
2.7 KiB
C++
// #include "behavior_tree/BtService.hpp"
|
|
#include "behaviortree_ros2/bt_service_node.hpp"
|
|
// #include "behaviortree_ros2/bt_action_node.hpp"
|
|
#include "rbs_skill_interfaces/srv/rbs_bt.hpp"
|
|
// #include "rbs_skill_interfaces/action/rbs_bt.hpp"
|
|
#include <behaviortree_cpp/tree_node.h>
|
|
#include <behaviortree_ros2/plugins.hpp>
|
|
#include <behaviortree_ros2/ros_node_params.hpp>
|
|
|
|
#define STR_ACTION "do"
|
|
#define STR_SID "sid"
|
|
#define STR_COMMAND "command"
|
|
#define NODE_NAME "rbs_interface"
|
|
|
|
template<typename T> std::string to_string(const T &t)
|
|
{
|
|
std::stringstream ss;
|
|
ss << t;
|
|
return ss.str();
|
|
}
|
|
|
|
using namespace BT;
|
|
using namespace std::chrono_literals;
|
|
using RbssConditionSrv = rbs_skill_interfaces::srv::RbsBt;
|
|
|
|
class RbssCondition : public RosServiceNode<RbssConditionSrv> {
|
|
public:
|
|
RbssCondition(const std::string &name, const NodeConfig& conf, const RosNodeParams& params)
|
|
: RosServiceNode<RbssConditionSrv>(name, conf, params) {
|
|
|
|
m_action_name = getInput<std::string>(STR_ACTION).value();
|
|
m_command = getInput<std::string>(STR_COMMAND).value();
|
|
RCLCPP_INFO_STREAM(logger(), "Start node RbssCondition: " + m_action_name + "/" + m_command);
|
|
|
|
m_params_client = std::make_shared<rclcpp::AsyncParametersClient>(node_.lock(), NODE_NAME);
|
|
|
|
while (!m_params_client->wait_for_service(1s)) {
|
|
if (!rclcpp::ok()) {
|
|
RCLCPP_ERROR(logger(), "Interrupted while waiting for the service. Exiting.");
|
|
break;
|
|
}
|
|
RCLCPP_WARN(logger(), NODE_NAME " service not available, waiting again...");
|
|
}
|
|
}
|
|
|
|
bool setRequest(Request::SharedPtr &request) override {
|
|
request->action = m_action_name;
|
|
request->command = m_command;
|
|
getInput(STR_SID, request->sid);
|
|
|
|
m_i++;
|
|
RCLCPP_INFO_STREAM(logger(), "setRequest: " + to_string(m_i) + ") " + m_action_name + "/" + m_command + " request->sid = " + to_string(request->sid));
|
|
return true;
|
|
}
|
|
|
|
NodeStatus onResponseReceived(const Response::SharedPtr &response) override {
|
|
m_i++;
|
|
RCLCPP_INFO_STREAM(logger(), "onResponseReceived: " + to_string(m_i) + ") " + m_action_name + "/" + m_command + " response->ok - " + to_string(response->ok));
|
|
|
|
if (!response->ok) {
|
|
return NodeStatus::FAILURE;
|
|
}
|
|
return NodeStatus::SUCCESS;
|
|
}
|
|
|
|
static PortsList providedPorts() {
|
|
return providedBasicPorts({
|
|
InputPort<std::string>(STR_SID),
|
|
InputPort<std::string>(STR_ACTION),
|
|
InputPort<std::string>(STR_COMMAND)
|
|
});
|
|
}
|
|
|
|
private:
|
|
int m_i{0};
|
|
std::string m_action_name{};
|
|
std::string m_command{};
|
|
std::shared_ptr<rclcpp::AsyncParametersClient> m_params_client;
|
|
};
|
|
|
|
CreateRosNodePlugin(RbssCondition, "RbssCondition")
|