Revision 0e9eb730

View differences:

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