Revision af7e0f94
Added new at_destination functionality.
Maze turns are buggy and are the next order of business.
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