Statistics
| Branch: | Revision:

root / scout / libscout / src / test_behaviors / Scheduler.cpp @ 339f64d2

History | View | Annotate | Download (5.57 KB)

1
#include "Scheduler.h"
2
#include "../helper_classes/Order.h"
3

    
4
using namespace std;
5

    
6
/** @Brief: Initialize data structures for the scheduler. */
7
Scheduler::Scheduler(std::string scoutname, Sensors* sensors):
8
            Behavior(scoutname, "Scheduler", sensors)
9
{
10
  unassignedOrders = new PQWrapper(NUM_TASKS);
11

    
12
        create_orders();
13
}
14

    
15
/** @Brief: Free allocatetd memory. */
16
Scheduler::~Scheduler()
17
{
18
  delete unassignedOrders;
19
}
20

    
21
/** @Brief: Add robot to the waiting queue. 
22
 *  A robot calls this function with itself
23
 *  and gets pushed on a list of waiting robots.
24
 *  When a task is availaible the scheduler, removes
25
 *  the robot of the waiting queue, and gives it a
26
 *  task.
27
 */
28
void Scheduler::get_task(int robot)
29
{
30
  Robot my_robot = robots[robot-1];
31
  if(my_robot.sched_status != WAITING_ROBOT)
32
  {
33
          waitingRobots.push(my_robot);
34
    my_robot.sched_status = WAITING_ROBOT;
35
  }
36
}
37

    
38
/** @Brief: Statically add orders to the Priority Queue Wrapper.*/
39
void Scheduler::create_orders()
40
{
41
  Path p;
42
  p.len = 0;
43
  p.path = NULL;
44
  Time t = ros::Time::now();
45
  Duration end(0);
46
  Duration five(5);
47
  Duration three(3);
48
  Duration ten(10);
49
        Order a(1,0,13,t+ten,p,end);
50
  Order b(2,0,14,t+five,p,end);
51
  Order c(3,0,15,t+three,p,end);
52
  Order d(4,0,16,t,p,end);
53
        
54
        unassignedOrders->insert(a);
55
        unassignedOrders->insert(b);
56
        unassignedOrders->insert(c);
57
        unassignedOrders->insert(d);
58
}
59

    
60
/** @Brief: This is a confirmation that the task is complete. 
61
    This function removes the order from assignedOrders. */
62
void Scheduler::task_complete(Order o)
63
{
64
  ROS_INFO("Scheduler: Task id %d was completed", o.getid());
65
  for (unsigned int i=0; i<assignedOrders.size(); i++)
66
  {
67
    if (assignedOrders[i].getid()==o.getid())
68
    {
69
      assignedOrders.erase(assignedOrders.begin()+i);
70
    }
71
  }
72
}
73

    
74
/** @Brief: This is a confirmation that the task failed. 
75
    This function places the order back on the PQWrapper. */
