Implement Octomap and Misc Bug Fixes

[amalnanavati]

Description

Joint PR with ada_ros2#21.

This PR does the following:

1. Implements multiple filters on the depth image that are necessary to make Octomap work. Without it, the Octomap gets populated with noisy/hallucinated obstacles behind the fork handle.
2. Toggle off the octomap in Acquisition (Grasp + Extract) and verify that acquisition still works.
3. Add extra safeguards to StopServo. Specifically:
   1. Update the timestamp of the 0-velocity message so MoveIt Servo will actually use it (before it was stale).
   2. Add a delay after publishing the o-velocity message to ensure MoveIt Servo acts on it.
   3. Stops the servo controller, in the off-chance that MoveIt Servo did not publish the 0-velocity command to the controller. (This is to be extra-safe).
4. Other small bug/typo fixes.

Testing procedure

Setup:

• Pull ada_ros2#21.
• Pull the amaln/attached_collision_primitive branch of pymoveit2.
• Re-build.
• Start the code as documented in the README (ideally on lovelace).

Testing:

• [ ] Using the app, go through an entire bite and ensure everything works.

Before opening a pull request

• [x] Format your code using black formatter python3 -m black .
• [x] Run your code through pylint and address all warnings/errors. The only warnings that are acceptable to not address is TODOs that should be addressed in a future PR. From the top-level ada_feeding directory, run: pylint --recursive=y --rcfile=.pylintrc ..

Before Merging

• [ ] Squash & Merge

[amalnanavati]

The only lingering TODO is to permanently add a spherical "bite" to the planning scene, attached to forkTip, so that Octomap doesn't hallucinate fixed collision voxels near the fork when it has acquired food.