Statistics
| Branch: | Revision:

root / scout / libscout / src / behaviors / Scheduler.cpp @ d140fd71

History | View | Annotate | Download (5.52 KB)

1 97b6298e unknown
#include "Scheduler.h"
2 2f025967 Priya
#include "../helper_classes/Order.h"
3 97b6298e unknown
4
using namespace std;
5
6
/** @Brief: Initialize data structures for the scheduler. */
7 d140fd71 Yuyang
Scheduler::Scheduler(std::string scoutname, Sensors* sensors):
8
            Behavior(scoutname, "Scheduler", sensors)
9 97b6298e unknown
{
10 2f025967 Priya
  unassignedOrders = new PQWrapper(NUM_TASKS);
11
12 97b6298e unknown
        create_orders();
13
}
14
15
/** @Brief: Free allocatetd memory. */
16
Scheduler::~Scheduler()
17
{
18 3f72678f Priya
  delete unassignedOrders;
19 97b6298e unknown
}
20
21 76cefba1 Priya
/** @Brief: Add robot to the waiting queue. 
22
 *  A robot calls this function with itself
23
 *  and gets pushed on a list of waiting robots.
24
 *  When a task is availaible the scheduler, removes
25
 *  the robot of the waiting queue, and gives it a
26
 *  task.
27
 */
28 3f72678f Priya
void Scheduler::get_task(int robot)
29 97b6298e unknown
{
30 ad5b5826 Priya
  Robot my_robot = robots[robot-1];
31
  if(my_robot.sched_status != WAITING_ROBOT)
32
  {
33
          waitingRobots.push(my_robot);
34
    my_robot.sched_status = WAITING_ROBOT;
35
  }
36 97b6298e unknown
}
37
38
/** @Brief: Statically add orders to the Priority Queue Wrapper.*/
39
void Scheduler::create_orders()
40
{
41 2f025967 Priya
  Path p;
42
  p.len = 0;
43
  p.path = NULL;
44
  Time t = ros::Time::now();
45
  Duration end(0);
46 5d0687a9 Priya
  Duration five(5);
47
  Duration three(3);
48
  Duration ten(10);
49 58c19c15 Priya
        Order a(1,0,13,t+ten,p,end);
50
  Order b(2,0,14,t+five,p,end);
51
  Order c(3,0,15,t+three,p,end);
52
  Order d(4,0,16,t,p,end);
53 97b6298e unknown
        
54 2f025967 Priya
        unassignedOrders->insert(a);
55
        unassignedOrders->insert(b);
56
        unassignedOrders->insert(c);
57
        unassignedOrders->insert(d);
58 97b6298e unknown
}
59
60
/** @Brief: This is a confirmation that the task is complete. 
61
    This function removes the order from assignedOrders. */
62
void Scheduler::task_complete(Order o)
63
{
64 3f72678f Priya
  ROS_INFO("Scheduler: Task id %d was completed", o.getid());
65
  for (unsigned int i=0; i<assignedOrders.size(); i++)
66
  {
67
    if (assignedOrders[i].getid()==o.getid())
68
    {
69
      assignedOrders.erase(assignedOrders.begin()+i);
70
    }
71
  }
72 97b6298e unknown
}
73
74
/** @Brief: This is a confirmation that the task failed. 
75
    This function places the order back on the PQWrapper. */