76
void Scheduler::task_failed(Order o)
77
{
78
  ROS_INFO("Scheduler: Task id %d was failed", o.getid());
79
        task_complete(o);
80
        unassignedOrders->insert(o);
81
}
82

    
83
/** @Brief: Do a waiting dance. */
84
void Scheduler::waiting_dance()
85
{ 
86
    //ROS_INFO("Scheduler: TEEHEE i do a dance!");
87
}
88

    
89
/** @Brief: The scheduling algorithm. Picks a task and pops from the PQWrapper. */
90
Order Scheduler::get_next_item()
91
{
92
  Time t = ros::TIME_MAX; 
93
  double time = t.toSec();
94

    
95
  //Order* best;        // Unused; restore when needed
96
  int best_index = 0;
97
  for(unsigned int i=0; i<unassignedOrders->arraySize(); i++)
98
  {
99
    Order order = unassignedOrders->peek(i);
100

    
101
    Time end_time = order.get_start_time() - order.get_est_time();
102

    
103
    if(end_time.toSec() + MAX_WAIT_TIME < time) //TODO: use another function
104
    {
105
      //best = &order;
106
      best_index = i;
107
      time = end_time.toSec() + MAX_WAIT_TIME;
108
    }
109
  }
110

    
111
  Order ret;
112
  ret = unassignedOrders->remove(best_index);
113
  assignedOrders.push_back(ret);
114
  return ret;
115
}
116

    
117
void Scheduler::msg_callback(const std_msgs::String::ConstPtr& msg)
118
{
119
  if(msg->data.compare(0, 6, "FAILED") == 0)
120
  {
121
    int order_id = atoi(msg->data.substr(7).c_str());
122
    for(unsigned int i=0; i<assignedOrders.size(); i++)
123
    {
124
      if(assignedOrders[i].getid() == order_id)
125
      {
126
        task_failed(assignedOrders[i]);
127
        break;
128
      }
129
    }
130
  }
131
  else if(msg->data.compare(0, 7, "SUCCESS") == 0)
132
  {
133
    int order_id = atoi(msg->data.substr(8).c_str());
134
    for(unsigned int i=0; i<assignedOrders.size(); i++)
135
    {
136
      if(assignedOrders[i].getid() == order_id)
137
      {
138
        task_complete(assignedOrders[i]);
139
        break;
140
      }
141
    }
142
  }
143
  else if(msg->data.compare(0, 8, "GET_TASK") == 0)
144
  {
145
    ROS_INFO("SCHEDULER: got get task message");
146
    int robot = atoi(msg->data.substr(9).c_str());
147
    get_task(robot);
148
  }
149
  else if(msg->data.compare(0, 8, "REGISTER") == 0)
150
  {
151
    string robot_name = msg->data.substr(9);
152
    for(unsigned int i=0; i<robots.size(); i++)
153
    {
154
      if(robots[i].name.compare(robot_name) == 0)
155
      { 
156
        return;
157
      }
158
    }
159

    
160
    int id = robots.size() +1;
161
    Robot new_robot;
162
    new_robot.name = robot_name;
163
    new_robot.topic = node.advertise<std_msgs::String>(new_robot.name + "_topic", 1000);
164
    new_robot.sched_status = NEW_ROBOT;
165
    robots.push_back(new_robot);
166

    
167
    std_msgs::String msg;
168
    std::stringstream ss;
169
    ss<<"REG_SUCCESS "<<id;
170
    msg.data = ss.str();
171
    new_robot.topic.publish(msg);
172
    spinOnce();
173

    
174
    ROS_INFO("Registration a success");
175

    
176
    assert(robots.size() > 0);
177
  }
178
  else
179
  {
180
    ROS_INFO("I got a bad message: %s", msg->data.c_str());
181
  }
182

    
183
  return;
184
}
185

    
186
/** @Brief: Continuously checks for waiting robots. If no robots are waiting,
187
    this function calls the waiting_dance() function. */
188
void Scheduler::run()
189
{
190
  robot_to_sched = node.subscribe("robot_to_sched", 1000, &Scheduler::msg_callback, this);
191
  ROS_INFO("I am a scheduler!. Now waiting for robots");
192
  while(robots.size() < 1 && ok())
193
  {
194
    spinOnce();
195
  }
196
  ROS_INFO("SCHEDULER: main loop");
197
        while (ok())
198
        {
199
    ROS_INFO("Scheduler running");
200
    spinOnce();
201
                while((waitingRobots.empty() || unassignedOrders->arraySize()==0) && ok())
202
    {
203
      waiting_dance();
204
      spinOnce();
205
    }
206
  
207
    ROS_INFO("SCHEDULER: Setting task");
208
    Order next = get_next_item();
209
    Robot r = waitingRobots.front();
210

    
211
    ROS_INFO("SCHEDULER: Setting task %d to robot %s", next.getid(), r.name.c_str());
212
    std_msgs::String msg;
213
    std::stringstream ss;
214
    ss<<"SET_TASK "<<next.getid()<<" "<<next.get_source()<<" "<<next.get_dest();
215
    std::cout<<"msg: "<<ss.str()<<endl;
216
    msg.data = ss.str();
217
    r.topic.publish(msg);
218
  
219
    spinOnce();
220

    
221
    waitingRobots.front().sched_status = ORDERED_ROBOT;
222
                waitingRobots.pop();
223
        }
224
}
225

    
226

    
227

    
228