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 |
} |
scout/libscout/src/behaviors/Scheduler.h | ||
---|---|---|
6 | 6 |
#include "../Behavior.h" |
7 | 7 |
|
8 | 8 |
#define NUM_TASKS 5 |
9 |
#define WAITING_ROBOT 1 |
|
10 |
#define NEW_ROBOT 2 |
|
11 |
#define ORDERED_ROBOT 3 |
|
9 | 12 |
|
10 | 13 |
typedef struct{ |
11 | 14 |
std::string name; |
12 | 15 |
ros::Publisher topic; |
16 |
int sched_status; |
|
13 | 17 |
} Robot; |
14 | 18 |
|
15 | 19 |
class Scheduler : Behavior { |
16 | 20 |
std::vector<Robot> robots; |
21 |
std::queue<Robot> waitingRobots; |
|
22 |
|
|
17 | 23 |
PQWrapper* unassignedOrders; |
18 | 24 |
std::vector<Order> assignedOrders; |
19 |
std::queue<Robot> waitingRobots; |
|
20 | 25 |
|
21 | 26 |
void create_orders(); |
22 | 27 |
|
scout/libscout/src/behaviors/WH_Robot.cpp | ||
---|---|---|
4 | 4 |
/** @Brief: warehouse robot constructor **/ |
5 | 5 |
WH_Robot::WH_Robot(std::string scoutname):Behavior(scoutname, "WH_Robot") |
6 | 6 |
{ |
7 |
nav_map = new navigationMap(scoutname); |
|
8 |
curr_task = DEFAULT_TASK; |
|
9 |
name = scoutname; |
|
10 |
reg_failed = 1; |
|
11 |
|
|
12 |
robot_to_sched = node.advertise<std_msgs::String>("robot_to_sched", 1000); |
|
13 |
sched_to_robot = node.subscribe(name + "_topic", 1000, &WH_Robot::robot_callback, this); |
|
14 |
ROS_INFO("A WH_Robot has been created"); |
|
7 |
nav_map = new navigationMap(scoutname); |
|
8 |
curr_task = DEFAULT_TASK; |
|
9 |
name = scoutname; |
|
10 |
reg_failed = 1; |
|
15 | 11 |
} |
16 | 12 |
|
17 | 13 |
WH_Robot::~WH_Robot() |
18 | 14 |
{ |
19 |
delete(nav_map);
|
|
15 |
delete(nav_map); |
|
20 | 16 |
} |
21 | 17 |
|
22 | 18 |
Duration WH_Robot::get_worst_case_time(State start_state, State target_state) |
... | ... | |
26 | 22 |
|
27 | 23 |
int WH_Robot::exec_task() |
28 | 24 |
{ |
29 |
ROS_INFO("WH_robot: Executing a task");
|
|
30 |
assert(curr_task != DEFAULT_TASK);
|
|
31 |
//TODO: do task
|
|
32 |
srand(0xDEADBEEF);
|
|
33 |
int error = rand() % 12;
|
|
34 |
if(error < 9) //Fail with 1/4 probability
|
|
35 |
{
|
|
36 |
ROS_INFO("WH_robot: TASK COMPLETE!");
|
|
37 |
return TASK_COMPLETED;
|
|
38 |
}
|
|
39 |
else
|
|
40 |
{
|
|
41 |
ROS_INFO("WH_robot: TASK FAILED!");
|
|
42 |
return TASK_FAILED;
|
|
43 |
}
|
|
25 |
ROS_INFO("WH_robot: Executing a task"); |
|
26 |
assert(curr_task != DEFAULT_TASK); |
|
27 |
//TODO: do task |
|
28 |
srand(0xDEADBEEF); |
|
29 |
int error = rand() % 12; |
|
30 |
if(error < 9) //Fail with 1/4 probability |
|
31 |
{ |
|
32 |
ROS_INFO("WH_robot: TASK COMPLETE!"); |
|
33 |
return TASK_COMPLETED; |
|
34 |
} |
|
35 |
else |
|
36 |
{ |
|
37 |
ROS_INFO("WH_robot: TASK FAILED!"); |
|
38 |
return TASK_FAILED; |
|
39 |
} |
|
44 | 40 |
} |
45 | 41 |
|
46 | 42 |
void WH_Robot::robot_callback(const std_msgs::String::ConstPtr& msg) |
47 | 43 |
{ |
44 |
std::cout << "robot callback called." << std::endl; |
|
48 | 45 |
if(msg->data.compare(0, 11, "REG_SUCCESS") == 0) |
49 | 46 |
{ |
50 | 47 |
id = atoi(msg->data.substr(12).c_str()); |
... | ... | |
52 | 49 |
} |
53 | 50 |
else if(msg->data.compare(0, 8, "SET_TASK") == 0) |
54 | 51 |
{ |
52 |
ROS_INFO("WH_ROBOT: Setting robot task"); |
|
55 | 53 |
char* string = (char*)msg->data.c_str(); |
56 | 54 |
char* data; |
57 | 55 |
data = strtok(string, " "); |
... | ... | |
76 | 74 |
|
77 | 75 |
void WH_Robot::run () |
78 | 76 |
{ |
79 |
ROS_INFO("I am a robot. Registering with scheduler..."); |
|
80 |
while(reg_failed) |
|
81 |
{ |
|
77 |
ROS_INFO("A WH_Robot has been created"); |
|
78 |
|
|
79 |
robot_to_sched = node.advertise<std_msgs::String>("robot_to_sched", 1000); |
|
80 |
sched_to_robot = node.subscribe(name + "_topic", 1000, &WH_Robot::robot_callback, this); |
|
81 |
|
|
82 |
ROS_INFO("I am a robot. Registering with scheduler..."); |
|
83 |
while(reg_failed && ok()) |
|
84 |
{ |
|
85 |
std_msgs::String msg; |
|
86 |
std::stringstream ss; |
|
87 |
ss << "REGISTER "<<name; |
|
88 |
msg.data = ss.str(); |
|
89 |
robot_to_sched.publish(msg); |
|
90 |
|
|
91 |
spinOnce(); |
|
92 |
} |
|
93 |
|
|
94 |
while(ok()) |
|
95 |
{ |
|
96 |
ROS_INFO("WH_ROBOT: Running main loop"); |
|
97 |
|
|
98 |
|
|
82 | 99 |
std_msgs::String msg; |
83 | 100 |
std::stringstream ss; |
84 |
ss << "REGISTER "<<name;
|
|
101 |
ss << "GET_TASK "<<id;
|
|
85 | 102 |
msg.data = ss.str(); |
86 | 103 |
robot_to_sched.publish(msg); |
87 | 104 |
|
105 |
while(curr_task == DEFAULT_TASK && ok()){ |
|
88 | 106 |
spinOnce(); |
89 | 107 |
} |
90 | 108 |
|
91 |
while(ok()) |
|
109 |
int error = exec_task(); |
|
110 |
if(error == TASK_COMPLETED) |
|
92 | 111 |
{ |
93 |
ROS_INFO("WH_ROBOT: Running main loop"); |
|
94 |
|
|
95 |
|
|
96 |
std_msgs::String msg; |
|
97 |
std::stringstream ss; |
|
98 |
ss << "GET_TASK "<<id; |
|
99 |
msg.data = ss.str(); |
|
100 |
robot_to_sched.publish(msg); |
|
101 |
|
|
102 |
spinOnce(); |
|
103 |
|
|
104 |
while(curr_task == DEFAULT_TASK){ |
|
105 |
continue; |
|
106 |
} |
|
107 |
|
|
108 |
int error = exec_task(); |
|
109 |
if(error == TASK_COMPLETED) |
|
110 |
{ |
|
111 |
|
|
112 |
|
|
113 |
ss << "SUCCESS "<<curr_task->getid(); |
|
114 |
msg.data = ss.str(); |
|
115 |
robot_to_sched.publish(msg); |
|
116 |
|
|
117 |
|
|
118 |
} |
|
119 |
else //error == TASK_FAILED |
|
120 |
{ |
|
121 |
|
|
122 |
|
|
123 |
ss << "FAILED "<<curr_task->getid(); |
|
124 |
msg.data = ss.str(); |
|
125 |
robot_to_sched.publish(msg); |
|
126 |
|
|
127 |
|
|
128 |
} |
|
129 |
delete curr_task; |
|
130 |
curr_task = DEFAULT_TASK; |
|
131 |
|
|
112 |
std_msgs::String msg; |
|
113 |
std::stringstream ss; |
|
114 |
ss << "SUCCESS "<<curr_task->getid(); |
|
115 |
msg.data = ss.str(); |
|
116 |
robot_to_sched.publish(msg); |
|
132 | 117 |
} |
118 |
else //error == TASK_FAILED |
|
119 |
{ |
|
120 |
std_msgs::String msg; |
|
121 |
std::stringstream ss; |
|
122 |
ss << "FAILED "<<curr_task->getid(); |
|
123 |
msg.data = ss.str(); |
|
124 |
robot_to_sched.publish(msg); |
|
125 |
} |
|
126 |
delete curr_task; |
|
127 |
curr_task = DEFAULT_TASK; |
|
128 |
|
|
129 |
} |
|
133 | 130 |
} |
134 | 131 |
|
135 | 132 |
void WH_Robot::set_task(Order order) |
136 | 133 |
{ |
137 |
curr_task = new Order(order.getid(), order.get_source(), order.get_dest(),
|
|
138 |
order.get_start_time(), order.get_path(), order.get_est_time());
|
|
139 |
return;
|
|
134 |
curr_task = new Order(order.getid(), order.get_source(), order.get_dest(), |
|
135 |
order.get_start_time(), order.get_path(), order.get_est_time()); |
|
136 |
return; |
|
140 | 137 |
} |
141 | 138 |
|
Also available in: Unified diff