Implement Octomap and Misc Bug Fixes (#125)

* Fixed watchdog F/T, added camera info to repub, added behavior logs

* Added mask to republisher

* added mask

* Add manually-defined shadow to mask

* Added temporal and spatial postprocessors

* Added thresholding

* Added extra safeguards when stopping Servo

* Add persistent attached food sphere to forkTip

* Updated save_image script

* Removed food collision object as Octomap doesn't properly filter it out

See here for more: moveit/moveit_ros#315

* Added a separate depth octomap topic that removes the forktip and food

* Addressed PR comments

.pylintrc

@@ -26,13 +26,15 @@ clear-cache-post-run=no
 # be loaded. Extensions are loading into the active Python interpreter and may
 # run arbitrary code.
 extension-pkg-allow-list=cv2,
+                         cv,
                          tf2_py
 
 # A comma-separated list of package or module names from where C extensions may
 # be loaded. Extensions are loading into the active Python interpreter and may
 # run arbitrary code. (This is an alternative name to extension-pkg-allow-list
 # for backward compatibility.)
 extension-pkg-whitelist=cv2,
+                        cv,
                         tf2_py
 
 # Return non-zero exit code if any of these messages/categories are detected,

ada_feeding/ada_feeding/behaviors/acquisition/compute_action_constraints.py

@@ -122,6 +122,7 @@ def setup(self, **kwargs):
     @override
     def update(self) -> py_trees.common.Status:
         # Docstring copied from @override
+        self.logger.info(f"{self.name} [{self.__class__.__name__}::update()]")
 
         # Input Validation
         if not self.blackboard_exists("action_select_response"):

ada_feeding/ada_feeding/idioms/__init__.py

@@ -6,3 +6,4 @@
 from .pre_moveto_config import pre_moveto_config
 from .retry_call_ros_service import retry_call_ros_service
 from .scoped_behavior import scoped_behavior
+from .wait_for_secs import wait_for_secs

ada_feeding/ada_feeding/idioms/wait_for_secs.py

@@ -0,0 +1,38 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+"""
+This module defines the wait_for_secs idiom, which returns a behavior that
+waits for a specified number of seconds.
+"""
+
+# Standard imports
+
+# Third-party imports
+import py_trees
+
+
+def wait_for_secs(
+    name: str,
+    secs: float,
+) -> py_trees.behaviour.Behaviour:
+    """
+    Creates a behavior that returns RUNNING for a specified number of secs
+    and then returns SUCCESS.
+
+    Parameters
+    ----------
+    name: The name of the behavior.
+    secs: The number of seconds to wait.
+    """
+
+    # pylint: disable=abstract-class-instantiated
+    # Creating a Running behavior is not instantiating an abstract class.
+
+    return py_trees.decorators.FailureIsSuccess(
+        name=name,
+        child=py_trees.decorators.Timeout(
+            name=name + "Timeout",
+            duration=secs,
+            child=py_trees.behaviours.Running(),
+        ),
+    )

ada_feeding/ada_feeding/trees/acquire_food_tree.py

@@ -97,6 +97,8 @@ def create_tree(
         )
 
         ### Define Tree Logic
+        # NOTE: If octomap clearing ends up being an issue, we should
+        # consider adding a call to the /clear_octomap service to this tree.
         # Root Sequence
         root_seq = py_trees.composites.Sequence(
             name=name,
@@ -160,7 +162,7 @@ def create_tree(
                     ),
                 ),
                 # Re-Tare FT Sensor and default to 4N threshold
-                pre_moveto_config(name="PreAquireFTTare"),
+                pre_moveto_config(name="PreAcquireFTTare"),
                 ### Move Above Food
                 MoveIt2PoseConstraint(
                     name="MoveAbovePose",
@@ -222,10 +224,10 @@ def create_tree(
                                 ],
                             ),
                             ToggleCollisionObject(
-                                name="AllowTable",
+                                name="AllowTableAndOctomap",
                                 ns=name,
                                 inputs={
-                                    "collision_object_ids": ["table"],
+                                    "collision_object_ids": ["table", "<octomap>"],
                                     "allow": True,
                                 },
                             ),
@@ -237,10 +239,10 @@ def create_tree(
                         children=[
                             pre_moveto_config(name="PostAcquireFTSet", re_tare=False),
                             ToggleCollisionObject(
-                                name="DisallowTable",
+                                name="DisallowTableAndOctomap",
                                 ns=name,
                                 inputs={
-                                    "collision_object_ids": ["table"],
+                                    "collision_object_ids": ["table", "<octomap>"],
                                     "allow": False,
                                 },
                             ),
@@ -362,7 +364,7 @@ def create_tree(
                                 ),  # Auto Zero-Twist on terminate()
                                 ### Extraction
                                 ComputeActionTwist(
-                                    name="ComputeGrasp",
+                                    name="ComputeExtract",
                                     ns=name,
                                     inputs={
                                         "action": BlackboardKey("action"),

ada_feeding/ada_feeding/trees/start_servo_tree.py

@@ -60,6 +60,7 @@ def create_tree(
             activate_controllers=[self.servo_controller_name],
             deactivate_controllers=[self.move_group_controller_name],
             activate_asap=True,
+            strictness=SwitchController.Request.BEST_EFFORT,
         )
         switch_controllers_key_response = Blackboard.separator.join(
             [name, "switch_controllers", "response"]

ada_feeding/ada_feeding/trees/stop_servo_tree.py

@@ -8,6 +8,7 @@
 import operator
 
 # Third-party imports
+from controller_manager_msgs.srv import SwitchController
 from geometry_msgs.msg import Twist, TwistStamped
 from overrides import override
 import py_trees
@@ -19,21 +20,28 @@
 from std_srvs.srv import Trigger
 
 # Local imports
-from ada_feeding.idioms import retry_call_ros_service
+from ada_feeding.behaviors import UpdateTimestamp
+from ada_feeding.helpers import BlackboardKey
+from ada_feeding.idioms import retry_call_ros_service, wait_for_secs
 from ada_feeding.trees import TriggerTree
 
 
 class StopServoTree(TriggerTree):
     """
     This behavior tree does the following:
       1. Pubishes one 0-velocity twist message to `~/servo_twist_cmds`.
-      2. Calls the `~/stop_servo` service to stop MoveIt Servo
+      2. Waits for `delay` seconds, to allow servo to publish stop commands
+         to the controller.
+      3. Calls the `~/stop_servo` service to stop MoveIt Servo.
+      4. Calls the `~/switch_controller` service to turn off the servo controller.
     """
 
     def __init__(
         self,
         node: Node,
         base_frame_id: str = "j2n6s200_link_base",
+        servo_controller_name: str = "jaco_arm_servo_controller",
+        delay: float = 0.5,
     ) -> None:
         """
         Initializes the behavior tree.
@@ -47,6 +55,8 @@ def __init__(
         # Initialize the TriggerTree class
         super().__init__(node=node)
         self.base_frame_id = base_frame_id
+        self.servo_controller_name = servo_controller_name
+        self.delay = delay
 
     @override
     def create_tree(
@@ -75,6 +85,17 @@ def create_tree(
             ),
         )
 
+        # Update the timestamp of the twist message so its not stale
+        update_timestamp = UpdateTimestamp(
+            name=name + "Update Timestamp",
+            inputs={
+                "stamped_msg": BlackboardKey(twist_key),
+            },
+            outputs={
+                "stamped_msg": BlackboardKey(twist_key),
+            },
+        )
+
         # Create the behavior to publish the twist message
         twist_pub = py_trees_ros.publishers.FromBlackboard(
             name=name + "Publish Twist",
@@ -84,6 +105,9 @@ def create_tree(
             blackboard_variable=twist_key,
         )
 
+        # Add a delay to allow servo to publish stop commands
+        delay_behavior = wait_for_secs(name + "Delay", self.delay)
+
         # Create the behavior to stop servo
         stop_servo_key_response = Blackboard.separator.join(
             [name, "stop_servo", "response"]
@@ -104,12 +128,44 @@ def create_tree(
             ],
         )
 
+        # Create the behavior to turn off the controllers
+        stop_controller_req = SwitchController.Request(
+            activate_controllers=[],
+            deactivate_controllers=[self.servo_controller_name],
+            activate_asap=True,
+            strictness=SwitchController.Request.BEST_EFFORT,
+        )
+        stop_controllers_key_response = Blackboard.separator.join(
+            [name, "switch_controllers", "response"]
+        )
+        stop_controllers = retry_call_ros_service(
+            name=name + "Activate Servo Controller",
+            service_type=SwitchController,
+            service_name="~/switch_controller",
+            key_request=None,
+            request=stop_controller_req,
+            key_response=stop_controllers_key_response,
+            response_checks=[
+                py_trees.common.ComparisonExpression(
+                    variable=stop_controllers_key_response + ".ok",
+                    value=True,
+                    operator=operator.eq,
+                )
+            ],
+        )
+
         # Put them together in a sequence with memory
         # pylint: disable=duplicate-code
         return py_trees.trees.BehaviourTree(
             root=py_trees.composites.Sequence(
                 name=name,
                 memory=True,
-                children=[twist_pub, stop_servo],
+                children=[
+                    update_timestamp,
+                    twist_pub,
+                    delay_behavior,
+                    stop_servo,
+                    stop_controllers,
+                ],
             )
         )

ada_feeding/config/ada_planning_scene.yaml

@@ -9,12 +9,17 @@ ada_planning_scene:
       - workspace_wall_front
       - workspace_wall_left
       - workspace_wall_top
+      # - food
     # For each object, specify:
     #   - Shape: EITHER `filepath` (for a mesh) OR `primitive_type` and
     #     `primitive_dims`(for a primitive -- see shape_msgs/SolidPrimitive.msg)
     #   - Pose: `position` and `quat_xyzw` (see geometry_msgs/Pose.msg)
     #   - Frame ID: `frame_id` (the frame_id of the object that the pose is
     #     relative to)
+    #   - [Optional] to attach the collision object specify `attached` to be True.
+    #     In that case, `frame_id` must be a link on the robot to attach the object
+    #     to, and `touch_links` must be a list of additional links that should be
+    #     ignored for collision checking.
     wheelchair: # the wheelchair mesh
       filename: wheelchair.stl
       position: [0.02, -0.02, -0.05]
@@ -55,4 +60,12 @@ ada_planning_scene:
       primitive_dims: [1.59, 1.5, 0.01] # Box has 3 dims: [x, y, z]
       position: [-0.05, 0.17, 1.05]
       quat_xyzw: [0.0, 0.0, 0.0, 1.0]
-      frame_id: root # the frame_id that the pose is relative to
+      frame_id: root # the frame_id that the pose is relative to
+    food:
+      primitive_type: 2 # Sphere=2. See shape_msgs/SolidPrimitive.msg
+      primitive_dims: [0.05] # Sphere has 1 dim: [radius]
+      position: [0.0, 0.0, 0.0]
+      quat_xyzw: [0.0, 0.0, 0.0, 1.0]
+      attached: True
+      frame_id: forkTip # the frame_id that the pose is relative to
+      touch_links: [forkTine] # the links, in addition to frame_id, that should be ignored for collision checking