Project

General

Profile

Revision 2f025967

ID2f0259679a0994dd9893ecace84dab0fa118eaeb

Added by Priya almost 12 years ago

Got scheduler to compile. onto wh robots

View differences:

scout/libscout/CMakeLists.txt
30 30
#target_link_libraries(example ${PROJECT_NAME})
31 31

  
32 32
set(MAIN_FILES src/Behavior.cpp src/BehaviorList.cpp src/BehaviorProcess.cpp)
33
set(BEHAVIOR_FILES src/behaviors/Odometry.cpp src/behaviors/draw_cw_circle.cpp src/behaviors/draw_ccw_circle.cpp src/behaviors/navigationMap.cpp)
33
set(BEHAVIOR_FILES src/behaviors/Odometry.cpp src/behaviors/draw_cw_circle.cpp src/behaviors/draw_ccw_circle.cpp src/behaviors/navigationMap.cpp src/behaviors/Scheduler.cpp src/behaviors/WH_Robot.cpp)
34 34
set(CONTROL_CLASSES src/MotorControl.cpp src/SonarControl.cpp src/HeadlightControl.cpp src/ButtonControl.cpp src/WirelessSender.cpp src/EncodersControl.cpp)
35 35

  
36 36
rosbuild_add_executable(libscout ${MAIN_FILES} ${BEHAVIOR_FILES} ${CONTROL_CLASSES})
scout/libscout/src/BehaviorList.cpp
6 6
  behavior_list.push_back((Behavior*)new draw_ccw_circle(scoutname));
7 7
  behavior_list.push_back((Behavior*)new Odometry(scoutname));
8 8
  behavior_list.push_back((Behavior*)new navigationMap(scoutname));
9
  behavior_list.push_back((Behavior*)new Scheduler(scoutname));
9 10
  return;
10 11
}
11 12

  
scout/libscout/src/BehaviorList.h
6 6
#include "behaviors/draw_ccw_circle.h"
7 7
#include "behaviors/Odometry.h"
8 8
#include "behaviors/navigationMap.h"
9
#include "behaviors/Scheduler.h"
9 10

  
10 11
class BehaviorList
11 12
{
scout/libscout/src/behaviors/Scheduler.cpp
1 1
#include "Scheduler.h"
2
#include "../helper_classes/Order.h"
2 3

  
3 4
using namespace std;
4 5

  
5 6
/** @Brief: Initialize data structures for the scheduler. */
6
Scheduler::Scheduler()
7
Scheduler::Scheduler(std::string scoutname):Behavior(scoutname, "Scheduler")
7 8
{
8
	WH_Robot rob1(/*params*/);
9
	WH_Robot rob2(/*params*/);
10
	WH_Robot rob3(/*params*/);
9
	WH_Robot* rob1 = new WH_Robot("Robot1", this);
10
	WH_Robot* rob2 = new WH_Robot("Robot2", this);
11
	WH_Robot* rob3 = new WH_Robot("Robot3", this);
11 12

  
12
	robots.insert(rob1);
13
	robots.insert(rob2);
14
	robots.insert(rob3);
13
  unassignedOrders = new PQWrapper(NUM_TASKS);
14

  
15
	robots.push_back(rob1);
16
	robots.push_back(rob2);
17
	robots.push_back(rob3);
15 18
	
16 19
	create_orders();
17 20
}
......
19 22
/** @Brief: Free allocatetd memory. */
20 23
Scheduler::~Scheduler()
21 24
{
22
   delete rob1;
23
   delete rob2;
24
   delete rob3;
25
   delete robots.at(0);
26
   delete robots.at(1);
27
   delete robots.at(2);
25 28
   
26
   delete robots;
27 29
   delete unassignedOrders;
28
   delete assigned Orders;
29
   delete waitingRobots;
30 30
}
31 31

  
32 32
/** @Brief: Add robot to the waiting queue. 
......
36 36
 *  the robot of the waiting queue, and gives it a
37 37
 *  task.
38 38
 */
39
void Scheduler::get_task(WH_Robot r)
39
void Scheduler::get_task(WH_Robot* r)
40 40
{
41 41
	waitingRobots.push(r);
42 42
}
......
44 44
/** @Brief: Statically add orders to the Priority Queue Wrapper.*/
45 45
void Scheduler::create_orders()
46 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);
47
  Path p;
48
  p.len = 0;
49
  p.path = NULL;
50
  Time t = ros::Time::now();
51
  Duration end(0);
52
	Order a(1,0,1,t,p,end);
53
  Order b(2,0,1,t,p,end);
54
  Order c(3,0,9,t,p,end);
55
  Order d(4,0,2,t,p,end);
56
  Order e(5,0,4,t,p,end);
52 57
	
53
	unassignedOrders.insert(a);
54
	unassignedOrders.insert(b);
55
	unassignedOrders.insert(c);
56
	unassignedOrders.insert(d);
57
	unassignedOrders.insert(e);
58
	unassignedOrders->insert(a);
59
	unassignedOrders->insert(b);
60
	unassignedOrders->insert(c);
61
	unassignedOrders->insert(d);
62
	unassignedOrders->insert(e);
58 63
}
59 64

  
60 65
/** @Brief: This is a confirmation that the task is complete. 
61 66
    This function removes the order from assignedOrders. */
