Loading files
This file is pinned to the selected release. Download the original to open it in your design tool.
#include "actions/arm_navigate_to_pose.hpp"
#include "actions/average_pose.hpp"
#include "actions/build_uv.hpp"
#include "actions/come_closer.hpp"
#include "actions/do_something.hpp"
#include "actions/get_next_goal.hpp"
#include "actions/ik_navigate_to_pose.hpp"
#include "actions/say_something.hpp"
#include "actions/show_board.hpp"
#include "conditions/has_next_goal.hpp"
#include "conditions/is_recent_detection.hpp"
#include "mission_state.hpp"
#include <behaviortree_cpp_v3/behavior_tree.h>
#include <behaviortree_cpp_v3/bt_factory.h>
#include <behaviortree_cpp_v3/loggers/bt_zmq_publisher.h> // optional (Groot)
#include <kalman_interfaces/action/arm_mission.hpp>
#include <memory>
#include <rclcpp/rclcpp.hpp>
#include <rclcpp_action/create_server.hpp>
#include <rclcpp_action/rclcpp_action.hpp>
#include <rclcpp_action/server.hpp>
#include <rclcpp_action/server_goal_handle.hpp>
#include <rclcpp_action/types.hpp>
#include <rclcpp_components/register_node_macro.hpp>
#include <std_msgs/msg/header.hpp>
#include <std_srvs/srv/trigger.hpp>
#include <unistd.h>
namespace kalman_arm2 {
class BTPanel : public rclcpp::Node {
public:
using ArmMission = kalman_interfaces::action::ArmMission;
using GoalHandle = rclcpp_action::ServerGoalHandle<ArmMission>;
BTPanel(const rclcpp::NodeOptions &options) : Node("bt_panel", options) {
declare_parameter<std::string>("tree_xml");
panel_layout_yaml_path_ = declare_parameter<std::string>("layout_yaml");
declare_parameter<bool>("enable_zmq_publisher", false);
declare_parameter<double>("tick_rate_hz", 20.0);
declare_parameter<bool>("auto_start", true);
mission_helper_ = std::make_shared<MissionHelper>(
// FIXME: assumption that the panel layout was read successfully
MissionState{
PanelLayout::read_yaml(panel_layout_yaml_path_, get_logger())
.value()
}
);
// Build the tree
configure();
if (get_parameter("auto_start").as_bool()) {
startTicking();
}
using namespace std::placeholders;
this->actionServer = rclcpp_action::create_server<ArmMission>(
this,
"arm_mission",
std::bind(&BTPanel::handle_goal, this, _1, _2),
std::bind(&BTPanel::handle_cancel, this, _1),
std::bind(&BTPanel::handle_accepted, this, _1)
);
}
void configure() {
const auto tree_xml = get_parameter("tree_xml").as_string();
// Load XML (supports package:// if you use BT factory helpers)
try {
factory_ = std::make_unique<BT::BehaviorTreeFactory>();
factory_->registerBuilder<DoSomething>(
"DoSomething", Builder<DoSomething>()
);
factory_->registerBuilder<ArmNavigateToPose>(
"ArmNavigateToPose", Builder<ArmNavigateToPose>()
);
factory_->registerBuilder<IsRecentDetection>(
"IsRecentDetection", Builder<IsRecentDetection>()
);
factory_->registerBuilder<ComeCloser>(
"ComeCloser", Builder<ComeCloser>()
);
factory_->registerBuilder<AveragePose>(
"AveragePose", Builder<AveragePose>()
);
factory_->registerBuilder<HasNextGoal>(
"HasNextGoal", Builder<HasNextGoal>()
);
factory_->registerBuilder<GetNextGoal>(
"GetNextGoal", Builder<GetNextGoal>()
);
factory_->registerBuilder<IKNavigateToPose>(
"IKNavigateToPose", Builder<IKNavigateToPose>()
);
factory_->registerBuilder<SaySomething>(
"Say", Builder<SaySomething>()
);
factory_->registerBuilder<BuildUV>("BuildUV", Builder<BuildUV>());
factory_->registerBuilder<ShowBoard>(
"ShowBoard", Builder<ShowBoard>()
);
// Build tree
tree_ = std::make_unique<BT::Tree>(
factory_->createTreeFromFile(tree_xml)
);
const auto bboard = tree_->rootBlackboard();
bboard->set("state", mission_helper_);
// Optional: publish to Groot (set enable_zmq_publisher=true)
if (get_parameter("enable_zmq_publisher").as_bool()) {
zmq_publisher_ = std::make_unique<BT::PublisherZMQ>(*tree_);
}
} catch (const std::exception &e) {
RCLCPP_ERROR(get_logger(), "Failed to configure BT: %s", e.what());
tree_.reset();
}
stop_requested_ = false;
}
private:
rclcpp_action::GoalResponse handle_goal(
const rclcpp_action::GoalUUID &uuid,
std::shared_ptr<const ArmMission::Goal> goal
) {
return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
}
rclcpp_action::CancelResponse
handle_cancel(const std::shared_ptr<GoalHandle> goal_handle) {
requestStop();
return rclcpp_action::CancelResponse::ACCEPT;
}
void handle_accepted(const std::shared_ptr<GoalHandle> goal_handle) {
using namespace std::placeholders;
auto feedback = std::make_shared<ArmMission::Feedback>();
feedback->progress = "checking";
goal_handle->publish_feedback(feedback);
uint8_t cmd_id = goal_handle->get_goal()->command_id;
if (cmd_id == 1) { // FIXME: make theese enum comparisons
currentHandle = goal_handle;
startTicking();
} else if (cmd_id == 2) {
feedback->progress = "stopping";
goal_handle->publish_feedback(feedback);
// tree.haltTree();
}
}
void startTicking() {
double hz = get_parameter("tick_rate_hz").as_double();
tick_period_ = std::chrono::duration<double>(1.0 / std::max(1e-3, hz));
stop_requested_ = false;
running_ = true;
worker_ = std::thread([this] {
tickLoop();
});
}
void tickLoop() {
while (!stop_requested_.load()) {
auto status = tree_->rootNode()->executeTick();
// Optionally react to terminal states (SUCCESS/FAILURE) by halting
// or restarting:
if (status != BT::NodeStatus::RUNNING) {
RCLCPP_INFO(
get_logger(),
"Tree finished with status: %s",
toStr(status, true).c_str()
);
auto result = std::make_shared<ArmMission::Result>();
result->result = (status == BT::NodeStatus::SUCCESS);
// Only transition if the goal is still active
if (currentHandle && currentHandle->is_active()) {
if (status == BT::NodeStatus::SUCCESS) {
currentHandle->succeed(result);
} else {
currentHandle->abort(result);
}
}
tree_->rootNode()->halt();
currentHandle.reset();
stop_requested_ = true;
break;
}
std::this_thread::sleep_for(tick_period_);
RCLCPP_ERROR(
get_logger(),
"Mission state: %s",
mission_helper_->to_string().c_str()
);
}
}
void requestStop() {
if (!stop_requested_.exchange(true)) {
if (worker_.joinable()) {
worker_.join();
}
if (tree_) {
tree_->rootNode()->halt();
}
}
}
template <typename T> const BT::NodeBuilder Builder() {
return [this](
const std::string &name, const BT::NodeConfiguration &config
) {
return std::make_unique<T>(name, config, this);
};
}
std::unique_ptr<BT::BehaviorTreeFactory> factory_;
std::unique_ptr<BT::Tree> tree_;
std::unique_ptr<BT::PublisherZMQ> zmq_publisher_; // optional
std::string panel_layout_yaml_path_;
std::thread worker_;
std::atomic<bool> stop_requested_{true};
std::atomic<bool> running_{false};
std::chrono::duration<double> tick_period_{0.05};
std::shared_ptr<MissionHelper> mission_helper_;
std::shared_ptr<GoalHandle> currentHandle;
rclcpp_action::Server<ArmMission>::SharedPtr actionServer;
};
} // namespace kalman_arm2
RCLCPP_COMPONENTS_REGISTER_NODE(kalman_arm2::BTPanel)
fc117d976This file is pinned to the selected release. Download the original to open it in your design tool.