Adrian/behavior tree

.gitmodules

@@ -11,3 +11,9 @@
 [submodule "vendor/gncea"]
 	path = vendor/gncea
 	url = https://github.com/palouserobosub/gncea
+[submodule "vendor/behaviortree_cpp"]
+	path = vendor/behaviortree_cpp
+	url = https://github.com/BehaviorTree/BehaviorTree.CPP
+[submodule "vendor/behaviortree_cpp.ROS2"]
+	path = vendor/behaviortree_cpp.ROS2
+	url = https://github.com/BehaviorTree/BehaviorTree.ROS2

src/guppy_nav/CMakeLists.txt

@@ -7,6 +7,7 @@ endif()
 
 # find dependencies
 find_package(ament_cmake REQUIRED)
+find_package(rosidl_default_generators REQUIRED)
 
 find_package(rclcpp REQUIRED)
 find_package(rclcpp_action REQUIRED)
@@ -22,35 +23,70 @@ find_package(Eigen3)
 
 find_package(control_toolbox REQUIRED)
 
-add_library(action_server SHARED src/navigate_action_server.cpp)
-add_library(pose_setter SHARED src/pose_setter.cpp)
+find_package(behaviortree_cpp REQUIRED)
+find_package(behaviortree_ros2 REQUIRED)
 
-#target_include_directories(action_server PRIVATE
-#  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
-#  $<INSTALL_INTERFACE:include>
+#add_library(navigate_action SHARED src/navigate_action_server.cpp)
+add_library(pose_setter SHARED src/pose_setter_server.cpp)
+add_executable(behavior_tree src/behavior_tree.cpp)
+
+#target_include_directories(navigate_action_server PUBLIC
+#    $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+#    $<INSTALL_INTERFACE:include>
+#)
+target_include_directories(pose_setter PUBLIC
+    $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+    $<INSTALL_INTERFACE:include>
+)
+target_include_directories(behavior_tree PUBLIC
+    $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+    $<INSTALL_INTERFACE:include>
+)
+
+#target_compile_definitions(navigate_action_server
+#    PRIVATE "GUPPY_NAV_BUILDING_DLL"
+#)
+target_compile_definitions(pose_setter
+    PRIVATE "GUPPY_NAV_BUILDING_DLL"
+)
+
+#ament_target_dependencies(navigate_action
+#    rclcpp rclcpp_action rclcpp_components guppy_msgs nav_msgs geometry_msgs Eigen3 control_toolbox
 #)
+ament_target_dependencies(pose_setter
+    rclcpp rclcpp_action rclcpp_components guppy_msgs nav_msgs geometry_msgs Eigen3 control_toolbox
+)
+ament_target_dependencies(behavior_tree
+    rclcpp guppy_msgs behaviortree_cpp behaviortree_ros2
+)
 
-include_directories(include)
+target_link_libraries(behavior_tree
+    behaviortree_cpp::behaviortree_cpp behaviortree_ros2::behaviortree_ros2
+)
 
-#add_executable(action_server src/navigate_action_server.cpp)
-ament_target_dependencies(action_server rclcpp rclcpp_action rclcpp_components guppy_msgs nav_msgs geometry_msgs Eigen3 control_toolbox)
-ament_target_dependencies(pose_setter rclcpp rclcpp_action rclcpp_components guppy_msgs nav_msgs geometry_msgs Eigen3 control_toolbox)
-rclcpp_components_register_node(action_server PLUGIN "NavigateActionServer" EXECUTABLE navigate_action_server)
-rclcpp_components_register_node(pose_setter PLUGIN "PoseSetterServer" EXECUTABLE the_pose_setter)
+#rclcpp_components_register_node(navigate_action
+#    PLUGIN "NavigateActionServer"
+#    EXECUTABLE navigate_action_server
+#)
+rclcpp_components_register_node(pose_setter
+    PLUGIN "PoseSetterServer"
+    EXECUTABLE pose_setter_server
+)
 
 install(TARGETS
-  action_server
-  pose_setter
+  #navigate_action navigate_action_server
+  pose_setter pose_setter_server
+  behavior_tree
 
   LIBRARY DESTINATION lib
   ARCHIVE DESTINATION lib
   RUNTIME DESTINATION bin
-  #DESTINATION lib/${PROJECT_NAME}
+  DESTINATION lib/${PROJECT_NAME}
 )
 
 install(DIRECTORY
   launch
-  
+
   DESTINATION share/${PROJECT_NAME}/
 )
 

src/guppy_nav/include/guppy_nav/acquire_detection.hpp

