Revision d37cfc69 scout/libscout/src/behaviors/WH_Robot.cpp

View differences:

scout/libscout/src/behaviors/WH_Robot.cpp
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)

Also available in: Unified diff