scoutos / scout / libscout / src / SonarControl.cpp @ 15b7e607
History | View | Annotate | Download (4.69 KB)
1 | 65c7f52b | pdeo | /**
|
---|---|---|---|
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 | 15b7e607 | pdeo | /** 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 | 65c7f52b | pdeo | sonar::sonar_set_scan msg; |
82 | |||
83 | 15b7e607 | pdeo | //TODO Enable some checks of start and end position
|
84 | 65c7f52b | pdeo | 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 | 15b7e607 | pdeo | * @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 | 65c7f52b | pdeo | * @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 | } |