Colony » Colony Scout » Colony Scout OS

#include "Scheduler.h"

#include "../helper_classes/Order.h"

using namespace std;

/** @Brief: Initialize data structures for the scheduler. */

Scheduler::Scheduler(std::string scoutname):Behavior(scoutname, "Scheduler")

{

        WH_Robot* rob1 = new WH_Robot("Robot1", this);

        WH_Robot* rob2 = new WH_Robot("Robot2", this);

        WH_Robot* rob3 = new WH_Robot("Robot3", this);

  unassignedOrders = new PQWrapper(NUM_TASKS);

        robots.push_back(rob1);

        robots.push_back(rob2);

        robots.push_back(rob3);

        create_orders();

}

/** @Brief: Free allocatetd memory. */

Scheduler::~Scheduler()

{

   delete robots.at(0);

   delete robots.at(1);

   delete robots.at(2);

   delete unassignedOrders;

}

/** @Brief: Add robot to the waiting queue. 

 *  A robot calls this function with itself

 *  and gets pushed on a list of waiting robots.

 *  When a task is availaible the scheduler, removes

 *  the robot of the waiting queue, and gives it a

 *  task.

 */

void Scheduler::get_task(WH_Robot* r)

{

        waitingRobots.push(r);

}

/** @Brief: Statically add orders to the Priority Queue Wrapper.*/

void Scheduler::create_orders()

{

  Path p;

  p.len = 0;

  p.path = NULL;

  Time t = ros::Time::now();

  Duration end(0);

        Order a(1,0,1,t,p,end);

  Order b(2,0,1,t,p,end);

  Order c(3,0,9,t,p,end);

  Order d(4,0,2,t,p,end);

  Order e(5,0,4,t,p,end);

        unassignedOrders->insert(a);

        unassignedOrders->insert(b);

        unassignedOrders->insert(c);

        unassignedOrders->insert(d);

        unassignedOrders->insert(e);

}

/** @Brief: This is a confirmation that the task is complete. 

    This function removes the order from assignedOrders. */

void Scheduler::task_complete(Order o)

{

    for (unsigned int i=0; i<assignedOrders.size(); i++)

        {

            if (assignedOrders[i].getid()==o.getid())

                {

                        assignedOrders.erase(assignedOrders.begin()+i);

                }

        }

}

/** @Brief: This is a confirmation that the task failed. 

    This function places the order back on the PQWrapper. */

void Scheduler::task_failed(Order o)

{

        task_complete(o);

        unassignedOrders->insert(o);

}

/** @Brief: Do a waiting dance. */

void Scheduler::waiting_dance()

{

}

/** @Brief: The scheduling algorithm. Picks a task and pops from the PQWrapper. */

Order Scheduler::get_next_item()

{

  Time t = ros::TIME_MAX; 

  double time = t.toSec();

  Order* best;

  for(unsigned int i=0; i<unassignedOrders->arraySize(); i++)

  {

    Order order = unassignedOrders->peek(i);

    Time end_time = order.get_start_time() - order.get_est_time();

    if(end_time.toSec() + MAX_WAIT_TIME < time) //TODO: use another function

    {

      best = ℴ

      time = end_time.toSec() + MAX_WAIT_TIME;

    }

  }

  return *best;

}

/** @Brief: Continuously checks for waiting robots. If no robots are waiting,

    this function calls the waiting_dance() function. */

void Scheduler::run()

{

        while (ok())

        {

                while(waitingRobots.empty() || unassignedOrders->arraySize()==0) 

      waiting_dance();

                (waitingRobots.front())->set_task(get_next_item());

                waitingRobots.pop();

        }

}