Added ROS action server to send hierarchical tasks to plan and execute

[marcvinci]

A version of this task server was already present in the ai-demo-day branch. The commits here are based on that and updated to the changes done in the refactored domain in this pull request.

Summary of changes:

• Adds a simple_action_server and corresponding ros actions to retrieve hierarchical tasks. When retrieving a task, the planning is triggered with the given task and the plan is executed.
• Adds a simple_action_client to give tasks easily over the command line e.g. rosrun tables_demo_planning task_server_client.py move_item mobipick multimeter_1 table_2

The action server receives all item locations as a parameter from the parameter server. This is needed so that tasks like move_item, which requires the knowledge of item positions, can be executed (item search is not part of the move_item task). Additionally this parameter is needed to create UP objects of what items are present in the environment, similar to the demo_items variable in the other demo nodes e.g. demo_items. The locations can also be set to be "unknown" and then the search task can be given as a separate task before other tasks as well.

[alexander-sung]

Sounds and looks great! To proceed one more step, wouldn't it be a good opportunity to get rid of the ambiguous parameter definitions (Python scripts and parameter server) we have?

The default values here could be removed: https://github.com/DFKI-NI/mobipick_labs/blob/task-server/tables_demo_planning/nodes/task_server_node.py#L26 - in the current implementation, they could be replaced but not deleted using the parameter server. Also regarding this hack: https://github.com/DFKI-NI/mobipick_labs/blob/task-server/tables_demo_planning/nodes/task_server_node.py#L88, if we forget the existing item locations one day, we probably will wonder why some configured values are not read in correctly. Or is it too early still?

[sebastianstock]

Looks good. I also agree with the suggestions by @alexander-sung to get rid of those parameter definitions.

[marcvinci]

@alexander-sung So how do we load the object locations onto the parameter server?

My suggestion is to then just create another config file like tables_demo_bringup/config/initial_items.yaml, which contains the objects used in the demo and their initial location. The contents would look like this:

initial_item_locations: {
  "multimeter_1": "table_3",
  "relay_1": "table_3",
  "screwdriver_1": "table_3",
  "power_drill_with_grip_1": "table_2",
  "hot_glue_gun_1": "table_3",
  "klt_1": "table_2",
  "klt_2": "table_1",
  "klt_3": "table_3",
}

This file can then replace the demo_items variable used in all of our demo starting ros nodes e.g. in tables_demo_node.py or in hierarchical_demo_node.py and others. We then also only use the hard-coded locations in the task_server_node and in the other nodes only the keys of the dictionary to specify, which items are present.

[alexander-sung]

Yes! The dict in the code shall be replaced by one configured in the yaml file. The naming and location are just fine as you proposed. As discussed, this config file would then be read and loaded onto the parameter server by the imported TablesDemoAPI.

[marcvinci]

@alexander-sung @sebastianstock I pushed the changes to use a config file loaded onto the parameter server to replace the demo_items variable present in all our demo starting nodes.

[alexander-sung]

How is the "task_server_client.py" command from your pull request description to be used? Nothing happens after launching the sim, then calling this command. Is it to be used in combination? Please update the README with a sample use-case.

[marcvinci]

@alexander-sung I will update the README with example usage.

For now you can just do the following:

roslaunch tables_demo_bringup demo_sim.launch
rosrun tables_demo_planning task_server_node.py
rosrun tables_demo_planning task_server_client.py task_name parameter1 parameter2 ...

One example to test could be rosrun tables_demo_planning task_server_client.py move_item mobipick multimeter_1 table_2

[alexander-sung]

Okay thanks, it works nicely - but only once for the same item because after placing it, the robot does not know where it is. We could keep it for now and let the item search handle this, or we could update the believed item locations. Would you agree on the second proposal?

[sebastianstock]

Okay thanks, it works nicely - but only once for the same item because after placing it, the robot does not know where it is. We could keep it for now and let the item search handle this, or we could update the believed item locations. Would you agree on the second proposal?

In simulation I have the issue that at one point it updates the believed_item_locations such that the object is on_robot. But this is not updated again after placing it. We should definitely fix this as the robot cannot do any object manipulations afterwards. I will take a look.

[alexander-sung]

https://github.com/DFKI-NI/mobipick_labs/blob/noetic/tables_demo_planning/src/tables_demo_planning/tables_demo.py#L366 https://github.com/DFKI-NI/mobipick_labs/blob/noetic/tables_demo_planning/src/tables_demo_planning/tables_demo.py#L374 https://github.com/DFKI-NI/mobipick_labs/blob/noetic/tables_demo_planning/src/tables_demo_planning/tables_demo.py#L383 are supposed to update the item location accordingly, and https://github.com/DFKI-NI/mobipick_labs/blob/noetic/tables_demo_planning/src/tables_demo_planning/tables_demo_api.py calls these functions. It would be interesting to see in which case there's no update and why.

[alexander-sung]

Anyway, the missing location update does not seem to be directly related to this pull request after all.

[sebastianstock]

Strangely, now this problem does not show up, anymore. The believed_item_locations are updated as expected.