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