@@ -0,0 +1,40 @@
+#include <algorithm>
+
+#include <behaviortree_cpp/action_node.h>
+#include <behaviortree_cpp/basic_types.h>
+#include <behaviortree_cpp/tree_node.h>
+
+#include <behaviortree_ros2/bt_topic_sub_node.hpp>
+#include "guppy_msgs/msg/corner_detection.hpp"
+#include "guppy_msgs/msg/corner_detection_list.hpp"
+
+class AcquireDetection : public BT::RosTopicSubNode<guppy_msgs::msg::CornerDetectionList> {
+public:
+    AcquireDetection(const std::string& name, const BT::NodeConfig& config, const BT::RosNodeParams& params)
+    : BT::RosTopicSubNode<guppy_msgs::msg::CornerDetectionList>(name, config, params) { }
+
+    static BT::PortsList providedPorts() {
+        return providedBasicPorts({
+            BT::InputPort<std::string>("target"),
+            BT::OutputPort<guppy_msgs::msg::CornerDetection>("detection")
+        });
+    }
+
+    BT::NodeStatus onTick(const std::shared_ptr<guppy_msgs::msg::CornerDetectionList>& msg) override {
+        if (!msg) return BT::NodeStatus::FAILURE; // no detections in list
+
+        std::string target;
+        getInput("target", target);
+
+        auto it = std::find_if(msg->detections.begin(), msg->detections.end(), [target](const guppy_msgs::msg::CornerDetection detection) { return matchesTarget(detection, target); });
+        if (it == msg->detections.end()) return BT::NodeStatus::FAILURE; // no detection matching target
+
+        setOutput("detection", *it);
+
+        return BT::NodeStatus::SUCCESS;
+    }
+private:
+    static bool matchesTarget(const guppy_msgs::msg::CornerDetection detection, const std::string& target) {
+        return detection.name == target;
+    }
+};

src/guppy_nav/include/guppy_nav/change_state_behavior.hpp

@@ -0,0 +1,34 @@
+#pragma once
+
+#include <behaviortree_ros2/bt_service_node.hpp>
+#include <guppy_msgs/srv/change_state.hpp>
+
+class ChangeStateBehavior : public BT::RosServiceNode<guppy_msgs::srv::ChangeState>
+{
+public:
+    ChangeStateBehavior(const std::string& name, const BT::NodeConfig& conf, const BT::RosNodeParams& params)
+    : RosServiceNode<guppy_msgs::srv::ChangeState>(name, conf, params)
+    {}
+
+    static BT::PortsList providedPorts()
+    {
+        return providedBasicPorts({
+            BT::InputPort<uint8_t>("state")
+        });
+    }
+
+    bool setRequest(Request::SharedPtr& request) override
+    {
+        getInput("state", request->new_state.state);
+
+        RCLCPP_DEBUG(logger(), "Request to change State");
+
+        return true;
+    }
+
+    BT::NodeStatus onResponseReceived(const Response::SharedPtr& response) override
+    {
+        RCLCPP_DEBUG(logger(), "ChangeStateNode success");
+        return BT::NodeStatus::SUCCESS;
+    }
+};

src/guppy_nav/include/guppy_nav/face_detection_behavior.hpp

@@ -0,0 +1,77 @@
+#pragma once
+
+#include "guppy_msgs/action/navigate.hpp"
+#include "guppy_msgs/msg/corner_detection.hpp"
+
+#include <behaviortree_ros2/bt_action_node.hpp>
+
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+
+#define CAMERA_RES_X 1920
+#define CAMERA_RES_Y 1080
+
+#define CAMERA_MAX_ANGLE_YAW 1.57079632679
+#define CAMERA_MAX_ANGLE_PITCH 1.57079632679
+
+// https://www.desmos.com/calculator/ivz6gpks8n
+
+class FaceDetectionBehavior: public BT::RosActionNode<guppy_msgs::action::Navigate> {
+public:
+    FaceDetectionBehavior(const std::string& name, const BT::NodeConfig& conf, const BT::RosNodeParams& params)
+    : BT::RosActionNode<guppy_msgs::action::Navigate>(name, conf, params) {}
+
+    static BT::PortsList providedPorts() {
+        return providedBasicPorts({
+            BT::InputPort<double>("detection"),
+            BT::InputPort<double>("timeout"), BT::InputPort<bool>("continueOnTimeout")
+        });
+    }
+
+    bool setGoal(BT::RosActionNode<guppy_msgs::action::Navigate>::Goal& goal) override {
+        guppy_msgs::msg::CornerDetection detection;
+        getInput("detection", detection);
+
+        auto x = 0.0, y = 0.0;
+        for (auto corner : detection.corners) x += corner.x, y += corner.y;
+
+        auto size = detection.corners.size();
+        x /= size, y /= size;
+
+        auto roll = 0.0, pitch = CAMERA_MAX_ANGLE_PITCH * (y / CAMERA_RES_Y),
+            yaw = CAMERA_MAX_ANGLE_YAW * (x / CAMERA_RES_X);
+        Eigen::Quaterniond q = Eigen::AngleAxisd(roll, Eigen::Vector3d::UnitX())
+            * Eigen::AngleAxisd(pitch, Eigen::Vector3d::UnitY())
+            * Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ());
+
+        goal.pose.position.x = goal.pose.position.y = goal.pose.position.z = 0.0; // doesn't move position
+        goal.pose.orientation.w = q.w(), goal.pose.orientation.x = q.x(),
+            goal.pose.orientation.y = q.y(),goal.pose.orientation.z = q.z();
+
+        goal.local = true; // local to cameras so has to be local
+
+        getInput("timeout", goal.timeout);
+        return true;
+    }
+
+    BT::NodeStatus onResultReceived(const WrappedResult& wrapped) override {
+        // should do?
+        return BT::NodeStatus::SUCCESS;
+    }
+
+    virtual BT::NodeStatus onFailure(BT::ActionNodeErrorCode error) override {
+        bool continueOnTimeout = false;
+        getInput("continueOnTimeout", continueOnTimeout);
+        if (continueOnTimeout) {
+            RCLCPP_INFO(logger(), "pose setter action aborted, continuing...");
+            return BT::NodeStatus::SUCCESS;
+        } else {
+            RCLCPP_ERROR(logger(), "pose setter node error... %s", BT::toStr(error));
+            return BT::NodeStatus::FAILURE;
+        }
+    }
+
+    BT::NodeStatus onFeedback(const std::shared_ptr<const Feedback> feedback) {
+        return BT::NodeStatus::RUNNING;
+    }
+};

