Implement behavior tree (#22)

* Started development of a cleaned up dockerfile

* Continued docker setup

* Finished up updated dockerfile

* here we go again

* Removed armv7

* Added requirement for vtk

* Replaced kinpy installation with fork

* Added updated docker compose script

* Moved tf to blue

* Started integration of a bezier curve planner

* Split kinematics into separate package

* Started arming behaviors

* Removed drake stuff and finished arming behaviors

* Created base controller

* Started implementing trajectory controller

* wip: continued joint trajectory controller implementation

* Finished initial version of trajectory interface

* Rebuild image to include blue changes

* Finished joint trajectory controller

* wip: finished initial version of behavior tree

* removed branch from pipeline

* Started debugging tree

* Fixed data gathering behaviors

* Finished debugging behavior tree

* Finished initial version of behavior tree

* Added gripper and cleanup behaviors

* started trajectory replanner

* Cleaned up params

* cleaned up behavior tree

* Resolved pr comments as much as i feel like resolving them

* Fixed bug in frame names

* updated issue templates

.dockerignore

@@ -3,10 +3,15 @@
 
 # Except the following
 !angler_bringup
+!angler_control
+!angler_description
 !angler_planning
-!angler_msgs
 !angler_mux
+!angler_utils
+!angler_kinematics
+!angler_behaviors
 !angler.repos
+!sim.repos
 !.docker/entrypoints
 !requirements-build.txt
 !requirements-dev.txt

.github/ISSUE_TEMPLATE/bug-report.yaml

@@ -1,7 +1,7 @@
 name: Bug Report
 description: Report a bug.
 title: "[BUG]: <Please write a descriptive title after the '[BUG]: ' prefix>"
-labels: [bug]
+labels: [bug, needs triage]
 
 body:
   - type: markdown

.github/ISSUE_TEMPLATE/documentation.yaml

@@ -1,7 +1,7 @@
 name: Documentation Improvement
 description: Report an issue related to the Angler documentation.
 title: "[DOC]: <Please write a descriptive title after the '[DOC]: ' prefix>"
-labels: [documentation]
+labels: [documentation, needs triage]
 
 body:
   - type: markdown

.github/ISSUE_TEMPLATE/feature-request.yaml

@@ -1,7 +1,7 @@
 name: Feature Request
 description: Suggest a new idea for the project.
 title: "[FEATURE]: <Please write a descriptive title after the '[FEATURE]: ' prefix>"
-labels: [enhancement]
+labels: [enhancement, needs triage]
 
 body:
   - type: markdown

.github/workflows/ci.yaml

@@ -31,7 +31,7 @@ jobs:
           submodules: recursive
 
       - name: Log into registry
-        uses: docker/login-action@v2.0.0
+        uses: docker/login-action@v2.2.0
         with:
           registry: ghcr.io
           username: ${{ github.actor }}

.github/workflows/docker.yaml

@@ -31,7 +31,7 @@ jobs:
 
       - name: Log into registry
         if: env.PUSH == 'true'
-        uses: docker/login-action@v2.0.0
+        uses: docker/login-action@v2.2.0
         with:
           registry: ghcr.io
           username: ${{ github.actor }}
@@ -40,14 +40,14 @@ jobs:
       - name: Extract Docker metadata
         if: env.PUSH == 'true'
         id: meta
-        uses: docker/metadata-action@v4.0.1
+        uses: docker/metadata-action@v4.6.0
         with:
           images: ghcr.io/${{ github.repository }}
           tags: |
             type=raw,value=${{ matrix.ROS_DISTRO }}-${{ github.job }}
 
       - name: Build and push Docker image
-        uses: docker/build-push-action@v3.0.0
+        uses: docker/build-push-action@v4.1.1
         with:
           context: .
           file: .docker/Dockerfile
@@ -56,3 +56,51 @@ jobs:
           tags: ${{ steps.meta.outputs.tags }}
           labels: ${{ steps.meta.outputs.labels }}
           push: ${{ env.PUSH }}
+
+  robot:
+    strategy:
+      fail-fast: false
+      matrix:
+        ROS_DISTRO: [humble, rolling, iron]
+    runs-on: ubuntu-latest
+    permissions:
+      packages: write
+      contents: read
+    steps:
+      - name: Checkout repository
+        uses: actions/checkout@v3.5.3
+
+      - name: Set up QEMU
+        uses: docker/setup-qemu-action@v2.2.0
+
+      - name: Set up Docker Buildx
+        uses: docker/setup-buildx-action@v2
+
+      - name: Log into registry
+        if: env.PUSH == 'true'
+        uses: docker/login-action@v2.2.0
+        with:
+          registry: ghcr.io
+          username: ${{ github.actor }}
+          password: ${{ secrets.GITHUB_TOKEN }}
+
+      - name: Extract Docker metadata
+        if: env.PUSH == 'true'
+        id: meta
+        uses: docker/metadata-action@v4.6.0
+        with:
+          images: ghcr.io/${{ github.repository }}
+          tags: |
+            type=raw,value=${{ matrix.ROS_DISTRO }}-${{ github.job }}
+
+      - name: Build and push Docker image
+        uses: docker/build-push-action@v4.1.1
+        with:
+          context: .
+          file: .docker/Dockerfile
+          build-args: ROS_DISTRO=${{ matrix.ROS_DISTRO }}
+          target: ${{ github.job }}
+          tags: ${{ steps.meta.outputs.tags }}
+          labels: ${{ steps.meta.outputs.labels }}
+          push: ${{ env.PUSH }}
+          platforms: linux/amd64,linux/arm64

angler.repos

@@ -10,17 +10,12 @@ repositories:
     url: https://github.com/evan-palmer/blue.git
     version: main
 
-  ros_gz:
+  kinpy:
     type: git
-    url: https://github.com/gazebosim/ros_gz
-    version: ros2
+    url: https://github.com/evan-palmer/kinpy.git
+    version: master
 
-  gz-plugin:
+  py_trees_ros_viewer:
     type: git
-    url: https://github.com/gazebosim/gz-plugin.git
-    version: gz-plugin2
-
-  gz_ros2_control:
-    type: git
-    url: https://github.com/evan-palmer/gz_ros2_control.git
-    version: patch-rolling-garden
+    url: https://github.com/splintered-reality/py_trees_ros_viewer.git
+    version: devel

angler_behaviors/LICENSE

@@ -0,0 +1,17 @@
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in
+all copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
+THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+THE SOFTWARE.

angler_behaviors/behavior_tree/behaviors/cleanup.py

@@ -0,0 +1,31 @@
+# Copyright 2023, Evan Palmer
+#
+# Permission is hereby granted, free of charge, to any person obtaining a copy
+# of this software and associated documentation files (the "Software"), to deal
+# in the Software without restriction, including without limitation the rights
+# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+# copies of the Software, and to permit persons to whom the Software is
+# furnished to do so, subject to the following conditions:
+#
+# The above copyright notice and this permission notice shall be included in
+# all copies or substantial portions of the Software.
+#
+# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
+# THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+# THE SOFTWARE.
+
+import py_trees
+
+
+def make_on_mission_complete_behavior() -> py_trees.behaviour.Behaviour:
+    """Make a behavior that initiates a post-mission completion sequence.
+
+    Returns:
+        A post-mission behavior.
+    """
+    # For now, we just idle on mission completion
+    return py_trees.behaviours.Running("Idle")

angler_behaviors/behavior_tree/behaviors/configure.py

@@ -0,0 +1,64 @@
+# Copyright 2023, Evan Palmer
+#
+# Permission is hereby granted, free of charge, to any person obtaining a copy
+# of this software and associated documentation files (the "Software"), to deal
+# in the Software without restriction, including without limitation the rights
+# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+# copies of the Software, and to permit persons to whom the Software is
+# furnished to do so, subject to the following conditions:
+#
+# The above copyright notice and this permission notice shall be included in
+# all copies or substantial portions of the Software.
+#
+# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
+# THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+# THE SOFTWARE.
+
+import operator
+
+import py_trees
+from behavior_tree.primitives.arming import make_system_arming_behavior
+
+
+def make_setup_behavior(
+    setup_finished_flag_key: str, armed_key: str
+) -> py_trees.behaviour.Behaviour:
+    """Make a behavior that sets up the system prior to beginning a mission.
+
+    Args:
+        setup_finished_flag_key: The key at which the setup flag is stored.
+
+    Returns:
+        A system setup behavior.
+    """
+    # Don't repeat setup if we have already done it
+    check_setup_finished = py_trees.behaviours.CheckBlackboardVariableValue(
+        name="Setup complete?",
+        check=py_trees.common.ComparisonExpression(
+            variable=setup_finished_flag_key, value=True, operator=operator.eq
+        ),
+    )
+
+    # Once we have a plan, arm so that we can start trajectory tracking
+    arming = make_system_arming_behavior(
+        arm=True, armed_key=armed_key, use_passthrough_mode=True
+    )
+
+    # Finish up by indicating that the setup has finished
+    finished_setup = py_trees.behaviours.SetBlackboardVariable(
+        "Setup finished!", setup_finished_flag_key, True, overwrite=True
+    )
+
+    setup = py_trees.composites.Sequence(
+        name="Setup the system for a mission",
+        memory=True,
+        children=[arming, finished_setup],
+    )
+
+    return py_trees.composites.Selector(
+        name="Run system setup", memory=False, children=[check_setup_finished, setup]
+    )

angler_behaviors/behavior_tree/behaviors/mission.py

@@ -0,0 +1,95 @@
+# Copyright 2023, Evan Palmer
+#
+# Permission is hereby granted, free of charge, to any person obtaining a copy
+# of this software and associated documentation files (the "Software"), to deal
+# in the Software without restriction, including without limitation the rights
+# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+# copies of the Software, and to permit persons to whom the Software is
+# furnished to do so, subject to the following conditions:
+#
+# The above copyright notice and this permission notice shall be included in
+# all copies or substantial portions of the Software.
+#
+# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
+# THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+# THE SOFTWARE.
+
+import operator
+
+import py_trees
+import py_trees_ros
+from behavior_tree.primitives.control import make_execute_multidof_trajectory_behavior
+from behavior_tree.primitives.planning import make_high_level_planning_behavior
+
+
+def make_save_start_mission_behavior(
+    start_mission_key: str,
+) -> py_trees.behaviour.Behaviour:
+    """Make a behavior that saves the "start mission" flag to the blackboard.
+
+    Args:
+        start_mission_key: The key at which the flag should be saved.
+
+    Returns:
+        A behavior that saves the key to trigger arming.
+    """
+    return py_trees_ros.subscribers.EventToBlackboard(
+        name="ROS2BB: Start mission",
+        topic_name="/angler/cmd/start_mission",
+        qos_profile=py_trees_ros.utilities.qos_profile_unlatched(),
+        variable_name=start_mission_key,
+    )
+
+
+def make_execute_mission_behavior(
+    start_mission_key: str,
+    robot_state_key: str,
+    planner_id: str,
+    controller_id: str,
+) -> py_trees.behaviour.Behaviour:
+    """Make a behavior that executes a mission.
+
+    Args:
+        start_mission_key: The key at which the signal indicating that a mission should
+            start is stored.
+        robot_state_key: The key at which the robot state is stored.
+        planner_id: The key at which the high-level planner ID is stored.
+        controller_id: The key at which the joint trajectory controller ID is
+            stored.
+
+    Returns:
+        A system setup behavior.
+    """
+    # Start by checking whether or not to start the mission
+    check_start_mission = py_trees.behaviours.CheckBlackboardVariableValue(
+        name="Start the mission?",
+        check=py_trees.common.ComparisonExpression(
+            variable=start_mission_key, value=True, operator=operator.eq
+        ),
+    )
+
+    planning_result_key = "planning_result"
+
+    get_mission_plan = make_high_level_planning_behavior(
+        robot_state_key=robot_state_key,
+        planner_id=planner_id,
+        planning_result_key=planning_result_key,
+    )
+
+    execute_mission = make_execute_multidof_trajectory_behavior(
+        trajectory_key=planning_result_key, controller_id=controller_id
+    )
+
+    return py_trees.composites.Sequence(
+        name="Execute mission",
+        memory=True,
+        children=[
+            check_start_mission,
+            get_mission_plan,
+            execute_mission,
+        ],
+    )