Statistics
| Branch: | Revision:

scoutos / scout / scoutsim / src / emitter.cpp @ cda6b590

History | View | Annotate | Download (5.77 KB)

1
/**
2
 BETA version of Emitter object for use in BOM simulator
3
 */
4

    
5
#include "emitter.h"
6

    
7
#include <wx/wx.h>
8

    
9
#define DEFAULT_PEN_R 0xb3
10
#define DEFAULT_PEN_G 0xb8
11
#define DEFAULT_PEN_B 0xff
12

    
13
using namespace std;
14

    
15
namespace scoutsim
16
{
17
    /**
18
     * The emitter object, which is responsible for refreshing itself and
19
     * updating its position.
20
     *
21
     */
22
    Emitter::Emitter(const ros::NodeHandle& nh,
23
                 const wxImage& emitter_image,
24
                 const Vector2& pos,
25
                 wxBitmap *path_bitmap,
26
                 float orient,
27
                 float aperture, 
28
                 int distance)
29
        : path_bitmap(path_bitmap)
30
          , ignore_behavior(false)
31
          , node (nh)
32
          , emitter_image(emitter_image)
33
          , pos(pos)
34
          , orient(orient)
35
          , aperture(aperture)
36
          , distance(distance)
37
          , pen_on(true)
38
          , emitter_visual_on(true)
39
          , pen(wxColour(DEFAULT_PEN_R, DEFAULT_PEN_G, DEFAULT_PEN_B))
40
    {
41
        pen.SetWidth(3);
42
        emitter = wxBitmap(emitter_image);
43
        color_pub = node.advertise<Color>("color_sensor", 1);
44
        set_pen_srv = node.advertiseService("set_pen",
45
                                            &Emitter::setPenCallback,
46
                                            this);
47
        // Init latch
48
        teleop_latch = 0;
49
    }
50

    
51
   
52
    bool Emitter::setPenCallback(scoutsim::SetPen::Request& req,
53
                               scoutsim::SetPen::Response&)
54
    {
55
        pen_on = !req.off;
56
        if (req.off)
57
        {
58
            return true;
59
        }
60

    
61
        wxPen pen(wxColour(req.r, req.g, req.b));
62
        if (req.width != 0)
63
        {
64
            pen.SetWidth(req.width);
65
        }
66

    
67
        pen = pen;
68
        return true;
69
    }
70

    
71
    // Scale to linesensor value
72
    unsigned int Emitter::rgb_to_grey(unsigned char r,
73
                                    unsigned char g,
74
                                    unsigned char b)
75
    {
76
        // Should be 0 to 255
77
        unsigned int grey = ((unsigned int) r + (unsigned int) g + (unsigned int) b) / 3;
78

    
79
        /// @todo Convert to the proper range
80
        return 255 - grey;
81
    }
82

    
83
    /// Sends back the position of this emitter so scoutsim can save
84
    /// the world state
85
    /// @todo remove dt param
86
    geometry_msgs::Pose2D Emitter::update(double dt, wxMemoryDC& path_dc,
87
                                        const wxImage& path_image,
88
                                        const wxImage& lines_image,
89
                                        const wxImage& walls_image,
90
                                        wxColour background_color,
91
                                        world_state state)
92
    {
93

    
94
        pos.x = min(max(pos.x, 0.0f), state.canvas_width);
95
        pos.y = min(max(pos.y, 0.0f), state.canvas_height);
96

    
97
        //int canvas_x = pos.x * PIX_PER_METER;
98
        //int canvas_y = pos.y * PIX_PER_METER;
99

    
100

    
101
        {
102
            wxImage rotated_image =
103
                emitter_image.Rotate(orient - PI/2.0,
104
                                   wxPoint(emitter_image.GetWidth() / 2,
105
                                           emitter_image.GetHeight() / 2),
106
                                   false);
107

    
108
            for (int y = 0; y < rotated_image.GetHeight(); ++y)
109
            {
110
                for (int x = 0; x < rotated_image.GetWidth(); ++x)
111
                {
112
                    if (rotated_image.GetRed(x, y) == 255
113
                            && rotated_image.GetBlue(x, y) == 255
114
                            && rotated_image.GetGreen(x, y) == 255)
115
                    {
116
                        rotated_image.SetAlpha(x, y, 0);
117
                    }
118
                }
119
            }
120

    
121
            emitter = wxBitmap(rotated_image);
122
        }
123

    
124
        if (emitter_visual_on) {
125
            path_dc.SelectObject(*path_bitmap);
126
            path_dc.SetBrush(*wxGREEN_BRUSH);
127
            path_dc.DrawLine(
128
                pos.x * PIX_PER_METER,
129
                pos.y * PIX_PER_METER,
130
                (pos.x+cos(orient-BOM_EM_APERTURE/2)*
131
                    BOM_EM_DISTANCE)*PIX_PER_METER,
132
                (pos.y-sin(orient-BOM_EM_APERTURE/2)*
133
                    BOM_EM_DISTANCE)*PIX_PER_METER);
134
            path_dc.DrawLine(
135
                pos.x * PIX_PER_METER,
136
                pos.y * PIX_PER_METER,
137
                (pos.x+cos(orient+BOM_EM_APERTURE/2)*
138
                    BOM_EM_DISTANCE)*PIX_PER_METER,
139
                (pos.y-sin(orient+BOM_EM_APERTURE/2)*
140
                    BOM_EM_DISTANCE)*PIX_PER_METER);
141

    
142

    
143
            path_dc.DrawCircle(
144
                wxPoint((pos.x+cos(orient-BOM_EM_APERTURE/2)*
145
                            BOM_EM_DISTANCE)*PIX_PER_METER,
146
                        (pos.y-sin(orient-BOM_EM_APERTURE/2)*
147
                            BOM_EM_DISTANCE)*PIX_PER_METER)
148
                ,2);
149
            path_dc.DrawCircle(
150
                wxPoint((pos.x+cos(orient+BOM_EM_APERTURE/2)*
151
                            BOM_EM_DISTANCE)*PIX_PER_METER,
152
                        (pos.y-sin(orient+BOM_EM_APERTURE/2)*
153
                            BOM_EM_DISTANCE)*PIX_PER_METER)
154
              ,2);
155
        }
156

    
157
        geometry_msgs::Pose2D my_pose;
158
        my_pose.x = pos.x;
159
        my_pose.y = pos.y;
160
        my_pose.theta = orient;
161

    
162
        return my_pose;
163
    }
164

    
165
    geometry_msgs::Pose2D Emitter::get_pos()
166
    {
167

    
168
        geometry_msgs::Pose2D my_pose;
169
        my_pose.x = pos.x;
170
        my_pose.y = pos.y;
171
        my_pose.theta = orient;
172

    
173
        return my_pose;
174
    }
175

    
176
    void Emitter::paint(wxDC& dc)
177
    {
178
        wxSize emitter_size = wxSize(emitter.GetWidth(), emitter.GetHeight());
179
        dc.DrawBitmap(emitter, pos.x * PIX_PER_METER - (emitter_size.GetWidth() / 2),
180
                      pos.y * PIX_PER_METER - (emitter_size.GetHeight() / 2), true);
181
    }
182

    
183
    void Emitter::set_emitter_visual(bool on)
184
    {
185
        emitter_visual_on = on;
186
    }
187
}
188

    
189
/** @} */