Revision 7ac5e9bc
ID | 7ac5e9bc2e048962b82aab3a995270bab56d816a |
ROS scheduler/whrobot behavior needs to be debugged
scout/libscout/src/behaviors/Scheduler.cpp | ||
---|---|---|
6 | 6 |
/** @Brief: Initialize data structures for the scheduler. */ |
7 | 7 |
Scheduler::Scheduler(std::string scoutname):Behavior(scoutname, "Scheduler") |
8 | 8 |
{ |
9 |
|
|
10 |
ROS_INFO("Scheduler: Creating order PQ..."); |
|
11 |
|
|
12 | 9 |
unassignedOrders = new PQWrapper(NUM_TASKS); |
13 | 10 |
|
14 |
ROS_INFO("Scheduler: PQ orders initialized. "); |
|
15 |
|
|
16 |
sched_to_robot = n.advertise("sched_to_robot", 1000); |
|
11 |
robot_to_sched = node.subscribe("robot_to_sched", 1000, &Scheduler::msg_callback, this); |
|
17 | 12 |
|
18 | 13 |
create_orders(); |
19 |
ROS_INFO("Scheduler: I am initialized!"); |
|
20 | 14 |
} |
21 | 15 |
|
22 | 16 |
/** @Brief: Free allocatetd memory. */ |
23 | 17 |
Scheduler::~Scheduler() |
24 | 18 |
{ |
25 |
while(robots.size() != 0) |
|
26 |
delete robots.at(0); |
|
27 |
|
|
28 | 19 |
delete unassignedOrders; |
29 | 20 |
} |
30 | 21 |
|
... | ... | |
120 | 111 |
|
121 | 112 |
void Scheduler::msg_callback(const std_msgs::String::ConstPtr& msg) |
122 | 113 |
{ |
114 |
ROS_INFO("I got a message: %s", msg->data.c_str()); |
|
123 | 115 |
if(msg->data.compare(0, 6, "FAILED") == 0) |
124 | 116 |
{ |
125 | 117 |
int order_id = atoi(msg->data.substr(7).c_str()); |
126 |
for(int i=0; i<assignedOrders.size(); i++) |
|
118 |
for(unsigned int i=0; i<assignedOrders.size(); i++)
|
|
127 | 119 |
{ |
128 |
if(assignedOrders[i].id == order_id)
|
|
120 |
if(assignedOrders[i].getid() == order_id)
|
|
129 | 121 |
{ |
130 | 122 |
task_failed(assignedOrders[i]); |
131 | 123 |
break; |
... | ... | |
135 | 127 |
else if(msg->data.compare(0, 7, "SUCCESS") == 0) |
136 | 128 |
{ |
137 | 129 |
int order_id = atoi(msg->data.substr(8).c_str()); |
138 |
for(int i=0; i<assignedOrders.size(); i++) |
|
130 |
for(unsigned int i=0; i<assignedOrders.size(); i++)
|
|
139 | 131 |
{ |
140 |
if(assignedOrders[i].id == order_id)
|
|
132 |
if(assignedOrders[i].getid() == order_id)
|
|
141 | 133 |
{ |
142 | 134 |
task_complete(assignedOrders[i]); |
143 | 135 |
break; |
... | ... | |
151 | 143 |
} |
152 | 144 |
else if(msg->data.compare(0, 8, "REGISTER") == 0) |
153 | 145 |
{ |
146 |
string robot_name = msg->data.substr(9); |
|
147 |
for(unsigned int i=0; i<robots.size(); i++) |
|
148 |
{ |
|
149 |
if(robots[i].name.compare(robot_name) == 0) |
|
150 |
return; |
|
151 |
} |
|
152 |
|
|
154 | 153 |
int id = robots.size() +1; |
155 | 154 |
Robot new_robot; |
156 |
new_robot.name = msg->data.substr(9);
|
|
157 |
new_robot.topic = n.subscribe(new_robot.name + "_topic", 1000, msg_callback);
|
|
155 |
new_robot.name = robot_name;
|
|
156 |
new_robot.topic = node.advertise<std_msgs::String>(new_robot.name + "_topic", 1000);
|
|
158 | 157 |
robots.push_back(new_robot); |
159 | 158 |
|
159 |
ROS_INFO("Registration a robot %s", new_robot.name.c_str()); |
|
160 | 160 |
std_msgs::String msg; |
161 | 161 |
std::stringstream ss; |
162 |
ss<<"REG_SUCCESS";
|
|
162 |
ss<<"REG_SUCCESS "<<id;
|
|
163 | 163 |
msg.data = ss.str(); |
164 |
sched_to_robot.publish(msg);
|
|
165 |
|
|
164 |
new_robot.topic.publish(msg);
|
|
165 |
ROS_INFO("Registration a success"); |
|
166 | 166 |
} |
167 | 167 |
else |
168 | 168 |
{ |
169 |
ROS_INFO("I got a bad message: %s", msg->data); |
|
169 |
ROS_INFO("I got a bad message: %s", msg->data.c_str());
|
|
170 | 170 |
} |
171 | 171 |
|
172 | 172 |
return; |
... | ... | |
176 | 176 |
this function calls the waiting_dance() function. */ |
177 | 177 |
void Scheduler::run() |
178 | 178 |
{ |
179 |
while(robots.size() < 3); |
|
179 |
ROS_INFO("I am a scheduler!. Now waiting for robots"); |
|
180 |
while(robots.size() < 3) |
|
181 |
spinOnce(); |
|
180 | 182 |
|
181 | 183 |
while (ok()) |
182 | 184 |
{ |
183 | 185 |
ROS_INFO("Scheduler running"); |
186 |
spinOnce(); |
|
184 | 187 |
while(waitingRobots.empty() || unassignedOrders->arraySize()==0) |
185 | 188 |
waiting_dance(); |
186 | 189 |
|
187 | 190 |
|
188 | 191 |
Order next = get_next_item(); |
192 |
Robot r = waitingRobots.front(); |
|
193 |
|
|
189 | 194 |
std_msgs::String msg; |
190 | 195 |
std::stringstream ss; |
191 |
ss<<"SET_TASK "<<next.id<<" "<<next.get_source()<<" "<<next.get_dest;
|
|
196 |
ss<<"SET_TASK "<<next.getid()<<" "<<next.get_source()<<" "<<next.get_dest();
|
|
192 | 197 |
msg.data = ss.str(); |
193 |
sched_to_robot.publish(msg);
|
|
198 |
r.topic.publish(msg);
|
|
194 | 199 |
|
195 | 200 |
waitingRobots.pop(); |
196 | 201 |
} |
Also available in: Unified diff