Statistics
| Branch: | Revision:

root / scout / scoutsim / src / scout.cpp @ c06735bb

History | View | Annotate | Download (18.6 KB)

1 c492be62 Alex Zirbel
/**
2
 * The code in this package was developed using the structure of Willow
3
 * Garage's turtlesim package.  It was modified by the CMU Robotics Club
4
 * to be used as a simulator for the Colony Scout robot.
5 266ae7f2 Alex Zirbel
 *
6 c492be62 Alex Zirbel
 * All redistribution of this code is limited to the terms of Willow Garage's
7
 * licensing terms, as well as under permission from the CMU Robotics Club.
8
 * 
9
 * Copyright (c) 2011 Colony Project
10
 * 
11
 * Permission is hereby granted, free of charge, to any person
12
 * obtaining a copy of this software and associated documentation
13
 * files (the "Software"), to deal in the Software without
14
 * restriction, including without limitation the rights to use,
15
 * copy, modify, merge, publish, distribute, sublicense, and/or sell
16
 * copies of the Software, and to permit persons to whom the
17
 * Software is furnished to do so, subject to the following
18
 * conditions:
19
 * 
20
 * The above copyright notice and this permission notice shall be
21
 * included in all copies or substantial portions of the Software.
22
 * 
23
 * Copyright (c) 2009, Willow Garage, Inc.
24 266ae7f2 Alex Zirbel
 * All rights reserved.
25 c492be62 Alex Zirbel
 * 
26
 * Redistribution and use in source and binary forms, with or without
27
 * modification, are permitted provided that the following conditions are met:
28
 * 
29
 *    Redistributions of source code must retain the above copyright
30
 *       notice, this list of conditions and the following disclaimer.
31
 *    Redistributions in binary form must reproduce the above copyright
32
 *       notice, this list of conditions and the following disclaimer in the
33
 *       documentation and/or other materials provided with the distribution.
34
 *    Neither the name of the Willow Garage, Inc. nor the names of its
35
 *       contributors may be used to endorse or promote products derived from
36
 *       this software without specific prior written permission.
37
 * 
38
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
39
 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
40
 * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
41
 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
42
 * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
43
 * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
44
 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
45
 * OTHER DEALINGS IN THE SOFTWARE.
46 266ae7f2 Alex Zirbel
 */
