Revision ad5b5826
ID | ad5b58260593f9a00266e17ccdb1aaed3d72460e |
some fixes to whrobot and scheduler
scout/libscout/src/behaviors/Scheduler.cpp | ||
---|---|---|
8 | 8 |
{ |
9 | 9 |
unassignedOrders = new PQWrapper(NUM_TASKS); |
10 | 10 |
|
11 |
robot_to_sched = node.subscribe("robot_to_sched", 1000, &Scheduler::msg_callback, this); |
|
12 |
|
|
13 | 11 |
create_orders(); |
14 | 12 |
} |
15 | 13 |
|
... | ... | |
28 | 26 |
*/ |
29 | 27 |
void Scheduler::get_task(int robot) |
30 | 28 |
{ |
31 |
waitingRobots.push(robots[robot-1]); |
|
29 |
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 |
} |
|
32 | 38 |
} |
33 | 39 |
|
34 | 40 |
/** @Brief: Statically add orders to the Priority Queue Wrapper.*/ |
35 | 41 |
void Scheduler::create_orders() |
36 | 42 |
{ |
37 |
ROS_INFO("Scheduler: Creating orders..."); |
|
38 |
|
|
39 | 43 |
Path p; |
40 | 44 |
p.len = 0; |
41 | 45 |
p.path = NULL; |
... | ... | |
47 | 51 |
Order d(4,0,2,t,p,end); |
48 | 52 |
Order e(5,0,4,t,p,end); |
49 | 53 |
|
50 |
ROS_INFO("Scheduler: Orders created. Adding to list"); |
|
51 |
|
|
52 | 54 |
unassignedOrders->insert(a); |
53 | 55 |
unassignedOrders->insert(b); |
54 | 56 |
unassignedOrders->insert(c); |
55 | 57 |
unassignedOrders->insert(d); |
56 | 58 |
unassignedOrders->insert(e); |
57 |
|
|
58 |
ROS_INFO("Scheduler: Orders initialized in list"); |
|
59 | 59 |
} |
60 | 60 |
|
61 | 61 |
/** @Brief: This is a confirmation that the task is complete. |
... | ... | |
84 | 84 |
/** @Brief: Do a waiting dance. */ |
85 | 85 |
void Scheduler::waiting_dance() |
86 | 86 |
{ |
87 |
ROS_INFO("Scheduler: TEEHEE i do a dance!"); |
|
87 |
//ROS_INFO("Scheduler: TEEHEE i do a dance!");
|
|
88 | 88 |
} |
89 | 89 |
|
90 | 90 |
/** @Brief: The scheduling algorithm. Picks a task and pops from the PQWrapper. */ |
... | ... | |
111 | 111 |
|
112 | 112 |
void Scheduler::msg_callback(const std_msgs::String::ConstPtr& msg) |
113 | 113 |
{ |
114 |
ROS_INFO("I got a message: %s", msg->data.c_str()); |
|
115 | 114 |
if(msg->data.compare(0, 6, "FAILED") == 0) |
116 | 115 |
{ |
117 | 116 |
int order_id = atoi(msg->data.substr(7).c_str()); |
... | ... | |
146 | 145 |
string robot_name = msg->data.substr(9); |
147 | 146 |
for(unsigned int i=0; i<robots.size(); i++) |
148 | 147 |
{ |
149 |
if(robots[i].name.compare(robot_name) == 0){ |
|
148 |
if(robots[i].name.compare(robot_name) == 0) |
|
149 |
{ |
|
150 | 150 |
return; |
151 | 151 |
} |
152 | 152 |
} |
... | ... | |
155 | 155 |
Robot new_robot; |
156 | 156 |
new_robot.name = robot_name; |
157 | 157 |
new_robot.topic = node.advertise<std_msgs::String>(new_robot.name + "_topic", 1000); |
158 |
new_robot.sched_status = NEW_ROBOT; |
|
158 | 159 |
robots.push_back(new_robot); |
159 | 160 |
|
160 |
ROS_INFO("Registration a robot %s", new_robot.name.c_str());
|
|
161 |
ROS_INFO("Registration a robot %s %d", new_robot.name.c_str(), robots.size());
|
|
161 | 162 |
std_msgs::String msg; |
162 | 163 |
std::stringstream ss; |
163 | 164 |
ss<<"REG_SUCCESS "<<id; |
164 | 165 |
msg.data = ss.str(); |
165 | 166 |
new_robot.topic.publish(msg); |
167 |
ros::spinOnce(); |
|
168 |
|
|
166 | 169 |
ROS_INFO("Registration a success"); |
170 |
|
|
171 |
assert(robots.size() > 0); |
|
167 | 172 |
} |
168 | 173 |
else |
169 | 174 |
{ |
... | ... | |
177 | 182 |
this function calls the waiting_dance() function. */ |
178 | 183 |
void Scheduler::run() |
179 | 184 |
{ |
185 |
robot_to_sched = node.subscribe("robot_to_sched", 1000, &Scheduler::msg_callback, this); |
|
180 | 186 |
ROS_INFO("I am a scheduler!. Now waiting for robots"); |
181 |
while(robots.size() < 3) |
|
187 |
while(robots.size() < 1 && ok()) |
|
188 |
{ |
|
189 |
ROS_INFO("Robots size: %d", robots.size()); |
|
182 | 190 |
spinOnce(); |
183 |
|
|
191 |
} |
|
192 |
ROS_INFO("SCHEDULER: main loop"); |
|
184 | 193 |
while (ok()) |
185 | 194 |
{ |
186 | 195 |
ROS_INFO("Scheduler running"); |
187 | 196 |
spinOnce(); |
188 | 197 |
while(waitingRobots.empty() || unassignedOrders->arraySize()==0) |
189 | 198 |
waiting_dance(); |
190 |
|
|
191 |
|
|
199 |
|
|
200 |
ROS_INFO("SCHEDULER: Setting task"); |
|
192 | 201 |
Order next = get_next_item(); |
193 | 202 |
Robot r = waitingRobots.front(); |
194 | 203 |
|
204 |
ROS_INFO("SCHEDULER: Setting task %d to robot %s", next.getid(), r.name.c_str()); |
|
195 | 205 |
std_msgs::String msg; |
196 | 206 |
std::stringstream ss; |
197 | 207 |
ss<<"SET_TASK "<<next.getid()<<" "<<next.get_source()<<" "<<next.get_dest(); |
198 | 208 |
msg.data = ss.str(); |
199 | 209 |
r.topic.publish(msg); |
200 | 210 |
|
211 |
waitingRobots.front().sched_status = ORDERED_ROBOT; |
|
201 | 212 |
waitingRobots.pop(); |
202 | 213 |
} |
203 | 214 |
} |
Also available in: Unified diff