Revision d37cfc69
ID | d37cfc692f95396c188e9bcf1a782e055381c308 |
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