scoutos / scout / libscout / src / behaviors / Scheduler.cpp @ fa5ca6e3
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 = ℴ |
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 |