root / scout / libscout / src / behaviors / WH_Robot.cpp @ 3db79f25
History | View | Annotate | Download (5.83 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 | 1905324e | Priya | WH_Robot::WH_Robot(std::string scoutname):line_follow(scoutname)
|
6 | bebd9bcb | Leon | { |
7 | ad5b5826 | Priya | nav_map = new navigationMap(scoutname);
|
8 | curr_task = DEFAULT_TASK; |
||
9 | name = scoutname; |
||
10 | reg_failed = 1;
|
||
11 | 58c19c15 | Priya | srand(0xDEADBEEF);
|
12 | bebd9bcb | Leon | } |
13 | |||
14 | 6761a531 | Priya | WH_Robot::~WH_Robot() |
15 | { |
||
16 | ad5b5826 | Priya | delete(nav_map);
|
17 | 6761a531 | Priya | } |
18 | |||
19 | Duration WH_Robot::get_worst_case_time(State start_state, State target_state) |
||
20 | bebd9bcb | Leon | { |
21 | 6761a531 | Priya | return nav_map->get_worst_case_time(start_state, target_state);
|
22 | bebd9bcb | Leon | } |
23 | |||
24 | 11aa087a | Priya | /** @brief Drives from state 2 to the robot home base and goes to sleep for
|
25 | * x seconds
|
||
26 | */
|
||
27 | void WH_Robot::go_home(int x) |
||
28 | { |
||
29 | unsigned int curr_state = nav_map->get_state(); |
||
30 | if(curr_state != 2) |
||
31 | { |
||
32 | ROS_WARN("Tried to go home from incorrect state %d", curr_state);
|
||
33 | return;
|
||
34 | } |
||
35 | turn_straight(); |
||
36 | follow_line(); |
||
37 | turn_left(); |
||
38 | follow_line(); |
||
39 | Duration sleep_time(x); |
||
40 | sleep_time.sleep(); |
||
41 | return;
|
||
42 | } |
||
43 | |||
44 | void WH_Robot::leave_home()
|
||
45 | { |
||
46 | unsigned int curr_state = nav_map->get_state(); |
||
47 | if(curr_state != 2) |
||
48 | { |
||
49 | ROS_WARN("Tried to go home from incorrect state %d", curr_state);
|
||
50 | return;
|
||
51 | } |
||
52 | turn_straight(); |
||
53 | follow_line(); |
||
54 | nav_map->update_state(ISTRAIGHT); |
||
55 | turn_left(); |
||
56 | follow_line(); |
||
57 | } |
||
58 | |||
59 | 0e9eb730 | Leon | /** @brief Based on the given path, make
|
60 | * the series of turns to follow this path,
|
||
61 | * updating the current state in the navigation map as we do so
|
||
62 | * @param the Path to follow
|
||
63 | */
|
||
64 | void WH_Robot::follow_path(Path path_to_follow){
|
||
65 | for(int i=0; i<path_to_follow.len; i++) |
||
66 | { |
||
67 | Turn t = path_to_follow.path[i]; |
||
68 | 58c19c15 | Priya | unsigned int curr_state = nav_map->get_state(); |
69 | |||
70 | 0e9eb730 | Leon | nav_map->update_state(t); |
71 | 1905324e | Priya | switch(t)
|
72 | { |
||
73 | case ISTRAIGHT:
|
||
74 | afa9104d | Priya | turn_straight(); |
75 | 1905324e | Priya | follow_line(); |
76 | 11aa087a | Priya | if(curr_state == 2) |
77 | { |
||
78 | turn_straight(); |
||
79 | follow_line(); |
||
80 | turn_straight(); |
||
81 | follow_line(); |
||
82 | } |
||
83 | 1905324e | Priya | break;
|
84 | case ILEFT:
|
||
85 | turn_left(); |
||
86 | follow_line(); |
||
87 | break;
|
||
88 | case IRIGHT:
|
||
89 | 58c19c15 | Priya | if(9 <= curr_state && curr_state <= 12) |
90 | { |
||
91 | turn_straight(); |
||
92 | follow_line(); |
||
93 | } |
||
94 | 1905324e | Priya | turn_right(); |
95 | follow_line(); |
||
96 | 58c19c15 | Priya | if(curr_state <= 4) |
97 | { |
||
98 | turn_straight(); |
||
99 | follow_line(); |
||
100 | } |
||
101 | 1905324e | Priya | break;
|
102 | case IUTURN:
|
||
103 | u_turn(); |
||
104 | follow_line(); |
||
105 | break;
|
||
106 | 58c19c15 | Priya | case ISPOTTURN:
|
107 | spot_turn(); |
||
108 | follow_line(); |
||
109 | break;
|
||
110 | 1905324e | Priya | } |
111 | 0e9eb730 | Leon | } |
112 | } |
||
113 | |||
114 | 9b4328d7 | Priya | int WH_Robot::exec_task()
|
115 | bebd9bcb | Leon | { |
116 | ad5b5826 | Priya | assert(curr_task != DEFAULT_TASK); |
117 | 5d0687a9 | Priya | |
118 | d37cfc69 | Leon | // remember where you started doing this task so we know where to return
|
119 | 11aa087a | Priya | // State home_state = nav_map->get_state();
|
120 | // For now, use state 2.
|
||
121 | 5d0687a9 | Priya | Path new_path = nav_map->shortest_path(curr_task->get_dest()); |
122 | curr_task->set_path(new_path); |
||
123 | d37cfc69 | Leon | |
124 | 0e9eb730 | Leon | follow_path(new_path); |
125 | d37cfc69 | Leon | |
126 | //TODO: forklift yaaaay
|
||
127 | |||
128 | int did_task_complete;
|
||
129 | |||
130 | ad5b5826 | Priya | int error = rand() % 12; |
131 | 58c19c15 | Priya | if(error < 9) //Fail with 1/4 probability |
132 | ad5b5826 | Priya | { |
133 | ROS_INFO("WH_robot: TASK COMPLETE!");
|
||
134 | d37cfc69 | Leon | did_task_complete = TASK_COMPLETED; |
135 | ad5b5826 | Priya | } |
136 | else
|
||
137 | { |
||
138 | ROS_INFO("WH_robot: TASK FAILED!");
|
||
139 | d37cfc69 | Leon | did_task_complete = TASK_FAILED; |
140 | ad5b5826 | Priya | } |
141 | 11aa087a | Priya | |
142 | // For now use state 2 as home state
|
||
143 | // Path return_path = nav_map->shortest_path(home_state);
|
||
144 | Path return_path = nav_map->shortest_path(2);
|
||
145 | d37cfc69 | Leon | curr_task->set_path(return_path); |
146 | |||
147 | follow_path(return_path); |
||
148 | |||
149 | return did_task_complete;
|
||
150 | } |
||
151 | |||
152 | 7ac5e9bc | Priya | void WH_Robot::robot_callback(const std_msgs::String::ConstPtr& msg) |
153 | 3f72678f | Priya | { |
154 | if(msg->data.compare(0, 11, "REG_SUCCESS") == 0) |
||
155 | { |
||
156 | 7ac5e9bc | Priya | id = atoi(msg->data.substr(12).c_str());
|
157 | 3f72678f | Priya | reg_failed = 0;
|
158 | } |
||
159 | else if(msg->data.compare(0, 8, "SET_TASK") == 0) |
||
160 | { |
||
161 | 7ac5e9bc | Priya | char* string = (char*)msg->data.c_str(); |
162 | 3f72678f | Priya | char* data;
|
163 | data = strtok(string, " "); |
||
164 | 58c19c15 | Priya | |
165 | 351f71d1 | Priya | data = strtok(NULL, " "); |
166 | 7ac5e9bc | Priya | int order_id = atoi(data);
|
167 | 351f71d1 | Priya | |
168 | data = strtok(NULL, " "); |
||
169 | 3f72678f | Priya | Address source = atoi(data); |
170 | 351f71d1 | Priya | |
171 | data = strtok(NULL, " "); |
||
172 | 3f72678f | Priya | Address dest = atoi(data); |
173 | 351f71d1 | Priya | |
174 | 3f72678f | Priya | Time start_time = ros::Time::now(); |
175 | 7ac5e9bc | Priya | Path path; |
176 | path.len = 0;
|
||
177 | path.path = NULL;
|
||
178 | 3f72678f | Priya | Duration est_time(0);
|
179 | |||
180 | 7ac5e9bc | Priya | curr_task = new Order(order_id, source, dest, start_time, path, est_time);
|
181 | 3f72678f | Priya | } |
182 | else
|
||
183 | { |
||
184 | 7ac5e9bc | Priya | ROS_INFO("I got a bad message: %s", msg->data.c_str());
|
185 | 3f72678f | Priya | } |
186 | } |
||
187 | |||
188 | void WH_Robot::run ()
|
||
189 | { |
||
190 | ad5b5826 | Priya | ROS_INFO("A WH_Robot has been created");
|
191 | |||
192 | robot_to_sched = node.advertise<std_msgs::String>("robot_to_sched", 1000); |
||
193 | sched_to_robot = node.subscribe(name + "_topic", 1000, &WH_Robot::robot_callback, this); |
||
194 | |||
195 | ROS_INFO("I am a robot. Registering with scheduler...");
|
||
196 | while(reg_failed && ok())
|
||
197 | { |
||
198 | std_msgs::String msg; |
||
199 | std::stringstream ss; |
||
200 | ss << "REGISTER "<<name;
|
||
201 | msg.data = ss.str(); |
||
202 | robot_to_sched.publish(msg); |
||
203 | |||
204 | spinOnce(); |
||
205 | } |
||
206 | |||
207 | 1905324e | Priya | follow_line(); |
208 | |||
209 | ad5b5826 | Priya | while(ok())
|
210 | { |
||
211 | ROS_INFO("WH_ROBOT: Running main loop");
|
||
212 | |||
213 | |||
214 | 7ac5e9bc | Priya | std_msgs::String msg; |
215 | std::stringstream ss; |
||
216 | ad5b5826 | Priya | ss << "GET_TASK "<<id;
|
217 | 7ac5e9bc | Priya | msg.data = ss.str(); |
218 | robot_to_sched.publish(msg); |
||
219 | |||
220 | ad5b5826 | Priya | while(curr_task == DEFAULT_TASK && ok()){
|
221 | 7ac5e9bc | Priya | spinOnce(); |
222 | } |
||
223 | 3f72678f | Priya | |
224 | ad5b5826 | Priya | int error = exec_task();
|
225 | 11aa087a | Priya | |
226 | /** Go to robot home base */
|
||
227 | go_home(2);
|
||
228 | leave_home(); |
||
229 | |||
230 | ad5b5826 | Priya | if(error == TASK_COMPLETED)
|
231 | bebd9bcb | Leon | { |
232 | ad5b5826 | Priya | std_msgs::String msg; |
233 | std::stringstream ss; |
||
234 | ss << "SUCCESS "<<curr_task->getid();
|
||
235 | msg.data = ss.str(); |
||
236 | robot_to_sched.publish(msg); |
||
237 | bebd9bcb | Leon | } |
238 | ad5b5826 | Priya | else //error == TASK_FAILED |
239 | { |
||
240 | std_msgs::String msg; |
||
241 | std::stringstream ss; |
||
242 | ss << "FAILED "<<curr_task->getid();
|
||
243 | msg.data = ss.str(); |
||
244 | robot_to_sched.publish(msg); |
||
245 | } |
||
246 | delete curr_task;
|
||
247 | curr_task = DEFAULT_TASK; |
||
248 | |||
249 | } |
||
250 | bebd9bcb | Leon | } |
251 | |||
252 | 9b4328d7 | Priya | void WH_Robot::set_task(Order order)
|
253 | { |
||
254 | ad5b5826 | Priya | curr_task = new Order(order.getid(), order.get_source(), order.get_dest(),
|
255 | order.get_start_time(), order.get_path(), order.get_est_time()); |
||
256 | return;
|
||
257 | bebd9bcb | Leon | } |
258 | 9b4328d7 | Priya |