scoutos / scout / scoutsim / src / emitter.cpp @ cda6b590
History | View | Annotate | Download (5.77 KB)
1 | 2d21076d | Hui Jun Tay | /**
|
---|---|---|---|
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 | 18800d29 | Yuyang (Misty) Guo | , emitter_visual_on(true)
|
39 | 2d21076d | Hui Jun Tay | , 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 | 18800d29 | Yuyang (Misty) Guo | 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 | cda6b590 | Yuyang (Misty) Guo | (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 | 18800d29 | Yuyang (Misty) Guo | path_dc.DrawLine( |
135 | pos.x * PIX_PER_METER, |
||
136 | pos.y * PIX_PER_METER, |
||
137 | cda6b590 | Yuyang (Misty) Guo | (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 | 18800d29 | Yuyang (Misty) Guo | |
142 | |||
143 | path_dc.DrawCircle( |
||
144 | cda6b590 | Yuyang (Misty) Guo | 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 | 18800d29 | Yuyang (Misty) Guo | ,2);
|
149 | path_dc.DrawCircle( |
||
150 | cda6b590 | Yuyang (Misty) Guo | 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 | 18800d29 | Yuyang (Misty) Guo | ,2);
|
155 | } |
||
156 | |||
157 | 2d21076d | Hui Jun Tay | 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 | /** @} */ |