Revision af7e0f94
| ID | af7e0f941c6ec3ce22eb88a8e49af36147359107 |
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