AcquireFood MVP (action 0 only)

[egordon]

Description

Pre-MVP (Action 0, straight up-and-down only) AcquireFood tree completed.

Also fixes a semantic bug where the Cartesian MoveIt2 Planner ignores the frame_id of constraints. So MoveIt2Plan now transforms position/orientation constraints into the base link frame if doing a cartesian plan.

Testing procedure

Copied and modified from #102 :

1. Run the Mock FT Sensor: ros2 run ada_feeding dummy_ft_sensor.py
2. Run the action servers: ros2 launch ada_feeding ada_feeding_launch.xml use_estop:=false
3. Run MoveIt in sim: ros2 launch ada_moveit demo.launch.py sim:=mock
4. Run this code with the default dummy carrot (oriented horizontally in the camera frame): ros2 launch feeding_web_app_ros2_test feeding_dummy_acquirefood_launch.py
5. Verify the Goal Succeeds (status=0). Open RViz and add an Axis Frame "food" (rescale to 0.1 for visibility), verify that +Z is against gravity and +X is horizontal relative to the camera.
6. Run this code with the dummy carrot 2 (oriented ~vertically in the camera frame): ros2 launch feeding_web_app_ros2_test feeding_dummy_acquirefood_launch.py request:=above_plate_2_carrot_request.pkl
7. Verify the Goal Succeeds (status=0). Open RViz and add an Axis Frame "food" (rescale to 0.1 for visibility), verify that +Z is against gravity and +X is vertical relative to the camera and to the "lower right" in the camera frame.
8. For both of the above, verify that the robot: moves above the food frame with fork tines perpendicular to the red +X axis, moves down so the center of the fork tines are located at the origin of the food frame, moves up 5cm above the food frame, and then returns to the resting position.

Testing can also follow the entire feeding demo from the app.

Currently, this has been tested only in simulation.

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