Auto dig server unit test

.devcontainer/devcontainer.json

@@ -48,35 +48,20 @@
 				"files.autoSave": "afterDelay",
 				"editor.formatOnSave": true,
 				"[python]": {
-					"editor.formatOnPaste": false,
-					"editor.formatOnSaveMode": "file"
-				},
-				"[workbench.view.extension]": {
-					"ms-python.python": false
+					"editor.defaultFormatter": "ms-python.black-formatter",
+					"editor.formatOnSave": true,
+					"editor.codeActionsOnSave": {
+					"source.fixAll": "explicit"
+					}
 				},
 				"black-formatter.args": [
 					"--line-length",
 					"120"
 				],
-				"[C_Cpp]": {
-					"default.cppStandard": "c++17",
-					"default.cStandard": "c99"
+				"[c++]": {
+					"editor.defaultFormatter": "ms-vscode.cpptools"
 				},
-				"ruff.lineLength": 99,
-				"ruff.lint.enable": true,
-				"ruff.lint.select": [
-					"A",
-					"C",
-					"CNL",
-					"D",
-					"E",
-					"F",
-					"I",
-					"Q",
-					"W"
-				],
-				"ruff.nativeServer": true,
-				"C_Cpp.clang_format_fallbackStyle": "Google",
+				"C_Cpp.clang_format_fallbackStyle": "{ ColumnLimit: 120 }",
 				"diffEditor.ignoreTrimWhitespace": false,
 				// Make bash the default shell in the container terminal
 				"terminal.integrated.defaultProfile.linux": "bash"

.gitmodules

@@ -30,3 +30,9 @@
 	path = src/isaac_ros/isaac_ros_nvblox
 	url = https://github.com/GOFIRST-Robotics/isaac_ros_nvblox.git
 	branch = main
+[submodule "src/behaviortree_ros2"]
+	path = src/behaviortree_ros2
+	url = https://github.com/BehaviorTree/BehaviorTree.ROS2.git
+[submodule "src/behaviortree_cpp"]
+	path = src/behaviortree_cpp
+	url = https://github.com/BehaviorTree/BehaviorTree.CPP.git

config/rovr_control.yaml

@@ -15,4 +15,5 @@
     # Locations of all dig sites to check, as [x, y] pairs
     # Ros param lists must be flattened, so this is a list of all x values followed by all y values
     # https://discord.com/channels/893292981094858754/896192143570403340/1345192968629387294
-    all_dig_locations: [.6, .37, .6, 1.1, .6, 1.83, 1.8, .37, 1.8, 1.1, 1.8, 1.83, 3.0, .37, 3.0, 1.1, 3.0, 1.83, .6, 2.5, 1.8, 2.5, 3.0, 2.5]
+    all_dig_locations: [2.0, 2.0, .7, .7, .7, 1.4, .7, 2.1, 1.9, .7, 1.9, 1.4, 1.9, 2.1, 3.0, .7, 3.0, 1.4, 3.0, 2.1, .7, 2.8, 1.9, 2.8, 3.0, 2.8]
+    # all_dig_locations: [.6, .37, .6, 1.1, .6, 1.83, 1.8, .37, 1.8, 1.1, 1.8, 1.83, 3.0, .37, 3.0, 1.1, 3.0, 1.83, .6, 2.5, 1.8, 2.5, 3.0, 2.5]

src/apriltag/apriltag/apriltag_node.py

@@ -100,7 +100,7 @@ def reset_callback(self, request, response):
     # Publish transform if the tag is detected
     def postTransform(self, tag):
         if tag and (self.get_clock().now().to_msg().sec == tag.header.stamp.sec):
-            self.get_logger().info(str("Resetting the map -> odom TF"))
+            self.get_logger().info("Resetting the map -> odom TF", throttle_duration_sec=5)
             self.map_transform = tag
             return True
         return False

src/behaviortree_executor/CMakeLists.txt

@@ -0,0 +1,47 @@
+cmake_minimum_required(VERSION 3.8)
+project(behaviortree_executor)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(behaviortree_cpp REQUIRED)
+find_package(behaviortree_ros2 REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rovr_interfaces REQUIRED)
+find_package(std_srvs REQUIRED)
+
+add_executable(behaviortree_executor src/behaviortree_executor.cpp)
+ament_target_dependencies(behaviortree_executor rclcpp std_srvs behaviortree_ros2 behaviortree_cpp rovr_interfaces)
+
+install(DIRECTORY
+    behavior_trees
+    config
+    launch
+    DESTINATION share/${PROJECT_NAME}/
+)
+
+install(TARGETS 
+  behaviortree_executor
+  DESTINATION lib/${PROJECT_NAME}
+)
+
+target_include_directories(behaviortree_executor PUBLIC
+  ${CMAKE_CURRENT_SOURCE_DIR}/include
+  ${CMAKE_CURRENT_SOURCE_DIR}/src/action)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  # the following line skips the linter which checks for copyrights
+  # comment the line when a copyright and license is added to all source files
+  set(ament_cmake_copyright_FOUND TRUE)
+  # the following line skips cpplint (only works in a git repo)
+  # comment the line when this package is in a git repo and when
+  # a copyright and license is added to all source files
+  set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()

src/behaviortree_executor/behavior_trees/main_tree.xml

@@ -0,0 +1,34 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<root BTCPP_format="4">
+  <BehaviorTree ID="MainTree">
+    <Fallback>
+      <Sequence>
+        <RetryUntilSuccessful num_attempts="5">
+          <CalibrateFieldCoordinates action_name="/calibrate_field_coordinates"/>
+        </RetryUntilSuccessful>
+        <KeepRunningUntilFailure>
+          <Sequence>
+            <GoToDigLocation action_name="/go_to_dig_location"/>
+            <AutoDig action_name="/auto_dig"/>
+            <Script code="msg:=&quot;Go To Offload Zone&quot;"/>
+            <AutoOffload action_name="/auto_offload"/>
+          </Sequence>
+        </KeepRunningUntilFailure>
+      </Sequence>
+      <Script code="msg:=&apos;Manual Override&apos;"/>
+    </Fallback>
+  </BehaviorTree>
+
+  <!-- Description of Node Models (used by Groot) -->
+  <TreeNodesModel>
+    <Action ID="AutoDig"
+            editable="true"/>
+    <Action ID="AutoOffload"
+            editable="true"/>
+    <Action ID="CalibrateFieldCoordinates"
+            editable="true"/>
+    <Action ID="GoToDigLocation"
+            editable="true"/>
+  </TreeNodesModel>
+
+</root>

src/behaviortree_executor/config/bt_executor.yaml

@@ -0,0 +1,9 @@
+bt_action_server:
+  ros__parameters:
+    action_name: "behavior_server" # Optional (defaults to `bt_action_server`)
+    tick_frequency: 100 # Optional (defaults to 100 Hz)
+    groot2_port: 1667 # Optional (defaults to 1667)
+    ros_plugins_timeout: 1000 # Optional (defaults 1000 ms)
+
+    behavior_trees:
+      - behaviortree_executor/behavior_trees

src/behaviortree_executor/launch/behavior_tree_launch.py

@@ -0,0 +1,53 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from launch.substitutions import PathJoinSubstitution
+from launch_ros.substitutions import FindPackageShare
+
+
+def generate_launch_description():
+    ld = LaunchDescription()
+
+    auto_dig_server = Node(
+        package="rovr_control",
+        executable="auto_dig_server",
+        name="auto_dig_server",
+    )
+    auto_offload_server = Node(
+        package="rovr_control",
+        executable="auto_offload_server",
+        name="auto_offload_server",
+    )
+
+    calibrate_field_coordinate_server = Node(
+        package="rovr_control",
+        executable="calibrate_field_coordinate_server",
+        name="calibrate_field_coordinate_server",
+    )
+
+    dig_location_server = Node(
+        package="rovr_control",
+        executable="dig_location_server",
+        name="dig_location_server",
+    )
+
+    behaviortree_executor = Node(
+        package="behaviortree_executor",
+        executable="behaviortree_executor",
+        name="bt_action_server",
+        output="screen",
+        parameters=[
+            PathJoinSubstitution([
+                FindPackageShare('behaviortree_executor'),
+                'config',
+                'bt_executor.yaml'
+            ]),
+        ],
+        emulate_tty=True,
+    )
+    ld.add_action(auto_dig_server)
+    ld.add_action(auto_offload_server)
+    ld.add_action(calibrate_field_coordinate_server)
+    ld.add_action(dig_location_server)
+    ld.add_action(behaviortree_executor)
+
+    return ld

src/behaviortree_executor/package.xml

@@ -0,0 +1,36 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>behaviortree_executor</name>
+  <version>0.1.0</version>
+  <description>This is the behavior tree of the robot</description>
+  <maintainer email="hwvxyeej@gmail.com">Adam Yaj</maintainer>
+  <license>MIT License</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <build_depend>rclpy</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_depend>geometry_msgs</build_depend>
+  <build_depend>sensor_msgs</build_depend>
+  <build_depend>tf2_msgs</build_depend>
+  <build_depend>rovr_interfaces</build_depend>
+
+  <exec_depend>rclpy</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+  <exec_depend>geometry_msgs</exec_depend>
+  <exec_depend>sensor_msgs</exec_depend>
+  <exec_depend>tf2_msgs</exec_depend>
+  <exec_depend>ros2launch</exec_depend>
+  <exec_depend>nav2_simple_commander</exec_depend>
+  <exec_depend>std_srvs</exec_depend>
+  <exec_depend>behaviortree_cpp</exec_depend>
+  <exec_depend>behaviortree_ros2</exec_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

src/behaviortree_executor/src/action/auto_dig.hpp

@@ -0,0 +1,33 @@
+#include "rovr_interfaces/action/auto_dig.hpp"
+
+#include "behaviortree_cpp/tree_node.h"
+#include "behaviortree_ros2/bt_action_node.hpp"
+#include "behaviortree_ros2/ros_node_params.hpp"
+
+using AutoDig = rovr_interfaces::action::AutoDig;
+using namespace BT;
+
+class AutoDigAction : public RosActionNode<AutoDig>
+{
+public:
+    AutoDigAction(const std::string &name, const BT::NodeConfig &conf,
+                  const BT::RosNodeParams &params)
+        : RosActionNode<AutoDig>(name, conf, params)
+    {
+    }
+
+    bool setGoal(Goal &goal) override
+    {
+        // get inputs from the Input port
+        getInput("lift_digging_start_position", goal.lift_digging_start_position);
+        getInput("lift_digging_end_position", goal.lift_digging_end_position);
+        getInput("digger_belt_power", goal.digger_belt_power);
+        // return true, if we were able to set the goal correctly.
+        return true;
+    }
+
+    NodeStatus onResultReceived(__attribute__((unused)) const WrappedResult &result) override
+    {
+        return NodeStatus::SUCCESS;
+    }
+};

src/behaviortree_executor/src/action/auto_offload.hpp

@@ -0,0 +1,35 @@
+// Someone please double check this is correct
+#include "rovr_interfaces/action/auto_offload.hpp"
+
+#include "behaviortree_cpp/tree_node.h"
+#include "behaviortree_ros2/bt_action_node.hpp"
+#include "behaviortree_ros2/ros_node_params.hpp"
+
+using AutoOffload = rovr_interfaces::action::AutoOffload;
+using namespace BT;
+
+class AutoOffloadAction : public RosActionNode<AutoOffload>
+{
+public:
+    AutoOffloadAction(const std::string &name, const BT::NodeConfig &conf,
+                      const BT::RosNodeParams &params)
+
+        : RosActionNode<AutoOffload>(name, conf, params)
+    {
+    }
+
+    bool setGoal(Goal &goal) override
+    {
+        // get inputs from the Input port
+        getInput("lift_dumping_position", goal.lift_dumping_position);
+        getInput("digger_belt_power", goal.digger_belt_power);
+        // return true, if we were able to set the goal correctly.
+        return true;
+    }
+
+    // TODO: finish this
+    NodeStatus onResultReceived(__attribute__((unused)) const WrappedResult &result) override
+    {
+        return NodeStatus::SUCCESS;
+    }
+};

src/behaviortree_executor/src/action/calibrate_field_coordinates.hpp

@@ -0,0 +1,28 @@
+#include "rovr_interfaces/action/calibrate_field_coordinates.hpp"
+
+#include "behaviortree_cpp/tree_node.h"
+#include "behaviortree_ros2/bt_action_node.hpp"
+#include "behaviortree_ros2/ros_node_params.hpp"
+
+using CalibrateFieldCoordinates = rovr_interfaces::action::CalibrateFieldCoordinates;
+using namespace BT;
+
+class CalibrateFieldCoordinateAction : public RosActionNode<CalibrateFieldCoordinates>
+{
+public:
+    CalibrateFieldCoordinateAction(const std::string &name, const BT::NodeConfig &conf,
+                                   const BT::RosNodeParams &params)
+        : RosActionNode<CalibrateFieldCoordinates>(name, conf, params)
+    {
+    }
+
+    bool setGoal(__attribute__((unused)) Goal &goal) override
+    {
+        return true;
+    }
+
+    NodeStatus onResultReceived(__attribute__((unused)) const WrappedResult &result) override
+    {
+        return NodeStatus::SUCCESS;
+    }
+};

src/behaviortree_executor/src/action/go_to_dig_location.hpp

@@ -0,0 +1,28 @@
+#include "rovr_interfaces/action/go_to_dig_location.hpp"
+
+#include "behaviortree_cpp/tree_node.h"
+#include "behaviortree_ros2/bt_action_node.hpp"
+#include "behaviortree_ros2/ros_node_params.hpp"
+
+using GoToDigLocation = rovr_interfaces::action::GoToDigLocation;
+using namespace BT;
+
+class GoToDigLocationAction : public RosActionNode<GoToDigLocation>
+{
+public:
+    GoToDigLocationAction(const std::string& name, const BT::NodeConfig& conf,
+        const BT::RosNodeParams& params)
+        : RosActionNode<GoToDigLocation>(name, conf, params)
+    {
+    }
+
+    bool setGoal(__attribute__((unused)) Goal &goal) override
+    {
+        return true;
+    }
+
+    NodeStatus onResultReceived(__attribute__((unused)) const WrappedResult &result) override
+    {
+        return NodeStatus::SUCCESS;
+    }
+};