Revision c63c9752

View differences:

scout/scoutsim/src/scout.cpp
105 105
            linesensor_readings.push_back(0);
106 106
        }
107 107

  
108
        meter = scout.GetHeight();
109

  
110 108
        // Initialize sonar
111 109
        sonar_position = 0;
112 110
        sonar_stop_l = 0;
......
140 138
        {
141 139
            motor_br_speed = msg->br_speed;
142 140
        }
143

  
144 141
    }
145 142

  
146 143
    bool Scout::setPenCallback(scoutsim::SetPen::Request& req,
......
194 191
    }
195 192

  
196 193
    unsigned int Scout::trace_sonar(const wxImage& walls_image, int x, int y,
197
                                    double robot_theta, int sonar_pos, wxMemoryDC& sonar_dc, bool sonar_on)
194
                                    double robot_theta, int sonar_pos,
195
                                    wxMemoryDC& sonar_dc, bool sonar_on)
198 196
    {
199 197
        double angle = robot_theta + (PI * ((float) sonar_pos) / 24.0) - PI / 2;
200 198
        unsigned int d = 0;
201 199

  
202 200
        unsigned int reading = 0;
203
	int d_x = 0;
204
	int d_y = 0;
201
        int d_x = 0;
202
        int d_y = 0;
203

  
205 204
        do
206 205
        {
207 206
            d_x = x + (int) floor(d * cos(angle));
......
233 232
        /// @todo Consider using different cutoffs for different features
234 233
        while (reading < 128); /// @todo Get rid of hardcoded stuff like this
235 234
        
236
	if(sonar_on)
237
	{       
238
		if(isFront)
239
		{
240
			// draw a circle at the wall_x, wall_y where reading > 128
241
			sonar_dc.SelectObject(*path_bitmap);
242
			sonar_dc.SetBrush(*wxRED_BRUSH); //old value
243
			sonar_dc.DrawCircle(wxPoint(old_front_dx,old_front_dy), 2);
244
			old_front_dx = d_x;
245
			old_front_dy = d_y;
246
		}
247
		else
248
		{
249
			// draw a circle at the wall_x, wall_y where reading > 128
250
			sonar_dc.SelectObject(*path_bitmap);
251
			sonar_dc.SetBrush(*wxRED_BRUSH); //old value
252
			sonar_dc.DrawCircle(wxPoint(old_back_dx,old_back_dy), 2);
253
			old_back_dx = d_x;
254
			old_back_dy = d_y;
255
		}
256

  
257
		sonar_dc.SetBrush(*wxGREEN_BRUSH);  //newest value
258
		sonar_dc.DrawCircle( wxPoint(d_x,d_y), 2);
259
		if (isFront) //for some reason isFront = (!isFront) is not working
260
		{
261
			isFront = FALSE;
262
		}
263
		else
264
		{
265
			isFront = TRUE;	
266
		}
267
			
268
	}
235
        if(sonar_on)
236
        {       
237
            if(isFront)
238
            {
239
                // draw a circle at the wall_x, wall_y where reading > 128
240
                sonar_dc.SelectObject(*path_bitmap);
241
                sonar_dc.SetBrush(*wxRED_BRUSH); //old value
242
                sonar_dc.DrawCircle(wxPoint(old_front_dx,old_front_dy), 2);
243
                old_front_dx = d_x;
244
                old_front_dy = d_y;
245
            }
246
            else
247
            {
248
                // draw a circle at the wall_x, wall_y where reading > 128
249
                sonar_dc.SelectObject(*path_bitmap);
250
                sonar_dc.SetBrush(*wxRED_BRUSH); //old value
251
                sonar_dc.DrawCircle(wxPoint(old_back_dx,old_back_dy), 2);
252
                old_back_dx = d_x;
253
                old_back_dy = d_y;
254
            }
255

  
256
            sonar_dc.SetBrush(*wxGREEN_BRUSH);  //newest value
257
            sonar_dc.DrawCircle( wxPoint(d_x,d_y), 2);
258
            if (isFront) //for some reason isFront = (!isFront) is not working
259
            {
260
                isFront = FALSE;
261
            }
262
            else
263
            {
264
                isFront = TRUE;	
265
            }
266
                
267
        }
269 268
        return d;
270 269
    }
271 270

  
......
333 332
    /// Sends back the position of this scout so scoutsim can save
334 333
    /// the world state
335 334
    geometry_msgs::Pose2D Scout::update(double dt, wxMemoryDC& path_dc,
336
					wxMemoryDC& sonar_dc,
337
					bool sonar_on,
335
                                        wxMemoryDC& sonar_dc,
336
                                        bool sonar_on,
338 337
                                        const wxImage& path_image,
339 338
                                        const wxImage& lines_image,
340 339
                                        const wxImage& walls_image,
341 340
                                        wxColour background_color,
342
					wxColour sonar_color,
341
                                        wxColour sonar_color,
343 342
                                        world_state state)
344 343
    {
345 344
        // Assume that the two motors on the same side will be set to
......
348 347
        float l_speed = (float (motor_fl_speed + motor_bl_speed)) / 2;
349 348
        float r_speed = (float (motor_fr_speed + motor_br_speed)) / 2;
350 349

  
351
        // Set the linear and angular speeds
352
        float lin_vel = SPEED_SCALE_FACTOR * (l_speed + r_speed) / 2;
353
        float ang_vel = SPEED_SCALE_FACTOR * (r_speed - l_speed);
350
        // Find linear and angular movement in m
351
        float lin_dist = SCOUTSIM_REFRESH_RATE * (l_speed + r_speed) / 2;
352
        float ang_dist = SCOUTSIM_REFRESH_RATE * (r_speed - l_speed);
354 353

  
355 354
        Vector2 old_pos = pos;
356 355

  
357 356
        // Update encoders
358
        /// @todo replace
359
        fl_ticks += (unsigned int) motor_fl_speed;
360
        fr_ticks += (unsigned int) motor_fr_speed;
361
        bl_ticks += (unsigned int) motor_bl_speed;
362
        br_ticks += (unsigned int) motor_br_speed;
357
        fl_ticks += (unsigned int) (motor_fl_speed * SCOUTSIM_REFRESH_RATE *
358
                                    ENCODER_TICKS_PER_METER);
359
        fr_ticks += (unsigned int) (motor_fr_speed * SCOUTSIM_REFRESH_RATE *
360
                                    ENCODER_TICKS_PER_METER);
361
        bl_ticks += (unsigned int) (motor_bl_speed * SCOUTSIM_REFRESH_RATE *
362
                                    ENCODER_TICKS_PER_METER);
363
        br_ticks += (unsigned int) (motor_br_speed * SCOUTSIM_REFRESH_RATE *
364
                                    ENCODER_TICKS_PER_METER);
365

  
366
        orient = fmod((float) (orient + ang_dist), (float) (2.0 * PI));
363 367

  
364
        orient = fmod(orient + ang_vel * dt, 2*PI);
365
        pos.x += sin(orient + PI/2.0) * lin_vel * dt;
366
        pos.y += cos(orient + PI/2.0) * lin_vel * dt;
368
        // Multiply by dt, the time passed (SCOUTSIM_REFRESH_RATE)
369
        pos.x += sin(orient + PI/2.0) * lin_dist;
370
        pos.y += cos(orient + PI/2.0) * lin_dist;
367 371

  
368 372
        // Clamp to screen size
369 373
        if (pos.x < 0 || pos.x >= state.canvas_width
370 374
                || pos.y < 0 || pos.y >= state.canvas_height)
371 375
        {
372
            ROS_WARN("Oh no! I hit the wall! (Clamping from [x=%f, y=%f])", pos.x, pos.y);
376
            ROS_WARN("I hit the wall! (Clamping from [x=%f, y=%f])", pos.x, pos.y);
373 377
        }
374 378

  
375 379
        pos.x = min(max(pos.x, 0.0f), state.canvas_width);
376 380
        pos.y = min(max(pos.y, 0.0f), state.canvas_height);
377 381

  
378
        int canvas_x = pos.x * meter;
379
        int canvas_y = pos.y * meter;
382
        int canvas_x = pos.x * PIX_PER_METER;
383
        int canvas_y = pos.y * PIX_PER_METER;
380 384

  
381 385
        {
382 386
            wxImage rotated_image =
......
405 409
        p.x = pos.x;
406 410
        p.y = state.canvas_height - pos.y;
407 411
        p.theta = orient;
408
        p.linear_velocity = lin_vel;
409
        p.angular_velocity = ang_vel;
412
        p.linear_velocity = l_speed;
413
        p.angular_velocity = r_speed;
410 414
        pose_pub.publish(p);
411 415

  
412 416
        update_linesensor(lines_image, canvas_x, canvas_y, p.theta);
......
434 438
            {
435 439
                path_dc.SelectObject(*path_bitmap);
436 440
                path_dc.SetPen(pen);
437
                path_dc.DrawLine(pos.x * meter, pos.y * meter,
438
                                 old_pos.x * meter, old_pos.y * meter);
441
                path_dc.DrawLine(pos.x * PIX_PER_METER, pos.y * PIX_PER_METER,
442
                                 old_pos.x * PIX_PER_METER, old_pos.y * PIX_PER_METER);
439 443
            }
440 444
        }
441 445

  
......
450 454
    void Scout::paint(wxDC& dc)
451 455
    {
452 456
        wxSize scout_size = wxSize(scout.GetWidth(), scout.GetHeight());
453
        dc.DrawBitmap(scout, pos.x * meter - (scout_size.GetWidth() / 2),
454
                      pos.y * meter - (scout_size.GetHeight() / 2), true);
457
        dc.DrawBitmap(scout, pos.x * PIX_PER_METER - (scout_size.GetWidth() / 2),
458
                      pos.y * PIX_PER_METER - (scout_size.GetHeight() / 2), true);
455 459
    }
456

  
457 460
}
scout/scoutsim/src/scout.h
70 70

  
71 71
#define PI 3.14159265
72 72

  
73
/// The scale factor so the speed of scout robots is reasonable for the sim
74
#define SPEED_SCALE_FACTOR 0.05
75

  
76 73
#define NUM_LINESENSORS 8
77 74

  
78 75
// Distance, pixels, from center of robot to the linesensors.
......
135 132
                                     unsigned char g,
136 133
                                     unsigned char b);
