Added initial batch of behaviors

toid_bt/include/toid_bt/bt_executor.hpp

@@ -0,0 +1,34 @@
+#pragma once
+#include "rclcpp/rclcpp.hpp"
+
+#include "behaviortree_ros2/tree_execution_server.hpp"
+#include "behaviortree_cpp/loggers/bt_cout_logger.h"
+
+#include "tf2_ros/buffer.hpp"
+#include "tf2_ros/transform_listener.hpp"
+
+#include "geometry_msgs/msg/pose_stamped.hpp"
+
+namespace toid {
+    class TreeExecutor : public BT::TreeExecutionServer {
+      public:
+        TreeExecutor(const rclcpp::NodeOptions opts);
+        void onTreeCreated(BT::Tree& tree) override;
+        
+        void registerNodesIntoFactory(BT::BehaviorTreeFactory& factory) override;
+
+        std::optional<std::string> onTreeExecutionCompleted(BT::NodeStatus status, bool was_cancelled) override;
+
+        void position(geometry_msgs::msg::PoseStamped &pose);
+
+        std::string describeCustomNodes();
+
+      private:
+        std::shared_ptr<BT::StdCoutLogger>          logger_cout_;
+        tf2_ros::Buffer::SharedPtr                  tf_buffer_;
+        std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
+
+        std::string base_frame_;
+        std::string global_frame_;
+    };
+}

toid_bt/include/toid_bt/plugin.hpp

@@ -0,0 +1,8 @@
+#pragma once
+#include <functional>
+
+#include "geometry_msgs/msg/pose_stamped.hpp"
+
+namespace toid {
+    using PoseFunc = std::function<void(geometry_msgs::msg::PoseStamped &)>;
+}

toid_bt/include/toid_bt/plugins/empty_srv_action.hpp

@@ -0,0 +1,30 @@
+#pragma once
+
+#include "angles/angles.h"
+#include "behaviortree_ros2/bt_service_node.hpp"
+#include "std_srvs/srv/empty.hpp"
+#include "tf2/utils.hpp"
+#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
+#include "toid_bt/plugin.hpp"
+
+namespace toid
+{
+
+class EmptySrvNode : public BT::RosServiceNode<std_srvs::srv::Empty>
+{
+public:
+  EmptySrvNode(
+    const std::string & name, const BT::NodeConfig & conf, const BT::RosNodeParams & params)
+  : BT::RosServiceNode<std_srvs::srv::Empty>(name, conf, params)
+  {
+  }
+
+  bool setRequest(typename Request::SharedPtr &) override { return true; }
+
+  BT::NodeStatus onResponseReceived(const typename Response::SharedPtr &) override
+  {
+    return BT::NodeStatus::SUCCESS;
+  }
+};
+
+}  // namespace toid

toid_bt/include/toid_bt/plugins/end_calib_action.hpp

@@ -0,0 +1,56 @@
+#pragma once
+
+#include "angles/angles.h"
+#include "behaviortree_ros2/bt_service_node.hpp"
+#include "tf2/utils.hpp"
+#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
+#include "toid_bt/plugin.hpp"
+#include "toid_msgs/srv/send_double.hpp"
+
+namespace toid
+{
+
+class EndCalibNode : public BT::RosServiceNode<toid_msgs::srv::SendDouble>
+{
+public:
+  EndCalibNode(
+    const std::string & name, const BT::NodeConfig & conf, const BT::RosNodeParams & params,
+    PoseFunc pose)
+  : BT::RosServiceNode<toid_msgs::srv::SendDouble>(name, conf, params), get_pose(pose)
+  {
+  }
+
+  static BT::PortsList providedPorts()
+  {
+    return providedBasicPorts({
+      BT::InputPort<double>("width"), BT::InputPort<int>("count", 1, {}),
+      BT::OutputPort<double>("new_width"),
+      //BT::InputPort<double>("options"),
+    });
+  }
+
+  bool setRequest(typename Request::SharedPtr & request) override
+  {
+    auto width = getInput<double>("width").value();
+    auto count = getInput<double>("count").value();
+
+    geometry_msgs::msg::PoseStamped pose;
+    get_pose(pose);
+
+    double theta = tf2::getYaw(pose.pose.orientation);
+    double new_width = width * (1 + (theta / (2 * M_PI * count)));
+    request->data = new_width;
+
+    setOutput("new_width", new_width);
+    return true;
+  }
+
+  BT::NodeStatus onResponseReceived(const typename Response::SharedPtr &) override
+  {
+    return BT::NodeStatus::SUCCESS;
+  }
+
+  PoseFunc get_pose;
+};
+
+}  // namespace toid

