Statistics
| Branch: | Revision:

root / scout / libscout / src / SonarControl.cpp @ 15b7e607

History | View | Annotate | Download (4.69 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 SonarControl.cpp
28
 * @brief Contains sonar function imeplementations
29
 * 
30
 * Contains functions for the use of sonar control.
31
 *
32
 * @author Colony Project, CMU Robotics Club
33
 * @author Priyanka Deo
34
 **/
35

    
36
#include "SonarControl.h"
37

    
38
/**
39
 * @brief Initialize the sonar module of libscout.
40
 *
41
 * Initialize the libscout node as a publisher of sonar_set_scan and 
42
 * sonar_toggle and a client of query_sonar.
43
 */
44
SonarControl::SonarControl(const ros::NodeHandle& libscout_node)
45
    : node(libscout_node)
46
{
47
    sonar_set_scan_pub =
48
        node.advertise<sonar::sonar_set_scan>("set_sonar_scan", QUEUE_SIZE);
49
    sonar_toggle_pub = 
50
        node.advertise<sonar::sonar_toggle>("toggle_sonar", QUEUE_SIZE);
51
    query_sonar =
52
        node.serviceClient<sonar::sonar_direction>("query_sonar");
53
}
54

    
55
/**
56
 * @brief Sets the sonar to a position.
57
 *
58
 * @param position Value between 0-180 of degree position to set sonar
59
 * @return NONE
60
 */
61
void SonarControl::set(int position)
62
{
63
    set_range(position, position);
64
}
65

    
66
/**
67
 * @brief Sets the sonar to scan between two positions
68
 *
69
 * @param start_pos The leftmost (smallest) value that the sonar can take
70
 * @param end_pos The rightmost (largest) value that the sonar can take
71
 * @return NONE
72
 */
73
void SonarControl::set_range(int start_pos, int end_pos)
74
{
75
    /** Turn sonar on */
76
    sonar::sonar_toggle on_msg;
77
    on_msg.set_on = true;
78
    sonar_toggle_pub.publish(on_msg);
79

    
80
    /** Set scan range */
81
    sonar::sonar_set_scan msg;
82

    
83
    //TODO Enable some checks of start and end position
84
    msg.stop_l = start_pos;
85
    msg.stop_r = end_pos;
86

    
87
    sonar_set_scan_pub.publish(msg);
88
    ros::spinOnce();
89
}
90

    
91
/**
92
 * @brief Turn off sonar readings
93
 *
94
 * Stops sonar from panning and taking readings.
95
 *
96
 * @return NONE
97
 */
98
float SonarControl::set_off()
99
{
100
    /** Turn sonar off */
101
    sonar::sonar_toggle on_msg;
102
    on_msg.set_on = false;
103
    sonar_toggle_pub.publish(on_msg);
104
}
105
/**
106
 * @brief Query the current front sonar readings
107
 *
108
 * Sends a request to the query_sonar service which will reply with the
109
 *  current readings of the sonar.
110
 *
111
 * @TODO Change so we can get multiple sonar readings with one call
112
 *
113
 * @return The reading of the front sonar or LIB_ERR.
114
 */
115
float SonarControl::query_front()
116
{
117
    sonar::sonar_direction srv;
118
    if(query_sonar.call(srv))
119
    {
120
        srv.response.distance0;
121
    }
122
    else
123
    {
124
        ROS_ERROR("Failed to call service query_motors");
125
        return LIB_ERROR;
126
    }
127

    
128
    return 0;
129
}
130

    
131
/**
132
 * @brief Query the current back sonar readings
133
 *
134
 * Sends a request to the query_sonar service which will reply with the
135
 *  current readings of the sonar.
136
 *
137
 * @TODO Change so we can get multiple sonar readings with one call
138
 *
139
 * @return The reading of the back sonar or LIB_ERR.
140
 */
141
float SonarControl::query_back()
142
{
143
    sonar::sonar_direction srv;
144
    if(query_sonar.call(srv))
145
    {
146
        srv.response.distance1;
147
    }
148
    else
149
    {
150
        ROS_ERROR("Failed to call service query_motors");
151
        return LIB_ERROR;
152
    }
153

    
154
    return 0;
155
}
156

    
157
/**
158
 * @brief Converts value returne by sonar to physical distances.
159
 *
160
 * @param sonar_value The returned value of the sonar
161
 * @return The physical distance measured by the sonar.
162
 **/
163
float sonar_to_dist(float sonar_value)
164
{
165
    //TODO impelement later based on sonar readings
166
    return sonar_value;
167
}
168

    
169
/**
170
 * @brief Converts values from physical distances to values read by sonar
171
 *
172
 * @param distance The physical distance as measured.
173
 * @return The value read by the sonar that corresponds to the given distance
174
 **/
175
float dist_to_sonar(float distance)
176
{
177
    //TODO implement later based on sonar readings
178
    return distance;
179
}