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