Revision af7e0f94 scout/libscout/src/LinesensorControl.cpp

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
}

Also available in: Unified diff