Revision a2e6bd4c

View differences:

scout/scoutsim/CMakeLists.txt
36 36
#rosbuild_add_executable(turtle_teleop_key tutorials/teleop_turtle_key.cpp)
37 37
#rosbuild_add_executable(draw_square tutorials/draw_square.cpp)
38 38
#rosbuild_add_executable(mimic tutorials/mimic.cpp)
39
rosbuild_add_executable(sonar_viz src/sonar_viz.cpp)
39 40
rosbuild_add_executable(scout_teleop src/scout_teleop.cpp)
40 41
rosbuild_add_executable(scout_teleop_userinput src/scout_teleop_userinput.cpp)
scout/scoutsim/manifest.xml
16 16
  <depend package="motors" />
17 17
  <depend package="encoders" />
18 18
  <depend package="linesensor" />
19
  <depend package="sonar" />
19 20
  <depend package="geometry_msgs" />
20 21
  <depend package="messages"/>
21 22
  
scout/scoutsim/src/scout.cpp
83 83

  
84 84
        pose_pub = node.advertise<Pose>("pose", 1);
85 85
        color_pub = node.advertise<Color>("color_sensor", 1);
86
        sonar_pub = node.advertise<sonar::sonar_distance>("sonar_distance", 1);
86 87
        set_pen_srv = node.advertiseService("set_pen",
87 88
                                            &Scout::setPenCallback,
88 89
                                            this);
......
102 103
        }
103 104

  
104 105
        meter = scout.GetHeight();
106

  
107
        // Initialize sonar
108
        sonar_position = 0;
109
        sonar_stop_l = 0;
110
        sonar_stop_r = 23;
111
        sonar_direction = 1;
112
        sonar_tick_time = ros::Duration(scoutsim::SONAR_HALF_SPIN_TIME / 24.0);
105 113
    }
