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