Revision 85538662
ID | 855386623f71e8f31cd5440dc59858a73954bf2a |
Updated the sonar module prototype.
The biggest change to the proposed node is a restructuring of the sonar code style. Two services (sonar_toggle and sonar_set_scan) allow another node to turn the sonar on and off. The sonar constantly scans, keeps track of its position, and publishes all its readings in the topic sonar_direction.msg.
Added the appropriate message and service files, and the code skeleton.
The most obvious missing part of the code is the part which controls the movement of the sonar sensor, which should constantly scan between two ranges without user intervention.
scout/sonar/src/sonar.cpp | ||
---|---|---|
31 | 31 |
* Implementation of functions for sonar use. |
32 | 32 |
* |
33 | 33 |
* @author Colony Project, CMU Robotics Club |
34 |
* @author Benjamin Wasserman
|
|
34 |
* @author Alex Zirbel
|
|
35 | 35 |
**/ |
36 | 36 |
|
37 | 37 |
#include "scout/sonar.h" |
... | ... | |
45 | 45 |
* @{ |
46 | 46 |
**/ |
47 | 47 |
|
48 |
int8_t sonar_stepper_pos; //This should only contain values in [0,47] |
|
48 |
/** Current position of the sonar. Should only contain values in [0,23]. */ |
|
49 |
int8_t sonar_stepper_pos; |
|
50 |
bool is_on; |
|
51 |
|
|
49 | 52 |
/** |
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 |
* Initialize the sonar system. This must be called before sonar_read. |
|
54 |
* |
|
55 |
* @see sonar_read |
|
56 |
* @see sonar_read_raw |
|
53 | 57 |
**/ |
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; |
|
58 |
void sonar_init(void) |
|
59 |
{ |
|
60 |
sonar_stepper_pos = 0; |
|
65 | 61 |
} |
66 | 62 |
|
67 | 63 |
/** |
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(); |
|
64 |
* Turns the sonar on and off. |
|
65 |
*/ |
|
66 |
bool handle_sonar_toggle(sonar::sonar_toggle::Request &req, |
|
67 |
sonar::sonar_toggle::Response &res) |
|
68 |
{ |
|
69 |
if (req.set_on) |
|
70 |
ROS_INFO("Turning on the sonar [unimplemented]"); |
|
71 |
else |
|
72 |
ROS_INFO("Turning off the sonar [unimplemented]"); |
|
73 |
|
|
74 |
res.ack = true; |
|
87 | 75 |
|
88 |
return 0;
|
|
76 |
return true;
|
|
89 | 77 |
} |
90 | 78 |
|
91 | 79 |
/** |
92 |
* Initialize the sonar system. This must be called before sonar_read.
|
|
80 |
* @brief Sets the sonar scan range. Values from 0 to 23 are valid.
|
|
93 | 81 |
* |
94 |
* @see sonar_read |
|
95 |
* @see sonar_read_raw |
|
96 |
**/ |
|
97 |
void sonar_init(void){ |
|
98 |
sonar_stepper_pos = 0; |
|
99 |
} |
|
82 |
* Handles setting the sonar scan range. The scan range is specified by |
|
83 |
* the front sonar, so a scan range of 0 to 23 is a full scan. |
|
84 |
* After the scan range is set, the robot will continue reading values |
|
85 |
* only from that range until the range is reset. Old values can be identified |
|
86 |
* by the header on the distance reading message. |
|
87 |
*/ |
|
88 |
bool handle_sonar_set_scan(sonar::sonar_set_scan::Request &req, |
|
89 |
sonar::sonar_set_scan::Response &res) |
|
90 |
{ |
|
91 |
// Code to set the sonar to scan from |
|
92 |
// req.stop_l to req.stop_r |
|
100 | 93 |
|
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 |
} |
|
94 |
ROS_INFO("Setting sonar scan range to [%s, %s] [unimplemented]", |
|
95 |
req.stop_l, |
|
96 |
req.stop_r); |
|
97 |
|
|
98 |
res.ack = true; |
|
99 |
|
|
100 |
return true; |
|
135 | 101 |
} |
136 | 102 |
|
137 | 103 |
/** |
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 |
|
104 |
* Initialize the sonar driver. This must be called before any sonar |
|
105 |
* requests are made. It will then run in the background until the sonar node is |
|
106 |
* shut down. |
|
145 | 107 |
**/ |
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; |
|
108 |
void main(int argc, char **argv) |
|
109 |
{ |
|
110 |
is_on = true; |
|
111 |
|
|
112 |
/* Initialize hardware for sonar */ |
|
113 |
//Hardware init functions here |
|
114 |
sonar_init(); |
|
115 |
|
|
116 |
/* Initialize the sonar driver node in ROS */ |
|
117 |
ros::init(argc, argv, "sonar_driver"); |
|
118 |
|
|
119 |
/* Advertise messages and services */ |
|
120 |
ros::NodeHandle n; |
|
121 |
ros:ServiceServer service0 = n.advertiseService("sonar_toggle", |
|
122 |
handle_sonar_toggle); |
|
123 |
ros:ServiceServer service1 = n.advertiseService("sonar_set_scan", |
|
124 |
handle_sonar_set_scan); |
|
125 |
|
|
126 |
ROS_INFO("Sonar: Ready"); |
|
127 |
ros::spin(); |
|
128 |
|
|
129 |
return 0; |
|
175 | 130 |
} |
131 |
|
|
132 |
/** @} **/ |
Also available in: Unified diff