137 134
            unsigned int trace_sonar(const wxImage& walls_image, int x, int y,
138
                                     double robot_theta, int sonar_pos,wxMemoryDC& sonar_dc, bool sonar_on);
135
                                     double robot_theta, int sonar_pos,
136
                                     wxMemoryDC& sonar_dc, bool sonar_on);
139 137
            void update_sonar(const wxImage& walls_image, int x, int y,
140 138
                              double robot_theta, wxMemoryDC& sonar_dc,bool sonar_on);
141 139
            void update_linesensor(const wxImage& lines_image, int x, int y,
142 140
                                   double theta);
143
	    int old_front_dx;
144
	    int old_front_dy;
145
	    int old_back_dx;
146
	    int old_back_dy;
147
	    bool isFront;
141
            int old_front_dx;
142
            int old_front_dy;
143
            int old_back_dx;
144
            int old_back_dy;
145
            bool isFront;
148 146

  
149
	    wxBitmap *path_bitmap;
150
	    bool sonar_on;
147
            wxBitmap *path_bitmap;
148
            bool sonar_on;
151 149

  
152 150
            ros::NodeHandle node;
153 151

  
......
175 173
            int sonar_stop_l;
176 174
            int sonar_stop_r;
177 175
            int sonar_direction;
176

  
178 177
            // The last time the sonar changed its position.
