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