Statistics
| Branch: | Revision:

root / scout / libscout / src / LinesensorControl.cpp @ af7e0f94

History | View | Annotate | Download (4.07 KB)

1 1034f829 Alex
/**
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 5d0687a9 Priya
static double last_ret;
39
40 1034f829 Alex
/**
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 5866cb38 Alex
vector<uint32_t> LinesensorControl::query()
55 1034f829 Alex
{
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 af7e0f94 Alex
    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 5866cb38 Alex
    return srv.response.readings;
71 1034f829 Alex
}
72
73 5d0687a9 Priya
/** 
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 360e3b6c Tom Mullins
    if (readings.size() < 8)
84
    {
85
        return last_ret;
86
    }
87
88 5d0687a9 Priya
    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 6fab3966 Alex
    double ret_val = last_ret = ((double) weighted_total / total_read) - 3.5;
100 5d0687a9 Priya
    return ret_val;
101
}
102
103 af7e0f94 Alex
bool LinesensorControl::fullline(vector<uint32_t> readings)
104 5d0687a9 Priya
{
105 af7e0f94 Alex
    if (readings.size() < 8)
106
    {
107
        return false;
108
    }
109
110 ae21730e Priya
    for(int i=0; i<8; i++)
111
    {
112
        if(readings[i] < 200)
113
          return false;
114
    }
115
    return true; 
116 5d0687a9 Priya
}
117
118 af7e0f94 Alex
/**
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
}