47
48 a8480867 Alex Zirbel
#include "scout.h"
49 266ae7f2 Alex Zirbel
50
#include <wx/wx.h>
51
52
#define DEFAULT_PEN_R 0xb3
53
#define DEFAULT_PEN_G 0xb8
54
#define DEFAULT_PEN_B 0xff
55
56 af0d9743 Alex
using namespace std;
57
58 266ae7f2 Alex Zirbel
namespace scoutsim
59
{
60
    Scout::Scout(const ros::NodeHandle& nh,
61 9f547ef7 Alex Zirbel
                 const wxImage& scout_image,
62
                 const Vector2& pos,
63 6639ce9c viki
                 wxBitmap *path_bitmap,
64 9f547ef7 Alex Zirbel
                 float orient)
65 6639ce9c viki
        : path_bitmap(path_bitmap)
66 43811241 Alex
          , sonar_visual_on(false)
67 093a1aea Alex
          , sonar_on(true)
68 60a90290 Hui Jun Tay
          , ignore_behavior(false)
69
          , current_teleop_scout("")
70 43811241 Alex
          , node (nh)
71 144137a1 Alex Zirbel
          , scout_image(scout_image)
72
          , pos(pos)
73
          , orient(orient)
74 9f547ef7 Alex Zirbel
          , motor_fl_speed(0)
75
          , motor_fr_speed(0)
76
          , motor_bl_speed(0)
77
          , motor_br_speed(0)
78
          , fl_ticks(0)
79
          , fr_ticks(0)
80
          , bl_ticks(0)
81
          , br_ticks(0)
82 144137a1 Alex Zirbel
          , pen_on(true)
83
          , pen(wxColour(DEFAULT_PEN_R, DEFAULT_PEN_G, DEFAULT_PEN_B))
84 266ae7f2 Alex Zirbel
    {
85 144137a1 Alex Zirbel
        pen.SetWidth(3);
86
        scout = wxBitmap(scout_image);
87
88
        motors_sub = node.subscribe("set_motors", 1, &Scout::setMotors, this);
89
90
        pose_pub = node.advertise<Pose>("pose", 1);
91
        color_pub = node.advertise<Color>("color_sensor", 1);
92 a2e6bd4c Alex
        sonar_pub = node.advertise<sonar::sonar_distance>("sonar_distance", 1);
93 144137a1 Alex Zirbel
        set_pen_srv = node.advertiseService("set_pen",
94 266ae7f2 Alex Zirbel
                                            &Scout::setPenCallback,
95
                                            this);
96 eb9cff77 Hui Jun Tay
        toggle_sonar_srv = node.advertiseService("sonar_toggle",
97
                                            &Scout::handle_sonar_toggle,
98
                                            this);
99
        set_sonar_srv = node.advertiseService("sonar_set_scan",
100
                                            &Scout::handle_sonar_set_scan,
101
                                            this);
102 9f547ef7 Alex Zirbel
        query_encoders_srv =
103
            node.advertiseService("query_encoders",
104
                                  &Scout::query_encoders_callback,
105
                                  this);
106 266ae7f2 Alex Zirbel
107 af0d9743 Alex
        query_linesensor_srv =
108
            node.advertiseService("query_linesensor",
109
                                  &Scout::query_linesensor_callback,
110
                                  this);
111
112 ade1b7f9 Alex
        for (unsigned int i = 0; i < NUM_LINESENSORS; i++)
113
        {
114
            linesensor_readings.push_back(0);
115
        }
116
117 a2e6bd4c Alex
        // Initialize sonar
118
        sonar_position = 0;
119
        sonar_stop_l = 0;
120
        sonar_stop_r = 23;
121
        sonar_direction = 1;
122
        sonar_tick_time = ros::Duration(scoutsim::SONAR_HALF_SPIN_TIME / 24.0);
123 266ae7f2 Alex Zirbel
    }
124
125 6257c97d Alex
    float Scout::absolute_to_mps(int absolute_speed)
126
    {
127
        return ((float) absolute_speed) * MAX_SPEED_MPS / MAX_ABSOLUTE_SPEED;
128
    }
129
130 a8480867 Alex Zirbel
    /**
131
     * A callback function that sets velocity based on a set_motors
132
     * request.
133 c492be62 Alex Zirbel
     * @todo Use "callback" in all callback function names? Or remove?
134 a8480867 Alex Zirbel
     */
135
    void Scout::setMotors(const motors::set_motors::ConstPtr& msg)
136
    {
137 144137a1 Alex Zirbel
        last_command_time = ros::WallTime::now();
138 a8480867 Alex Zirbel
139 60a90290 Hui Jun Tay
        //ignore non-teleop commands if commands if teleop is ON
140
        if (node.getNamespace() != current_teleop_scout || msg->teleop_ON)
141 a8480867 Alex Zirbel
        {
142 60a90290 Hui Jun Tay
            if(msg->fl_set)
143
            {
144
                motor_fl_speed = absolute_to_mps(msg->fl_speed);
145
            }
146
            if(msg->fr_set)
147
            {
148
                motor_fr_speed = absolute_to_mps(msg->fr_speed);
149
            }
150
            if(msg->bl_set)
151
            {
152
                motor_bl_speed = absolute_to_mps(msg->bl_speed);
153
            }
154
            if(msg->br_set)
155
            {
156
                motor_br_speed = absolute_to_mps(msg->br_speed);
157
            }
158 a8480867 Alex Zirbel
        }
159 60a90290 Hui Jun Tay
160 266ae7f2 Alex Zirbel
    }
161 eb9cff77 Hui Jun Tay
162
    bool Scout::handle_sonar_toggle(sonar::sonar_toggle::Request  &req,
163 04114d13 Alex
                         sonar::sonar_toggle::Response &res)
164 eb9cff77 Hui Jun Tay
    {
165
        if (req.set_on && !sonar_on)
166
        {
167 04114d13 Alex
            ROS_INFO("Turning on the sonar");
168 eb9cff77 Hui Jun Tay
            sonar_on = true;
169
170
        }
171
        else if (!req.set_on && sonar_on)
172
        {
173 04114d13 Alex
            ROS_INFO("Turning off the sonar");
174 eb9cff77 Hui Jun Tay
            sonar_on = false;
175
        }
176
        else
177
        {
178 04114d13 Alex
            ROS_INFO("Sonar state remains unchanged");
179 eb9cff77 Hui Jun Tay
        }
180 04114d13 Alex
        res.ack = true;
181
        return true;
182 eb9cff77 Hui Jun Tay
    }
183
184 04114d13 Alex
    bool Scout::handle_sonar_set_scan(sonar::sonar_set_scan::Request  &req,
185
                                      sonar::sonar_set_scan::Response &res)
186 eb9cff77 Hui Jun Tay
    {
187 04114d13 Alex
        // Code to set the sonar to scan from
188
        // req.stop_l to req.stop_r
189
        if (req.stop_l>=0 and req.stop_r<=23 and req.stop_l<=req.stop_r)
190
        {
191
            ROS_INFO("Setting sonar scan range to [%i, %i]",
192
                     req.stop_l,
193
                     req.stop_r);
194
            sonar_stop_l = req.stop_l;
195
            sonar_stop_r = req.stop_r;
196
            sonar_position = req.stop_l;
197
            sonar_direction = 1;
198
            res.ack = true;
199
        }
200
        else
201
        {
202
            ROS_INFO("Bad Input: Input should be integers 0-23, stop_l<stop_r");
203
        }
204
        return true;
205 eb9cff77 Hui Jun Tay
    }
206 266ae7f2 Alex Zirbel
207
    bool Scout::setPenCallback(scoutsim::SetPen::Request& req,
208
                               scoutsim::SetPen::Response&)
209
    {
210 144137a1 Alex Zirbel
        pen_on = !req.off;
211 266ae7f2 Alex Zirbel
        if (req.off)
212
        {
213
            return true;
214
        }
215
216
        wxPen pen(wxColour(req.r, req.g, req.b));
217
        if (req.width != 0)
218
        {
219
            pen.SetWidth(req.width);
220
        }
221
222 144137a1 Alex Zirbel
        pen = pen;
223 266ae7f2 Alex Zirbel
        return true;
224
    }
225
226 9f547ef7 Alex Zirbel
    bool Scout::query_encoders_callback(encoders::query_encoders::Request&,
227
                                        encoders::query_encoders::Response& res)
228
    {
229
        res.fl_distance = fl_ticks;
230
        res.fr_distance = fr_ticks;
231
        res.bl_distance = bl_ticks;
232
        res.br_distance = br_ticks;
233
234
        return true;
235
    }
236
237 af0d9743 Alex
    bool Scout::query_linesensor_callback(linesensor::query_linesensor::Request&,
238
                                          linesensor::query_linesensor::Response& res)
239
    {
240 ade1b7f9 Alex
        res.readings = linesensor_readings;
241
242
        return true;
243
    }
244 af0d9743 Alex
245 ade1b7f9 Alex
    // Scale to linesensor value
246 a2e6bd4c Alex
    unsigned int Scout::rgb_to_grey(unsigned char r,
247
                                    unsigned char g,
248
                                    unsigned char b)
249 ade1b7f9 Alex
    {
250
        // Should be 0 to 255
251
        unsigned int grey = ((unsigned int) r + (unsigned int) g + (unsigned int) b) / 3;
252
253
        /// @todo Convert to the proper range
254
        return 255 - grey;
255
    }
256
257 a2e6bd4c Alex
    unsigned int Scout::trace_sonar(const wxImage& walls_image, int x, int y,
258 96ec9388 Hui Jun Tay
                                    double robot_theta, int sonar_pos, 
259
                                    wxMemoryDC& sonar_dc)
260 a2e6bd4c Alex
    {
261 71e1154d Alex
        double angle = robot_theta + (PI * ((float) sonar_pos) / 24.0) - PI / 2;
262 a2e6bd4c Alex
        unsigned int d = 0;
263
264
        unsigned int reading = 0;
265 96ec9388 Hui Jun Tay
266 c63c9752 Alex
        int d_x = 0;
267
        int d_y = 0;
268
269 a2e6bd4c Alex
        do
270
        {
271 6639ce9c viki
            d_x = x + (int) floor(d * cos(angle));
272 eb9cff77 Hui Jun Tay
            d_y = y - (int) floor(d * sin(angle));
273 a2e6bd4c Alex
274
            // Out of image boundary
275
            if (d_x < 0 || d_x >= walls_image.GetWidth() ||
276
                d_y < 0 || d_y >= walls_image.GetHeight())
277
            {
278 dd065971 Alex
                break;
279 a2e6bd4c Alex
            }
280
281
            // Max range
282 dd065971 Alex
            if (d > scoutsim::SONAR_MAX_RANGE_PIX)
283 a2e6bd4c Alex
            {
284 dd065971 Alex
                break;
285 a2e6bd4c Alex
            }
286
287
            // Get the sonar reading at the current position of the sonar
288
            unsigned char r = walls_image.GetRed(d_x, d_y);
289
            unsigned char g = walls_image.GetGreen(d_x, d_y);
290
            unsigned char b = walls_image.GetBlue(d_x, d_y);
291 04114d13 Alex
    
292 a2e6bd4c Alex
            reading = rgb_to_grey(r, g, b);
293 04114d13 Alex
        
294 a2e6bd4c Alex
            d++;
295
        }
296 71e1154d Alex
        /// @todo Consider using different cutoffs for different features
297
        while (reading < 128); /// @todo Get rid of hardcoded stuff like this
298 6639ce9c viki
        
299 04114d13 Alex
        if (sonar_visual_on)
300
        {   
301
            if (isFront)
302
            {
303
                // draw a circle at the wall_x, wall_y where reading > 128
304
                sonar_dc.SelectObject(*path_bitmap);
305
                sonar_dc.SetBrush(*wxRED_BRUSH); //old value
306 9cb9623b Alex
                sonar_dc.DrawCircle(wxPoint(old_front_dx, old_front_dy), 2);
307 04114d13 Alex
                old_front_dx = d_x;
308
                old_front_dy = d_y;
309
            }
310
            else
311
            {
312
                // draw a circle at the wall_x, wall_y where reading > 128
313
                sonar_dc.SelectObject(*path_bitmap);
314
                sonar_dc.SetBrush(*wxRED_BRUSH); //old value
315
                sonar_dc.DrawCircle(wxPoint(old_back_dx,old_back_dy), 2);
316
                old_back_dx = d_x;
317
                old_back_dy = d_y;
318
            }
319
320
            sonar_dc.SetBrush(*wxGREEN_BRUSH);  //newest value
321 43811241 Alex
            sonar_dc.DrawCircle(wxPoint(d_x,d_y), 2);
322
            if (isFront) // @todo for some reason isFront = (!isFront) is not working
323 04114d13 Alex
            {
324
                isFront = FALSE;
325
            }
326
            else
327
            {
328
                isFront = TRUE; 
329
            }
330
        }
331
332 e5ac3afb Alex
        // Convert from pixels to mm and return
333 dd065971 Alex
        return (unsigned int) ((1000 / PIX_PER_METER) * d);
334 a2e6bd4c Alex
    }
335
336 71e1154d Alex
    // x and y is current position of the sonar
337 a2e6bd4c Alex
    void Scout::update_sonar(const wxImage& walls_image, int x, int y,
338 eb9cff77 Hui Jun Tay
                             double robot_theta,wxMemoryDC& sonar_dc)
339 a2e6bd4c Alex
    {
340
        // Only rotate the sonar at the correct rate.
341
        if (ros::Time::now() - last_sonar_time < sonar_tick_time)
342
        {
343
            return;
344
        }
345
        last_sonar_time = ros::Time::now();
346
347
        unsigned int d_front = trace_sonar(walls_image, x, y, robot_theta,
348 43811241 Alex
                                           sonar_position, sonar_dc);
349 a2e6bd4c Alex
        unsigned int d_back = trace_sonar(walls_image, x, y, robot_theta,
350 43811241 Alex
                                          sonar_position + 24, sonar_dc);
351 a2e6bd4c Alex
352
        // Publish
353
        sonar::sonar_distance msg;
354
        msg.pos = sonar_position;
355
        msg.distance0 = d_front;
356
        msg.distance1 = d_back;
357 c06735bb Hui Jun Tay
        msg.stamp = ros::Time::now();
358 a2e6bd4c Alex
359
        sonar_pub.publish(msg);
360
361
        // Update the sonar rotation
362
        if (sonar_position + sonar_direction <= sonar_stop_r &&
363
            sonar_position + sonar_direction >= sonar_stop_l)
364
        {
365
            sonar_position = sonar_position + sonar_direction;
366
        }
367
        else
368
        {
369
            sonar_direction = -sonar_direction;
370
        }
371
    }
372
373
    void Scout::update_linesensor(const wxImage& lines_image, int x, int y,
374
                                  double robot_theta)
375 ade1b7f9 Alex
    {
376
        linesensor_readings.clear();
377
378
        double spacing = scout_image.GetWidth() / (NUM_LINESENSORS - 1);
379
        for (int s = 0; s < NUM_LINESENSORS; s++)
380 af0d9743 Alex
        {
381 ade1b7f9 Alex
            double offset = -(scout_image.GetWidth() / 2) + s * spacing;
382 af0d9743 Alex
383 a2e6bd4c Alex
            int sensor_x = (int) (x - LNSNSR_D * cos(robot_theta) -
384
                                  offset * sin(robot_theta));
385
            int sensor_y = (int) (y + LNSNSR_D * sin(robot_theta) -
386
                                  offset * cos(robot_theta));
387 af0d9743 Alex
388 ade1b7f9 Alex
            unsigned char r = lines_image.GetRed(sensor_x, sensor_y);
389
            unsigned char g = lines_image.GetGreen(sensor_x, sensor_y);
390
            unsigned char b = lines_image.GetBlue(sensor_x, sensor_y);
391
392 a2e6bd4c Alex
            unsigned int reading = rgb_to_grey(r, g, b);
393 ade1b7f9 Alex
394
            linesensor_readings.push_back(reading);
395
        }
396 af0d9743 Alex
    }
397
398 9b3564f3 Alex Zirbel
    /// Sends back the position of this scout so scoutsim can save
399
    /// the world state
400 04114d13 Alex
    /// TODO remove dt param
401 9b3564f3 Alex Zirbel
    geometry_msgs::Pose2D Scout::update(double dt, wxMemoryDC& path_dc,
402 04114d13 Alex
                                        wxMemoryDC& sonar_dc,
403 9b3564f3 Alex Zirbel
                                        const wxImage& path_image,
404 ade1b7f9 Alex
                                        const wxImage& lines_image,
405 a2e6bd4c Alex
                                        const wxImage& walls_image,
406 9b3564f3 Alex Zirbel
                                        wxColour background_color,
407 c63c9752 Alex
                                        wxColour sonar_color,
408 60a90290 Hui Jun Tay
                                        world_state state, 
409
                                        string teleop_scoutname)
410 266ae7f2 Alex Zirbel
    {
411 9f547ef7 Alex Zirbel
        // Assume that the two motors on the same side will be set to
412
        // roughly the same speed. Does not account for slip conditions
413
        // when they are set to different speeds.
414
        float l_speed = (float (motor_fl_speed + motor_bl_speed)) / 2;
415
        float r_speed = (float (motor_fr_speed + motor_br_speed)) / 2;
416
417 c63c9752 Alex
        // Find linear and angular movement in m
418 7f095440 Priya
        float lin_dist = SIM_TIME_REFRESH_RATE * (l_speed + r_speed) / 2;
419 fa857318 Priya
        float ang_dist = SIM_TIME_REFRESH_RATE * (r_speed - l_speed) / SCOUT_WIDTH;
420 9f547ef7 Alex Zirbel
421 60a90290 Hui Jun Tay
        //store currently teleop'd scoutname
422
        std::stringstream ss;
423
        ss << "/" << teleop_scoutname;
424
        current_teleop_scout = ss.str();
425
426 144137a1 Alex Zirbel
        Vector2 old_pos = pos;
427 266ae7f2 Alex Zirbel
428 9f547ef7 Alex Zirbel
        // Update encoders
429 7f095440 Priya
        fl_ticks += (unsigned int) (motor_fl_speed * SIM_TIME_REFRESH_RATE *
430 c63c9752 Alex
                                    ENCODER_TICKS_PER_METER);
431 7f095440 Priya
        fr_ticks += (unsigned int) (motor_fr_speed * SIM_TIME_REFRESH_RATE *
432 c63c9752 Alex
                                    ENCODER_TICKS_PER_METER);
433 7f095440 Priya
        bl_ticks += (unsigned int) (motor_bl_speed * SIM_TIME_REFRESH_RATE *
434 c63c9752 Alex
                                    ENCODER_TICKS_PER_METER);
435 7f095440 Priya
        br_ticks += (unsigned int) (motor_br_speed * SIM_TIME_REFRESH_RATE *
436 c63c9752 Alex
                                    ENCODER_TICKS_PER_METER);
437
438
        orient = fmod((float) (orient + ang_dist), (float) (2.0 * PI));
439 9f547ef7 Alex Zirbel
440 fa857318 Priya
        pos.x += cos(orient) * lin_dist;
441
        pos.y -= sin(orient) * lin_dist; //Subtract because of the way the simulator handles y directions.
442 266ae7f2 Alex Zirbel
443
        // Clamp to screen size
444 e3f69e61 Alex
        if (pos.x < 0 || pos.x >= state.canvas_width
445
                || pos.y < 0 || pos.y >= state.canvas_height)
446 266ae7f2 Alex Zirbel
        {
447 c63c9752 Alex
            ROS_WARN("I hit the wall! (Clamping from [x=%f, y=%f])", pos.x, pos.y);
448 266ae7f2 Alex Zirbel
        }
449
450 af0d9743 Alex
        pos.x = min(max(pos.x, 0.0f), state.canvas_width);
451
        pos.y = min(max(pos.y, 0.0f), state.canvas_height);
452 266ae7f2 Alex Zirbel
453 c63c9752 Alex
        int canvas_x = pos.x * PIX_PER_METER;
454
        int canvas_y = pos.y * PIX_PER_METER;
455 266ae7f2 Alex Zirbel
456 60a90290 Hui Jun Tay
457 266ae7f2 Alex Zirbel
        {
458
            wxImage rotated_image =
459 144137a1 Alex Zirbel
                scout_image.Rotate(orient - PI/2.0,
460
                                   wxPoint(scout_image.GetWidth() / 2,
461 ade1b7f9 Alex
                                           scout_image.GetHeight() / 2),
462 5e2c3dc1 Alex
                                   false);
463 266ae7f2 Alex Zirbel
464
            for (int y = 0; y < rotated_image.GetHeight(); ++y)
465
            {
466
                for (int x = 0; x < rotated_image.GetWidth(); ++x)
467
                {
468
                    if (rotated_image.GetRed(x, y) == 255
469
                            && rotated_image.GetBlue(x, y) == 255
470
                            && rotated_image.GetGreen(x, y) == 255)
471
                    {
472
                        rotated_image.SetAlpha(x, y, 0);
473
                    }
474
                }
475
            }
476
477 144137a1 Alex Zirbel
            scout = wxBitmap(rotated_image);
478 266ae7f2 Alex Zirbel
        }
479
480
        Pose p;
481 144137a1 Alex Zirbel
        p.x = pos.x;
482 ddfeb111 Priya
        p.y = pos.y;
483 144137a1 Alex Zirbel
        p.theta = orient;
484 c63c9752 Alex
        p.linear_velocity = l_speed;
485
        p.angular_velocity = r_speed;
486 144137a1 Alex Zirbel
        pose_pub.publish(p);
487 266ae7f2 Alex Zirbel
488 ade1b7f9 Alex
        update_linesensor(lines_image, canvas_x, canvas_y, p.theta);
489 eb9cff77 Hui Jun Tay
        if (sonar_on)
490
        {
491
            update_sonar(walls_image,
492
                         canvas_x + scoutsim::SCOUT_SONAR_X,
493
                         canvas_y + scoutsim::SCOUT_SONAR_Y,
494
                         p.theta,sonar_dc);
495 ade1b7f9 Alex
496 eb9cff77 Hui Jun Tay
        }
497 43811241 Alex
498 266ae7f2 Alex Zirbel
        // Figure out (and publish) the color underneath the scout
499
        {
500 23b8119f Alex
            //wxSize scout_size = wxSize(scout.GetWidth(), scout.GetHeight());
501 266ae7f2 Alex Zirbel
            Color color;
502
            color.r = path_image.GetRed(canvas_x, canvas_y);
503
            color.g = path_image.GetGreen(canvas_x, canvas_y);
504
            color.b = path_image.GetBlue(canvas_x, canvas_y);
505 144137a1 Alex Zirbel
            color_pub.publish(color);
506 266ae7f2 Alex Zirbel
        }
507
508
        ROS_DEBUG("[%s]: pos_x: %f pos_y: %f theta: %f",
509 144137a1 Alex Zirbel
                  node.getNamespace().c_str(), pos.x, pos.y, orient);
510 266ae7f2 Alex Zirbel
511 144137a1 Alex Zirbel
        if (pen_on)
512 266ae7f2 Alex Zirbel
        {
513 144137a1 Alex Zirbel
            if (pos != old_pos)
514 266ae7f2 Alex Zirbel
            {
515 6639ce9c viki
                path_dc.SelectObject(*path_bitmap);
516 144137a1 Alex Zirbel
                path_dc.SetPen(pen);
517 c63c9752 Alex
                path_dc.DrawLine(pos.x * PIX_PER_METER, pos.y * PIX_PER_METER,
518
                                 old_pos.x * PIX_PER_METER, old_pos.y * PIX_PER_METER);
519 266ae7f2 Alex Zirbel
            }
520
        }
521 e3f69e61 Alex
522
        geometry_msgs::Pose2D my_pose;
523
        my_pose.x = pos.x;
524
        my_pose.y = pos.y;
525
        my_pose.theta = orient;
526
527
        return my_pose;
528 266ae7f2 Alex Zirbel
    }
529
530
    void Scout::paint(wxDC& dc)
531
    {
532 144137a1 Alex Zirbel
        wxSize scout_size = wxSize(scout.GetWidth(), scout.GetHeight());
533 c63c9752 Alex
        dc.DrawBitmap(scout, pos.x * PIX_PER_METER - (scout_size.GetWidth() / 2),
534
                      pos.y * PIX_PER_METER - (scout_size.GetHeight() / 2), true);
535 266ae7f2 Alex Zirbel
    }
536 43811241 Alex
537
    void Scout::set_sonar_visual(bool on)
538
    {
539
        sonar_visual_on = on;
540
    }
541 266ae7f2 Alex Zirbel
}