Revision d37cfc69
WH_robot goes home in exec_task, follow_path function added; untested
| scout/libscout/src/behaviors/WH_Robot.cpp | ||
|---|---|---|
| 25 | 25 |
ROS_INFO("WH_robot: Executing a task");
|
| 26 | 26 |
assert(curr_task != DEFAULT_TASK); |
| 27 | 27 |
|
| 28 |
// remember where you started doing this task so we know where to return |
|
| 29 |
State home_state = nav_map->get_state; |
|
| 30 |
ROS_INFO("WH_robot: starting from home state %d", (int)home_state);
|
|
| 31 |
|
|
| 28 | 32 |
Path new_path = nav_map->shortest_path(curr_task->get_dest()); |
| 29 | 33 |
curr_task->set_path(new_path); |
| 30 |
for(int i=0; i<new_path.len; i++) |
|
| 31 |
{
|
|
| 32 |
Turn t = new_path.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());
|
|
| 34 |
|
|
| 35 |
followPath(new_path); |
|
| 36 |
|
|
| 37 |
//TODO: forklift yaaaay |
|
| 38 |
|
|
| 39 |
int did_task_complete; |
|
| 40 |
|
|
| 40 | 41 |
srand(0xDEADBEEF); |
| 41 | 42 |
int error = rand() % 12; |
| 42 | 43 |
if(error < 6) //Fail with 1/2 probability |
| 43 | 44 |
{
|
| 44 | 45 |
ROS_INFO("WH_robot: TASK COMPLETE!");
|
| 45 |
return TASK_COMPLETED;
|
|
| 46 |
did_task_complete = TASK_COMPLETED;
|
|
| 46 | 47 |
} |
| 47 | 48 |
else |
| 48 | 49 |
{
|
| 49 | 50 |
ROS_INFO("WH_robot: TASK FAILED!");
|
| 50 |
return TASK_FAILED;
|
|
| 51 |
did_task_complete = TASK_FAILED;
|
|
| 51 | 52 |
} |
| 53 |
|
|
| 54 |
ROS_INFO("WH_robot: Now I am attempting to go home from state %d to state %d",
|
|
| 55 |
(int)nav_map->get_state(), (int)home_state); |
|
| 56 |
|
|
| 57 |
Path return_path = nav_map->shortest_path(home_state); |
|
| 58 |
curr_task->set_path(return_path); |
|
| 59 |
|
|
| 60 |
follow_path(return_path); |
|
| 61 |
|
|
| 62 |
return did_task_complete; |
|
| 63 |
} |
|
| 64 |
|
|
| 65 |
/** @brief Based on the given path, make |
|
| 66 |
* the series of turns to follow this path, |
|
| 67 |
* updating the current state in the navigation map as we do so |
|
| 68 |
* @param the Path to follow |
|
| 69 |
*/ |
|
| 70 |
void WH_Robot::follow_path(Path path_to_follow){
|
|
| 71 |
|
|
| 72 |
for(int i=0; i<path_to_follow.len; i++) |
|
| 73 |
{
|
|
| 74 |
Turn t = path_to_follow.path[i]; |
|
| 75 |
ROS_INFO("making turn %d at state %d", (int)t, (int)nav_map->get_state());
|
|
| 76 |
nav_map->update_state(t); |
|
| 77 |
Duration time = nav_map->get_time_remaining(); |
|
| 78 |
while(time.sec > 0) |
|
| 79 |
time = nav_map->get_time_remaining(); |
|
| 80 |
} |
|
| 81 |
ROS_INFO("Path complete at state %d", (int)nav_map->get_state());
|
|
| 52 | 82 |
} |
| 53 | 83 |
|
| 54 | 84 |
void WH_Robot::robot_callback(const std_msgs::String::ConstPtr& msg) |
Also available in: Unified diff