Revision ad5b5826
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