136 
136 
wireless_send = nh.subscribe("/wireless/send", 1000,

137 
137 
&SimFrame::wirelessCallback, this);

138 
138 

139 

//Emitter default values

140 

em_aperture = PI / 3.0;

141 

em_distance = 1;

142 


143 


144 
139 
// Teleop

145 
140 
teleop_type = TELEOP_OFF;

146 
141 
teleop_l_speed = 0;

147 
142 
teleop_r_speed = 0;

148 
143 
teleop_scoutname = "scout1";

149 
144 


145 
//Emitter default values


146 
em_aperture = PI / 3.0;


147 
em_distance = 1.0;


148 


149 

150 
150 
teleop_pub = nh.advertise< ::messages::set_motors>("/scout1/set_motors", 1000);

151 
151 

152 
152 
ROS_INFO("Starting scoutsim with node name %s",

...  ...  
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 

721 
725 
//iterate over Emitters:

722 
726 
for (; m_it != m_end; ++m_it)

723 
727 
{

724 
728 
geometry_msgs::Pose2D emitter_pos;

725 
729 
emitter_pos = m_it>second>get_pos();

726 

if (is_inrange(scout_pos, emitter_pos)) { //if scout is within emitter range


730 
if (is_inrange(scout_pos, emitter_pos, em_aperture, em_distance)) {


731 
//if scout is within emitter range

727 
732 
//check if emitter is within each BOM range

728 



733 
double x_offset;


734 
double y_offset;


735 
double offset_angle;


736 
double diag_dis = pow((pow((SCOUT_W/2.0),2) +


737 
pow((SCOUT_H/2.0),2)),(0.5));


738 

729 
739 
for (int i = 0; i<10; i++) {

730 
740 
geometry_msgs::Pose2D bom_pos;

731 

bom_pos.x = scout_pos.x;

732 

bom_pos.y = scout_pos.y;

733 

bom_pos.theta = (scout_pos.theta+(2*PI/10.0)*i);


741 


742 
if (i == 2) { //middle left


743 
offset_angle = PI/2.0;


744 
bom_pos.theta = scout_pos.theta + PI/2.0;


745 
}


746 
else if (i == 7) { //middle right


747 
offset_angle = 3*PI/2.0;


748 
bom_pos.theta = scout_pos.theta + 3.0*PI/2.0;


749 
}


750 
else if (i < 2) { //top left


751 
offset_angle = atan(abs((SCOUT_W/2.0)/(SCOUT_H/2.0)));


752 
bom_pos.theta = scout_pos.theta + PI/2.0 * i;


753 
}


754 
else if (i >= 2 && i <5) { //bottom left


755 
offset_angle = PI  atan(abs((SCOUT_W/2.0)/(SCOUT_H/2.0)));


756 
bom_pos.theta = scout_pos.theta + PI/2.0 + PI/2.0 * (i/4);


757 
}


758 
else if (i >= 5 && i < 8) { //bottom right


759 
offset_angle = PI + atan(abs((SCOUT_W/2.0)/(SCOUT_H/2.0)));


760 
bom_pos.theta = scout_pos.theta + PI + PI/2.0 * (i/6);


761 
}


762 
else { //top right


763 
offset_angle = 2*PI  atan(abs((SCOUT_W/2.0)/(SCOUT_H/2.0)));


764 
bom_pos.theta = scout_pos.theta + 3*PI/2.0 + PI/2.0 * (i/9);


765 
}


766 
offset_angle += scout_pos.theta;


767 


768 
if (i == 2  i == 7) {


769 
x_offset = (SCOUT_W/2.0)*cos(offset_angle);


770 
y_offset = (SCOUT_W/2.0)*sin(offset_angle);


771 
}


772 
else {


773 
x_offset = diag_dis*cos(offset_angle);


774 
y_offset = diag_dis*sin(offset_angle);


775 
}


776 


777 
bom_pos.x = scout_pos.x + x_offset;


778 
bom_pos.y = scout_pos.y  y_offset;

734 
779 

735 
780 
if (bom_pos.theta > 2*PI) {

736 
781 
bom_pos.theta = 2*PI;

...  ...  
739 
784 
bom_pos.theta += 2*PI;

740 
785 
}

741 
786 

742 

if(is_inrange(emitter_pos, bom_pos)) {

743 

//ROS_INFO("BOM%d %f: true", i, bom_pos.theta);

744 



787 
if(is_inrange(emitter_pos, bom_pos, bom_aperture, bom_distance)) {


788 

745 
789 
it>second>update_BOM(i, true);


790 

746 
791 
}

747 
792 
else {

748 

//ROS_INFO("BOM%d %f: false", i, bom_pos.theta);

749 



793 

750 
794 
it>second>update_BOM(i, false);


795 

751 
796 
}

752 
797 
}

753 
798 

...  ...  
783 
828 

784 
829 

785 
830 
bool SimFrame::is_inrange(geometry_msgs::Pose2D scout_pos,

786 

geometry_msgs::Pose2D emitter_pos)


831 
geometry_msgs::Pose2D emitter_pos,


832 
double aperture, double distance)

787 
833 

788 
834 
{

789 
835 
//ROS_INFO("SCOUT: %f %f %f ", scout_pos.x, scout_pos.x, scout_pos.theta);

...  ...  
803 
849 

804 
850 
//ROS_INFO("distance: %f, angle: %f, orient: %f", se_distance, se_angle, emitter_orient);

805 
851 

806 

if ((se_distance <= em_distance)) { //distance check


852 
if ((se_distance <= distance)) { //distance check

807 
853 

808 

double upper_limit = (emitter_orient + em_aperture/2);

809 

double lower_limit = (emitter_orient  em_aperture/2);


854 
double upper_limit = (emitter_orient + aperture/2);


855 
double lower_limit = (emitter_orient  aperture/2);

810 
856 

811 
857 
//ROS_INFO("upper: %f, lower: %f", upper_limit, lower_limit+2*PI);

812 
858 
