Revision 98ed4757
Basic BOM works (only 1 BOM in 'centre' of scout)
Added BOM topic to scouts
Added 'kill' for emitters
scout/scoutsim/src/sim_frame.cpp | ||
---|---|---|
137 | 137 |
&SimFrame::wirelessCallback, this); |
138 | 138 |
|
139 | 139 |
//Emitter default values |
140 |
em_aperture = PI / 6.0; |
|
141 |
em_distance = 2; |
|
140 |
em_aperture = PI / 3.0; |
|
141 |
em_distance = 1; |
|
142 |
|
|
142 | 143 |
|
143 | 144 |
// Teleop |
144 | 145 |
teleop_type = TELEOP_OFF; |
... | ... | |
221 | 222 |
scoutsim::Kill::Response&) |
222 | 223 |
{ |
223 | 224 |
M_Scout::iterator it = scouts.find(req.name); |
225 |
M_Emitter::iterator m_it = emitters.find(req.name); |
|
224 | 226 |
if (it == scouts.end()) |
225 | 227 |
{ |
226 |
ROS_WARN("Tried to kill scout [%s], which does not exist", |
|
227 |
req.name.c_str()); |
|
228 |
return false; |
|
228 |
if (m_it == emitters.end()) { |
|
229 |
ROS_WARN("Tried to kill scout/emitter [%s], which does not exist", |
|
230 |
req.name.c_str()); |
|
231 |
return false; |
|
232 |
} |
|
233 |
emitters.erase(m_it); |
|
234 |
return true; |
|
229 | 235 |
} |
230 | 236 |
|
231 | 237 |
scouts.erase(it); |
... | ... | |
667 | 673 |
state.canvas_width = width_in_meters; |
668 | 674 |
state.canvas_height = height_in_meters; |
669 | 675 |
|
670 |
for (; it != end; ++it)
|
|
676 |
for (; m_it != m_end; ++m_it)
|
|
671 | 677 |
{ |
672 | 678 |
|
673 |
it->second->update(SIM_TIME_REFRESH_RATE, |
|
674 |
path_dc, sonar_dc,
|
|
679 |
m_it->second->update(SIM_TIME_REFRESH_RATE,
|
|
680 |
path_dc, |
|
675 | 681 |
path_image, lines_image, walls_image, |
676 | 682 |
path_dc.GetBackground().GetColour(), |
677 |
sonar_dc.GetBackground().GetColour(), |
|
678 | 683 |
state); |
679 | 684 |
} |
680 | 685 |
|
681 | 686 |
|
682 |
for (; m_it != m_end; ++m_it)
|
|
687 |
for (; it != end; ++it)
|
|
683 | 688 |
{ |
684 |
|
|
685 |
m_it->second->update(SIM_TIME_REFRESH_RATE, |
|
686 |
path_dc, |
|
689 |
//ROS_INFO(it->second); |
|
690 |
geometry_msgs::Pose2D scout_pos; |
|
691 |
scout_pos = it->second->update(SIM_TIME_REFRESH_RATE, |
|
692 |
path_dc, sonar_dc, |
|
687 | 693 |
path_image, lines_image, walls_image, |
688 | 694 |
path_dc.GetBackground().GetColour(), |
695 |
sonar_dc.GetBackground().GetColour(), |
|
689 | 696 |
state); |
697 |
|
|
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 |
|
|
690 | 706 |
} |
691 | 707 |
|
708 |
|
|
692 | 709 |
for (unsigned int i = 0; i < ghost_scouts.size(); ++i) |
693 | 710 |
{ |
694 | 711 |
ghost_scouts.at(i)->update(SIM_TIME_REFRESH_RATE, path_dc, sonar_dc, |
... | ... | |
725 | 742 |
} |
726 | 743 |
|
727 | 744 |
|
728 |
void SimFrame::readBOM() |
|
745 |
void SimFrame::readBOM(geometry_msgs::Pose2D scout_pos, |
|
746 |
geometry_msgs::Pose2D emitter_pos, |
|
747 |
M_Scout::iterator it) |
|
729 | 748 |
{ |
749 |
//ROS_INFO("SCOUT: %f %f %f ", scout_pos.x, scout_pos.x, scout_pos.theta); |
|
750 |
//ROS_INFO("EMITTER: %f %f %f ", emitter_pos.x, emitter_pos.x, emitter_pos.theta); |
|
751 |
double se_distance = pow((pow((scout_pos.x - emitter_pos.x),2) + |
|
752 |
pow((scout_pos.y - emitter_pos.y),2)),(0.5)); |
|
753 |
double se_angle = atan((scout_pos.x - emitter_pos.x)/(scout_pos.y - emitter_pos.y)); |
|
754 |
if (scout_pos.y < emitter_pos.y) |
|
755 |
{ |
|
756 |
se_angle = se_angle + PI/2; |
|
757 |
} |
|
758 |
else |
|
759 |
{ |
|
760 |
se_angle = se_angle + PI*3/2; |
|
761 |
} |
|
762 |
double emitter_orient = emitter_pos.theta; |
|
763 |
|
|
764 |
//ROS_INFO("distance: %f, angle: %f, orient: %f", se_distance, se_angle, emitter_orient); |
|
765 |
|
|
766 |
if ((se_distance <= em_distance)) { //distance check |
|
730 | 767 |
|
768 |
double upper_limit = (emitter_orient + em_aperture/2); |
|
769 |
double lower_limit = (emitter_orient - em_aperture/2); |
|
770 |
|
|
771 |
//ROS_INFO("upper: %f, lower: %f", upper_limit, lower_limit+2*PI); |
|
772 |
|
|
773 |
if (upper_limit > 2*PI) { |
|
774 |
if (((se_angle < 2*PI) && (se_angle > (lower_limit))) || |
|
775 |
((se_angle < upper_limit-2*PI) && (se_angle >= 0))) { |
|
776 |
it->second->pub_BOM(true); |
|
777 |
} |
|
778 |
|
|
779 |
} |
|
780 |
else if (lower_limit < 0) { |
|
781 |
if (((se_angle < 2*PI) && (se_angle > (lower_limit+2*PI))) || |
|
782 |
((se_angle < upper_limit) && (se_angle >= 0))) { |
|
783 |
it->second->pub_BOM(true); |
|
784 |
} |
|
785 |
|
|
786 |
|
|
787 |
} |
|
788 |
else { |
|
789 |
if ((se_angle < upper_limit) && //angle check |
|
790 |
(se_angle > lower_limit)) { |
|
791 |
it->second->pub_BOM(true); |
|
792 |
} |
|
793 |
} |
|
794 |
} |
|
795 |
|
|
731 | 796 |
} |
732 | 797 |
|
733 | 798 |
} |
Also available in: Unified diff