Project

General

Profile

Revision 11aa087a

ID11aa087a867c52e55873dbfb387b75e4bf85d6f2

Added by Priya almost 12 years ago

Demo almost working Scouts go home! a

View differences:

scout/libscout/src/behaviors/WH_Robot.cpp
21 21
  return nav_map->get_worst_case_time(start_state, target_state);
22 22
}
23 23

  
24
/** @brief Drives from state 2 to the robot home base and goes to sleep for
25
 *  x seconds
26
 */
27
void WH_Robot::go_home(int x)
28
{
29
  unsigned int curr_state = nav_map->get_state();
30
  if(curr_state != 2)
31
  {
32
    ROS_WARN("Tried to go home from incorrect state %d", curr_state);
33
    return;
34
  }
35
  turn_straight();
36
  follow_line();
37
  turn_left();
38
  follow_line();
39
  Duration sleep_time(x);
40
  sleep_time.sleep();
41
  return;
42
}
43

  
44
void WH_Robot::leave_home()
45
{
46
  unsigned int curr_state = nav_map->get_state();
47
  if(curr_state != 2)
48
  {
49
    ROS_WARN("Tried to go home from incorrect state %d", curr_state);
50
    return;
51
  }
52
  turn_straight();
53
  follow_line();
54
  nav_map->update_state(ISTRAIGHT);
55
  turn_left();
56
  follow_line();
57
}
58

  
24 59
/** @brief Based on the given path, make
25 60
 *         the series of turns to follow this path,
26 61
 *         updating the current state in the navigation map as we do so   
......
32 67
    Turn t = path_to_follow.path[i];
33 68
    unsigned int curr_state = nav_map->get_state();
34 69

  
35
    ROS_INFO("making turn %d", t);
36

  
37 70
    nav_map->update_state(t);
38 71
    switch(t)
39 72
    {
40 73
      case ISTRAIGHT:
41 74
        turn_straight();
42 75
        follow_line();
76
        if(curr_state == 2)
77
        {
78
          turn_straight();
79
          follow_line();
80
          turn_straight();
81
          follow_line();
82
        }
43 83
        break;
44 84
      case ILEFT:
45 85
        turn_left();
......
76 116
  assert(curr_task != DEFAULT_TASK);
77 117
  
78 118
  // remember where you started doing this task so we know where to return
79
  State home_state = nav_map->get_state();
119
  // State home_state = nav_map->get_state();
120
  // For now, use state 2.
80 121
  Path new_path = nav_map->shortest_path(curr_task->get_dest());
81 122
  curr_task->set_path(new_path);
82 123

  
......
97 138
    ROS_INFO("WH_robot: TASK FAILED!");
98 139
    did_task_complete = TASK_FAILED;
99 140
  }
100

  
101
  Path return_path = nav_map->shortest_path(home_state);
141
  
142
  // For now use state 2 as home state
143
  // Path return_path = nav_map->shortest_path(home_state);
144
  Path return_path = nav_map->shortest_path(2);
102 145
  curr_task->set_path(return_path);
103 146
  
104 147
  follow_path(return_path);
......
179 222
    }
180 223

  
181 224
    int error = exec_task();
225

  
226
    /** Go to robot home base */
227
    go_home(2);
228
    leave_home();
229

  
182 230
    if(error == TASK_COMPLETED)
183 231
    {
184 232
      std_msgs::String msg;
scout/libscout/src/behaviors/WH_Robot.h
26 26

  
27 27
        void robot_callback(const std_msgs::String::ConstPtr& msg);
28 28

  
29
        void go_home(int x);
30
        void leave_home();
31

  
29 32
    public:
30 33
        WH_Robot(std::string scoutname);
31 34
        ~WH_Robot();
scout/libscout/src/behaviors/navigationMap.cpp
51 51
navigationMap::navigationMap(string scoutname) : Behavior(scoutname, "navigationMap")
52 52
{
53 53
  /** Initialize Map 
54
   *
55
   *   1           2          3         4
56
   *  ----|-----------|----------|---------|---------->
54
   *                     _____
55
   *   1           2     |    | 3         4
56
   *  ----|-----------|--|----|--|---------|---------->
57 57
   *  <---|--5--------|--6-------|--7------|--8-------
58 58
   *      |           |          |         |
59 59
   *     9|         10|        11|       12|
60 60
   *      |           |          |         |
61
   *     ---         ---        ---       ---
61
   *     ---13       ---14      ---15     ---16
62 62
   */
63 63
  
64 64
    Edge* a1 = new Edge[ARRAY_SIZE];

Also available in: Unified diff