Revision 11aa087a
ID | 11aa087a867c52e55873dbfb387b75e4bf85d6f2 |
Demo almost working Scouts go home! a
scout/libscout/src/behaviors/WH_Robot.cpp | ||
---|---|---|
21 | 21 |
return nav_map->get_worst_case_time(start_state, target_state); |
22 | 22 |
} |
23 | 23 |
|
24 |
/** @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 |
|
|
24 | 59 |
/** @brief Based on the given path, make |
25 | 60 |
* the series of turns to follow this path, |
26 | 61 |
* updating the current state in the navigation map as we do so |
... | ... | |
32 | 67 |
Turn t = path_to_follow.path[i]; |
33 | 68 |
unsigned int curr_state = nav_map->get_state(); |
34 | 69 |
|
35 |
ROS_INFO("making turn %d", t); |
|
36 |
|
|
37 | 70 |
nav_map->update_state(t); |
38 | 71 |
switch(t) |
39 | 72 |
{ |
40 | 73 |
case ISTRAIGHT: |
41 | 74 |
turn_straight(); |
42 | 75 |
follow_line(); |
76 |
if(curr_state == 2) |
|
77 |
{ |
|
78 |
turn_straight(); |
|
79 |
follow_line(); |
|
80 |
turn_straight(); |
|
81 |
follow_line(); |
|
82 |
} |
|
43 | 83 |
break; |
44 | 84 |
case ILEFT: |
45 | 85 |
turn_left(); |
... | ... | |
76 | 116 |
assert(curr_task != DEFAULT_TASK); |
77 | 117 |
|
78 | 118 |
// remember where you started doing this task so we know where to return |
79 |
State home_state = nav_map->get_state(); |
|
119 |
// State home_state = nav_map->get_state(); |
|
120 |
// For now, use state 2. |
|
80 | 121 |
Path new_path = nav_map->shortest_path(curr_task->get_dest()); |
81 | 122 |
curr_task->set_path(new_path); |
82 | 123 |
|
... | ... | |
97 | 138 |
ROS_INFO("WH_robot: TASK FAILED!"); |
98 | 139 |
did_task_complete = TASK_FAILED; |
99 | 140 |
} |
100 |
|
|
101 |
Path return_path = nav_map->shortest_path(home_state); |
|
141 |
|
|
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); |
|
102 | 145 |
curr_task->set_path(return_path); |
103 | 146 |
|
104 | 147 |
follow_path(return_path); |
... | ... | |
179 | 222 |
} |
180 | 223 |
|
181 | 224 |
int error = exec_task(); |
225 |
|
|
226 |
/** Go to robot home base */ |
|
227 |
go_home(2); |
|
228 |
leave_home(); |
|
229 |
|
|
182 | 230 |
if(error == TASK_COMPLETED) |
183 | 231 |
{ |
184 | 232 |
std_msgs::String msg; |
Also available in: Unified diff