Revision 0e9eb730
ID | 0e9eb730c5589af1e21c3d66284fe5307f7b18ff |
some testing done on WH_robot exec_task, issue pinpointed to curr_task's dest being stored as 0, not fixed yet
scout/libscout/src/behaviors/WH_Robot.cpp | ||
---|---|---|
20 | 20 |
return nav_map->get_worst_case_time(start_state, target_state); |
21 | 21 |
} |
22 | 22 |
|
23 |
/** @brief Based on the given path, make |
|
24 |
* the series of turns to follow this path, |
|
25 |
* updating the current state in the navigation map as we do so |
|
26 |
* @param the Path to follow |
|
27 |
*/ |
|
28 |
void WH_Robot::follow_path(Path path_to_follow){ |
|
29 |
ROS_INFO("Path length: %d", path_to_follow.len); |
|
30 |
for(int i=0; i<path_to_follow.len; i++) |
|
31 |
{ |
|
32 |
Turn t = path_to_follow.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()); |
|
40 |
} |
|
41 |
|
|
23 | 42 |
int WH_Robot::exec_task() |
24 | 43 |
{ |
25 | 44 |
ROS_INFO("WH_robot: Executing a task"); |
26 | 45 |
assert(curr_task != DEFAULT_TASK); |
27 | 46 |
|
28 | 47 |
// remember where you started doing this task so we know where to return |
29 |
State home_state = nav_map->get_state; |
|
48 |
State home_state = nav_map->get_state();
|
|
30 | 49 |
ROS_INFO("WH_robot: starting from home state %d", (int)home_state); |
31 |
|
|
50 |
ROS_INFO("WH_robot: aiming for state %d", (int)curr_task->get_dest()); |
|
32 | 51 |
Path new_path = nav_map->shortest_path(curr_task->get_dest()); |
33 | 52 |
curr_task->set_path(new_path); |
34 | 53 |
|
35 |
followPath(new_path);
|
|
54 |
follow_path(new_path);
|
|
36 | 55 |
|
37 | 56 |
//TODO: forklift yaaaay |
38 | 57 |
|
... | ... | |
62 | 81 |
return did_task_complete; |
63 | 82 |
} |
64 | 83 |
|
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()); |
|
82 |
} |
|
83 |
|
|
84 | 84 |
void WH_Robot::robot_callback(const std_msgs::String::ConstPtr& msg) |
85 | 85 |
{ |
86 | 86 |
std::cout << "robot callback called." << std::endl; |
Also available in: Unified diff