root / scout / libscout / src / behaviors / Scheduler.cpp @ dc742c14
History | View | Annotate | Download (2.93 KB)
1 |
#include "Scheduler.h" |
---|---|
2 |
#include "../helper_classes/Order.h" |
3 |
|
4 |
using namespace std; |
5 |
|
6 |
/** @Brief: Initialize data structures for the scheduler. */
|
7 |
Scheduler::Scheduler(std::string scoutname):Behavior(scoutname, "Scheduler") |
8 |
{ |
9 |
WH_Robot* rob1 = new WH_Robot("Robot1", this); |
10 |
WH_Robot* rob2 = new WH_Robot("Robot2", this); |
11 |
WH_Robot* rob3 = new WH_Robot("Robot3", this); |
12 |
|
13 |
unassignedOrders = new PQWrapper(NUM_TASKS);
|
14 |
|
15 |
robots.push_back(rob1); |
16 |
robots.push_back(rob2); |
17 |
robots.push_back(rob3); |
18 |
|
19 |
create_orders(); |
20 |
} |
21 |
|
22 |
/** @Brief: Free allocatetd memory. */
|
23 |
Scheduler::~Scheduler() |
24 |
{ |
25 |
delete robots.at(0); |
26 |
delete robots.at(1); |
27 |
delete robots.at(2); |
28 |
|
29 |
delete unassignedOrders;
|
30 |
} |
31 |
|
32 |
/** @Brief: Add robot to the waiting queue.
|
33 |
* A robot calls this function with itself
|
34 |
* and gets pushed on a list of waiting robots.
|
35 |
* When a task is availaible the scheduler, removes
|
36 |
* the robot of the waiting queue, and gives it a
|
37 |
* task.
|
38 |
*/
|
39 |
void Scheduler::get_task(WH_Robot* r)
|
40 |
{ |
41 |
waitingRobots.push(r); |
42 |
} |
43 |
|
44 |
/** @Brief: Statically add orders to the Priority Queue Wrapper.*/
|
45 |
void Scheduler::create_orders()
|
46 |
{ |
47 |
Path p; |
48 |
p.len = 0;
|
49 |
p.path = NULL;
|
50 |
Time t = ros::Time::now(); |
51 |
Duration end(0);
|
52 |
Order a(1,0,1,t,p,end); |
53 |
Order b(2,0,1,t,p,end); |
54 |
Order c(3,0,9,t,p,end); |
55 |
Order d(4,0,2,t,p,end); |
56 |
Order e(5,0,4,t,p,end); |
57 |
|
58 |
unassignedOrders->insert(a); |
59 |
unassignedOrders->insert(b); |
60 |
unassignedOrders->insert(c); |
61 |
unassignedOrders->insert(d); |
62 |
unassignedOrders->insert(e); |
63 |
} |
64 |
|
65 |
/** @Brief: This is a confirmation that the task is complete.
|
66 |
This function removes the order from assignedOrders. */
|
67 |
void Scheduler::task_complete(Order o)
|
68 |
{ |
69 |
for (unsigned int i=0; i<assignedOrders.size(); i++) |
70 |
{ |
71 |
if (assignedOrders[i].getid()==o.getid())
|
72 |
{ |
73 |
assignedOrders.erase(assignedOrders.begin()+i); |
74 |
} |
75 |
} |
76 |
} |
77 |
|
78 |
/** @Brief: This is a confirmation that the task failed.
|
79 |
This function places the order back on the PQWrapper. */
|
80 |
void Scheduler::task_failed(Order o)
|
81 |
{ |
82 |
task_complete(o); |
83 |
unassignedOrders->insert(o); |
84 |
} |
85 |
|
86 |
/** @Brief: Do a waiting dance. */
|
87 |
void Scheduler::waiting_dance()
|
88 |
{ |
89 |
|
90 |
} |
91 |
|
92 |
/** @Brief: The scheduling algorithm. Picks a task and pops from the PQWrapper. */
|
93 |
Order Scheduler::get_next_item() |
94 |
{ |
95 |
Time t = ros::TIME_MAX; |
96 |
double time = t.toSec();
|
97 |
|
98 |
Order* best; |
99 |
for(unsigned int i=0; i<unassignedOrders->arraySize(); i++) |
100 |
{ |
101 |
Order order = unassignedOrders->peek(i); |
102 |
|
103 |
Time end_time = order.get_start_time() - order.get_est_time(); |
104 |
|
105 |
if(end_time.toSec() + MAX_WAIT_TIME < time) //TODO: use another function |
106 |
{ |
107 |
best = ℴ |
108 |
time = end_time.toSec() + MAX_WAIT_TIME; |
109 |
} |
110 |
} |
111 |
return *best;
|
112 |
} |
113 |
|
114 |
/** @Brief: Continuously checks for waiting robots. If no robots are waiting,
|
115 |
this function calls the waiting_dance() function. */
|
116 |
void Scheduler::run()
|
117 |
{ |
118 |
while (ok())
|
119 |
{ |
120 |
while(waitingRobots.empty() || unassignedOrders->arraySize()==0) |
121 |
waiting_dance(); |
122 |
|
123 |
(waitingRobots.front())->set_task(get_next_item()); |
124 |
waitingRobots.pop(); |
125 |
} |
126 |
} |
127 |
|
128 |
|
129 |
|
130 |
|