Revision 71ae6e3f
Basic sim framework for multiple BOMs written
scout/scoutsim/src/sim_frame.cpp | ||
---|---|---|
673 | 673 |
state.canvas_width = width_in_meters; |
674 | 674 |
state.canvas_height = height_in_meters; |
675 | 675 |
|
676 |
|
|
676 | 677 |
for (; m_it != m_end; ++m_it) |
677 | 678 |
{ |
678 | 679 |
|
... | ... | |
695 | 696 |
sonar_dc.GetBackground().GetColour(), |
696 | 697 |
state); |
697 | 698 |
|
698 |
//iterate over Emitters: |
|
699 |
for (m_it = emitters.begin(); m_it != m_end; ++m_it) |
|
700 |
{ |
|
701 |
geometry_msgs::Pose2D emitter_pos; |
|
702 |
emitter_pos = m_it->second->get_pos(); |
|
703 |
readBOM(scout_pos, emitter_pos,it); |
|
704 |
} |
|
705 |
|
|
699 |
checkBOM(scout_pos, it); |
|
706 | 700 |
} |
707 | 701 |
|
708 | 702 |
|
... | ... | |
717 | 711 |
} |
718 | 712 |
|
719 | 713 |
|
714 |
void SimFrame::checkBOM(geometry_msgs::Pose2D scout_pos, |
|
715 |
M_Scout::iterator it) |
|
716 |
{ |
|
717 |
|
|
718 |
M_Emitter::iterator m_it = emitters.begin(); |
|
719 |
M_Emitter::iterator m_end = emitters.end(); |
|
720 |
|
|
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)) { //if scout is within emitter range |
|
727 |
//check if emitter is within each BOM range |
|
728 |
|
|
729 |
for (int i = 0; i<10; i++) { |
|
730 |
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); |
|
734 |
|
|
735 |
if (bom_pos.theta > 2*PI) { |
|
736 |
bom_pos.theta -= 2*PI; |
|
737 |
} |
|
738 |
else if (bom_pos.theta < 0) { |
|
739 |
bom_pos.theta += 2*PI; |
|
740 |
} |
|
741 |
|
|
742 |
if(is_inrange(emitter_pos, bom_pos)) { |
|
743 |
//ROS_INFO("BOM%d %f: true", i, bom_pos.theta); |
|
744 |
|
|
745 |
it->second->update_BOM(i, true); |
|
746 |
} |
|
747 |
else { |
|
748 |
//ROS_INFO("BOM%d %f: false", i, bom_pos.theta); |
|
749 |
|
|
750 |
it->second->update_BOM(i, false); |
|
751 |
} |
|
752 |
} |
|
753 |
|
|
754 |
} |
|
755 |
} |
|
756 |
|
|
757 |
} |
|
758 |
|
|
759 |
|
|
720 | 760 |
bool SimFrame::clearCallback(std_srvs::Empty::Request&, |
721 | 761 |
std_srvs::Empty::Response&) |
722 | 762 |
{ |
... | ... | |
742 | 782 |
} |
743 | 783 |
|
744 | 784 |
|
745 |
void SimFrame::readBOM(geometry_msgs::Pose2D scout_pos,
|
|
746 |
geometry_msgs::Pose2D emitter_pos,
|
|
747 |
M_Scout::iterator it) |
|
785 |
bool SimFrame::is_inrange(geometry_msgs::Pose2D scout_pos,
|
|
786 |
geometry_msgs::Pose2D emitter_pos)
|
|
787 |
|
|
748 | 788 |
{ |
749 | 789 |
//ROS_INFO("SCOUT: %f %f %f ", scout_pos.x, scout_pos.x, scout_pos.theta); |
750 | 790 |
//ROS_INFO("EMITTER: %f %f %f ", emitter_pos.x, emitter_pos.x, emitter_pos.theta); |
... | ... | |
773 | 813 |
if (upper_limit > 2*PI) { |
774 | 814 |
if (((se_angle < 2*PI) && (se_angle > (lower_limit))) || |
775 | 815 |
((se_angle < upper_limit-2*PI) && (se_angle >= 0))) { |
776 |
it->second->pub_BOM(true);
|
|
816 |
return true;
|
|
777 | 817 |
} |
778 | 818 |
|
779 | 819 |
} |
780 | 820 |
else if (lower_limit < 0) { |
781 | 821 |
if (((se_angle < 2*PI) && (se_angle > (lower_limit+2*PI))) || |
782 | 822 |
((se_angle < upper_limit) && (se_angle >= 0))) { |
783 |
it->second->pub_BOM(true);
|
|
823 |
return true;
|
|
784 | 824 |
} |
785 | 825 |
|
786 | 826 |
|
... | ... | |
788 | 828 |
else { |
789 | 829 |
if ((se_angle < upper_limit) && //angle check |
790 | 830 |
(se_angle > lower_limit)) { |
791 |
it->second->pub_BOM(true);
|
|
831 |
return true;
|
|
792 | 832 |
} |
793 | 833 |
} |
794 | 834 |
} |
835 |
return false; |
|
795 | 836 |
|
796 | 837 |
} |
797 | 838 |
|
Also available in: Unified diff