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")

{

  unassignedOrders = new PQWrapper(NUM_TASKS);

  robot_to_sched = node.subscribe("robot_to_sched", 1000, &Scheduler::msg_callback, this);

        create_orders();

}

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

Scheduler::~Scheduler()

{

  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(int robot)

{

        waitingRobots.push(robots[robot-1]);

}

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

void Scheduler::create_orders()

{

  ROS_INFO("Scheduler: Creating 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);

  ROS_INFO("Scheduler: Orders created. Adding to list");

        unassignedOrders->insert(a);

        unassignedOrders->insert(b);

        unassignedOrders->insert(c);

        unassignedOrders->insert(d);

        unassignedOrders->insert(e);

  ROS_INFO("Scheduler: Orders initialized in list");

}

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

    This function removes the order from assignedOrders. */

void Scheduler::task_complete(Order o)

{

  ROS_INFO("Scheduler: Task id %d was completed", o.getid());

  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)

{

  ROS_INFO("Scheduler: Task id %d was failed", o.getid());

        task_complete(o);

        unassignedOrders->insert(o);

}

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

void Scheduler::waiting_dance()

{ 

    ROS_INFO("Scheduler: TEEHEE i do a 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;

}

void Scheduler::msg_callback(const std_msgs::String::ConstPtr& msg)

{

  ROS_INFO("I got a message: %s", msg->data.c_str());

  if(msg->data.compare(0, 6, "FAILED") == 0)

  {

    int order_id = atoi(msg->data.substr(7).c_str());

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

    {

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

      {

        task_failed(assignedOrders[i]);

        break;

      }

    }

  }

  else if(msg->data.compare(0, 7, "SUCCESS") == 0)

  {

    int order_id = atoi(msg->data.substr(8).c_str());

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

    {

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

      {

        task_complete(assignedOrders[i]);

        break;

      }

    }

  }

  else if(msg->data.compare(0, 8, "GET_TASK") == 0)

  {

    int robot = atoi(msg->data.substr(9).c_str());

    get_task(robot);

  }

  else if(msg->data.compare(0, 8, "REGISTER") == 0)

  {

    string robot_name = msg->data.substr(9);

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

    {

      if(robots[i].name.compare(robot_name) == 0){

        return;

      }

    }

    int id = robots.size() +1;

    Robot new_robot;

    new_robot.name = robot_name;

    new_robot.topic = node.advertise<std_msgs::String>(new_robot.name + "_topic", 1000);

    robots.push_back(new_robot);

    ROS_INFO("Registration a robot %s", new_robot.name.c_str());

    std_msgs::String msg;

    std::stringstream ss;

    ss<<"REG_SUCCESS "<<id;

    msg.data = ss.str();

    new_robot.topic.publish(msg);

    ROS_INFO("Registration a success");

  }

  else

  {

    ROS_INFO("I got a bad message: %s", msg->data.c_str());

  }

  return;

}

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

    this function calls the waiting_dance() function. */

void Scheduler::run()

{

  ROS_INFO("I am a scheduler!. Now waiting for robots");

  while(robots.size() < 3)

    spinOnce();

        while (ok())

        {

    ROS_INFO("Scheduler running");

    spinOnce();

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

      waiting_dance();

    Order next = get_next_item();

    Robot r = waitingRobots.front();

    std_msgs::String msg;

    std::stringstream ss;

    ss<<"SET_TASK "<<next.getid()<<" "<<next.get_source()<<" "<<next.get_dest();

    msg.data = ss.str();

    r.topic.publish(msg);

                waitingRobots.pop();

        }

}