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