Project

General

Profile

Revision 2002

added the warehouse behavior section that will be used on the colony 3's. Currently only contains the c++ files that we are going to port over

View differences:

trunk/code/projects/warehouse/Scheduler.cpp
1
#include "Scheduler.h"
2

  
3
using namespace std;
4

  
5
/** @Brief: Initialize data structures for the scheduler. */
6
Scheduler::Scheduler()
7
{
8
	WH_Robot rob1(/*params*/);
9
	WH_Robot rob2(/*params*/);
10
	WH_Robot rob3(/*params*/);
11

  
12
	robots.insert(rob1);
13
	robots.insert(rob2);
14
	robots.insert(rob3);
15
	
16
	create_orders();
17
}
18

  
19
/** @Brief: Free allocatetd memory. */
20
Scheduler::~Scheduler()
21
{
22
   delete rob1;
23
   delete rob2;
24
   delete rob3;
25
   
26
   delete robots;
27
   delete unassignedOrders;
28
   delete assigned Orders;
29
   delete waitingRobots;
30
}
31

  
32
/** @Brief: Add robot to the waiting queue. 
33
 *  A robot calls this function with itself
34
 *  and gets pushed on a list of waiting robots.
35
 *  When a task is availaible the scheduler, removes
36
 *  the robot of the waiting queue, and gives it a
37
 *  task.
38
 */
39
void Scheduler::get_task(WH_Robot r)
40
{
41
	waitingRobots.push(r);
42
}
43

  
44
/** @Brief: Statically add orders to the Priority Queue Wrapper.*/
45
void Scheduler::create_orders()
46
{
47
	Order a(1,0,0,101,0,0,0);
48
  Order b(2,0,0,11,0,0,0);
49
  Order c(3,0,0,91,0,0,0);
50
  Order d(4,0,0,21,0,0,0);
51
  Order e(5,0,0,41,0,0,0);
52
	
53
	unassignedOrders.insert(a);
54
	unassignedOrders.insert(b);
55
	unassignedOrders.insert(c);
56
	unassignedOrders.insert(d);
57
	unassignedOrders.insert(e);
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
    for (int i=0; i< assignedOrders.size(); i++)
65
	{
66
	    if (assignedOrders[i].getid()==o.getid())
67
		{
68
			assignedOrders.erase(assignedOrders.begin()+i);
69
		}
70
	}
71
}
72

  
73
/** @Brief: This is a confirmation that the task failed. 
74
    This function places the order back on the PQWrapper. */
75
void Scheduler::task_failed(Order o)
76
{
77
	task_complete(o);
78
	unassignedOrders.insert(o);
79
}
80

  
81
/** @Brief: Do a waiting dance. */
82
void Scheduler::waiting_dance()
83
{
84
	
85
}
86

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

  
92
  Order best;
93
  for(int i=0; i<unassigned_orders.arraySize(); i++)
94
  {
95
    Order order = unassigned_orders.peek(i);
96

  
97
    if(order.orderDeadline < time) //TODO: use another function
98
    {
99
      best = order;
100
      time = order.orderDeadline;
101
    }
102
  }
103

  
104
}
105

  
106
/** @Brief: Continuously checks for waiting robots. If no robots are waiting,
107
    this function calls the waiting_dance() function. */
108
void Scheduler::run()
109
{
110
	while (ok())
111
	{
112
		while(waitingRobots.empty() || unassignedOrders.arraySize()==0) 
113
      waiting_dance();
114

  
115
		waitingRobots.front().set_task(get_next_item());
116
		waitingRobots.pop();
117
	}
118
}
119

  
120

  
121

  
122

  
trunk/code/projects/warehouse/Scheduler.h
1
#ifndef _SCHEDULER_
2
#define _SCHEDULER_
3

  
4
#include "../helper_classes/PQWrapper.h"
5
#include "../helper_classes/Order.h"
6
#include "WH_Robot.h"
7
#include "../Behavior.h"
8

  
9
class Scheduler : Behavior {
10
  std::vector<WH_Robot> robots;
11
	PQWrapper unassignedOrders;
12
	std::vector<Order> assignedOrders;
13
	std::queue<WH_Robot> waitingRobots;
14

  
15
	void create_orders();
16

  
17
	void waiting_dance();
18

  
19
public:
20
  Scheduler(std::string scoutname):Behavior(scoutname, "Scheduler"){};
21
	~Scheduler();
22
	
23
	void get_task(WH_Robot robot);
24
	
25
	void task_complete(Order o);
26
	void task_failed(Order o);
27
	
28
	
29
	Order get_next_item();
30
	
31
	void run();
32
    
33
};
34
#endif
trunk/code/projects/warehouse/WH_Robot.cpp
1
#include "WH_Robot.h"
2

  
3

  
4
/** @Brief: warehouse robot constructor **/
5
WH_Robot::WH_Robot(std::string scoutname):Behavior(scoutname, "Scheduler")
6
{
7
    nav_map = navigationMap(scoutname);
8
    curr_task = DEFAULT_TASK;
9
    scheduler = Scheduler(scoutname);
10
}
11

  
12
Time WH_Robot::get_wc_time(State dest)
13
{
14
    //TODO: integrate Lalitha's code
15
    return 0.0;
16
}
17

  
18
int WH_Robot::exec_task()
19
{
20
    assert(curr_task != DEFAULT_TASK);
21
    //TODO: do task
22
    srand(0xDEADBEEF);
23
    int error = rand() % 12;
24
    if(error < 9) //Fail with 1/4 probability
25
    {
26
      return TASK_COMPLETED;
27
    }
28
    else
29
    {
30
      return TASK_FAILED;
31
    }
32
}
33

  
34
void WH_Robot::run (){
35
    Scheduler.get_task(*this);
36
    while(curr_task == DEFAULT_TASK)
37
      continue;
38
    int error = exec_task();
39
    if(error == TASK_COMPLETE)
40
    {
41
        schedule.task_complete(*curr_task);
42
    }
43
    else //error == TASK_FAILED
44
    {
45
        schedule.task_failed(*curr_task);
46
    }
47
    curr_task = DEFAULT_TASK;
48
}
49

  
50
void WH_Robot::set_task(Order order)
51
{
52
    curr_task = new Order(order.getid(), order.get_source(), order.get_dest(), 
53
        order.get_start_time(), order.get_path(), order.get_est_time()); 
54
    return;
55
}
56

  
trunk/code/projects/warehouse/WH_Robot.h
1
#ifndef _WH_ROBOT_
2
#define _WH_ROBOT_
3

  
4
#define DEFAULT_TASK NULL;
5
#define TASK_COMPLETED 0
6
#define TASK_FAILED -1
7

  
8
#include "../Behavior.h"
9
#include "navigationMap.h"
10
#include "Scheduler.h" 
11
#include "../helper_classes/Order.h"
12
#include <assert.h>
13
#include <stdlib.h>
14

  
15
class WH_Robot : Behavior{
16

  
17
        Order* curr_task;
18
        navigationMap nav_map;
19
        Scheduler scheduler;
20

  
21
        Time get_wc_time(State dest);
22
        int exec_task();
23

  
24
    public:
25
        WH_Robot(std::string scoutname);
26
        ~WH_Robot();
27
        void run();
28

  
29
        void set_task(Order order);
30

  
31
};
32

  
33
#endif

Also available in: Unified diff