Implement Full Schema using MoveIt2Servo

[egordon]

Description

Implement the full acquisition schema.

Testing procedure

Tested w/ Action 0 (i.e. just straight down and up).

• Pull the updated pr_ros_controllers: https://github.com/personalrobotics/pr_ros_controllers/pull/30
• Tested with dummy carrots in sim
• Tested with real carrots.

Observed occasional errors:

• Misses the carrot (camera calibration? or detection before robot has finished moving above plate, maybe add an 0.5s delay?)
• Delay on twist execution means that the robot does not move as far as expected during extraction. Okay for now, need to investigate later
• Carrot depth occasionally off (TODO later: verify against depth camera)

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]

Re. the following:

or detection before robot has finished moving above plate, maybe add an 0.5s delay?

The app shows the user a live video stream, so assuming the user has at least a 0.5s delay before selecting a point on the screen, that delay is essentially there.

[egordon]

Updates tested again in sim.