Revision 11bb78c2 scout/scoutsim/src/sim_frame.cpp
scout/scoutsim/src/sim_frame.cpp  

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 
Also available in: Unified diff