Revision 76cefba1
Merged scheduler implementations and modified order structrure class
scout/libscout/src/behaviors/Scheduler.cpp | ||
---|---|---|
29 | 29 |
delete waitingRobots; |
30 | 30 |
} |
31 | 31 |
|
32 |
/** @Brief: Add robot to the waiting queue. */ |
|
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 |
*/ |
|
33 | 39 |
void Scheduler::get_task(WH_Robot r) |
34 | 40 |
{ |
35 | 41 |
waitingRobots.push(r); |
... | ... | |
39 | 45 |
void Scheduler::create_orders() |
40 | 46 |
{ |
41 | 47 |
Order a(1,0,0,101,0,0,0); |
42 |
Order b(2,0,0,11,0,0,0);
|
|
43 |
Order c(3,0,0,91,0,0,0);
|
|
44 |
Order d(4,0,0,21,0,0,0);
|
|
45 |
Order e(5,0,0,41,0,0,0);
|
|
48 |
Order b(2,0,0,11,0,0,0); |
|
49 |
Order c(3,0,0,91,0,0,0); |
|
50 |
Order d(4,0,0,21,0,0,0); |
|
51 |
Order e(5,0,0,41,0,0,0); |
|
46 | 52 |
|
47 | 53 |
unassignedOrders.insert(a); |
48 | 54 |
unassignedOrders.insert(b); |
... | ... | |
81 | 87 |
/** @Brief: The scheduling algorithm. Picks a task and pops from the PQWrapper. */ |
82 | 88 |
Order Scheduler::get_next_item() |
83 | 89 |
{ |
90 |
Time time = ros::TIME_MAX; |
|
91 |
|
|
92 |
Order best; |
|
93 |
for(int i=0; i<unassigned_orders.arraySize(); i++) |
|
94 |
{ |
|
95 |
Order order = unassigned_orders.peek(i); |
|
96 |
|
|
97 |
if(order.orderDeadline < time) //TODO: use another function |
|
98 |
{ |
|
99 |
best = order; |
|
100 |
time = order.orderDeadline; |
|
101 |
} |
|
102 |
} |
|
84 | 103 |
|
85 | 104 |
} |
86 | 105 |
|
... | ... | |
90 | 109 |
{ |
91 | 110 |
while (ok()) |
92 | 111 |
{ |
93 |
while(waitingRobots.empty() || unassignedOrders.arraySize()==0) waiting_dance(); |
|
112 |
while(waitingRobots.empty() || unassignedOrders.arraySize()==0) |
|
113 |
waiting_dance(); |
|
114 |
|
|
94 | 115 |
waitingRobots.front().set_task(get_next_item()); |
95 | 116 |
waitingRobots.pop(); |
96 | 117 |
} |
scout/libscout/src/behaviors/Scheduler.h | ||
---|---|---|
1 | 1 |
#ifndef _SCHEDULER_ |
2 | 2 |
#define _SCHEDULER_ |
3 | 3 |
|
4 |
#include "helper_classes/PQWrapper.h" |
|
5 |
#include "helper_classes/Order.h" |
|
4 |
#include "../helper_classes/PQWrapper.h"
|
|
5 |
#include "../helper_classes/Order.h"
|
|
6 | 6 |
#include "WH_Robot.h" |
7 |
#include "../Behavior.h" |
|
7 | 8 |
|
8 |
class Scheduler { |
|
9 |
std::vector<WH_Robot> robots;
|
|
9 |
class Scheduler : Behavior {
|
|
10 |
std::vector<WH_Robot> robots; |
|
10 | 11 |
PQWrapper unassignedOrders; |
11 | 12 |
std::vector<Order> assignedOrders; |
12 | 13 |
std::queue<WH_Robot> waitingRobots; |
14 |
|
|
15 |
void create_orders(); |
|
16 |
|
|
17 |
void waiting_dance(); |
|
18 |
|
|
13 | 19 |
public: |
14 | 20 |
Scheduler(); |
15 | 21 |
~Scheduler(); |
16 | 22 |
|
17 | 23 |
void get_task(WH_Robot robot); |
18 | 24 |
|
19 |
void create_orders(); |
|
20 |
|
|
21 | 25 |
void task_complete(Order o); |
22 | 26 |
void task_failed(Order o); |
23 | 27 |
|
24 |
void waiting_dance(); |
|
25 | 28 |
|
26 | 29 |
Order get_next_item(); |
27 | 30 |
|
... | ... | |
31 | 34 |
|
32 | 35 |
|
33 | 36 |
}; |
34 |
#endif |
|
37 |
#endif |
scout/libscout/src/behaviors/scheduler.cpp | ||
---|---|---|
1 |
#include "scheduler.cpp" |
|
2 |
|
|
3 |
/** @brief Initialize the scheduler */ |
|
4 |
scheduler::scheduler(std::string scoutname) : Behavior(scoutname) |
|
5 |
{ |
|
6 |
waitingOrders = emptyPQ(); |
|
7 |
} |
|
8 |
|
|
9 |
scheduler::~scheduler() |
|
10 |
{ |
|
11 |
//TODO:Free the priority queue here |
|
12 |
return; |
|
13 |
} |
|
14 |
|
|
15 |
void scheduler::run() |
|
16 |
{ |
|
17 |
while(1) |
|
18 |
{ |
|
19 |
//TODO: Everything in here |
|
20 |
//listen for wireless messages, if you here one parse it |
|
21 |
//if message is a addItem message, call addItem on the data |
|
22 |
//if message is a getItem message, call getNextItem and send back the item |
|
23 |
} |
|
24 |
} |
|
25 |
|
|
26 |
void scheduler::addItem(item job) |
|
27 |
{ |
|
28 |
Time job_time = getCompletionTime(item); |
|
29 |
start_by_time = job.deadline - job_time; |
|
30 |
waitingOrders.enQ(job, start_by_time); |
|
31 |
} |
|
32 |
|
|
33 |
item getNextItem() |
|
34 |
{ |
|
35 |
Time time = 999999;//Infinity |
|
36 |
item best; |
|
37 |
for(int i=0; i<10; i++) |
|
38 |
{ |
|
39 |
pq_item order = waitingOrders.get(i); |
|
40 |
//A checker to see if the order must go now? |
|
41 |
if(order.job.deadline < time) |
|
42 |
{ |
|
43 |
best = order.job; |
|
44 |
time = order.job.deadline; |
|
45 |
} |
|
46 |
} |
|
47 |
|
|
48 |
return best; |
|
49 |
|
|
50 |
} |
scout/libscout/src/behaviors/scheduler.h | ||
---|---|---|
1 |
#ifndef _SCHEDULER_ |
|
2 |
#define _SCHEDULER_ |
|
3 |
|
|
4 |
#include <queue> |
|
5 |
#include "../Behavior.h" |
|
6 |
|
|
7 |
//location: where the robot has to go; |
|
8 |
//job_type: what the robot is supposed to do when it gets there |
|
9 |
typedef struct{ |
|
10 |
int location; |
|
11 |
int job_type; |
|
12 |
int deadline; |
|
13 |
} item; |
|
14 |
|
|
15 |
//struct for the items in the pq. TODO: check its validity |
|
16 |
typedef struct { |
|
17 |
item job; |
|
18 |
Time start_time; |
|
19 |
} pq_item; |
|
20 |
|
|
21 |
|
|
22 |
class compare |
|
23 |
{ |
|
24 |
public: |
|
25 |
bool operator() (pq_item a, pq_item b) const |
|
26 |
{ |
|
27 |
return (a.start_time) >= (b.start_time); |
|
28 |
} |
|
29 |
}; |
|
30 |
|
|
31 |
|
|
32 |
|
|
33 |
class scheduler : Behavior |
|
34 |
{ |
|
35 |
public: |
|
36 |
scheduler(std::scoutname); |
|
37 |
~scheduler(); |
|
38 |
|
|
39 |
void run(); |
|
40 |
|
|
41 |
|
|
42 |
private: |
|
43 |
void addItem(item); |
|
44 |
item getNextItem(); |
|
45 |
Time getCompletionTime(item job); |
|
46 |
|
|
47 |
bool compare(pq_item x, pq_item y); |
|
48 |
//TODO:Check the validity of this line. |
|
49 |
priority_queue<pq_item, vector<pq_item>, compare> waitingOrders; |
|
50 |
} |
|
51 |
|
|
52 |
#endif |
scout/libscout/src/helper_classes/Order.cpp | ||
---|---|---|
2 | 2 |
|
3 | 3 |
using namespace std; |
4 | 4 |
|
5 |
/** @Brief: Default order constructor */ |
|
6 |
Order::Order() |
|
5 |
/** @Brief: Regular order constructor */ |
|
6 |
Order::Order(int order_id, Address order_source, Address order_dest, Time order_deadline, Time order_start_time, Path order_path, Time order_est_time) |
|
7 |
{ |
|
8 |
id = order_id; |
|
9 |
source = order_source; |
|
10 |
dest = order_dest; |
|
11 |
start_time = order_start_time; |
|
12 |
path = order_path; |
|
13 |
est_time = order_est_time; |
|
14 |
} |
|
15 |
|
|
16 |
/** @Brief: Get order ID */ |
|
17 |
int Order::getid() const |
|
7 | 18 |
{ |
8 |
orderID = 0; |
|
9 |
orderSource = 0; |
|
10 |
orderDest = 0; |
|
11 |
orderDeadline = 0; |
|
12 |
orderStartTime = 0; |
|
13 |
orderPath = 0; |
|
14 |
orderEstTime = 0; |
|
19 |
return id; |
|
20 |
} |
|
21 |
|
|
22 |
Address get_source() const |
|
23 |
{ |
|
24 |
return source; |
|
15 | 25 |
} |
16 | 26 |
|
17 |
/** @Brief: Regular order constructor */ |
|
18 |
Order::Order(int ID, Address source, Address dest, Time deadline, Time start_time, Path path, Time est_time) { |
|
19 |
orderID = ID; |
|
20 |
orderSource = source; |
|
21 |
orderDest = dest; |
|
22 |
orderDeadline = deadline; |
|
23 |
orderStartTime = start_time; |
|
24 |
orderPath = path; |
|
25 |
orderEstTime = est_time; |
|
27 |
Address get_dest() const |
|
28 |
{ |
|
29 |
return dest; |
|
26 | 30 |
} |
27 | 31 |
|
28 |
/** @Brief: Get priority for the PQWrapper */
|
|
29 |
double Order::getpriority() const {
|
|
30 |
return orderDeadline - orderStartTime;
|
|
32 |
Time get_start_time() const
|
|
33 |
{
|
|
34 |
return start_time;
|
|
31 | 35 |
} |
32 | 36 |
|
33 |
/** @Brief: Get order ID */
|
|
34 |
int Order::getid() const {
|
|
35 |
return orderID;
|
|
36 |
}
|
|
37 |
Path get_path() const
|
|
38 |
{ |
|
39 |
return path;
|
|
40 |
} |
|
37 | 41 |
|
38 |
/** @Brief: Order comparison function for PQWrapper */ |
|
39 |
bool CompareOrder::operator()(Order& o1, Order& o2) { |
|
40 |
return o1.getpriority() > o2.getpriority(); |
|
42 |
void set_path(Path order_path) |
|
43 |
{ |
|
44 |
path = order_path; |
|
45 |
return; |
|
46 |
} |
|
47 |
/** @Brief: Order comparison function for PQWrapper |
|
48 |
* NOTE: In order to have a min priority queue, using c++'s pq |
|
49 |
* implementation, the compare function must return true if |
|
50 |
* o1 is greater than o2. |
|
51 |
*/ |
|
52 |
bool CompareOrder::operator()(Order& o1, Order& o2) |
|
53 |
{ |
|
54 |
int pq_value1 = o1.get_start_time + MAX_WAIT_TIME - o1.get_est_time(); |
|
55 |
int pq_value2 = o2.get_start_time + MAX_WAIT_TIME - o2.get_est_time(); |
|
56 |
return pq_value1 > pq_value2; |
|
41 | 57 |
} |
58 |
|
scout/libscout/src/helper_classes/Order.h | ||
---|---|---|
1 | 1 |
#ifndef _ORDER_ |
2 | 2 |
#define _ORDER_ |
3 | 3 |
|
4 |
#define MAX_WAIT_TIME 120 //2 minutes in seconds |
|
5 |
|
|
4 | 6 |
typedef unsigned int Address; |
5 | 7 |
typedef unsigned int Path; |
6 | 8 |
typedef double Time; |
7 | 9 |
|
8 | 10 |
class Order { |
9 |
int orderID;
|
|
10 |
Address orderSource, orderDest;
|
|
11 |
Time orderDeadline, orderStartTime;
|
|
12 |
Path orderPath;
|
|
13 |
Time orderEstTime;
|
|
11 |
int id;
|
|
12 |
Address source, dest;
|
|
13 |
Time start_time;
|
|
14 |
Path path;
|
|
15 |
Time est_time;
|
|
14 | 16 |
public: |
15 |
Order(); |
|
16 |
Order(int ID, Address source, Address dest, Time deadline, Time start_time, Path path, Time est_time); |
|
17 |
Time getpriority() const; |
|
17 |
Order(int order_id, Address order_source, Address order_dest, Time order_deadline, Time order_start_time, Path order_path, Time order_est_time); |
|
18 |
|
|
18 | 19 |
int getid() const; |
20 |
Address get_source() const; |
|
21 |
Address get_dest() const; |
|
22 |
Time get_start_time() const; |
|
23 |
Path get_path() const; |
|
24 |
void set_path(Path order_path); |
|
25 |
|
|
19 | 26 |
}; |
20 | 27 |
|
21 | 28 |
class CompareOrder { |
22 |
public:
|
|
23 |
bool operator()(Order& o1, Order& o2);
|
|
29 |
public: |
|
30 |
bool operator()(Order& o1, Order& o2);
|
|
24 | 31 |
}; |
25 |
#endif |
|
32 |
|
|
33 |
#endif |
Also available in: Unified diff