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 |
/** @} **/
|