Revision ade1b7f9

View differences:

scout/libscout/src/LinesensorControl.cpp
64 64

  
65 65
    for (int i = 0; i < 8; i++)
66 66
    {
67
        cout << "[" << i << "]: " << srv.response.readings[i] << ", " << endl;
67
        cout << "[" << i << "]: " << srv.response.readings[i] << ", ";
68 68
    }
69

  
70
    cout << endl;
69 71
}
70 72

  
scout/libscout/src/behaviors/sim_line.cpp
29 29
{
30 30
    while(ok())
31 31
    {
32
        motors->set_sides(40, 80, MOTOR_ABSOLUTE);
32
        motors->set_sides(10, 20, MOTOR_ABSOLUTE);
33 33

  
34 34
        linesensor->query();
35 35

  
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());
scout/scoutsim/src/scout.h
73 73
/// The scale factor so the speed of scout robots is reasonable for the sim
74 74
#define SPEED_SCALE_FACTOR 0.05
75 75

  
76
#define NUM_LINESENSORS 8
77

  
78
// Distance, pixels, from center of robot to the linesensors.
79
#define LNSNSR_D 20
80

  
76 81
namespace scoutsim
77 82
{
78 83
    struct Vector2
......
109 114

  
110 115
            geometry_msgs::Pose2D update(double dt, wxMemoryDC& path_dc,
111 116
                                         const wxImage& path_image,
117
                                         const wxImage& lines_image,
112 118
                                         wxColour background_color,
113 119
                                         world_state state);
114 120
            void paint(wxDC& dc);
......
122 128
                                         encoders::query_encoders::Response&);
123 129
            bool query_linesensor_callback(linesensor::query_linesensor::Request&,
124 130
                                           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);
125 135

  
126 136
            ros::NodeHandle node;
127 137

  
......
145 155
            unsigned int bl_ticks;
146 156
            unsigned int br_ticks;
147 157

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

  
148 161
            // Each scout has a unique id number, which is also displayed on its image.
149 162
            int scout_id;
150 163

  
scout/scoutsim/src/sim_frame.cpp
92 92
        /// @todo This should change.
93 93
        meter = scout_images[0].GetHeight();
94 94

  
95
        base_map_name = map_name;
96
        display_map_name = ros::package::getPath("scoutsim") + "/maps/" + map_name + ".bmp";
95
        map_base_name =  ros::package::getPath("scoutsim") + "/maps/" +
96
                           map_name + ".bmp";
97
        map_lines_name = ros::package::getPath("scoutsim") + "/maps/" +
98
                           map_name + "_lines.bmp";
99
        display_map_name = map_base_name;
100

  
101
        wxBitmap lines_bitmap;
102
        lines_bitmap.LoadFile(wxString::FromAscii(map_lines_name.c_str()));
103
        lines_image = lines_bitmap.ConvertToImage();
104

  
97 105
        clear();
98 106

  
99 107
        clear_srv = nh.advertiseService("clear",
......
227 235

  
228 236
    void SimFrame::showMap(wxCommandEvent& WXUNUSED(event))
229 237
    {
230
        display_map_name = ros::package::getPath("scoutsim") + "/maps/" +
231
                           base_map_name + ".bmp";
238
        display_map_name = map_base_name;
232 239
        clear();
233 240
    }
234 241

  
235 242
    void SimFrame::showLines(wxCommandEvent& WXUNUSED(event))
236 243
    {
237
        display_map_name = ros::package::getPath("scoutsim") + "/maps/" +
238
                           base_map_name + "_lines.bmp";
244
        display_map_name = map_lines_name;
239 245
        clear();
240 246
    }
241 247

  
......
298 304

  
299 305
        for (; it != end; ++it)
300 306
        {
301
            it->second->update(0.016, path_dc, path_image,
307
            it->second->update(0.016, path_dc, path_image, lines_image,
302 308
                               path_dc.GetBackground().GetColour(),
303 309
                               state);
304 310
        }
scout/scoutsim/src/sim_frame.h
103 103
            wxTimer* update_timer;
104 104
            wxBitmap path_bitmap;
105 105
            wxImage path_image;
106
            wxImage lines_image;
106 107
            wxMemoryDC path_dc;
107 108

  
108 109
            uint64_t frame_count;
......
124 125
            float width_in_meters;
125 126
            float height_in_meters;
126 127

  
127
            std::string base_map_name;
128
            std::string map_base_name;
129
            std::string map_lines_name;
128 130
            std::string display_map_name;
129 131
    };
130 132

  

Also available in: Unified diff