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