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