Statistics
| Branch: | Revision:

scoutos / scout / libscout / src / behaviors / WH_Robot.cpp @ d140fd71

History | View | Annotate | Download (5.88 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, Sensors* sensors):
6
            line_follow(scoutname, sensors) 
7
{
8
  nav_map = new navigationMap(scoutname, sensors);
9
  curr_task = DEFAULT_TASK;
10
  name = scoutname;
11
  reg_failed = 1;
12
  srand(0xDEADBEEF);
13
}
14

    
15
WH_Robot::~WH_Robot()
16
{
17
  delete(nav_map);
18
}
19

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

    
25
/** @brief Drives from state 2 to the robot home base and goes to sleep for
26
 *  x seconds
27
 */
28
void WH_Robot::go_home(int x)
29
{
30
  unsigned int curr_state = nav_map->get_state();
31
  if(curr_state != 2)
32
  {
33
    ROS_WARN("Tried to go home from incorrect state %d", curr_state);
34
    return;
35
  }
36
  turn_straight();
37
  follow_line();
38
  turn_left();
39
  follow_line();
40
  Duration sleep_time(x);
41
  sleep_time.sleep();
42
  return;
43
}
44

    
45
void WH_Robot::leave_home()
46
{
47
  unsigned int curr_state = nav_map->get_state();
48
  if(curr_state != 2)
49
  {
50
    ROS_WARN("Tried to go home from incorrect state %d", curr_state);
51
    return;
52
  }
53
  turn_straight();
54
  follow_line();
55
  nav_map->update_state(ISTRAIGHT);
56
  turn_left();
57
  follow_line();
58
}
59

    
60
/** @brief Based on the given path, make
61
 *         the series of turns to follow this path,
62
 *         updating the current state in the navigation map as we do so   
63
 *  @param the Path to follow
64
 */
65
void WH_Robot::follow_path(Path path_to_follow){
66
  for(int i=0; i<path_to_follow.len; i++)
67
  {
68
    Turn t = path_to_follow.path[i];
69
    unsigned int curr_state = nav_map->get_state();
70

    
71
    nav_map->update_state(t);
72
    switch(t)
73
    {
74
      case ISTRAIGHT:
75
        turn_straight();
76
        follow_line();
77
        if(curr_state == 2)
78
        {
79
          turn_straight();
80
          follow_line();
81
          turn_straight();
82
          follow_line();
83
        }
84
        break;
85
      case ILEFT:
86
        turn_left();
87
        follow_line();
88
        break;
89
      case IRIGHT:
90
        if(9 <= curr_state && curr_state <= 12)
91
        {
92
          turn_straight();
93
          follow_line();
94
        }
95
        turn_right();
96
        follow_line();
97
        if(curr_state <= 4)
98
        {
99
          turn_straight();
100
          follow_line();
101
        }
102
        break;
103
      case IUTURN:
104
        u_turn();
105
        follow_line();
106
        break;
107
      case ISPOTTURN:
108
        spot_turn();
109
        follow_line();
110
        break;
111
    }
112
  }  
113
}
114

    
115
int WH_Robot::exec_task()
116
{
117
  assert(curr_task != DEFAULT_TASK);
118
  
119
  // remember where you started doing this task so we know where to return
120
  // State home_state = nav_map->get_state();
121
  // For now, use state 2.
122
  Path new_path = nav_map->shortest_path(curr_task->get_dest());
123
  curr_task->set_path(new_path);
124

    
125
  follow_path(new_path);
126
   
127
  //TODO: forklift yaaaay
128

    
129
  int did_task_complete;
130

    
131
  int error = rand() % 12;
132
  if(error < 9) //Fail with 1/4 probability
133
  {
134
    ROS_INFO("WH_robot: TASK COMPLETE!");
135
    did_task_complete = TASK_COMPLETED;
136
  }
137
  else
138
  {
139
    ROS_INFO("WH_robot: TASK FAILED!");
140
    did_task_complete = TASK_FAILED;
141
  }
142
  
143
  // For now use state 2 as home state
144
  // Path return_path = nav_map->shortest_path(home_state);
145
  Path return_path = nav_map->shortest_path(2);
146
  curr_task->set_path(return_path);
147
  
148
  follow_path(return_path);
149

    
150
  return did_task_complete;
151
}
152

    
153
void WH_Robot::robot_callback(const std_msgs::String::ConstPtr& msg)
154
{
155
  if(msg->data.compare(0, 11, "REG_SUCCESS") == 0)
156
  {
157
    id = atoi(msg->data.substr(12).c_str());
158
    reg_failed = 0;
159
  }
160
  else if(msg->data.compare(0, 8, "SET_TASK") == 0)
161
  {
162
    char* string = (char*)msg->data.c_str();
163
    char* data;
164
    data = strtok(string, " ");
165

    
166
    data = strtok(NULL, " ");
167
    int order_id = atoi(data);
168

    
169
    data = strtok(NULL, " ");
170
    Address source = atoi(data);
171

    
172
    data = strtok(NULL, " ");
173
    Address dest = atoi(data);
174

    
175
    Time start_time = ros::Time::now();
176
    Path path;
177
    path.len = 0;
178
    path.path = NULL;
179
    Duration est_time(0);
180

    
181
    curr_task = new Order(order_id, source, dest, start_time, path, est_time);
182
  }
183
  else
184
  {
185
    ROS_INFO("I got a bad message: %s", msg->data.c_str());
186
  }
187
}
188

    
189
void WH_Robot::run ()
190
{
191
  ROS_INFO("A WH_Robot has been created");
192

    
193
  robot_to_sched = node.advertise<std_msgs::String>("robot_to_sched", 1000);
194
  sched_to_robot = node.subscribe(name + "_topic", 1000, &WH_Robot::robot_callback, this);
195

    
196
  ROS_INFO("I am a robot. Registering with scheduler...");
197
  while(reg_failed && ok())
198
  {
199
    std_msgs::String msg;
200
    std::stringstream ss;
201
    ss << "REGISTER "<<name;
202
    msg.data = ss.str();
203
    robot_to_sched.publish(msg);
204

    
205
    spinOnce();
206
  }
207

    
208
  follow_line();
209

    
210
  while(ok())
211
  {
212
    ROS_INFO("WH_ROBOT: Running main loop");
213

    
214

    
215
      std_msgs::String msg;
216
      std::stringstream ss;
217
      ss << "GET_TASK "<<id;
218
      msg.data = ss.str();
219
      robot_to_sched.publish(msg);
220

    
221
    while(curr_task == DEFAULT_TASK && ok()){
222
      spinOnce();
223
    }
224

    
225
    int error = exec_task();
226

    
227
    /** Go to robot home base */
228
    go_home(2);
229
    leave_home();
230

    
231
    if(error == TASK_COMPLETED)
232
    {
233
      std_msgs::String msg;
234
      std::stringstream ss;
235
      ss << "SUCCESS "<<curr_task->getid();
236
      msg.data = ss.str();
237
      robot_to_sched.publish(msg);
238
    }
239
    else //error == TASK_FAILED
240
    {
241
      std_msgs::String msg;
242
      std::stringstream ss;
243
      ss << "FAILED "<<curr_task->getid();
244
      msg.data = ss.str();
245
      robot_to_sched.publish(msg);
246
    }
247
    delete curr_task;
248
    curr_task = DEFAULT_TASK;
249

    
250
  }
251
}
252

    
253
void WH_Robot::set_task(Order order)
254
{
255
  curr_task = new Order(order.getid(), order.get_source(), order.get_dest(), 
256
      order.get_start_time(), order.get_path(), order.get_est_time()); 
257
  return;
258
}
259