Statistics
| Branch: | Revision:

root / scout / libscout / src / behaviors / WH_Robot.cpp @ 0e9eb730

History | View | Annotate | Download (4.72 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

    
13
WH_Robot::~WH_Robot()
14
{
15
  delete(nav_map);
16
}
17

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

    
23
/** @brief Based on the given path, make
24
 *         the series of turns to follow this path,
25
 *         updating the current state in the navigation map as we do so   
26
 *  @param the Path to follow
27
 */
28
void WH_Robot::follow_path(Path path_to_follow){
29
  ROS_INFO("Path length: %d", path_to_follow.len);
30
  for(int i=0; i<path_to_follow.len; i++)
31
  {
32
    Turn t = path_to_follow.path[i];
33
    ROS_INFO("making turn %d at state %d", (int)t, (int)nav_map->get_state());
34
    nav_map->update_state(t);
35
    Duration time = nav_map->get_time_remaining();
36
    while(time.sec > 0)
37
      time = nav_map->get_time_remaining();
38
  }  
39
  ROS_INFO("Path complete at state %d", (int)nav_map->get_state());
40
}
41

    
42
int WH_Robot::exec_task()
43
{
44
  ROS_INFO("WH_robot: Executing a task");
45
  assert(curr_task != DEFAULT_TASK);
46
  
47
  // remember where you started doing this task so we know where to return
48
  State home_state = nav_map->get_state();
49
  ROS_INFO("WH_robot: starting from home state %d", (int)home_state);
50
  ROS_INFO("WH_robot: aiming for state %d", (int)curr_task->get_dest());
51
  Path new_path = nav_map->shortest_path(curr_task->get_dest());
52
  curr_task->set_path(new_path);
53

    
54
  follow_path(new_path);
55
   
56
  //TODO: forklift yaaaay
57

    
58
  int did_task_complete;
59

    
60
  srand(0xDEADBEEF);
61
  int error = rand() % 12;
62
  if(error < 6) //Fail with 1/2 probability
63
  {
64
    ROS_INFO("WH_robot: TASK COMPLETE!");
65
    did_task_complete = TASK_COMPLETED;
66
  }
67
  else
68
  {
69
    ROS_INFO("WH_robot: TASK FAILED!");
70
    did_task_complete = TASK_FAILED;
71
  }
72

    
73
  ROS_INFO("WH_robot: Now I am attempting to go home from state %d to state %d", 
74
        (int)nav_map->get_state(), (int)home_state);
75

    
76
  Path return_path = nav_map->shortest_path(home_state);
77
  curr_task->set_path(return_path);
78
  
79
  follow_path(return_path);
80

    
81
  return did_task_complete;
82
}
83

    
84
void WH_Robot::robot_callback(const std_msgs::String::ConstPtr& msg)
85
{
86
  std::cout << "robot callback called." << std::endl;
87
  if(msg->data.compare(0, 11, "REG_SUCCESS") == 0)
88
  {
89
    id = atoi(msg->data.substr(12).c_str());
90
    reg_failed = 0;
91
  }
92
  else if(msg->data.compare(0, 8, "SET_TASK") == 0)
93
  {
94
    ROS_INFO("WH_ROBOT: Setting robot task");
95
    char* string = (char*)msg->data.c_str();
96
    char* data;
97
    data = strtok(string, " ");
98
    int order_id = atoi(data);
99
    data = strtok(string, " ");
100
    Address source = atoi(data);
101
    data = strtok(string, " ");
102
    Address dest = atoi(data);
103
    Time start_time = ros::Time::now();
104
    Path path;
105
    path.len = 0;
106
    path.path = NULL;
107
    Duration est_time(0);
108

    
109
    curr_task = new Order(order_id, source, dest, start_time, path, est_time);
110
  }
111
  else
112
  {
113
    ROS_INFO("I got a bad message: %s", msg->data.c_str());
114
  }
115
}
116

    
117
void WH_Robot::run ()
118
{
119
  ROS_INFO("A WH_Robot has been created");
120

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

    
124
  ROS_INFO("I am a robot. Registering with scheduler...");
125
  while(reg_failed && ok())
126
  {
127
    std_msgs::String msg;
128
    std::stringstream ss;
129
    ss << "REGISTER "<<name;
130
    msg.data = ss.str();
131
    robot_to_sched.publish(msg);
132

    
133
    spinOnce();
134
  }
135

    
136
  while(ok())
137
  {
138
    ROS_INFO("WH_ROBOT: Running main loop");
139

    
140

    
141
      std_msgs::String msg;
142
      std::stringstream ss;
143
      ss << "GET_TASK "<<id;
144
      msg.data = ss.str();
145
      robot_to_sched.publish(msg);
146

    
147
    while(curr_task == DEFAULT_TASK && ok()){
148
      spinOnce();
149
    }
150

    
151
    int error = exec_task();
152
    if(error == TASK_COMPLETED)
153
    {
154
      std_msgs::String msg;
155
      std::stringstream ss;
156
      ss << "SUCCESS "<<curr_task->getid();
157
      msg.data = ss.str();
158
      robot_to_sched.publish(msg);
159
    }
160
    else //error == TASK_FAILED
161
    {
162
      std_msgs::String msg;
163
      std::stringstream ss;
164
      ss << "FAILED "<<curr_task->getid();
165
      msg.data = ss.str();
166
      robot_to_sched.publish(msg);
167
    }
168
    delete curr_task;
169
    curr_task = DEFAULT_TASK;
170

    
171
  }
172
}
173

    
174
void WH_Robot::set_task(Order order)
175
{
176
  curr_task = new Order(order.getid(), order.get_source(), order.get_dest(), 
177
      order.get_start_time(), order.get_path(), order.get_est_time()); 
178
  return;
179
}
180