Revision 2009 trunk/code/projects/warehouse/WH_Robot.cpp

View differences:

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)

Also available in: Unified diff