Revision 7b94db2c
Fixed bug with BOM sensors reading from emitters (edge detection)
Added basic starter code for ParkScout behavior
scout/scoutsim/src/sim_frame.cpp  

713  713  
714  714 
void SimFrame::checkBOM(geometry_msgs::Pose2D scout_pos, 
715  715 
M_Scout::iterator it) 
716 
{


716 
{ 

717  717  
718 
M_Emitter::iterator m_it = emitters.begin(); 

719 
M_Emitter::iterator m_end = emitters.end(); 

718 
geometry_msgs::Pose2D bom_pos; 

719 
double x_offset; 

720 
double y_offset; 

721 
double offset_angle; 

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

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

724 
int emitter_id; 

720  725  
721 
//iterate over Emitters: 

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

723 
{ 

724 
geometry_msgs::Pose2D emitter_pos; 

725 
emitter_pos = m_it>second>get_pos(); 

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

727 
//if scout is within emitter range 

728 
//check if emitter is within each BOM range 

729 
double x_offset; 

730 
double y_offset; 

731 
double offset_angle; 

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

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

727 
emitter_id = 1; 

728 


729 
diag_dis = pow((pow((SCOUT_W/2.0),2) + 

733  730 
pow((SCOUT_H/2.0),2)),(0.5)); 
734  731  
735 
for (int i = 0; i<10; i++) { 

736 
geometry_msgs::Pose2D bom_pos; 

737 


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

739 
offset_angle = PI/2.0; 

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

741 
} 

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

743 
offset_angle = 3*PI/2.0; 

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

745 
} 

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

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

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

749 
} 

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

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

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

753 
} 

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

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

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

757 
} 

758 
else { //top right 

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

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

761 
} 

762 
offset_angle += scout_pos.theta; 

763 


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

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

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

767 
} 

768 
else { 

769 
x_offset = diag_dis*cos(offset_angle); 

770 
y_offset = diag_dis*sin(offset_angle); 

771 
} 

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

733 
offset_angle = PI/2.0; 

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

735 
} 

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

737 
offset_angle = 3*PI/2.0; 

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

739 
} 

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

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

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

743 
} 

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

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

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

747 
} 

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

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

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

751 
} 

752 
else { //top right 

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

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

755 
} 

756 
offset_angle += scout_pos.theta; 

757 


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

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

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

761 
} 

762 
else { 

763 
x_offset = diag_dis*cos(offset_angle); 

764 
y_offset = diag_dis*sin(offset_angle); 

765 
} 

772  766  
773 
bom_pos.x = scout_pos.x + x_offset;


774 
bom_pos.y = scout_pos.y  y_offset;


767 
bom_pos.x = scout_pos.x + x_offset; 

768 
bom_pos.y = scout_pos.y  y_offset; 

775  769  
776 
if (bom_pos.theta > 2*PI) {


777 
bom_pos.theta = 2*PI;


778 
}


779 
else if (bom_pos.theta < 0) {


780 
bom_pos.theta += 2*PI;


781 
}


770 
if (bom_pos.theta > 2*PI) { 

771 
bom_pos.theta = 2*PI; 

772 
} 

773 
else if (bom_pos.theta < 0) { 

774 
bom_pos.theta += 2*PI; 

775 
} 

782  776  
783 
if(is_inrange(emitter_pos, bom_pos, BOM_APERTURE, BOM_DISTANCE)) { 

784 
string em_id_str = m_it>first.substr(m_it>first.find("_")+1); 

785 
int emitter_id = atoi(em_id_str.c_str()); 

786 
it>second>update_BOM(i, 1, emitter_id); 

787 
} 

788 
else { 

789 
it>second>update_BOM(i, 0, 0); 

790 
} 

791 
} 

792 
} else { 

793 
// nothing in the emitter range, set everything to 0 

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

795 
it>second>update_BOM(i, 0, 0); 

796 
} 

777 
emitter_id = getActiveEmitter(bom_pos); 

778  
779 
if (emitter_id >= 0) { 

780 
it>second>update_BOM(i, 1, emitter_id); 

797  781 
} 
782 
else { 

783 
it>second>update_BOM(i, 0, 1); 

784 
} 

785  
798  786 
} 
799  787  
788  
789 
} 

790  
791 
int SimFrame::getActiveEmitter(geometry_msgs::Pose2D bom_pos) { 

792  
793 
M_Emitter::iterator m_it = emitters.begin(); 

794 
M_Emitter::iterator m_end = emitters.end(); 

795 
int emitter_id = 1; 

796 
for (; m_it != m_end; ++m_it) { 

797 
geometry_msgs::Pose2D emitter_pos; 

798 
emitter_pos = m_it>second>get_pos(); 

799  
800 
if(is_inrange(bom_pos, emitter_pos, em_aperture, em_distance) && 

801 
is_inrange(emitter_pos, bom_pos, BOM_APERTURE, BOM_DISTANCE)) { 

802 
string em_id_str = m_it>first.substr(m_it>first.find("_")+1); 

803 
emitter_id = atoi(em_id_str.c_str()); 

804 
return emitter_id; 

805 
} 

806 
} 

807 
return 1; 

800  808 
} 
801  809  
802  810 
Also available in: Unified diff