Revision 1905324e
Trying to make the warehouse robot drive around
scout/libscout/src/behaviors/WH_Robot.cpp | ||
---|---|---|
2 | 2 |
#include "../helper_classes/Order.h" |
3 | 3 |
|
4 | 4 |
/** @Brief: warehouse robot constructor **/ |
5 |
WH_Robot::WH_Robot(std::string scoutname):Behavior(scoutname, "WH_Robot")
|
|
5 |
WH_Robot::WH_Robot(std::string scoutname):line_follow(scoutname)
|
|
6 | 6 |
{ |
7 | 7 |
nav_map = new navigationMap(scoutname); |
8 | 8 |
curr_task = DEFAULT_TASK; |
... | ... | |
26 | 26 |
* @param the Path to follow |
27 | 27 |
*/ |
28 | 28 |
void WH_Robot::follow_path(Path path_to_follow){ |
29 |
ROS_INFO("Path length: %d", path_to_follow.len); |
|
30 | 29 |
for(int i=0; i<path_to_follow.len; i++) |
31 | 30 |
{ |
32 | 31 |
Turn t = path_to_follow.path[i]; |
33 |
ROS_INFO("making turn %d at state %d", (int)t, (int)nav_map->get_state());
|
|
32 |
ROS_INFO("making turn %d", t);
|
|
34 | 33 |
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(); |
|
34 |
switch(t) |
|
35 |
{ |
|
36 |
case ISTRAIGHT: |
|
37 |
follow_line(); |
|
38 |
break; |
|
39 |
case ILEFT: |
|
40 |
turn_left(); |
|
41 |
follow_line(); |
|
42 |
break; |
|
43 |
case IRIGHT: |
|
44 |
turn_right(); |
|
45 |
follow_line(); |
|
46 |
break; |
|
47 |
case IUTURN: |
|
48 |
u_turn(); |
|
49 |
follow_line(); |
|
50 |
break; |
|
51 |
} |
|
38 | 52 |
} |
39 |
ROS_INFO("Path complete at state %d", (int)nav_map->get_state()); |
|
40 | 53 |
} |
41 | 54 |
|
42 | 55 |
int WH_Robot::exec_task() |
... | ... | |
141 | 154 |
spinOnce(); |
142 | 155 |
} |
143 | 156 |
|
157 |
follow_line(); |
|
158 |
|
|
144 | 159 |
while(ok()) |
145 | 160 |
{ |
146 | 161 |
ROS_INFO("WH_ROBOT: Running main loop"); |
Also available in: Unified diff