Statistics
| Branch: | Revision:

root / scout / libscout / src / LinesensorControl.cpp @ 3a73516c

History | View | Annotate | Download (4 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
 * @defgroup linesensorcontrol LinesensorControl
31
 * @brief Functions which a behavior can use to control the line sensor.
32
 * @ingroup behavior
33
 * 
34
 * @author Colony Project, CMU Robotics Club
35
 * @author Alex Zirbel
36
 *
37
 * @{
38
 */
39

    
40
#include "LinesensorControl.h"
41

    
42
using namespace std;
43

    
44
static double last_ret;
45

    
46
/**
47
 * @brief Initialize the line following module of libscout.
48
 */
49
LinesensorControl::LinesensorControl(const ros::NodeHandle& libscout_node,
50
                                 string scoutname)
51
    : node(libscout_node)
52
{
53
    query_client =
54
        node.serviceClient< ::messages::query_linesensor>(scoutname+"/query_linesensor");
55
}
56

    
57
/**
58
 * @brief Returns the current readings of the line follwing sensor.
59
 */
60
vector<uint32_t> LinesensorControl::query()
61
{
62
    // Set scan range
63
    ::messages::query_linesensor srv;
64

    
65
    if (!query_client.call(srv))
66
    {
67
        ROS_ERROR("LinesensorControl query failed.");
68
    }
69

    
70
    if (srv.response.readings.size() != 8)
71
    {
72
        ROS_WARN("Linesensor reading vector has %d readings, 8 expected.",
73
                 int(srv.response.readings.size()));
74
    }
75

    
76
    return srv.response.readings;
77
}
78

    
79
/** 
80
 *  @brief Returns the average readings of the line following sensors. 
81
 */
82
double LinesensorControl::readline()
83
{
84
    unsigned int total_read = 0;
85
    unsigned int weighted_total = 0;
86

    
87
    vector<uint32_t> readings = query();
88

    
89
    if (readings.size() < 8)
90
    {
91
        return last_ret;
92
    }
93

    
94
    for (int i = 0; i < 8; i++)
95
    {
96
        total_read += readings[i];
97
        weighted_total += i * readings[i];
98
    }
99

    
100
    if (total_read == 0)
101
    {
102
        return last_ret;
103
    }
104
    
105
    double ret_val = last_ret = ((double) weighted_total / total_read) - 3.5;
106
    return ret_val;
107
}
108

    
109
bool LinesensorControl::fullline(vector<uint32_t> readings)
110
{
111
    if (readings.size() < 8)
112
    {
113
        return false;
114
    }
115

    
116
    for(int i=0; i<8; i++)
117
    {
118
        if(readings[i] < 200)
119
          return false;
120
    }
121
    return true; 
122
}
123

    
124
/**
125
 * We define a destination as two gaps of white between the line and patches
126
 * of 95-shade color (dark grey) on either side.
127
 * I use 95-shade color in an attempt to not throw off the line localization.
128
 */
129
bool LinesensorControl::destination(vector<uint32_t> readings)
130
{
131
    if (readings.size() < 8)
132
    {
133
        return false;
134
    }
135

    
136
    // Try to match this pattern to what we see
137
    int expected_pattern[] = {95, 0, 255, 0, 95};
138
    int pat_idx = 0;
139

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

    
149
        // The whole pattern was found
150
        if (pat_idx > 4)
151
        {
152
            return true;
153
        }
154
    }
155
    return false;
156
}
157

    
158
/** @} */