Revision a2e6bd4c scout/scoutsim/src/scout.cpp

View differences:

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
        {

Also available in: Unified diff