179 178
            ros::Time last_sonar_time;
180 179
            ros::Duration sonar_tick_time;
......
197 196
            ros::ServiceServer query_linesensor_srv;
198 197

  
199 198
            ros::WallTime last_command_time;
200

  
201
            float meter;
202

  
203 199
    };
204 200
    typedef boost::shared_ptr<Scout> ScoutPtr;
205

  
206 201
}
207 202

  
208 203
#endif
scout/scoutsim/src/scout_constants.h
49 49

  
50 50
namespace scoutsim
51 51
{
52
    // Pixels in scoutsim per real-world meter.
53
    // Note that scout image size has been set based on this.
54
    const float PIX_PER_METER = 200.0;
55

  
52 56
    // Scout dimensions, in meters
53 57
    const float SCOUT_WIDTH = 0.05;
54 58
    const float SCOUT_LENGTH = 0.2;
......
57 61
    const float SCOUT_SONAR_X = -0.01;
58 62
    const float SCOUT_SONAR_Y = 0.0;
59 63

  
64
    // @todo Inaccurate. Update
65
    const float ENCODER_TICKS_PER_METER = 363.78;
66

  
60 67
    const unsigned int SONAR_MAX_RANGE = 255;
68

  
61 69
    // Time it takes for the sonar to spin from position 0 to position 23
62 70
    const float SONAR_HALF_SPIN_TIME = 0.5;
71

  
72
    const float SCOUTSIM_REFRESH_RATE = 0.01;   // s
63 73
}
scout/scoutsim/src/sim_frame.cpp
68 68
        srand(time(NULL));
