Revision ade1b7f9 scout/scoutsim/src/scout.cpp

View differences:

scout/scoutsim/src/scout.cpp
96 96
                                  &Scout::query_linesensor_callback,
97 97
                                  this);
98 98

  
99
        for (unsigned int i = 0; i < NUM_LINESENSORS; i++)
100
        {
101
            linesensor_readings.push_back(0);
102
        }
103

  
99 104
        meter = scout.GetHeight();
100 105
    }
101 106

  
......
166 171
    bool Scout::query_linesensor_callback(linesensor::query_linesensor::Request&,
167 172
                                          linesensor::query_linesensor::Response& res)
168 173
    {
169
        vector<unsigned int> readings;
174
        res.readings = linesensor_readings;
175

  
176
        return true;
177
    }
170 178

  
171
        for (unsigned int i = 0; i < 8; i++)
179
    // Scale to linesensor value
180
    unsigned int Scout::rgb_to_linesensor_val(unsigned char r,
181
                                              unsigned char g,
182
                                              unsigned char b)
183
    {
184
        // Should be 0 to 255
185
        unsigned int grey = ((unsigned int) r + (unsigned int) g + (unsigned int) b) / 3;
186

  
187
        /// @todo Convert to the proper range
188
        return 255 - grey;
189
    }
190

  
191
    void Scout::update_linesensor(const wxImage& lines_image, int x, int y, double theta)
192
    {
193
        linesensor_readings.clear();
194

  
195
        double spacing = scout_image.GetWidth() / (NUM_LINESENSORS - 1);
196
        for (int s = 0; s < NUM_LINESENSORS; s++)
172 197
        {
173
            readings.push_back(8 - i);
174
        }
198
            double offset = -(scout_image.GetWidth() / 2) + s * spacing;
175 199

  
176
        res.readings = readings;
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));
177 202

  
178
        return true;
203
            /*
204
            if (s == 0)
205
            {
206
                cout << "s: " << s << ", offset: " << offset << ", scout: (" << x <<
207
                        ", " << y << "), sensor: (" << sensor_x <<
208
                        ", " << sensor_y << ")" << endl;
209
            }
210
            */
211

  
212
            unsigned char r = lines_image.GetRed(sensor_x, sensor_y);
213
            unsigned char g = lines_image.GetGreen(sensor_x, sensor_y);
214
            unsigned char b = lines_image.GetBlue(sensor_x, sensor_y);
215

  
216
            unsigned int reading = rgb_to_linesensor_val(r, g, b);
217

  
218
            linesensor_readings.push_back(reading);
219
        }
179 220
    }
180 221

  
181 222
    /// Sends back the position of this scout so scoutsim can save
182 223
    /// the world state
183 224
    geometry_msgs::Pose2D Scout::update(double dt, wxMemoryDC& path_dc,
184 225
                                        const wxImage& path_image,
226
                                        const wxImage& lines_image,
185 227
                                        wxColour background_color,
186 228
                                        world_state state)
187 229
    {
......
225 267
            wxImage rotated_image =
226 268
                scout_image.Rotate(orient - PI/2.0,
227 269
                                   wxPoint(scout_image.GetWidth() / 2,
228
                                            scout_image.GetHeight() / 2),
270
                                           scout_image.GetHeight() / 2),
229 271
                                   false);
230 272

  
231 273
            for (int y = 0; y < rotated_image.GetHeight(); ++y)
......
252 294
        p.angular_velocity = ang_vel;
253 295
        pose_pub.publish(p);
254 296

  
297
        update_linesensor(lines_image, canvas_x, canvas_y, p.theta);
298

  
255 299
        // Figure out (and publish) the color underneath the scout
256 300
        {
257 301
            wxSize scout_size = wxSize(scout.GetWidth(), scout.GetHeight());

Also available in: Unified diff