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)
|