Added initial batch of behaviors

2026-03-06 20:52:01 +01:00
parent 1a2661fa77
commit 5c8ed91ccf
16 changed files with 899 additions and 0 deletions
--- a/toid_bt/src/bt_executor.cpp
+++ b/toid_bt/src/bt_executor.cpp
@@ -0,0 +1,103 @@
+#include "toid_bt/bt_executor.hpp"
+
+#include "behaviortree_cpp/xml_parsing.h"
+#include "nav2_util/node_utils.hpp"
+#include "nav2_util/robot_utils.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "toid_bt/plugins/empty_srv_action.hpp"
+#include "toid_bt/plugins/end_calib_action.hpp"
+#include "toid_bt/plugins/move_coords_action.hpp"
+#include "toid_bt/plugins/rotate_action.hpp"
+#include "toid_bt/plugins/rotate_towards_action.hpp"
+
+namespace toid
+{
+TreeExecutor::TreeExecutor(const rclcpp::NodeOptions opts)
+: BT::TreeExecutionServer(std::make_shared<rclcpp::Node>("toid_bt", opts))
+{
+  /* 
+  executeRegistration();
+  std::cout << BT::writeTreeNodesModelXML(factory()) << std::endl;
+  tf_buffer_ = std::make_shared<tf2_ros::Buffer>(node()->get_clock());
+  tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
+  */
+
+  nav2_util::declare_parameter_if_not_declared(
+    node(), "base_frame", rclcpp::ParameterValue("base_footprint"));
+  node()->get_parameter("base_frame", base_frame_);
+
+  nav2_util::declare_parameter_if_not_declared(
+    node(), "global_frame", rclcpp::ParameterValue("map"));
+  node()->get_parameter("global_frame", global_frame_);
+}
+
+void TreeExecutor::onTreeCreated(BT::Tree & tree)
+{
+  logger_cout_ = std::make_shared<BT::StdCoutLogger>(tree);
+}
+
+void TreeExecutor::registerNodesIntoFactory(BT::BehaviorTreeFactory & factory)
+{
+  PoseFunc get_pose = [this](geometry_msgs::msg::PoseStamped & pose) { this->position(pose); };
+  rclcpp::Node::SharedPtr nh = node();
+  factory.registerNodeType<toid::MovePointNode>(
+    "MovePointSimple", BT::RosNodeParams(nh, "/moveCoords"), get_pose);
+
+  factory.registerNodeType<toid::RotateNode>(
+    "RotateSimple", BT::RosNodeParams(nh, "/rotate"), get_pose);
+
+  factory.registerNodeType<toid::RotateTowardsNode>(
+    "RotateTowards", BT::RosNodeParams(nh, "/rotate"), get_pose);
+
+  factory.registerNodeType<toid::EndCalibNode>(
+    "SetWidth", BT::RosNodeParams(nh, "/set_width"), get_pose);
+
+  factory.registerNodeType<toid::EmptySrvNode>(
+    "ZeroOdom", BT::RosNodeParams(nh, "/zero"));
+
+  factory.registerNodeType<toid::EmptySrvNode>(
+    "EndCalib", BT::RosNodeParams(nh, "/end_calib"));
+
+  std::cout << describeCustomNodes() << std::endl;
+
+}
+
+void TreeExecutor::position(geometry_msgs::msg::PoseStamped & pose)
+{
+  nav2_util::getCurrentPose(pose, *tf_buffer_, global_frame_, base_frame_);
+}
+
+std::optional<std::string> TreeExecutor::onTreeExecutionCompleted(
+  BT::NodeStatus status, bool was_cancelled)
+{
+  (void)status;
+  logger_cout_.reset();
+
+  if (was_cancelled) {
+    std::cout << "Behavior tree was cancled" << std::endl;
+  }
+
+  return std::nullopt;
+}
+
+std::string TreeExecutor::describeCustomNodes() { return BT::writeTreeNodesModelXML(factory()); }
+
+}  // namespace toid
+
+int main(const int argc, const char * const * argv)
+{
+  rclcpp::init(argc, argv);
+
+  rclcpp::NodeOptions options;
+  auto bt_exec_server = std::make_shared<toid::TreeExecutor>(options);
+
+  rclcpp::executors::MultiThreadedExecutor executor(
+    rclcpp::ExecutorOptions(), 0, false, std::chrono::milliseconds(250));
+
+  executor.add_node(bt_exec_server->node());
+  executor.spin();
+  executor.remove_node(bt_exec_server->node());
+
+  rclcpp::shutdown();
+  return 0;
+}