toid_bt/include/toid_bt/plugins/move_coords_action.hpp

@@ -0,0 +1,65 @@
+#pragma once
+
+#include "angles/angles.h"
+#include "behaviortree_ros2/bt_action_node.hpp"
+#include "tf2/utils.hpp"
+#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
+#include "toid_bt/plugin.hpp"
+#include "toid_msgs/action/simple_move_coords.hpp"
+
+namespace toid
+{
+
+class MovePointNode : public BT::RosActionNode<toid_msgs::action::SimpleMoveCoords>
+{
+public:
+  MovePointNode(
+    const std::string & name, const BT::NodeConfig & conf, const BT::RosNodeParams & params,
+    PoseFunc pose)
+  : BT::RosActionNode<toid_msgs::action::SimpleMoveCoords>(name, conf, params), get_pose(pose)
+  {
+    std::cout << "wtf? " << params.default_port_value << std::endl;
+  }
+
+  static BT::PortsList providedPorts()
+  {
+    return providedBasicPorts({
+      BT::InputPort<double>("x"), BT::InputPort<double>("y"), BT::InputPort<double>("theta"),
+      //BT::InputPort<double>("options"),
+    });
+  }
+
+  bool setGoal(Goal & goal) override
+  {
+    auto x_goal = getInput<double>("x");
+    auto y_goal = getInput<double>("y");
+    auto theta = getInput<double>("theta");
+    goal.x = x_goal.value();
+    goal.y = y_goal.value();
+    if (!theta.has_value()) {
+      geometry_msgs::msg::PoseStamped pose;
+      get_pose(pose);
+      goal.theta = tf2::getYaw(pose.pose.orientation);
+    } else {
+      goal.theta = angles::from_degrees(theta.value_or(0));
+    }
+    return true;
+  }
+
+  void onHalt() override { RCLCPP_INFO(logger(), "%s: onHalt", name().c_str()); }
+
+  BT::NodeStatus onResultReceived(const WrappedResult & wr) override
+  {
+    return (wr.result->error_code == 0) ? BT::NodeStatus::SUCCESS : BT::NodeStatus::FAILURE;
+  }
+
+  BT::NodeStatus onFailure(BT::ActionNodeErrorCode error) override
+  {
+    RCLCPP_ERROR(logger(), "%s: onFailure with error: %s", name().c_str(), toStr(error));
+    return BT::NodeStatus::FAILURE;
+  }
+
+  PoseFunc get_pose;
+};
+
+}  // namespace toid

toid_bt/include/toid_bt/plugins/rotate_action.hpp

