Revision af7e0f94

View differences:

scout/libscout/src/LinesensorControl.cpp
61 61
        ROS_ERROR("LinesensorControl query failed.");
62 62
    }
63 63

  
64
    if (srv.response.readings.size() != 8)
65
    {
66
        ROS_WARN("Linesensor reading vector has %d readings, 8 expected.",
67
                 int(srv.response.readings.size()));
68
    }
69

  
64 70
    return srv.response.readings;
65 71
}
66 72

  
......
94 100
    return ret_val;
95 101
}
96 102

  
97
bool LinesensorControl::fullline()
103
bool LinesensorControl::fullline(vector<uint32_t> readings)
98 104
{
99
    vector<uint32_t> readings = query();
105
    if (readings.size() < 8)
106
    {
107
        return false;
108
    }
109

  
100 110
    for(int i=0; i<8; i++)
101 111
    {
102 112
        if(readings[i] < 200)
......
105 115
    return true; 
106 116
}
107 117

  
118
/**
119
 * We define a destination as two gaps of white between the line and patches
120
 * of 15-shade color (light grey) on either side.
121
 * I use 15-shade color in an attempt to not throw off the line localization.
122
 */
123
bool LinesensorControl::destination(vector<uint32_t> readings)
124
{
125
    if (readings.size() < 8)
126
    {
127
        return false;
128
    }
129

  
130
    ROS_INFO("Destination call. Readings: %d %d %d %d %d %d %d %d",
131
            readings[0], readings[1], readings[2], readings[3],
132
            readings[4], readings[5], readings[6], readings[7]);
133

  
134
    // Try to match this pattern to what we see
135
    int expected_pattern[] = {15, 0, 255, 0, 15};
136
    int pat_idx = 0;
137

  
138
    for (int i = 0; i < 8; i++)
139
    {
140
        // If the difference between what we have and expect is less than 5
141
        if (abs(readings[i] - expected_pattern[pat_idx]) < 5)
142
        {
143
            // Check. Keep looking for the rest of the pattern.
144
            pat_idx++;
145
        }
146

  
147
        // The whole pattern was found
148
        if (pat_idx > 4)
149
        {
150
            ROS_INFO("FOUND DESTINATION");
151
            return true;
152
        }
153
    }
154
    return false;
155
}
scout/libscout/src/LinesensorControl.h
53 53
        double readline();
54 54

  
55 55
        /** Check if we are at an intersecetion */
56
        bool fullline();
56
        bool fullline(std::vector<uint32_t> readings);
57

  
58
        /** Check if we are at the destination for a maze
59
         *  (a special line pattern) */
60
        bool destination(std::vector<uint32_t> readings);
57 61

  
58 62
    private:
59 63
        /* ROS publisher and client declaration */
scout/libscout/src/behaviors/line_follow.cpp
34 34

  
35 35
void line_follow::follow_line()
36 36
{
37
    vector<uint32_t> readings;
37 38
    do
38 39
    {
39 40
        double line_loc = linesensor->readline();
41
        ROS_INFO("Line loc: %lf", line_loc);
40 42

  
41 43
        if (line_loc == 0.0)
42 44
        {
......
48 50
        motor_r = min(max((int) (-MOTOR_BASE - KP * line_loc), -128), 127);
49 51

  
50 52
        motors->set_sides(motor_l, motor_r, MOTOR_ABSOLUTE);
53
        readings = linesensor->query();
51 54
    }
52
    while(!linesensor->fullline());
55
    while(!linesensor->fullline(readings) &&
56
          !linesensor->destination(readings) &&
57
          ok());
53 58
    halt();
54 59
}
55 60

  
56 61
void line_follow::turn_straight()
57 62
{
63
  vector<uint32_t> readings;
58 64
  do
59 65
  {
60 66
    motor_l = -MOTOR_BASE;
61 67
    motor_r = -MOTOR_BASE;
62 68

  
63 69
    motors->set_sides(motor_l, motor_r, MOTOR_ABSOLUTE);
70
    readings = linesensor->query();
64 71
  }
65
  while(linesensor->fullline());
72
  while(!linesensor->fullline(readings) &&
73
        !linesensor->destination(readings) &&
74
        ok());
66 75
}
67 76

  
68 77
void line_follow::turn_left()
......
84 93

  
85 94
    line_loc = linesensor->readline();
86 95
  }
87
  while(!(-1 < line_loc && line_loc < 1));
96
  while(!(-1 < line_loc && line_loc < 1) && ok());
88 97
}
89 98

  
90 99
void line_follow::halt()
......
109 118
        {
110 119
          line_loc = linesensor->readline();
111 120
        }
112
        while(line_loc < 2 && line_loc > -2);
121
        while(line_loc < 2 && line_loc > -2 && ok());
113 122

  
114 123
        first = false;
115 124
    }
116 125
    line_loc = linesensor->readline();
117 126
  }
118
  while(!(-1 < line_loc && line_loc < 1));
127
  while(!(-1 < line_loc && line_loc < 1) && ok());
119 128
}
120 129

  
121 130
void line_follow::turn_right()
......
137 146

  
138 147
    line_loc = linesensor->readline();
139 148
  }
140
  while(!(-1 < line_loc && line_loc < 1));
149
  while(!(-1 < line_loc && line_loc < 1) && ok());
141 150
}
142 151

  
143 152
void line_follow::u_turn()

Also available in: Unified diff