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

launching a unified behavior tree (+rbss)

Browse source
This commit is contained in:
shalenikol 2025-03-11 15:58:38 +03:00
parent 4c64536b80
commit cf692ff4c1
8 changed files with 460 additions and 325 deletions
Download patch file Download diff file Expand all files Collapse all files

84
rbs_bt_executor/src/rbssAction.cpp Normal file
View file

@ -0,0 +1,84 @@
// #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 RbssActionSrv = rbs_skill_interfaces::action::RbsBt;
class RbssAction : public RosActionNode<RbssActionSrv> {
public:
RbssAction(const std::string &name, const NodeConfig &conf, const RosNodeParams &params)
: RosActionNode<RbssActionSrv>(name, conf, params) {
m_action = getInput<std::string>(STR_ACTION).value();
m_command = getInput<std::string>(STR_COMMAND).value();
RCLCPP_INFO_STREAM(logger(), "Start node RbssAction: " + m_action + "/" + m_command);
m_client = std::make_shared<rclcpp::AsyncParametersClient>(node_.lock(), NODE_NAME);
while (!m_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...");
}
}
static BT::PortsList providedPorts() {
return providedBasicPorts({
BT::InputPort<std::string>(STR_SID),
BT::InputPort<std::string>(STR_ACTION),
BT::InputPort<std::string>(STR_COMMAND)
});
}
bool setGoal(RosActionNode::Goal& goal) override {
getInput(STR_ACTION, goal.action);
getInput(STR_COMMAND, goal.command);
getInput(STR_SID, goal.sid);
m_action = goal.action;
m_command = goal.command;
m_i++;
RCLCPP_INFO_STREAM(logger(), "setGoal: " + to_string(m_i) + ") " + goal.action + "/" + goal.command + " goal.sid = " + to_string(goal.sid));
return true;
}
NodeStatus onResultReceived(const WrappedResult& wr) override {
m_i++;
RCLCPP_INFO_STREAM(logger(), "onResultReceived: " + to_string(m_i) + ") " + m_action + "/" + m_command + " wr.result->ok - " + to_string(wr.result->ok));
if (!wr.result->ok) {
return NodeStatus::FAILURE;
}
return NodeStatus::SUCCESS;
}
private:
int m_i{0};
std::string m_action{};
std::string m_command{};
std::shared_ptr<rclcpp::AsyncParametersClient> m_client;
};
CreateRosNodePlugin(RbssAction, "RbssAction")