62 67
void Scheduler::task_complete(Order o)
63 68
{
64
    for (int i=0; i< assignedOrders.size(); i++)
69
    for (int i=0; i<assignedOrders.size(); i++)
65 70
	{
66 71
	    if (assignedOrders[i].getid()==o.getid())
67 72
		{
......
75 80
void Scheduler::task_failed(Order o)
76 81
{
77 82
	task_complete(o);
78
	unassignedOrders.insert(o);
83
	unassignedOrders->insert(o);
79 84
}
80 85

  
81 86
/** @Brief: Do a waiting dance. */
......
87 92
/** @Brief: The scheduling algorithm. Picks a task and pops from the PQWrapper. */
88 93
Order Scheduler::get_next_item()
89 94
{
90
  Time time = ros::TIME_MAX; 
95
  Time t = ros::TIME_MAX; 
96
  double time = t.toSec();
91 97

  
92
  Order best;
93
  for(int i=0; i<unassigned_orders.arraySize(); i++)
98
  Order* best;
99
  for(int i=0; i<unassignedOrders->arraySize(); i++)
94 100
  {
95
    Order order = unassigned_orders.peek(i);
101
    Order order = unassignedOrders->peek(i);
102

  
103
    Time end_time = order.get_start_time() - order.get_est_time();
96 104

  
97
    if(order.orderDeadline < time) //TODO: use another function
105
    if(end_time.toSec() + MAX_WAIT_TIME < time) //TODO: use another function
98 106
    {
99
      best = order;
100
      time = order.orderDeadline;
107
      best = &order;
108
      time = end_time.toSec() + MAX_WAIT_TIME;
101 109
    }
102 110
  }
103

  
111
  return *best;
104 112
}
105 113

  
106 114
/** @Brief: Continuously checks for waiting robots. If no robots are waiting,
......
109 117
{
110 118
	while (ok())
111 119
	{
112
		while(waitingRobots.empty() || unassignedOrders.arraySize()==0) 
120
		while(waitingRobots.empty() || unassignedOrders->arraySize()==0) 
113 121
      waiting_dance();
114 122

  
115
		waitingRobots.front().set_task(get_next_item());
123
		(waitingRobots.front())->set_task(get_next_item());
116 124
		waitingRobots.pop();
117 125
	}
118 126
}
scout/libscout/src/behaviors/Scheduler.h
6 6
#include "WH_Robot.h"
7 7
#include "../Behavior.h"
8 8

  
9
#define NUM_TASKS 5
10

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

  
15 17
	void create_orders();
16 18

  
17 19
	void waiting_dance();
18 20

  
19 21
public:
20
  Scheduler(std::string scoutname):Behavior(scoutname, "Scheduler"){};
22
  Scheduler(std::string scoutname);
21 23
	~Scheduler();
22 24
	
23
	void get_task(WH_Robot robot);
25
	void get_task(WH_Robot* robot);
24 26
	
25 27
	void task_complete(Order o);
26 28
	void task_failed(Order o);
scout/libscout/src/behaviors/WH_Robot.cpp
1 1
#include "WH_Robot.h"
2
#include "Scheduler.h" 
2 3

  
3 4

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

  
12 13
Duration WH_Robot::get_wc_time(State start_state, State target_state)
......
31 32
}
32 33

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

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

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

  
21 20
        Duration get_worst_case_time(State start_state, State target_state);
22 21
        int exec_task();
23 22

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

  
scout/libscout/src/helper_classes/Order.cpp
4 4

  
5 5
/** @Brief: Regular order constructor */
6 6
Order::Order(int order_id, Address order_source, Address order_dest, 
7
    Time order_start_time, Path order_path, Time order_est_time) 
7
    Time order_start_time, Path order_path, Duration order_est_time) 
8 8
{ 
9 9
    id = order_id;
10 10
    source = order_source;
......
40 40
    return path;
41 41
}
42 42

  
43
Time Order::get_est_time() const
43
Duration Order::get_est_time() const
44 44
{
45 45
    return est_time;
46 46
}
47 47

  
48
bool Order::operator==(Order& o1, Order& o2)
48
bool Order::operator==(Order& order)
49 49
{
50
  return o1.id == o2.id;
50
  return this->id == order.id;
51 51
}
52 52

  
53 53
void Order::set_path(Path order_path)
scout/libscout/src/helper_classes/Order.h
1 1
#ifndef _ORDER_
2 2
#define _ORDER_
3 3

  
4
#include "../Behavior.h"
5
#include "../behaviors/navigationMap.h"
6

  
4 7
#define MAX_WAIT_TIME 120 //2 minutes in seconds
5 8

  
6 9
typedef unsigned int Address;
7
typedef unsigned int Path;
8
typedef double Time;
9 10

  
10 11
class Order {
11 12
    int id;
......
15 16
    Time est_time;
16 17
public:
17 18
    Order(int order_id, Address order_source, Address order_dest, 
18
        Time order_start_time, Path order_path, Time order_est_time);
19
        Time order_start_time, Path order_path, Duration order_est_time);
19 20

  
20 21
    int getid() const;
21 22
    Address get_source() const;
22 23
    Address get_dest() const;
23 24
    Time get_start_time() const;
24 25
    Path get_path() const;
25
    Time get_est_time() const;
26
    Duration get_est_time() const;
26 27
    void set_path(Path order_path);
27 28

  
28
    bool operator==(Order& o1, Order& o2);
29
    bool operator==(Order& order);
29 30
    
30 31
};
31 32

  

Also available in: Unified diff