Tables demo using plexmo

[sebastianstock]

This pull request adds a variant of the tables demo that uses plexmo. It moves the main functionality of the tables_demo node into TableDemoRobotAPI such that it can be used by plexmo as well.

This closes #9

[alexander-sung]

Small suggestion to rename tables_demo_robotapi.py to tables_demo_api.py since it contains more than a robot class and is not limited to the robot_api. The name is quite unspecified, though, so improvements are welcome.

[alexander-sung]

I receive a lot of unsatisfied preconditions messages during the uplexmo_tables_demo. Is it the same for you, @sebastianstock?

Additionally, the State is: message is very long.

Edit: Both obsolete due to following messages.

[alexander-sung]

The action execution and result resolution unfortunately introduces a bug, which I encountered before myself: While searching for the multimeter to pick it up, the Mobipick finds the box, thus aborts the search. But instead of replanning right away, it first tries the next action in the plan of picking up the multimeter, which fails with a warning: [WARN] Cannot find multimeter_1 at table_2. Pick up FAILED! The original tables demo handles this case with an unintuitive fall-through to replanning directly, which is not nice. I think we need to rework the overall flow of the demo. Root cause of this problem is that there is more than successful and failed actions, i.e. we need to distinguish between the reasons of success and failure. Additionally, the search branch in contrast to the overall sequential plan continues on failure and ends on success. This differentiation is addressed by, e.g., behavior trees better than in our current execution loops.

Edit: Here the fall-through is triggered: https://github.com/DFKI-NI/mobipick_labs/blob/noetic/tables_demo_planning/src/tables_demo_planning/tables_demo.py#L482, and https://github.com/DFKI-NI/mobipick_labs/blob/noetic/tables_demo_planning/src/tables_demo_planning/tables_demo.py#L517 is the corresponding replanning. Yes, it's not nice, and you can also see the reason here: We distinguish between the action succeeding, failing, or being canceled. Can uplexmo handle this?

[alexander-sung]

The monitor.check_preconditions() call in uplexmo_tables_demo_node.py will fail for the search action because we use symbolic planning symbols there (tool_search_pose, tool_search_location) which do not match to actual poses. Reason for this is that we don't know at planning time if and where the robot will find the item it is searching for. The resolve_search_location() method is resolving exactly this, using the environment representation for symbols in the application domain. For the preconditions check we will need a similar resolution (once the wrong state problem is resolved).

[alexander-sung]

I just rebased on changes to noetic in order to avoid a "merge commit hell". See https://stackoverflow.com/questions/16358418/how-to-avoid-merge-commit-hell-on-github-bitbucket for explanations.

Edit: No, this actually doesn't work as intended. It creates duplicate commits (same contents, different hash), so I reverted it.

[sebastianstock]

Thank you @alexander-sung for the detailed review!

I agree with you that we can merge it now and resolve those issues later on.

The action execution and result resolution unfortunately introduces a bug, which I encountered before myself: While searching for the multimeter to pick it up, the Mobipick finds the box, thus aborts the search. But instead of replanning right away, it first tries the next action in the plan of picking up the multimeter, which fails with a warning: [WARN] Cannot find multimeter_1 at table_2. Pick up FAILED! The original tables demo handles this case with an unintuitive fall-through to replanning directly, which is not nice. I think we need to rework the overall flow of the demo. Root cause of this problem is that there is more than successful and failed actions, i.e. we need to distinguish between the reasons of success and failure. Additionally, the search branch in contrast to the overall sequential plan continues on failure and ends on success. This differentiation is addressed by, e.g., behavior trees better than in our current execution loops.

Edit: Here the fall-through is triggered: https://github.com/DFKI-NI/mobipick_labs/blob/noetic/tables_demo_planning/src/tables_demo_planning/tables_demo.py#L482, and https://github.com/DFKI-NI/mobipick_labs/blob/noetic/tables_demo_planning/src/tables_demo_planning/tables_demo.py#L517 is the corresponding replanning. Yes, it's not nice, and you can also see the reason here: We distinguish between the action succeeding, failing, or being canceled. Can uplexmo handle this?

Thanks for pointing this out. I think currently, the SequentialPlanDispatcher in uplexmo does not distinguish cancelled and failed. I think this goes into the direction of those re-planning rules, that we were discussing.

[sebastianstock]

Thank you @alexander-sung for the detailed review!

I agree that we merge it now and resolve those issues afterwards.