ARC ROS: Task Coordination (Honours Research Project)

technical: Directed to those in CS. General reader may still keep up.

Previous Post: Behaviour Component Demo

Next Post: ~

Previously in the ARC ROS project, I implemented behavior which allowed robots to interact and act in the environment.
However, behaviours must be put together, in order for robots to complete useful work (ie: a task).
As part of my undergraduate honours project at the UofM, I've extended to a second release of ARC ROS (codename: Anderson), which introduces 4 tasks that robots may perform. Three of the tasks outlined below are isolated to single robots; whereas the last (victim confirmation) task requires coordination and consensus between multiple robots.

Implementing Tasks

A task is defined as some collection of behaviour wrapped in a state machine that completes useful work. Thus if we look at each perceptual/motor schema pair as a lower level controller, then a single task is just a supervisory controller that coordinates behaviours.

Explore

By far the easiest task, involving 3 states which toggle the RandomWander motor schema and let the robot explore the environment for some duration.

Guided Clean Debris

The next task was a bit more difficult to implement, requiring 6 states and 2 behaviours. Given a list of debris locations in the environment, the MoveToGoal behaviour is used to bring the robot near the debris. Once the robot detects the debris, the CleanDebris behaviour is activated to well.. do the cleaning. This process is repeated until the list of debris is clean.
It's sounds easy to do this in theory; but there are many considerations to keep in mind in practice. For example the robot may approach the debris coordinates, but the debris moved. As such the robot must actually see the debris... What happens if it can't see said debris? What if it can't get to the desired debris location? All of this must be handled by the state machine.

Unguided Clean Debris

With 8 states and 3 behaviours this task is an extention of Guided Clean Debris; however no initial list of Debris coordinates is given. As such, the robot must randomly wander until it located debris, then Move to that established goal, and then clean.

Confirm Victim

This is the most intricate task, requiring coordination between multiple robots. Robots are now equipped with either simple, or advanced victim detection sensors in the environment. A simple detection sensor can only detect "potential victims"; while an advanced sensor is required to correctly classify the victim as positive (human) or negative (a tree, for example). You can think of the advanced sensor as being a more complex deep learning approach, whereas the simple sensor is a more primitive heatmap and sonor approach.

Primitive testing

Here is a test showing an isolated robot performing the confirm_victim task. The input is a list of potential victim locations. The robot navigates to each and must broadcast whether or not a victim was found there. If the robot doesn't have an advanced victim detection sensor it will reject the task.

In this clip the first debris bot random wanders until it finds a potential victim, and then broadcasts that it needs assistance. You can see the other robot following it to the victim location. The robot with the advanced detector will broadcast its' result and both robots will then update their internal knowledge-base (victim trackers) with the new information.

Coordination and victim tracking

A VictimTracker module was created to allow robots to manage information on located victims. When a victim is confirmed or not, we store this information in the tracker. Upon receiving future confirm_victim task requests, the robot will check if the potential victim is already in its' tracker (the position is within close proximity of another victim it already detected).

The Task Handler

The biggest challenge was figuring out how to implement tasks in ROS, which at this time, doesn't provide any specific primitives for developing tasks, nor many other aspects of a multi-robot system.
In the current ARC ROS design, a task is received over the /wifi/request topic and fed through to the robots task handler node.

Each task is implemented as a Server and Client. A task (such as cleaning debris) must receive a goal, and then take some amount of time to complete before results are sent to the caller. Furthermore, a task must be preemptable. This means ROS topics and services alone aren't enough; however we can use the actionlib package as a starting point.  The server is a ROS actionlib server which encapsulates a state machine for performing the task. The corresponding task client is an actionlib client which calls upon the corresponding task server and gathers feedback.
The TaskHandler is then implemented as a single node which contains all of the task clients, dynamically loaded as a map from the task name (string) to the action client it corresponds to. This means upon adding a task, you don't have to ever change the task handler. Containing all tasks clients within a single node means that a user doesn't have to worry about managing each of the tasks. Instead, when a task is complete, the task handler can simply publish the result. The task handler is also in charge of ensuring only 1 task can be active at a given time.

Tasks are scheduled using a priority based FIFO policy, meaning there is a queue of tasks waiting to execute and they are dispatched by default in order of arrival. If a task has higher priority or other tasks are unable to execute yet, the order may change.

High-level outline of the task handler. 

Scenario Footage

Here is some demo footage of a full scenario made using ARC ROS. It employs all of the tasks at once, fully autonomous.
This video shows multiple scouts (blue) exploring the environment randomly. They only have basic victim sensors meaning they can at most detect potential victims and then broadcast those location estimates.
The leader bot (red) has an advanced victim sensor and is capable of navigating to the potential victim upon receiving confirm_victim requests. Since it is a heavy robot it doesn't explore at random unless called upon.
The 2 debris bots (grey) can clean debris and locate victims. They cannot classify victims..

Summary

This project is a demonstration of ROS as a functional platform for multi-robot coordination and task management. A lot of future work needs to go into making this code production ready. This proof-of-concept is exciting in the academic setting, as it shows the trend towards easier development of complex robotic systems. My goal is to help bring such proof of concepts to reality.

Major TODOs
I began this project in spring of 2017. This is the first time I've noticed just how drastically my coding style changes over just a single year. I've learned a lot of better software design practices which I've tried to incorporate into the project now but there still exists technical debt I'd like to address.The main source of this debt is that I've created a lot of manual launch file tests. I'd like to make these automated. I also think better unit testing of core components is required. Once this is complete, setting up a continuous integration workflow would be sufficient.

Show me the Code!

Relax! Once my honours project is complete I plan on publishing the code and I'll link to it here. I'd like to make sure I receive permission first.