Statistics
| Revision:

root / trunk / code / projects / warehouse / WH_Robot.cpp @ 2009

History | View | Annotate | Download (3.22 KB)

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

    
4
/** @Brief: warehouse robot constructor **/
5
WH_Robot::WH_Robot(std::string scoutname):Behavior(scoutname, "WH_Robot")
6
{
7
    nav_map = new navigationMap(scoutname);
8
    curr_task = DEFAULT_TASK;
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");
15
}
16

    
17
WH_Robot::~WH_Robot()
18
{
19
    delete(nav_map);
20
}
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

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

    
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)
81
    {
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();
89
    }
90

    
91
    while(ok())
92
    {
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;
129
    }
130
}
131

    
132
void WH_Robot::set_task(Order order)
133
{
134
    curr_task = new Order(order.getid(), order.get_source(), order.get_dest(), 
135
        order.get_start_time(), order.get_path(), order.get_est_time()); 
136
    return;
137
}
138