root / scout / sonar / src / sonar.cpp @ 0121ead7
History | View | Annotate | Download (5.73 KB)
1 |
/**
|
---|---|
2 |
* Copyright (c) 2007 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 |
/**
|
28 |
* @file sonar.cpp
|
29 |
* @brief Sonar
|
30 |
*
|
31 |
* Implementation of functions for sonar use.
|
32 |
*
|
33 |
* @author Colony Project, CMU Robotics Club
|
34 |
* @author Benjamin Wasserman
|
35 |
**/
|
36 |
|
37 |
#include "scout/sonar.h" |
38 |
#include "ros/ros.h" |
39 |
#include <cstdlib> |
40 |
|
41 |
/**
|
42 |
* @defgroup sonar Sonar
|
43 |
* @brief Functions for using the sonar
|
44 |
*
|
45 |
* @{
|
46 |
**/
|
47 |
|
48 |
int8_t sonar_stepper_pos; //This should only contain values in [0,47]
|
49 |
/**
|
50 |
* Sonar driver/service. This is a ROS node that communicates directly with the sonar
|
51 |
hardware. It serves the query_sonar service, and takes a direction, and responds with a distance. It is registered by sonar_driver_init.
|
52 |
*
|
53 |
**/
|
54 |
bool sonar_get_distance(sonar::query_sonar::Request &req,
|
55 |
sonar::query_sonar::Response &res){ |
56 |
/* Set the stepper position (position from 0-47 for direction)*/
|
57 |
//stepper_set(req.direction)
|
58 |
/* Delay if necessary for stepper to get in position */
|
59 |
/* Get value from sonar */
|
60 |
//res.distance0 = sonar_sensor(0);
|
61 |
//res.distance1 = sonar_sensor(1);
|
62 |
ROS_INFO("request: sonar direction #%d", req.direction);
|
63 |
ROS_INFO("response: sonar distances %d %d", res.distance0, res.distance1);
|
64 |
return true; |
65 |
} |
66 |
|
67 |
/**
|
68 |
* Initialize the sonar driver. This must be called before any sonar
|
69 |
* requests are made. It will then run in the background until the sonar node is
|
70 |
* shut down.
|
71 |
*
|
72 |
* @see sonar_get_distance
|
73 |
**/
|
74 |
void main(int argc, char **argv){ |
75 |
/* Initialize in ROS the sonar driver node */
|
76 |
ros::init(argc, argv, "sonar_driver");
|
77 |
|
78 |
/* Advertise that this serves the query_sonar service */
|
79 |
ros::NodeHandle n; |
80 |
ros:ServiceServer service = n.advertiseService("query_sonar", sonar_get_distance); |
81 |
|
82 |
/* Initialize hardware for sonar */
|
83 |
//Hardware init functions here
|
84 |
|
85 |
ROS_INFO("Ready to read sonar.");
|
86 |
ros::spin(); |
87 |
|
88 |
return 0; |
89 |
} |
90 |
|
91 |
/**
|
92 |
* Initialize the sonar system. This must be called before sonar_read.
|
93 |
*
|
94 |
* @see sonar_read
|
95 |
* @see sonar_read_raw
|
96 |
**/
|
97 |
void sonar_init(void){ |
98 |
sonar_stepper_pos = 0;
|
99 |
} |
100 |
|
101 |
/**
|
102 |
* Read raw sonar data from the sonar module via the query_sonar service.
|
103 |
*
|
104 |
* @param direction The direction to set the stepper motor to. (0-47)
|
105 |
* @param sensor The sonar sensor that should have its value returned. (0-1)
|
106 |
* @return distance The distance in the specified direction. Or -1 if an
|
107 |
* illegal parameter or error.
|
108 |
**/
|
109 |
int sonar_read_raw(int direction, int sensor){ |
110 |
/* Check for illegal arguments */
|
111 |
if(direction < 0 || direction > 47 || sensor < 0 || sensor > 1){ |
112 |
ROS_WARNING("Illegal argument");
|
113 |
return -1; |
114 |
} |
115 |
/* Register client to query_sonar service. */
|
116 |
ros::NodeHandle n; |
117 |
ros::ServiceClient client = n.serviceClient<messages::query_sonar>("query_sonar");
|
118 |
/* Create and populate query */
|
119 |
messages::query_sonar srv; |
120 |
srv.request.direction = direction; |
121 |
/* Send query */
|
122 |
if(client.call(srv)){
|
123 |
/* Return value of correct sensor */
|
124 |
if(sensor){
|
125 |
ROS_INFO("Sonar 1 at %d reads %d", direction, srv.response.distance1);
|
126 |
return srv.response.distance1;
|
127 |
}else{
|
128 |
ROS_INFO("Sonar 0 at %d reads %d", direction, srv.response.distance0);
|
129 |
return srv.response.distance0;
|
130 |
} |
131 |
}else{
|
132 |
ROS_ERROR("Failed to call service query_sonar");
|
133 |
return -1; |
134 |
} |
135 |
} |
136 |
|
137 |
/**
|
138 |
* Read sonar values in human readable fashion. Wrapper for
|
139 |
* sonar_read_raw.
|
140 |
*
|
141 |
* @param direction Direction to get sonar value at. Will be rounded to
|
142 |
* closest possible position. In degrees. Can be negative or > 360.
|
143 |
* @return distance Distance that sonar reads in the given direction.
|
144 |
* @see sonar_read_raw
|
145 |
**/
|
146 |
int sonar_read(int direction){ |
147 |
int dir1; //The position of the second sonar head; |
148 |
int distance;
|
149 |
/* Get angle in 0<=dir<359 */
|
150 |
direction %= 360;
|
151 |
/* Get stepper positions */
|
152 |
direction /= 48;
|
153 |
/* Move stepper to closest position to get reading */
|
154 |
if(direction == sonar_stepper_pos){
|
155 |
/* If direction is the same */
|
156 |
distance = sonar_read_raw(direction, 0);
|
157 |
}else if(direction == sonar_stepper_pos + 24 || direction + 24 == sonar_stepper_pos){ |
158 |
/* If direction is opposite last direction */
|
159 |
distance = sonar_read_raw(sonar_stepper_pos, 1);
|
160 |
}else{
|
161 |
/*TODO: Replace this next line with code that will move the stepper the
|
162 |
** minimum necessary distance to get one of the heads in the desired
|
163 |
** position. Update sonar_stepper_pos to this new position, and call
|
164 |
** sonar_read_raw with the stepper position (not necessarily the
|
165 |
** direction of the reading) and which sonar sensor should be
|
166 |
** returned (0 or 1).
|
167 |
*/
|
168 |
distance = sonar_read_raw(direction,0);
|
169 |
} |
170 |
/*TODO: Make a function to convert raw sonar distances to cm for
|
171 |
** usability. Replace next line with line that calls function.
|
172 |
*/
|
173 |
//return convert_to_cm(distance);
|
174 |
return distance;
|
175 |
} |