Revision 2009

View differences:

trunk/code/projects/warehouse/WH_Robot.cpp
1 1
#include "WH_Robot.h"
2
#include "../helper_classes/Order.h"
2 3

  
3

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

  
12
    robot_to_sched = node.advertise<std_msgs::String>("robot_to_sched", 1000);
13
    sched_to_robot = node.subscribe(name + "_topic", 1000, &WH_Robot::robot_callback, this);
14
    ROS_INFO("A WH_Robot has been created");
10 15
}
11 16

  
12
Time WH_Robot::get_wc_time(State dest)
17
WH_Robot::~WH_Robot()
13 18
{
14
    //TODO: integrate Lalitha's code
15
    return 0.0;
19
    delete(nav_map);
16 20
}
17 21

  
22
Duration WH_Robot::get_worst_case_time(State start_state, State target_state)
23
{
24
  return nav_map->get_worst_case_time(start_state, target_state);
25
}
26

  
18 27
int WH_Robot::exec_task()
19 28
{
29
    ROS_INFO("WH_robot: Executing a task");
20 30
    assert(curr_task != DEFAULT_TASK);
21 31
    //TODO: do task
22 32
    srand(0xDEADBEEF);
23 33
    int error = rand() % 12;
24 34
    if(error < 9) //Fail with 1/4 probability
25 35
    {
36
      ROS_INFO("WH_robot: TASK COMPLETE!");
26 37
      return TASK_COMPLETED;
27 38
    }
28 39
    else
29 40
    {
41
      ROS_INFO("WH_robot: TASK FAILED!");
30 42
      return TASK_FAILED;
31 43
    }
32 44
}
33 45

  
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)
46
void WH_Robot::robot_callback(const std_msgs::String::ConstPtr& msg)
47
{
48
  if(msg->data.compare(0, 11, "REG_SUCCESS") == 0)
49
  {
50
    id = atoi(msg->data.substr(12).c_str());
51
    reg_failed = 0;
52
  }
53
  else if(msg->data.compare(0, 8, "SET_TASK") == 0)
54
  {
55
    char* string = (char*)msg->data.c_str();
56
    char* data;
57
    data = strtok(string, " ");
58
    int order_id = atoi(data);
59
    data = strtok(string, " ");
60
    Address source = atoi(data);
61
    data = strtok(string, " ");
62
    Address dest = atoi(data);
63
    Time start_time = ros::Time::now();
64
    Path path;
65
    path.len = 0;
66
    path.path = NULL;
67
    Duration est_time(0);
68

  
69
    curr_task = new Order(order_id, source, dest, start_time, path, est_time);
70
  }
71
  else
72
  {
73
    ROS_INFO("I got a bad message: %s", msg->data.c_str());
74
  }
75
}
76

  
77
void WH_Robot::run ()
78
{
79
    ROS_INFO("I am a robot. Registering with scheduler...");
80
    while(reg_failed)
40 81
    {
41
        schedule.task_complete(*curr_task);
82
      std_msgs::String msg;
83
      std::stringstream ss;
84
      ss << "REGISTER "<<name;
85
      msg.data = ss.str();
86
      robot_to_sched.publish(msg);
87

  
88
      spinOnce();
42 89
    }
43
    else //error == TASK_FAILED
90

  
91
    while(ok())
44 92
    {
45
        schedule.task_failed(*curr_task);
93
        ROS_INFO("WH_ROBOT: Running main loop");
94

  
95

  
96
        std_msgs::String msg;
97
        std::stringstream ss;
98
        ss << "GET_TASK "<<id;
99
        msg.data = ss.str();
100
        robot_to_sched.publish(msg);
101

  
102
        spinOnce();
103

  
104
        while(curr_task == DEFAULT_TASK)
105
          continue;
106
        int error = exec_task();
107
        if(error == TASK_COMPLETED)
108
        {
109

  
110

  
111
        ss << "SUCCESS "<<curr_task->getid();
112
        msg.data = ss.str();
113
        robot_to_sched.publish(msg);
114

  
115

  
116
        }
117
        else //error == TASK_FAILED
118
        {
119
 
120

  
121
        ss << "FAILED "<<curr_task->getid();
122
        msg.data = ss.str();
123
        robot_to_sched.publish(msg);
124

  
125

  
126
       }
127
        delete curr_task;
128
        curr_task = DEFAULT_TASK;
46 129
    }
47
    curr_task = DEFAULT_TASK;
48 130
}
49 131

  
50 132
void WH_Robot::set_task(Order order)
trunk/code/projects/warehouse/WH_Robot.h
1 1
#ifndef _WH_ROBOT_
2 2
#define _WH_ROBOT_
3 3

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

  
8
#include "navigationMap.h"
9
#include "Scheduler.h" 
10
#include "/helper_classes/Order.h"
8
/*Deleted Behaviors.h*/
9
#include "navigationMap.h"/*Do we do something here?*/
10
#include "helper_classes/Order.h"
11 11
#include <assert.h>
12 12
#include <stdlib.h>
13 13

  
14
/* ~~Begin Variable Declarations~~ */
15
int id;
14 16

  
17
int reg_failed;
18

  
15 19
Order* curr_task;
16
navigationMap nav_map;
17
Scheduler scheduler;
20
navigationMap* nav_map;
21
/* ^ Is this going to be changed? Do we need to conv this to c?*/
22
/* ~~The End of all declarations : variable related~~*/
18 23

  
19
Time get_wc_time(State dest);
24

  
25

  
26
/*On colony 3 bots time is just an int (being ms || 1/16 of a sec)*/
27
int get_worst_case_time(State start_state, State target_state);
20 28
int exec_task();
21 29

  
22
WH_Robot(std::string scoutname);
23
~WH_Robot();
24
void run();
30
void robot_callback(const std_msgs::String::ConstPtr& msg);
25 31

  
32
/*WH_Robot(std::string scoutname); ->Coalesce into the main function*/
33
/*~WH_Robot(); ->Deconstructors don't exist in C*/
34
/*void run(); ->Coalesce into the main function*/
35

  
26 36
void set_task(Order order);
27 37

  
38
/* These subscribers must be dealt with through wireless checking
39
ros::Publisher robot_to_sched;
40
ros::Subscriber sched_to_robot;
41
*/
28 42
#endif
trunk/code/projects/warehouse/helper_classes/Order.h
1
#include "Order.h"
1
#ifndef _ORDER_
2
#define _ORDER_
2 3

  
3
using namespace std;
4
#include "../Behavior.h"
5
#include "../behaviors/navigationMap.h"
4 6

  
5
/** @Brief: Regular order constructor */
6
Order::Order(int order_id, Address order_source, Address order_dest, 
7
    Time order_start_time, Path order_path, Time order_est_time) 
