Revision 18800d29
added BOM emitter crappy (but working) visualization
| scout/scoutsim/src/emitter.cpp | ||
|---|---|---|
| 35 | 35 |
, aperture(aperture) |
| 36 | 36 |
, distance(distance) |
| 37 | 37 |
, pen_on(true) |
| 38 |
, emitter_visual_on(true) |
|
| 38 | 39 |
, pen(wxColour(DEFAULT_PEN_R, DEFAULT_PEN_G, DEFAULT_PEN_B)) |
| 39 | 40 |
{
|
| 40 | 41 |
pen.SetWidth(3); |
| ... | ... | |
| 120 | 121 |
emitter = wxBitmap(rotated_image); |
| 121 | 122 |
} |
| 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)*BOM_DISTANCE)*PIX_PER_METER, |
|
| 131 |
(pos.y-sin(orient-BOM_APERTURE)*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)*BOM_DISTANCE)*PIX_PER_METER, |
|
| 136 |
(pos.y-sin(orient+BOM_APERTURE)*BOM_DISTANCE)*PIX_PER_METER); |
|
| 137 |
|
|
| 138 |
|
|
| 139 |
path_dc.DrawCircle( |
|
| 140 |
wxPoint((pos.x+cos(orient-BOM_APERTURE)*BOM_DISTANCE)*PIX_PER_METER, |
|
| 141 |
(pos.y-sin(orient-BOM_APERTURE)*BOM_DISTANCE)*PIX_PER_METER) |
|
| 142 |
,2); |
|
| 143 |
path_dc.DrawCircle( |
|
| 144 |
wxPoint((pos.x+cos(orient+BOM_APERTURE)*BOM_DISTANCE)*PIX_PER_METER, |
|
| 145 |
(pos.y-sin(orient+BOM_APERTURE)*BOM_DISTANCE)*PIX_PER_METER) |
|
| 146 |
,2); |
|
| 147 |
} |
|
| 148 |
|
|
| 123 | 149 |
geometry_msgs::Pose2D my_pose; |
| 124 | 150 |
my_pose.x = pos.x; |
| 125 | 151 |
my_pose.y = pos.y; |
| scout/scoutsim/src/emitter.h | ||
|---|---|---|
| 24 | 24 |
// Distance, pixels, from center of robot to the linesensors. |
| 25 | 25 |
#define LNSNSR_D 20 |
| 26 | 26 |
|
| 27 |
#define BOM_APERTURE (PI / 3.0) |
|
| 28 |
#define BOM_DISTANCE 1.0 |
|
| 29 |
|
|
| 27 | 30 |
namespace scoutsim |
| 28 | 31 |
{
|
| 29 | 32 |
|
| scout/scoutsim/src/sim_frame.cpp | ||
|---|---|---|
| 718 | 718 |
M_Emitter::iterator m_it = emitters.begin(); |
| 719 | 719 |
M_Emitter::iterator m_end = emitters.end(); |
| 720 | 720 |
|
| 721 |
//Emitter default values |
|
| 722 |
double bom_aperture = PI / 3.0; |
|
| 723 |
double bom_distance = 1.0; |
|
| 724 |
|
|
| 725 | 721 |
//iterate over Emitters: |
| 726 | 722 |
for (; m_it != m_end; ++m_it) |
| 727 | 723 |
{
|
| ... | ... | |
| 784 | 780 |
bom_pos.theta += 2*PI; |
| 785 | 781 |
} |
| 786 | 782 |
|
| 787 |
if(is_inrange(emitter_pos, bom_pos, bom_aperture, bom_distance)) {
|
|
| 783 |
if(is_inrange(emitter_pos, bom_pos, BOM_APERTURE, BOM_DISTANCE)) {
|
|
| 788 | 784 |
string em_id_str = m_it->first.substr(m_it->first.find("_")+1);
|
| 789 | 785 |
int emitter_id = atoi(em_id_str.c_str()); |
| 790 | 786 |
it->second->update_BOM(i, 1, emitter_id); |
Also available in: Unified diff