76
void Scheduler::task_failed(Order o)
77
{
78 3f72678f Priya
  ROS_INFO("Scheduler: Task id %d was failed", o.getid());
79 97b6298e unknown
        task_complete(o);
80 2f025967 Priya
        unassignedOrders->insert(o);
81 97b6298e unknown
}
82
83
/** @Brief: Do a waiting dance. */
84
void Scheduler::waiting_dance()
85 3f72678f Priya
{ 
86 ad5b5826 Priya
    //ROS_INFO("Scheduler: TEEHEE i do a dance!");
87 97b6298e unknown
}
88
89
/** @Brief: The scheduling algorithm. Picks a task and pops from the PQWrapper. */
90
Order Scheduler::get_next_item()
91
{
92 2f025967 Priya
  Time t = ros::TIME_MAX; 
93
  double time = t.toSec();
94 76cefba1 Priya
95 2f025967 Priya
  Order* best;
96 5d0687a9 Priya
  int best_index;
97 dc742c14 Alex
  for(unsigned int i=0; i<unassignedOrders->arraySize(); i++)
98 76cefba1 Priya
  {
99 2f025967 Priya
    Order order = unassignedOrders->peek(i);
100
101
    Time end_time = order.get_start_time() - order.get_est_time();
102 76cefba1 Priya
103 2f025967 Priya
    if(end_time.toSec() + MAX_WAIT_TIME < time) //TODO: use another function
104 76cefba1 Priya
    {
105 2f025967 Priya
      best = &order;
106 5d0687a9 Priya
      best_index = i;
107 2f025967 Priya
      time = end_time.toSec() + MAX_WAIT_TIME;
108 76cefba1 Priya
    }
109
  }
110 5d0687a9 Priya
111
  Order ret;
112
  ret = unassignedOrders->remove(best_index);
113
  assignedOrders.push_back(ret);
114
  return ret;
115 97b6298e unknown
}
116
117 3f72678f Priya
void Scheduler::msg_callback(const std_msgs::String::ConstPtr& msg)
118
{
119
  if(msg->data.compare(0, 6, "FAILED") == 0)
120
  {
121
    int order_id = atoi(msg->data.substr(7).c_str());
122 7ac5e9bc Priya
    for(unsigned int i=0; i<assignedOrders.size(); i++)
123 3f72678f Priya
    {
124 7ac5e9bc Priya
      if(assignedOrders[i].getid() == order_id)
125 3f72678f Priya
      {
126
        task_failed(assignedOrders[i]);
127
        break;
128
      }
129
    }
130
  }
131
  else if(msg->data.compare(0, 7, "SUCCESS") == 0)
132
  {
133
    int order_id = atoi(msg->data.substr(8).c_str());
134 7ac5e9bc Priya
    for(unsigned int i=0; i<assignedOrders.size(); i++)
135 3f72678f Priya
    {
136 7ac5e9bc Priya
      if(assignedOrders[i].getid() == order_id)
137 3f72678f Priya
      {
138
        task_complete(assignedOrders[i]);
139
        break;
140
      }
141
    }
142
  }
143
  else if(msg->data.compare(0, 8, "GET_TASK") == 0)
144
  {
145 5d0687a9 Priya
    ROS_INFO("SCHEDULER: got get task message");
146 3f72678f Priya
    int robot = atoi(msg->data.substr(9).c_str());
147
    get_task(robot);
148
  }
149
  else if(msg->data.compare(0, 8, "REGISTER") == 0)
150
  {
151 7ac5e9bc Priya
    string robot_name = msg->data.substr(9);
152
    for(unsigned int i=0; i<robots.size(); i++)
153
    {
154 ad5b5826 Priya
      if(robots[i].name.compare(robot_name) == 0)
155
      { 
156 7ac5e9bc Priya
        return;
157 b2876335 roboclub
      }
158 7ac5e9bc Priya
    }
159
160 3f72678f Priya
    int id = robots.size() +1;
161
    Robot new_robot;
162 7ac5e9bc Priya
    new_robot.name = robot_name;
163
    new_robot.topic = node.advertise<std_msgs::String>(new_robot.name + "_topic", 1000);
164 ad5b5826 Priya
    new_robot.sched_status = NEW_ROBOT;
165 3f72678f Priya
    robots.push_back(new_robot);
166
167
    std_msgs::String msg;
168
    std::stringstream ss;
169 7ac5e9bc Priya
    ss<<"REG_SUCCESS "<<id;
170 3f72678f Priya
    msg.data = ss.str();
171 7ac5e9bc Priya
    new_robot.topic.publish(msg);
172 5d0687a9 Priya
    spinOnce();
173 ad5b5826 Priya
174 7ac5e9bc Priya
    ROS_INFO("Registration a success");
175 ad5b5826 Priya
176
    assert(robots.size() > 0);
177 3f72678f Priya
  }
178
  else
179
  {
180 7ac5e9bc Priya
    ROS_INFO("I got a bad message: %s", msg->data.c_str());
181 3f72678f Priya
  }
182
183
  return;
184
}
185
186 97b6298e unknown
/** @Brief: Continuously checks for waiting robots. If no robots are waiting,
187
    this function calls the waiting_dance() function. */
188
void Scheduler::run()
189
{
190 ad5b5826 Priya
  robot_to_sched = node.subscribe("robot_to_sched", 1000, &Scheduler::msg_callback, this);
191 7ac5e9bc Priya
  ROS_INFO("I am a scheduler!. Now waiting for robots");
192 ad5b5826 Priya
  while(robots.size() < 1 && ok())
193
  {
194 7ac5e9bc Priya
    spinOnce();
195 ad5b5826 Priya
  }
196
  ROS_INFO("SCHEDULER: main loop");
197 97b6298e unknown
        while (ok())
198
        {
199 3f72678f Priya
    ROS_INFO("Scheduler running");
200 7ac5e9bc Priya
    spinOnce();
201 5d0687a9 Priya
                while((waitingRobots.empty() || unassignedOrders->arraySize()==0) && ok())
202
    {
203 76cefba1 Priya
      waiting_dance();
204 5d0687a9 Priya
      spinOnce();
205
    }
206 ad5b5826 Priya
  
207
    ROS_INFO("SCHEDULER: Setting task");
208 3f72678f Priya
    Order next = get_next_item();
209 7ac5e9bc Priya
    Robot r = waitingRobots.front();
210
211 ad5b5826 Priya
    ROS_INFO("SCHEDULER: Setting task %d to robot %s", next.getid(), r.name.c_str());
212 3f72678f Priya
    std_msgs::String msg;
213
    std::stringstream ss;
214 7ac5e9bc Priya
    ss<<"SET_TASK "<<next.getid()<<" "<<next.get_source()<<" "<<next.get_dest();
215 351f71d1 Priya
    std::cout<<"msg: "<<ss.str()<<endl;
216 3f72678f Priya
    msg.data = ss.str();
217 7ac5e9bc Priya
    r.topic.publish(msg);
218 5d0687a9 Priya
  
219
    spinOnce();
220 3f72678f Priya
221 ad5b5826 Priya
    waitingRobots.front().sched_status = ORDERED_ROBOT;
222 97b6298e unknown
                waitingRobots.pop();
223
        }
224
}
225
226
227