Revision 1905324e
Trying to make the warehouse robot drive around
scout/libscout/src/behaviors/Scheduler.cpp | ||
---|---|---|
26 | 26 |
*/ |
27 | 27 |
void Scheduler::get_task(int robot) |
28 | 28 |
{ |
29 |
ROS_INFO("SCHEDULER: robots vector size is: %d", robots.size()); |
|
30 | 29 |
Robot my_robot = robots[robot-1]; |
31 |
ROS_INFO("SCHEDULER: My robot is %s", my_robot.name.c_str()); |
|
32 | 30 |
if(my_robot.sched_status != WAITING_ROBOT) |
33 | 31 |
{ |
34 | 32 |
waitingRobots.push(my_robot); |
35 | 33 |
my_robot.sched_status = WAITING_ROBOT; |
36 |
ROS_INFO("SCHEDULER: Added to scheduler %d", my_robot.sched_status); |
|
37 | 34 |
} |
38 | 35 |
} |
39 | 36 |
|
... | ... | |
48 | 45 |
Duration five(5); |
49 | 46 |
Duration three(3); |
50 | 47 |
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 |
Order d(4,0,2,t,p,end);
|
|
55 |
Order e(5,0,4,t,p,end);
|
|
48 |
Order a(1,0,2,t+ten,p,end);
|
|
49 |
Order b(2,0,4,t+five,p,end);
|
|
50 |
Order c(3,0,5,t+three,p,end);
|
|
51 |
Order d(4,0,6,t,p,end);
|
|
52 |
Order e(5,0,7,t,p,end);
|
|
56 | 53 |
|
57 | 54 |
unassignedOrders->insert(a); |
58 | 55 |
unassignedOrders->insert(b); |
... | ... | |
168 | 165 |
new_robot.sched_status = NEW_ROBOT; |
169 | 166 |
robots.push_back(new_robot); |
170 | 167 |
|
171 |
ROS_INFO("Registration a robot %s %d", new_robot.name.c_str(), robots.size()); |
|
172 | 168 |
std_msgs::String msg; |
173 | 169 |
std::stringstream ss; |
174 | 170 |
ss<<"REG_SUCCESS "<<id; |
... | ... | |
196 | 192 |
ROS_INFO("I am a scheduler!. Now waiting for robots"); |
197 | 193 |
while(robots.size() < 1 && ok()) |
198 | 194 |
{ |
199 |
ROS_INFO("Robots size: %d", robots.size()); |
|
200 | 195 |
spinOnce(); |
201 | 196 |
} |
202 | 197 |
ROS_INFO("SCHEDULER: main loop"); |
Also available in: Unified diff