root / scout / libscout / src / behaviors / WH_Robot.cpp @ c840fbe6
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 |