src/guppy_nav/include/guppy_nav/pose_setter_behavior.hpp

@@ -0,0 +1,53 @@
+#pragma once
+
+#include "guppy_msgs/action/navigate.hpp"
+
+#include <behaviortree_ros2/bt_action_node.hpp>
+
+class NavigateBehavior: public BT::RosActionNode<guppy_msgs::action::Navigate> {
+public:
+    NavigateBehavior(const std::string& name, const BT::NodeConfig& conf, const BT::RosNodeParams& params) : BT::RosActionNode<guppy_msgs::action::Navigate>(name, conf, params) {}
+
+    static BT::PortsList providedPorts() {
+        return providedBasicPorts({
+            BT::InputPort<double>("x"), BT::InputPort<double>("y"), BT::InputPort<double>("z"),
+            BT::InputPort<double>("qw"), BT::InputPort<double>("qx"), BT::InputPort<double>("qy"), BT::InputPort<double>("qz"),
+            BT::InputPort<bool>("local"), BT::InputPort<double>("timeout"),
+            BT::InputPort<bool>("continueOnTimeout")
+        });
+    }
+
+    bool setGoal(BT::RosActionNode<guppy_msgs::action::Navigate>::Goal& goal) override {
+        getInput("x", goal.pose.position.x);
+        getInput("y", goal.pose.position.y);
+        getInput("z", goal.pose.position.z);
+        getInput("qw", goal.pose.orientation.w);
+        getInput("qx", goal.pose.orientation.x);
+        getInput("qy", goal.pose.orientation.y);
+        getInput("qz", goal.pose.orientation.z);
+        getInput("local", goal.local);
+        getInput("timeout", goal.timeout);
+        return true;
+    }
+
+    BT::NodeStatus onResultReceived(const WrappedResult& wrapped) override {
+        // should do?
+        return BT::NodeStatus::SUCCESS;
+    }
+
+    virtual BT::NodeStatus onFailure(BT::ActionNodeErrorCode error) override {
+        bool continueOnTimeout = false;
+        getInput("continueOnTimeout", continueOnTimeout);
+        if (continueOnTimeout) {
+            RCLCPP_INFO(logger(), "navigate action aborted, continuing...");
+            return BT::NodeStatus::SUCCESS;
+        } else {
+            RCLCPP_ERROR(logger(), "pose setter treenode error... %s", BT::toStr(error));
+            return BT::NodeStatus::FAILURE;
+        }
+    }
+
+    BT::NodeStatus onFeedback(const std::shared_ptr<const Feedback> feedback) {
+        return BT::NodeStatus::RUNNING;
+    }
+};

src/guppy_nav/launch/core.xml

@@ -1,5 +1,6 @@
 <?xml version="1.0" encoding="UTF-8"?>
 <launch>
-    <!-- <node pkg="guppy_nav" exec="navigate_action_server"/> -->
-    <node pkg="guppy_nav" exec="the_pose_setter"/>
-</launch>
+    <!-- <node pkg="guppy_nav" exec="navigation_action_server"/> -->
+    <node pkg="guppy_nav" exec="pose_setter_server"/>
+    <node pkg="guppy_nav" exec="behavior_tree"/>
+</launch>

src/guppy_nav/package.xml

@@ -14,6 +14,9 @@
 
   <exec_depend>control_toolbox</exec_depend>
 
+  <depend>behaviortree_cpp</depend>
+  <depend>behaviortree_ros2</depend>
+
   <buildtool_depend>ament_cmake</buildtool_depend>
 
   <test_depend>ament_lint_auto</test_depend>