Retreat to charger in between tasks if the next task will bring the robot below the safety threshold

[mxgrey]

Reported in open-rmf/rmf#566 there are situations where a robot's battery can be brought dangerously low if it is given a long series of small tasks.

The task planner will only insert charging tasks if it estimates that the robot will become unable to make it back to the charger without the charging. However the real battery usage may be greater than the task planner estimates, so the robot's battery may become critical if the robot keeps performing tasks.

Each time before beginning a new task we should estimate if doing the next task would bring the battery level below the threshold and redirect the robot to the charger if it would. We could also consider doing a replan of all tasks since the charging would disrupt the schedule, but this should probably be opt-in behavior.

[alex-roba]

Hi @mxgrey, here's an update regarding the issue: I have only one robot in one of the fleets, and I wrote an automated script to continuously check for running and queued tasks. The script ensures that there is always at least one running task and one queued task. In this scenario, the TaskPlanner will never create a charging task, and the robot will continue performing tasks until the battery is completely drained.

[mxgrey]

If you're able to share that script with us, it would be super helpful for us to validate any fix that we come up with.

Due to a combination of end-of-year administrative housekeeping and folks going on holiday, we might not be able to produce a fix until mid-January, but any community contributions that help with the testing and validation will speed up our ability to fix the problem.

[TanJunKiat]

Not sure if this is the right location, but maybe we can add in a condition check here?

rmf_ros2/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp

Lines 1459 to 1470 in bee9778

	void TaskManager::_begin_next_task()
	{
	if (_active_task)
	{
	return;
	}
	if (_emergency_active)
	{
	return;
	}

The TaskManager can check if the SoC has dropped below the threshold and queue a charging task.

[mxgrey]

A naive and straightforward way to fix this is to just add in a battery check check prior to running each next task. But there are two concerns I have:

1. How much should the inserted task charge the battery? To full? High enough to perform the remaining tasks until the next scheduled charge? To a user-specified threshold?
2. What do we do with the previously planned charging tasks? It would be silly to charge the battery to full, perform one minor task, and then charge the battery to full again just because that second charging task is what the planner originally specified. We should probably replan the charging sequence for the individual robot, probably by generating a task plan that only includes the one robot and its currently assigned tasks.

[TanJunKiat]

Do you think it is right to take references from existing fleet management systems?

For example, Mir Fleet has a few settings that dictate such conditions (reference to doc)

We can add these to the fleet configuration file and pass it downstream to the task manager.

Following this will solve some of our problems, like how long should the robot be charging for, and how much charge the robot needs before leaving the charger.

But that also brings us to a potential situation of a robot being held at the charger station with a queue task and another idle robot with sufficient charge to finish the mission.

[alex-roba]

@mxgrey here the script

import requests
import time
import json
from datetime import datetime

# Constants for API Endpoints
TASK_URL = "http://192.168.10.11:8000/tasks/dispatch_task"
STATUS_URL = "http://192.168.10.11:8000/tasks"
FLEET_STATE_URL = "http://192.168.10.11:8000/fleets/Robot500xw/state"

status_params = {
    'assigned_to': 'nano500xw01',
    'status': 'underway,queued'
}


# Define the robot and fleet we are focusing on
TARGET_ROBOT = "nano500xw01"
TARGET_FLEET = "Robot500xw"


def generate_task_payload():
    current_time = int(time.time() * 1000)  # Current time in milliseconds
    payload = {
        "type": "dispatch_task_request",
        "request": {
            "unix_millis_earliest_start_time": current_time,
            "unix_millis_request_time": current_time,
            "priority": {
                "type": "binary",
                "value": 0
            },
            "category": "compose",
            "description": {
                "category": "transfer_material",
                "phases": [
                    {
                        "activity": {
                            "category": "sequence",
                            "description": {
                                "activities": [
                                    {
                                        "category": "go_to_place",
                                        "description": "pgv_dock5"
                                    }
                                    {
                                        "category": "go_to_place",
                                        "description": "pgv_dock7"
                                    }
                                ]
                            }
                        }
                    }
                ]
            },
            "labels": [""],
            "requester": "rmf_demos_tasks",
            "fleet_name": TARGET_FLEET
        }
    }
    return payload

def send_task():
    task_payload = generate_task_payload()
    # Log the payload before sending
    print(f"Sending task payload: {json.dumps(task_payload, indent=2)}")

    response = requests.post(TASK_URL, json=task_payload)
    if response.status_code == 200:
        print("Task submitted successfully")
    else:
        print(f"Failed to submit task: {response.status_code} - {response.text}")

def get_tasks_status():
    response = requests.get(STATUS_URL, params=status_params, headers={'accept': 'application/json'})

    if response.status_code == 200:
        tasks = response.json()
        return tasks
    else:
        print(f"Failed to retrieve tasks: {response.status_code} - {response.text}")
        return []
    



def manage_tasks():
    while True:

        tasks = get_tasks_status()
        running_tasks = [task for task in tasks if task.get('status') == 'underway']
        queued_tasks = [task for task in tasks if task.get('status') == 'queued']
        
        print(f"Running tasks for {TARGET_ROBOT}: {len(running_tasks)}, Queued tasks: {len(queued_tasks)}")

        # Ensure only 1 task is running
        if len(running_tasks) == 0:
            if len(queued_tasks) < 2:
                send_task()
                print("No running task, adding a new task to queue (up to 2 queued).")
        elif len(running_tasks) == 1 and len(queued_tasks) == 0:
            # Add a task to ensure there's always 1 running and 1 queued
            send_task()
            print("Adding a task to ensure 1 running and 1 queued task.")


        # Sleep for 5 seconds before the next status check
        time.sleep(5)

if __name__ == "__main__":
    manage_tasks()