@@ -0,0 +1,63 @@
+#pragma once
+
+#include "angles/angles.h"
+#include "behaviortree_ros2/bt_action_node.hpp"
+#include "tf2/utils.hpp"
+#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
+#include "toid_bt/plugin.hpp"
+#include "toid_msgs/action/simple_rotate.hpp"
+
+namespace toid
+{
+
+class RotateNode : public BT::RosActionNode<toid_msgs::action::SimpleRotate>
+{
+public:
+  RotateNode(
+    const std::string & name, const BT::NodeConfig & conf, const BT::RosNodeParams & params,
+    PoseFunc pose)
+  : BT::RosActionNode<toid_msgs::action::SimpleRotate>(name, conf, params), get_pose(pose)
+  {
+  }
+
+  static BT::PortsList providedPorts()
+  {
+    return providedBasicPorts({
+      BT::InputPort<double>("angle", {}), 
+      BT::InputPort<double>("min_angle", 0, {}),
+      BT::InputPort<bool>("radians", false, {}),
+      //BT::InputPort<double>("options"),
+    });
+  }
+
+  bool setGoal(Goal & goal) override
+  {
+    auto angle = getInput<double>("angle");
+    auto min_angle = getInput<double>("min_angle");
+    auto radians = getInput<bool>("radians");
+    goal.angle = angle.value();
+    goal.min_angle = min_angle.value();
+    if (!radians.value()) {
+      goal.angle = angles::from_degrees(goal.angle);
+      goal.min_angle = angles::from_degrees(goal.min_angle);
+    }
+    return true;
+  }
+
+  void onHalt() override { RCLCPP_INFO(logger(), "%s: onHalt", name().c_str()); }
+
+  BT::NodeStatus onResultReceived(const WrappedResult & wr) override
+  {
+    return (wr.result->error_code == 0) ? BT::NodeStatus::SUCCESS : BT::NodeStatus::FAILURE;
+  }
+
+  BT::NodeStatus onFailure(BT::ActionNodeErrorCode error) override
+  {
+    RCLCPP_ERROR(logger(), "%s: onFailure with error: %s", name().c_str(), toStr(error));
+    return BT::NodeStatus::FAILURE;
+  }
+
+  PoseFunc get_pose;
+};
+
+}  // namespace toid

toid_bt/include/toid_bt/plugins/rotate_towards_action.hpp

@@ -0,0 +1,70 @@
+#pragma once
+
+#include "angles/angles.h"
+#include "behaviortree_ros2/bt_action_node.hpp"
+#include "tf2/utils.hpp"
+#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
+#include "toid_bt/plugin.hpp"
+#include "toid_msgs/action/simple_rotate.hpp"
+
+namespace toid
+{
+
+class RotateTowardsNode : public BT::RosActionNode<toid_msgs::action::SimpleRotate>
+{
+public:
+  RotateTowardsNode(
+    const std::string & name, const BT::NodeConfig & conf, const BT::RosNodeParams & params,
+    PoseFunc pose)
+  : BT::RosActionNode<toid_msgs::action::SimpleRotate>(name, conf, params), get_pose(pose)
+  {
+  }
+
+  static BT::PortsList providedPorts()
+  {
+    return providedBasicPorts({
+      BT::InputPort<double>("x", {}), 
+      BT::InputPort<double>("y", {}), 
+      BT::InputPort<double>("min_angle", 0, {}),
+      BT::InputPort<bool>("radians", false, {}),
+      //BT::InputPort<double>("options"),
+    });
+  }
+
+  bool setGoal(Goal & goal) override
+  {
+    auto x = getInput<double>("x").value();
+    auto y = getInput<double>("y").value();
+    auto min_angle = getInput<double>("min_angle");
+    auto radians = getInput<bool>("radians");
+
+    geometry_msgs::msg::PoseStamped pose;
+    get_pose(pose);
+
+    goal.angle = std::atan2(y - pose.pose.position.y, x - pose.pose.position.x);
+    goal.min_angle = min_angle.value();
+
+    if (!radians.value()) {
+      goal.min_angle = angles::from_degrees(goal.min_angle);
+    }
+
+    return true;
+  }
+
+  void onHalt() override { RCLCPP_INFO(logger(), "%s: onHalt", name().c_str()); }
+
+  BT::NodeStatus onResultReceived(const WrappedResult & wr) override
+  {
+    return (wr.result->error_code == 0) ? BT::NodeStatus::SUCCESS : BT::NodeStatus::FAILURE;
+  }
+
+  BT::NodeStatus onFailure(BT::ActionNodeErrorCode error) override
+  {
+    RCLCPP_ERROR(logger(), "%s: onFailure with error: %s", name().c_str(), toStr(error));
+    return BT::NodeStatus::FAILURE;
+  }
+
+  PoseFunc get_pose;
+};
+
+}  // namespace toid