root / scout / scoutsim / src / emitter.cpp @ 1e84c699
History | View | Annotate | Download (5.53 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_APERTURE/2)*BOM_DISTANCE)*PIX_PER_METER,
|
131 |
(pos.y-sin(orient-BOM_APERTURE/2)*BOM_DISTANCE)*PIX_PER_METER);
|
132 |
path_dc.DrawLine( |
133 |
pos.x * PIX_PER_METER, |
134 |
pos.y * PIX_PER_METER, |
135 |
(pos.x+cos(orient+BOM_APERTURE/2)*BOM_DISTANCE)*PIX_PER_METER,
|
136 |
(pos.y-sin(orient+BOM_APERTURE/2)*BOM_DISTANCE)*PIX_PER_METER);
|
137 |
|
138 |
|
139 |
path_dc.DrawCircle( |
140 |
wxPoint((pos.x+cos(orient-BOM_APERTURE/2)*BOM_DISTANCE)*PIX_PER_METER,
|
141 |
(pos.y-sin(orient-BOM_APERTURE/2)*BOM_DISTANCE)*PIX_PER_METER)
|
142 |
,2);
|
143 |
path_dc.DrawCircle( |
144 |
wxPoint((pos.x+cos(orient+BOM_APERTURE/2)*BOM_DISTANCE)*PIX_PER_METER,
|
145 |
(pos.y-sin(orient+BOM_APERTURE/2)*BOM_DISTANCE)*PIX_PER_METER)
|
146 |
,2);
|
147 |
} |
148 |
|
149 |
geometry_msgs::Pose2D my_pose; |
150 |
my_pose.x = pos.x; |
151 |
my_pose.y = pos.y; |
152 |
my_pose.theta = orient; |
153 |
|
154 |
return my_pose;
|
155 |
} |
156 |
|
157 |
geometry_msgs::Pose2D Emitter::get_pos() |
158 |
{ |
159 |
|
160 |
geometry_msgs::Pose2D my_pose; |
161 |
my_pose.x = pos.x; |
162 |
my_pose.y = pos.y; |
163 |
my_pose.theta = orient; |
164 |
|
165 |
return my_pose;
|
166 |
} |
167 |
|
168 |
void Emitter::paint(wxDC& dc)
|
169 |
{ |
170 |
wxSize emitter_size = wxSize(emitter.GetWidth(), emitter.GetHeight()); |
171 |
dc.DrawBitmap(emitter, pos.x * PIX_PER_METER - (emitter_size.GetWidth() / 2),
|
172 |
pos.y * PIX_PER_METER - (emitter_size.GetHeight() / 2), true); |
173 |
} |
174 |
|
175 |
void Emitter::set_emitter_visual(bool on) |
176 |
{ |
177 |
emitter_visual_on = on; |
178 |
} |
179 |
} |
180 |
|
181 |
/** @} */
|