69 69

  
70 70
        update_timer = new wxTimer(this);
71
        update_timer->Start(16);
71
        update_timer->Start(SCOUTSIM_REFRESH_RATE * 1000);
72 72

  
73 73
        Connect(update_timer->GetId(), wxEVT_TIMER,
74 74
                wxTimerEventHandler(SimFrame::onUpdate), NULL, this);
......
77 77

  
78 78
        images_path = ros::package::getPath("scoutsim") + "/images/";
79 79

  
80
        // 200 pixels per meter. @todo make this a constant elsewhere.
81
        meter = 200;
82

  
83 80
        map_base_name =  ros::package::getPath("scoutsim") + "/maps/" +
84 81
                           map_name + ".bmp";
85 82
        map_lines_name = ros::package::getPath("scoutsim") + "/maps/" +
......
160 157

  
161 158
        SetMenuBar(menuBar);
162 159

  
163
        width_in_meters = GetSize().GetWidth() / meter;
164
        height_in_meters = GetSize().GetHeight() / meter;
160
        width_in_meters = GetSize().GetWidth() / PIX_PER_METER;
161
        height_in_meters = GetSize().GetHeight() / PIX_PER_METER;
165 162
    }
166 163

  
167 164
    SimFrame::~SimFrame()
......
314 311
        SetSize(wxSize(path_bitmap.GetWidth(), path_bitmap.GetHeight()));
315 312
    }
316 313

  
314
    // Runs every SCOUTSIM_REFRESH_RATE.
317 315
    void SimFrame::onUpdate(wxTimerEvent& evt)
318 316
    {
319 317
        ros::spinOnce();
......
393 391
        }
394 392
        else if (wxGetKeyState(WXK_LEFT))
395 393
        {
396
            teleop_l_speed = -TELEOP_PRECISE_SPEED;
397
            teleop_r_speed = TELEOP_PRECISE_SPEED;
394
            teleop_l_speed = -TELEOP_PRECISE_SPEED * 2;
395
            teleop_r_speed = TELEOP_PRECISE_SPEED * 2;
398 396
        }
399 397
        else if (wxGetKeyState(WXK_RIGHT))
400 398
        {
401
            teleop_l_speed = TELEOP_PRECISE_SPEED;
402
            teleop_r_speed = -TELEOP_PRECISE_SPEED;
399
            teleop_l_speed = TELEOP_PRECISE_SPEED * 2;
400
            teleop_r_speed = -TELEOP_PRECISE_SPEED * 2;
403 401
        }
