Revision b9e59a3c scout/libscout/src/test_behaviors/smart_runaround.cpp

View differences:

scout/libscout/src/test_behaviors/smart_runaround.cpp
178 178
    spinOnce();
179 179

  
180 180
    // Assumption: readings are given in millimeters - Zane
181

  
182
<<<<<<< HEAD
183
    // matrices for going from robot's frame to base frame
184
    float rightMat[2][2] = {{0, 1}, {-1, 0}};
185
    float downMat[2][2] = {{-1, 0}, {0, -1}};
186
    float leftMat[2][2] = {{0, 1}, {1, 0}};
187

  
188
    // Look to the left (and update map).
189
    float left_distance = readings[0]; // w.r.t. robot
190
    if(left_distance == 0)
191
        return false;
192
    int left_idx = -mm_to_idx(left_distance); // w.r.t. map
193
    // plot to map if indices are valid
194
    if (0 <= left_idx && left_idx < MAP_LENGTH)
195
        map[row][col+left_idx] = SEEN;
196

  
197
    // Look in the other directions (and update map).
198
    for (int i = 24; i < 48; i++) {
199
        float distance = readings[i]; // w.r.t. robot
200
        if(distance == 0)
201
            return false;
202
        if(distance >= 500)
203
            break; // too far to be accurate
204
        float theta = (M_PI/24)*i - M_PI;
205
        float xDist = distance*cos(theta); // w.r.t. robot
206
        float yDist = distance*sin(theta); // w.r.t. robot
207
        float inputs[2] = {xDist, yDist};
208
        float *ans;
209

  
210
        // re-orient x and y distances based on direction
211
        switch(dir) {
212
            case UP:
213
                ans[0] = xDist;
214
                ans[1] = yDist;
215
                break;
216
            case RIGHT:
217
                ans = matrix_mult(inputs, rightMat);
218
                break;
219
            case DOWN:
220
                ans = matrix_mult(inputs, downMat);
221
                break;
222
            case LEFT:
223
                ans = matrix_mult(inputs, leftMat);
224
                break;
225
        }
226
        // indices into the map
227
        int pixDistX = row + mm_to_idx(ans[0]);
228
        int pixDistY = col + mm_to_idx(ans[1]);
229
        // plot to map if indices are valid
230
        if (0 <= pixDistX && pixDistX < MAP_LENGTH
231
            && 0 <= pixDistY && pixDistY < MAP_LENGTH)
232
                map[pixDistX][pixDistY] = SEEN;
233
=======
234 181
    // distances with respect to robot, NOT map
235 182
    // Look to the left.
236 183
    float left_distance = readings[0]/1000.0;
237
    int left_idx = m_to_idx(left_distance);
184
    int left_idx = mm_to_idx(left_distance);
238 185
    // Look to the front.
239 186
    float front_distance = readings[36]/1000.0;
240
    int front_idx = m_to_idx(front_distance);
187
    int front_idx = mm_to_idx(front_distance);
241 188
    // Look to the right.
242 189
    float right_distance = readings[24]/1000.0;
243
    int right_idx = m_to_idx(right_distance);
190
    int right_idx = mm_to_idx(right_distance);
244 191

  
245 192
    ROS_INFO("front: %lf  left: %lf  right: %lf",
246 193
        front_distance, left_distance, right_distance);
......
323 270
                break;
324 271
        }
325 272
        map[row][col-l] = SEEN;
326
>>>>>>> dbbad8ed5cba08f5500e7d333beab2d089f9956c
327 273
    }
328 274
    return true;
329 275
}

Also available in: Unified diff