Statistics
| Branch: | Revision:

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

History | View | Annotate | Download (5.88 KB)

1 bebd9bcb Leon
#include "WH_Robot.h"
2 6761a531 Priya
#include "../helper_classes/Order.h"
3 bebd9bcb Leon
4
/** @Brief: warehouse robot constructor **/
5 d140fd71 Yuyang
WH_Robot::WH_Robot(std::string scoutname, Sensors* sensors):
6
            line_follow(scoutname, sensors) 
7 bebd9bcb Leon
{
8 d140fd71 Yuyang
  nav_map = new navigationMap(scoutname, sensors);
9 ad5b5826 Priya
  curr_task = DEFAULT_TASK;
10
  name = scoutname;
11
  reg_failed = 1;
12 58c19c15 Priya
  srand(0xDEADBEEF);
13 bebd9bcb Leon
}
14
15 6761a531 Priya
WH_Robot::~WH_Robot()
16
{
17 ad5b5826 Priya
  delete(nav_map);
18 6761a531 Priya
}
19
20
Duration WH_Robot::get_worst_case_time(State start_state, State target_state)
21 bebd9bcb Leon
{
22 6761a531 Priya
  return nav_map->get_worst_case_time(start_state, target_state);
23 bebd9bcb Leon
}
24
25 11aa087a Priya
/** @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 0e9eb730 Leon
/** @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 58c19c15 Priya
    unsigned int curr_state = nav_map->get_state();
70
71 0e9eb730 Leon
    nav_map->update_state(t);
72 1905324e Priya
    switch(t)
73
    {
74
      case ISTRAIGHT:
75 afa9104d Priya
        turn_straight();
76 1905324e Priya
        follow_line();
77 11aa087a Priya
        if(curr_state == 2)
78
        {
79
          turn_straight();
80
          follow_line();
81
          turn_straight();
82
          follow_line();
83
        }
84 1905324e Priya
        break;
85
      case ILEFT:
86
        turn_left();
87
        follow_line();
88
        break;
89
      case IRIGHT:
90 58c19c15 Priya
        if(9 <= curr_state && curr_state <= 12)
91
        {
92
          turn_straight();
93
          follow_line();
94
        }
95 1905324e Priya
        turn_right();
96
        follow_line();
97 58c19c15 Priya
        if(curr_state <= 4)
98
        {
99
          turn_straight();
100
          follow_line();
101
        }
102 1905324e Priya
        break;
103
      case IUTURN:
104
        u_turn();
105
        follow_line();
106
        break;
107 58c19c15 Priya
      case ISPOTTURN:
108
        spot_turn();
109
        follow_line();
110
        break;
111 1905324e Priya
    }
112 0e9eb730 Leon
  }  
113
}
114
115 9b4328d7 Priya
int WH_Robot::exec_task()
116 bebd9bcb Leon
{
117 ad5b5826 Priya
  assert(curr_task != DEFAULT_TASK);
118 5d0687a9 Priya
  
119 d37cfc69 Leon
  // remember where you started doing this task so we know where to return
120 11aa087a Priya
  // State home_state = nav_map->get_state();
121
  // For now, use state 2.
122 5d0687a9 Priya
  Path new_path = nav_map->shortest_path(curr_task->get_dest());
123
  curr_task->set_path(new_path);
124 d37cfc69 Leon
125 0e9eb730 Leon
  follow_path(new_path);
126 d37cfc69 Leon
   
127
  //TODO: forklift yaaaay
128
129
  int did_task_complete;
130
131 ad5b5826 Priya
  int error = rand() % 12;
132 58c19c15 Priya
  if(error < 9) //Fail with 1/4 probability
133 ad5b5826 Priya
  {
134
    ROS_INFO("WH_robot: TASK COMPLETE!");
135 d37cfc69 Leon
    did_task_complete = TASK_COMPLETED;
136 ad5b5826 Priya
  }
137
  else
138
  {
139
    ROS_INFO("WH_robot: TASK FAILED!");
140 d37cfc69 Leon
    did_task_complete = TASK_FAILED;
141 ad5b5826 Priya
  }
142 11aa087a Priya
  
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 d37cfc69 Leon
  curr_task->set_path(return_path);
147
  
148
  follow_path(return_path);
149
150
  return did_task_complete;
151
}
152
153 7ac5e9bc Priya
void WH_Robot::robot_callback(const std_msgs::String::ConstPtr& msg)
154 3f72678f Priya
{
155
  if(msg->data.compare(0, 11, "REG_SUCCESS") == 0)
156
  {
157 7ac5e9bc Priya
    id = atoi(msg->data.substr(12).c_str());
158 3f72678f Priya
    reg_failed = 0;
159
  }
160
  else if(msg->data.compare(0, 8, "SET_TASK") == 0)
161
  {
162 7ac5e9bc Priya
    char* string = (char*)msg->data.c_str();
163 3f72678f Priya
    char* data;
164
    data = strtok(string, " ");
165 58c19c15 Priya
166 351f71d1 Priya
    data = strtok(NULL, " ");
167 7ac5e9bc Priya
    int order_id = atoi(data);
168 351f71d1 Priya
169
    data = strtok(NULL, " ");
170 3f72678f Priya
    Address source = atoi(data);
171 351f71d1 Priya
172
    data = strtok(NULL, " ");
173 3f72678f Priya
    Address dest = atoi(data);
174 351f71d1 Priya
175 3f72678f Priya
    Time start_time = ros::Time::now();
176 7ac5e9bc Priya
    Path path;
177
    path.len = 0;
178
    path.path = NULL;
179 3f72678f Priya
    Duration est_time(0);
180
181 7ac5e9bc Priya
    curr_task = new Order(order_id, source, dest, start_time, path, est_time);
182 3f72678f Priya
  }
183
  else
184
  {
185 7ac5e9bc Priya
    ROS_INFO("I got a bad message: %s", msg->data.c_str());
186 3f72678f Priya
  }
187
}
188
189
void WH_Robot::run ()
190
{
191 ad5b5826 Priya
  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 1905324e Priya
  follow_line();
209
210 ad5b5826 Priya
  while(ok())
211
  {
212
    ROS_INFO("WH_ROBOT: Running main loop");
213
214
215 7ac5e9bc Priya
      std_msgs::String msg;
216
      std::stringstream ss;
217 ad5b5826 Priya
      ss << "GET_TASK "<<id;
218 7ac5e9bc Priya
      msg.data = ss.str();
219
      robot_to_sched.publish(msg);
220
221 ad5b5826 Priya
    while(curr_task == DEFAULT_TASK && ok()){
222 7ac5e9bc Priya
      spinOnce();
223
    }
224 3f72678f Priya
225 ad5b5826 Priya
    int error = exec_task();
226 11aa087a Priya
227
    /** Go to robot home base */
228
    go_home(2);
229
    leave_home();
230
231 ad5b5826 Priya
    if(error == TASK_COMPLETED)
232 bebd9bcb Leon
    {
233 ad5b5826 Priya
      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 bebd9bcb Leon
    }
239 ad5b5826 Priya
    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 bebd9bcb Leon
}
252
253 9b4328d7 Priya
void WH_Robot::set_task(Order order)
254
{
255 ad5b5826 Priya
  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 bebd9bcb Leon
}
259 9b4328d7 Priya