404 402
    }
405 403

  
......
407 405
    {
408 406
        if (wxGetKeyState(WXK_UP))
409 407
        {
410
            teleop_fluid_speed += 2;
408
            teleop_fluid_speed += TELEOP_FLUID_INC * 2;
411 409
        }
412 410
        else if (wxGetKeyState(WXK_DOWN))
413 411
        {
414
            teleop_fluid_speed -= 2;
412
            teleop_fluid_speed -= TELEOP_FLUID_INC * 2;
415 413
        }
416 414
        else if (teleop_fluid_speed > 0)
417 415
        {
418
            teleop_fluid_speed -= 1;
416
            teleop_fluid_speed -= TELEOP_FLUID_INC;
419 417
        }
420 418
        else if (teleop_fluid_speed < 0)
421 419
        {
422
            teleop_fluid_speed += 1;
420
            teleop_fluid_speed += TELEOP_FLUID_INC;
423 421
        }
424 422

  
425 423
        if (wxGetKeyState(WXK_LEFT))
426 424
        {
427
            teleop_fluid_omega -= 1;
425
            teleop_fluid_omega -= TELEOP_FLUID_INC;
428 426
        }
429 427
        else if (wxGetKeyState(WXK_RIGHT))
430 428
        {
431
            teleop_fluid_omega += 1;
429
            teleop_fluid_omega += TELEOP_FLUID_INC;
432 430
        }
433 431
        else
434 432
        {
......
439 437
        {
440 438
            teleop_fluid_speed = TELEOP_FLUID_MAX_SPEED;
441 439
        }
440
        else if (teleop_fluid_speed < -TELEOP_FLUID_MAX_SPEED)
441
        {
442
            teleop_fluid_speed = -TELEOP_FLUID_MAX_SPEED;
443
        }
442 444
        if (teleop_fluid_omega > TELEOP_FLUID_MAX_SPEED / 2)
443 445
        {
444 446
            teleop_fluid_omega = TELEOP_FLUID_MAX_SPEED / 2;
445 447
        }
448
        else if (teleop_fluid_omega < -TELEOP_FLUID_MAX_SPEED / 2)
449
        {
450
            teleop_fluid_omega = -TELEOP_FLUID_MAX_SPEED / 2;
451
        }
446 452

  
447 453
        teleop_l_speed = teleop_fluid_speed + teleop_fluid_omega;
448 454
        teleop_r_speed = teleop_fluid_speed - teleop_fluid_omega;
scout/scoutsim/src/sim_frame.h
77 77
#define ID_TELEOP_FLUID 9
78 78
#define ID_SONAR 10
79 79

  
80
#define TELEOP_PRECISE_SPEED 30
81
#define TELEOP_FLUID_MAX_SPEED 80
80
#define TELEOP_PRECISE_SPEED 1.0f
81
#define TELEOP_FLUID_MAX_SPEED 1.5f
82
#define TELEOP_FLUID_INC 0.1f
82 83

  
83 84
// Teleop types
84 85
#define TELEOP_OFF 0
......
134 135
            ros::Publisher wireless_receive;
135 136

  
136 137
            // Teleop
137
            short teleop_l_speed;
138
            short teleop_r_speed;
138
            float teleop_l_speed;
139
            float teleop_r_speed;
139 140
            ros::Publisher teleop_pub;
140 141
            int teleop_type;
141 142

  
142
            short teleop_fluid_speed;
143
            short teleop_fluid_omega;
143
            float teleop_fluid_speed;
144
            float teleop_fluid_omega;
144 145

  
145 146
            ros::NodeHandle nh;
146 147
            wxTimer* update_timer;
......
166 167

  
167 168
            std::string images_path;
168 169

  
169
            float meter;
170 170
            float width_in_meters;
171 171
            float height_in_meters;
172 172

  

Also available in: Unified diff