106 114

  
107 115
    /**
......
177 185
    }
178 186

  
179 187
    // Scale to linesensor value
180
    unsigned int Scout::rgb_to_linesensor_val(unsigned char r,
181
                                              unsigned char g,
182
                                              unsigned char b)
188
    unsigned int Scout::rgb_to_grey(unsigned char r,
189
                                    unsigned char g,
190
                                    unsigned char b)
183 191
    {
184 192
        // Should be 0 to 255
185 193
        unsigned int grey = ((unsigned int) r + (unsigned int) g + (unsigned int) b) / 3;
......
188 196
        return 255 - grey;
189 197
    }
190 198

  
191
    void Scout::update_linesensor(const wxImage& lines_image, int x, int y, double theta)
199
    unsigned int Scout::trace_sonar(const wxImage& walls_image, int x, int y,
200
                                    double robot_theta, int sonar_pos)
201
    {
202
        double angle = robot_theta + (180.0 * ((float) sonar_pos) / 24.0);
203
        unsigned int d = 0;
204

  
205
        unsigned int reading = 0;
206
        do
207
        {
208
            int d_x = x + (int) floor(d * cos(angle));
209
            int d_y = y + (int) floor(d * sin(angle));
210

  
211
            // Out of image boundary
212
            if (d_x < 0 || d_x >= walls_image.GetWidth() ||
213
                d_y < 0 || d_y >= walls_image.GetHeight())
214
            {
215
                return d;
216
            }
217

  
218
            // Max range
219
            if (d > scoutsim::SONAR_MAX_RANGE)
220
            {
221
                return d;
222
            }
223

  
224
            // Get the sonar reading at the current position of the sonar
225
            unsigned char r = walls_image.GetRed(d_x, d_y);
226
            unsigned char g = walls_image.GetGreen(d_x, d_y);
227
            unsigned char b = walls_image.GetBlue(d_x, d_y);
228

  
229
            reading = rgb_to_grey(r, g, b);
230

  
231
            d++;
232
        }
233
        while (reading < 128);
234

  
235
        return d;
236
    }
237

  
238
    void Scout::update_sonar(const wxImage& walls_image, int x, int y,
239
                             double robot_theta)
240
    {
241
        // Only rotate the sonar at the correct rate.
242
        if (ros::Time::now() - last_sonar_time < sonar_tick_time)
243
        {
244
            return;
245
        }
246
        last_sonar_time = ros::Time::now();
247

  
248
        unsigned int d_front = trace_sonar(walls_image, x, y, robot_theta,
249
                                           sonar_position);
250
        unsigned int d_back = trace_sonar(walls_image, x, y, robot_theta,
251
                                          sonar_position + 24);
252

  
253
        // Publish
254
        sonar::sonar_distance msg;
255
        msg.pos = sonar_position;
256
        msg.distance0 = d_front;
257
        msg.distance1 = d_back;
258

  
259
        sonar_pub.publish(msg);
260

  
261
        // Update the sonar rotation
262
        if (sonar_position + sonar_direction <= sonar_stop_r &&
263
            sonar_position + sonar_direction >= sonar_stop_l)
264
        {
265
            sonar_position = sonar_position + sonar_direction;
266
        }
267
        else
268
        {
269
            sonar_direction = -sonar_direction;
270
        }
271
    }
272

  
273
    void Scout::update_linesensor(const wxImage& lines_image, int x, int y,
274
                                  double robot_theta)
192 275
    {
193 276
        linesensor_readings.clear();
194 277

  
......
197 280
        {
198 281
            double offset = -(scout_image.GetWidth() / 2) + s * spacing;
199 282

  
200
            int sensor_x = (int) (x - LNSNSR_D * cos(theta) - offset * sin(theta));
201
            int sensor_y = (int) (y + LNSNSR_D * sin(theta) - offset * cos(theta));
283
            int sensor_x = (int) (x - LNSNSR_D * cos(robot_theta) -
284
                                  offset * sin(robot_theta));
285
            int sensor_y = (int) (y + LNSNSR_D * sin(robot_theta) -
286
                                  offset * cos(robot_theta));
202 287

  
203 288
            unsigned char r = lines_image.GetRed(sensor_x, sensor_y);
204 289
            unsigned char g = lines_image.GetGreen(sensor_x, sensor_y);
205 290
            unsigned char b = lines_image.GetBlue(sensor_x, sensor_y);
206 291

  
207
            unsigned int reading = rgb_to_linesensor_val(r, g, b);
292
            unsigned int reading = rgb_to_grey(r, g, b);
208 293

  
209 294
            linesensor_readings.push_back(reading);
210 295
        }
......
215 300
    geometry_msgs::Pose2D Scout::update(double dt, wxMemoryDC& path_dc,
216 301
                                        const wxImage& path_image,
217 302
                                        const wxImage& lines_image,
303
                                        const wxImage& walls_image,
218 304
                                        wxColour background_color,
219 305
                                        world_state state)
220 306
    {
......
286 372
        pose_pub.publish(p);
287 373

  
288 374
        update_linesensor(lines_image, canvas_x, canvas_y, p.theta);
375
        update_sonar(walls_image,
376
                     canvas_x + scoutsim::SCOUT_SONAR_X,
377
                     canvas_y + scoutsim::SCOUT_SONAR_Y,
378
                     p.theta);
289 379

  
290 380
        // Figure out (and publish) the color underneath the scout
291 381
        {
scout/scoutsim/src/scout.h
55 55
#include <motors/set_motors.h>
56 56
#include <encoders/query_encoders.h>
57 57
#include <linesensor/query_linesensor.h>
58
#include <sonar/sonar_distance.h>
58 59

  
59 60
#include <scoutsim/Pose.h>
60 61
#include <scoutsim/SetPen.h>
......
65 66
#include <wx/wx.h>
66 67

  
67 68
#include "scoutsim_internal.h"
68

  
69
//#include "scout_constants.h"
69
#include "scout_constants.h"
70 70

  
71 71
#define PI 3.14159265
72 72

  
......
115 115
            geometry_msgs::Pose2D update(double dt, wxMemoryDC& path_dc,
116 116
                                         const wxImage& path_image,
117 117
                                         const wxImage& lines_image,
118
                                         const wxImage& walls_image,
118 119
                                         wxColour background_color,
119 120
                                         world_state state);
120 121
            void paint(wxDC& dc);
......
128 129
                                         encoders::query_encoders::Response&);
129 130
            bool query_linesensor_callback(linesensor::query_linesensor::Request&,
130 131
                                           linesensor::query_linesensor::Response&);
131
            unsigned int rgb_to_linesensor_val(unsigned char r,
132
                                               unsigned char g,
133
                                               unsigned char b);
134
            void update_linesensor(const wxImage& lines_image, int x, int y, double theta);
132
            unsigned int rgb_to_grey(unsigned char r,
133
                                     unsigned char g,
134
                                     unsigned char b);
135
            unsigned int trace_sonar(const wxImage& walls_image, int x, int y,
136
                                     double robot_theta, int sonar_pos);
137
            void update_sonar(const wxImage& walls_image, int x, int y,
138
                              double robot_theta);
139
            void update_linesensor(const wxImage& lines_image, int x, int y,
140
                                   double theta);
135 141

  
136 142
            ros::NodeHandle node;
137 143

  
......
155 161
            unsigned int bl_ticks;
156 162
            unsigned int br_ticks;
157 163

  
164
            int sonar_position;
165
            int sonar_stop_l;
166
            int sonar_stop_r;
167
            int sonar_direction;
168
            // The last time the sonar changed its position.
169
            ros::Time last_sonar_time;
170
            ros::Duration sonar_tick_time;
171

  
158 172
            // A vector of the 8 linesensor readings
159 173
            std::vector<unsigned int> linesensor_readings;
160 174

  
......
167 181
            ros::Subscriber motors_sub;
168 182
            ros::Publisher pose_pub;
169 183
            ros::Publisher color_pub;
184
            ros::Publisher sonar_pub;
170 185
            ros::ServiceServer set_pen_srv;
171 186
            ros::ServiceServer query_encoders_srv;
172 187
            ros::ServiceServer query_linesensor_srv;
scout/scoutsim/src/scout_constants.h
56 56
    // Position of sonar relative to robot center
57 57
    const float SCOUT_SONAR_X = -0.01;
58 58
    const float SCOUT_SONAR_Y = 0.0;
59

  
60
    const unsigned int SONAR_MAX_RANGE = 255;
61
    // Time it takes for the sonar to spin from position 0 to position 23
62
    const float SONAR_HALF_SPIN_TIME = 24.0;
59 63
}
scout/scoutsim/src/sim_frame.cpp
323 323

  
324 324
        for (; it != end; ++it)
325 325
        {
326
            it->second->update(0.016, path_dc, path_image, lines_image,
326
            it->second->update(0.016, path_dc,
327
                               path_image, lines_image, walls_image,
327 328
                               path_dc.GetBackground().GetColour(),
328 329
                               state);
329 330
        }
scout/scoutsim/src/sonar_viz.cpp
1
#include <iostream>
2
#include <iomanip>
3

  
4
#include <ros/ros.h>
5
#include <sonar/sonar_distance.h>
6

  
7
#include "scout_teleop.h"
8

  
9
#define QUEUE_SIZE 10
10

  
11
using namespace std;
12

  
13
vector<unsigned int> distances;
14

  
15
void sonar_distance_callback(const sonar::sonar_distance::ConstPtr& msg)
16
{
17
    distances[msg->pos] = msg->distance0;
18
    distances[msg->pos + 24] = msg->distance1;
19

  
20
    cout << endl << endl << endl << "[ ";
21
    for (int i = 0; i < 48; i++)
22
    {
23
        cout << setw(3) << distances[i] << " ";
24
    }
25
    cout << "]" << endl;
26
}
27

  
28
int main(int argc, char **argv)
29
{
30
    if (argc < 2)
31
    {
32
        cout << "Usage: " << argv[0] << " <scout_name>" << endl;
33
        exit(1);
34
    }
35

  
36
    ros::init(argc, argv, "sonar_viz");
37

  
38
    distances = vector<unsigned int>(48, 0);
39
    ros::NodeHandle n;
40

  
41
    string scoutname = argv[1];
42
    ros::Subscriber sonar_distance_sub = n.subscribe(scoutname + "/sonar_distance",
43
                                                     QUEUE_SIZE,
44
                                                     sonar_distance_callback);
45

  
46
    ros::spin();
47
}
scout/sonar/msg/sonar_distance.msg
5 5
int8 pos
6 6

  
7 7
# Distance reading from the front sonar in mm
8
int8 distance0
8
uint8 distance0
9 9
# Distance reading from the back sonar in mm
10
int8 distance1
10
uint8 distance1

Also available in: Unified diff