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