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