Statistics
| Branch: | Revision:

root / scout / libscout / src / LinesensorControl.cpp @ 414d2b48

History | View | Annotate | Download (3.84 KB)

1
/**
2
 * Copyright (c) 2011 Colony Project
3
 * 
4
 * Permission is hereby granted, free of charge, to any person
5
 * obtaining a copy of this software and associated documentation
6
 * files (the "Software"), to deal in the Software without
7
 * restriction, including without limitation the rights to use,
8
 * copy, modify, merge, publish, distribute, sublicense, and/or sell
9
 * copies of the Software, and to permit persons to whom the
10
 * Software is furnished to do so, subject to the following
11
 * conditions:
12
 * 
13
 * The above copyright notice and this permission notice shall be
14
 * included in all copies or substantial portions of the Software.
15
 * 
16
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
17
 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
18
 * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
19
 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
20
 * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
21
 * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
22
 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
23
 * OTHER DEALINGS IN THE SOFTWARE.
24
 */
25

    
26
/**
27
 * @file LinesensorControl.cpp
28
 * @brief Contains line following function implementation
29
 * 
30
 * @author Colony Project, CMU Robotics Club
31
 * @author Alex Zirbel
32
 */
33

    
34
#include "LinesensorControl.h"
35

    
36
using namespace std;
37

    
38
static double last_ret;
39

    
40
/**
41
 * @brief Initialize the line following module of libscout.
42
 */
43
LinesensorControl::LinesensorControl(const ros::NodeHandle& libscout_node,
44
                                 string scoutname)
45
    : node(libscout_node)
46
{
47
    query_client =
48
        node.serviceClient<linesensor::query_linesensor>(scoutname+"/query_linesensor");
49
}
50

    
51
/**
52
 * @brief Returns the current readings of the line follwing sensor.
53
 */
54
vector<uint32_t> LinesensorControl::query()
55
{
56
    // Set scan range
57
    linesensor::query_linesensor srv;
58

    
59
    if (!query_client.call(srv))
60
    {
61
        ROS_ERROR("LinesensorControl query failed.");
62
    }
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

    
70
    return srv.response.readings;
71
}
72

    
73
/** 
74
 *  @brief Returns the average readings of the line following sensors. 
75
 */
76
double LinesensorControl::readline()
77
{
78
    unsigned int total_read = 0;
79
    unsigned int weighted_total = 0;
80

    
81
    vector<uint32_t> readings = query();
82

    
83
    if (readings.size() < 8)
84
    {
85
        return last_ret;
86
    }
87

    
88
    for (int i = 0; i < 8; i++)
89
    {
90
        total_read += readings[i];
91
        weighted_total += i * readings[i];
92
    }
93

    
94
    if (total_read == 0)
95
    {
96
        return last_ret;
97
    }
98
    
99
    double ret_val = last_ret = ((double) weighted_total / total_read) - 3.5;
100
    return ret_val;
101
}
102

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

    
110
    for(int i=0; i<8; i++)
111
    {
112
        if(readings[i] < 200)
113
          return false;
114
    }
115
    return true; 
116
}
117

    
118
/**
119
 * We define a destination as two gaps of white between the line and patches
120
 * of 95-shade color (dark grey) on either side.
121
 * I use 95-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
    // Try to match this pattern to what we see
131
    int expected_pattern[] = {95, 0, 255, 0, 95};
132
    int pat_idx = 0;
133

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

    
143
        // The whole pattern was found
144
        if (pat_idx > 4)
145
        {
146
            return true;
147
        }
148
    }
149
    return false;
150
}