Table Detection

[sriramk117]

Description

Overview

This PR implements a node for table detection. As of now, the system uses a fixed table position and orientation in the planning scene. This PR addresses the issue of tables having different heights by computing the pose of the detected table from a camera image and moving the table mesh object to the calculated pose in the planning scene.

The table detection node depends on a few assumptions to accurately detect the table's pose: (1) a plate should be in the camera view for the table detection node to publish the table's pose, (2) the plate is circular and is the largest circle in the camera view, and (3) the area within the buffer radius of 50 pixels around the plate is mostly obstacle-free.

ROS2 Interfaces

The node exposes a toggle service (toggle_table_detection) that turns a table detection publisher on and off. The publisher (table_detection) publishes a PoseStamped message consisting of the table center and the table’s orientation as a quaternion from the perspective of the camera frame.

Implementation Details

We use OpenCV’s Hough Circle detection to detect a circle around the plate, draw a larger concentric circle around the plate with a specific buffer radius, and capture points in the area between these two circles representative of the table. We then clean this data (remove noise and outliers), deproject these points to a point cloud with respect to the perspective of the camera frame, and then fit a plane to the table. From this table plane, we derive the center of the table and its orientation.

We noticed that large obstructions in the immediate area around the plate (the area between the detected plate and the larger concentric circle drawn around the plate) such as a caregiver moving their hand across the plate, can significantly alter the calculated orientation of the table. Additionally, these obstructions can be pretty frequent. So, to counteract this we use a threshold for the angular distance between the table's orientation and the table's default orientation in the planning scene. If the latest calculated orientation is within the threshold, it is accepted and otherwise, it is rejected resulting in the table using the default orientation.

Testing procedures

Test by running web app with mock robot:

• Run the mock robot, start the web app, and run dummy nodes for perception: python3 src/ada_feeding/start.py --sim mock
• Terminate the dummy nodes in the perception screen session and launch the real perception nodes within that session:
  • Reattach to perception screen session: 'screen -r perception'
  • Terminate the dummy nodes via Ctrl+C
  • Launch the real perception nodes within the screen: ros2 launch ada_feeding_perception ada_feeding_perception.launch.py
• On the web app, click the “Start Feeding” button and stay on the bite selection page.
• To visually test: on RViz, see that the updated table location is representative of the actual position and orientation of the table from the web app’s camera image.
  • Also, create a visualization for the pose that gets published by the /table_detection topic.
  • Set the shape of the pose to “Axes” from “Arrow”

Test by running web app with real robot:

• Run the real robot and start the web app: python3 src/ada_feeding/start.py --dev
• On the web app, click the “Start Feeding” button and stay on the bite selection page.
• To visually test: on RViz, see that the updated table location is representative of the actual position and orientation of the table from the web app’s camera image.
  • Also, create a visualization for the pose that gets published by the /table_detection topic.
  • Set the shape of the pose to “Axes” from “Arrow”

Test plate detection with real or mock robot:

• Follow either of the testing procedures above to run the web app with the real or mock robot.
• Set the value of the "viz" parameter in the table_detection.yaml file to True.
• On the web app, click the “Start Feeding” button and stay on the bite selection page.
• Then, on RViz, create a visualization for the Image that gets published by the /plate_detection_img topic
  • See that a circle is being drawn around the plate in the camera image so that the plate is being accurately detected by the table detection node.

Testing details:

The functionality of the table detection node was assessed using various tests by running the web app with the real robot. To verify dynamic table pose detection, we slowly lift one side of the table and check if the orientation of the table mesh object in RViz updates accordingly.

As stated in the implementation details, a threshold is used to prevent any obstructions in the camera image from significantly altering the detected table pose. To test this threshold, we place objects in the area between the plate and the larger concentric circle around the plate drawn from the plate detection algorithm (see details in the testing procedure above for creating a visualization for the plate detection images in RViz to refer to). Then, we test that the pose of the table mesh in RViz is still representative of the real table's pose from the camera's frame of perspective and does not change to a noticeably inaccurate pose.

Another way to test the threshold is by placing your hand directly over the plate or around the plate such that much of the area between the plate and the larger concentric circle around the plate drawn from plate detection is covered by your hand, and once again seeing that the pose of the table mesh either changes to the default table pose or doesn't change significantly.

Testing Results:

Here, we list the results of the table detection node using various camera images to test the accuracy of table detection.

Test 1 Camera image with plate detection: Table position and orientation in the planning scene:

Test 2 Camera image with plate detection: Table position and orientation in the planning scene:

Test 3 Camera image with plate detection: Table position and orientation in the planning scene:

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]

Also, a few small things about your PR description:

First:

This PR addresses the issue of tables having different heights and various orientations

Correct me if I’m wrong, but this PR does not address the issue of tables having various orientations, right? Since it just picks an arbitrary orientation relative to the camera, not a perceived table orientation.

Second, your test order is incorrect / unnecessary. Because you launch perception nodes, but then run the startup script, which also launches perception nodes. So either you’ll have two copies of the perception nodes running, or the first copy will be terminated as soon as the second copy launches.

Please edit the PR description accordingly