8
{ 
9
    id = order_id;
10
    source = order_source;
11
    dest = order_dest;
12
    start_time = order_start_time;
13
    path = order_path;
14
    est_time = order_est_time;
15
}
7
#define MAX_WAIT_TIME 120 //2 minutes in seconds
16 8

  
17
/** @Brief: Get order ID */
18
int Order::getid() const 
19
{
20
    return id;
21
}  
9
typedef unsigned int Address;
22 10

  
23
Address Order::get_source() const
24
{
25
    return source;
26
}
11
class Order {
12
    int id;
13
    Address source, dest;
14
    Time start_time;
15
    Path path;
16
    Duration est_time;
17
public:
18
    Order(int order_id, Address order_source, Address order_dest, 
19
        Time order_start_time, Path order_path, Duration order_est_time);
20
  
21
    Order() {};
27 22

  
28
Address Order::get_dest() const
29
{
30
    return dest;
31
}
23
    int getid() const;
24
    Address get_source() const;
25
    Address get_dest() const;
26
    Time get_start_time() const;
27
    Path get_path() const;
28
    Duration get_est_time() const;
29
    void set_path(Path order_path);
30
    int get_priority() const;
32 31

  
33
Time Order::get_start_time() const
34
{
35
    return start_time;
36
}
32
    bool operator==(Order& order);
33
    
34
};
37 35

  
38
Path Order::get_path() const
39
{
40
    return path;
41
}
36
class CompareOrder {
37
  public:
38
    bool operator()(Order& o1, Order& o2);   
39
};
42 40

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

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

  
53
void Order::set_path(Path order_path)
54
{
55
    path = order_path;
56
    return;
57
}
58
/** @Brief: Order comparison function for PQWrapper 
59
 *  NOTE: In order to have a min priority queue, using c++'s pq
60
 *  implementation, the compare function must return true if
61
 *  o1 is greater than o2.
62
 */
63
bool CompareOrder::operator()(Order& o1, Order& o2) 
64
{
65
  int pq_value1 = o1.get_start_time + MAX_WAIT_TIME - o1.get_est_time();
66
  int pq_value2 = o2.get_start_time + MAX_WAIT_TIME - o2.get_est_time();
67
  return pq_value1 > pq_value2;
68
}
69

  
41
#endif
trunk/code/projects/warehouse/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
int Order::get_priority() const
49 49
{
50
  return o1.id == o2.id;
50
    return start_time.toSec() + MAX_WAIT_TIME - est_time.toSec();
51 51
}
52 52

  
53
bool Order::operator==(Order& order)
54
{
55
  return this->id == order.id;
56
}
57

  
53 58
void Order::set_path(Path order_path)
54 59
{
55 60
    path = order_path;
......
62 67
 */
63 68
bool CompareOrder::operator()(Order& o1, Order& o2) 
64 69
{
65
  int pq_value1 = o1.get_start_time + MAX_WAIT_TIME - o1.get_est_time();
66
  int pq_value2 = o2.get_start_time + MAX_WAIT_TIME - o2.get_est_time();
67
  return pq_value1 > pq_value2;
70
  return o1.get_priority() > o2.get_priority();
68 71
}
69 72

  

Also available in: Unified diff