Revision 7b94db2c
Fixed bug with BOM sensors reading from emitters (edge detection)
Added basic starter code for ParkScout behavior
scout/libscout/src/test_behaviors/ParkScout.cpp | ||
---|---|---|
1 |
/** |
|
2 |
* Copyright (c) 2011 Colony Project |
|
3 |
* |
|
4 |
* Permission is hereby granted, free of charge, to any person |
|
5 |
* obtaining a copy of this software and associated documentation |
|
6 |
* files (the "Software"), to deal in the Software without |
|
7 |
* restriction, including without limitation the rights to use, |
|
8 |
* copy, modify, merge, publish, distribute, sublicense, and/or sell |
|
9 |
* copies of the Software, and to permit persons to whom the |
|
10 |
* Software is furnished to do so, subject to the following |
|
11 |
* conditions: |
|
12 |
* |
|
13 |
* The above copyright notice and this permission notice shall be |
|
14 |
* included in all copies or substantial portions of the Software. |
|
15 |
* |
|
16 |
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, |
|
17 |
* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES |
|
18 |
* OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND |
|
19 |
* NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT |
|
20 |
* HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, |
|
21 |
* WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING |
|
22 |
* FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR |
|
23 |
* OTHER DEALINGS IN THE SOFTWARE. |
|
24 |
*/ |
|
25 |
|
|
26 |
#include "ParkScout.h" |
|
27 |
#define LEFT_EMITTER_ID 110 |
|
28 |
#define RIGHT_EMITTER_ID 111 |
|
29 |
|
|
30 |
|
|
31 |
#define LEFT 0 |
|
32 |
#define RIGHT 1 |
|
33 |
|
|
34 |
using namespace std; |
|
35 |
|
|
36 |
ParkScout::ParkScout(string scoutname, Sensors* sensors) |
|
37 |
: Behavior(scoutname, "Park Scout", sensors) { |
|
38 |
name = scoutname; |
|
39 |
scout_pos = new pos; |
|
40 |
} |
|
41 |
|
|
42 |
void ParkScout::run() { |
|
43 |
|
|
44 |
/* update_readings(); |
|
45 |
start_side = get_side(readings); |
|
46 |
|
|
47 |
while ((start_side == LEFT && checkBOM(3, RIGHT_EMITTER_ID)) || |
|
48 |
(start_side == RIGHT && checkBOM(6, RIGHT_EMITTER_ID))) { |
|
49 |
motors->set_sides(100, 100, MOTOR_PERCENT); |
|
50 |
bomR = bom->query(); |
|
51 |
update_readings(); |
|
52 |
} |
|
53 |
|
|
54 |
motors->set_sides(0, 0, MOTOR_PERCENT); |
|
55 |
|
|
56 |
|
|
57 |
while(ok) { |
|
58 |
reverse_park(); |
|
59 |
} */ |
|
60 |
|
|
61 |
|
|
62 |
} |
|
63 |
/* |
|
64 |
void reverse_park() { |
|
65 |
if(checkBOM(5, LEFT_EMITTER_ID) || |
|
66 |
checkBOM(4, RIGHT_EMITTER_ID)) { //move forward |
|
67 |
motors->set_sides(10, 10, MOTOR_PERCENT); |
|
68 |
} |
|
69 |
else if(checkBOM(4, LEFT_EMITTER_ID) && |
|
70 |
checkBOM(5, RIGHT_EMITTER_ID)) { |
|
71 |
motors->set_sides(-20, -20, MOTOR_PERCENT); |
|
72 |
} |
|
73 |
else { |
|
74 |
if (start_side == RIGHT) { //reverseright |
|
75 |
motors->set_sides(100, 20, MOTOR_PERCENT); |
|
76 |
} |
|
77 |
else { //reverseleft |
|
78 |
motors->set_sides(20, 100, MOTOR_PERCENT); |
|
79 |
|
|
80 |
} |
|
81 |
} |
|
82 |
|
|
83 |
} |
|
84 |
|
|
85 |
int get_side() { |
|
86 |
|
|
87 |
return RIGHT; |
|
88 |
} |
|
89 |
|
|
90 |
bool checkBOM(int bNum, int ID) { |
|
91 |
if(readings[bNum] == true) { |
|
92 |
ROS_INFO("Emitter ID: %d", senders[bNum]); |
|
93 |
return senders[bNum] == ID; |
|
94 |
} |
|
95 |
return false; |
|
96 |
|
|
97 |
} |
|
98 |
|
|
99 |
void update_readings() { |
|
100 |
BomReadings bomR = bom->query(); |
|
101 |
readings = bomR.readings; |
|
102 |
senders = bomR.senders; |
|
103 |
|
|
104 |
}*/ |
scout/libscout/src/test_behaviors/ParkScout.h | ||
---|---|---|
1 |
/** |
|
2 |
* Copyright (c) 2011 Colony Project |
|
3 |
* |
|
4 |
* Permission is hereby granted, free of charge, to any person |
|
5 |
* obtaining a copy of this software and associated documentation |
|
6 |
* files (the "Software"), to deal in the Software without |
|
7 |
* restriction, including without limitation the rights to use, |
|
8 |
* copy, modify, merge, publish, distribute, sublicense, and/or sell |
|
9 |
* copies of the Software, and to permit persons to whom the |
|
10 |
* Software is furnished to do so, subject to the following |
|
11 |
* conditions: |
|
12 |
* |
|
13 |
* The above copyright notice and this permission notice shall be |
|
14 |
* included in all copies or substantial portions of the Software. |
|
15 |
* |
|
16 |
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, |
|
17 |
* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES |
|
18 |
* OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND |
|
19 |
* NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT |
|
20 |
* HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, |
|
21 |
* WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING |
|
22 |
* FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR |
|
23 |
* OTHER DEALINGS IN THE SOFTWARE. |
|
24 |
*/ |
|
25 |
|
|
26 |
#ifndef _PARK_SCOUT_H_ |
|
27 |
#define _PARK_SCOUT_H_ |
|
28 |
#include "../Behavior.h" |
|
29 |
#include "../Sensors.h" |
|
30 |
#include "../helper_classes/ScoutPosition.h" |
|
31 |
/* Details about map: |
|
32 |
* 1 meter = 200 pixels |
|
33 |
* 1 block is a 13x13 pixel section |
|
34 |
* actual meter x meter map has 15 blocks in one row/column |
|
35 |
* # blocks per robot map row = 2*(block per map's row)-1 |
|
36 |
*/ |
|
37 |
|
|
38 |
// number of blocks in one row/column of robot's map |
|
39 |
#define MAP_LENGTH 29 |
|
40 |
|
|
41 |
|
|
42 |
class ParkScout : public Behavior |
|
43 |
{ |
|
44 |
public: |
|
45 |
ParkScout(std::string scoutname, Sensors* sensors); |
|
46 |
void run(); |
|
47 |
private: |
|
48 |
std::string name; |
|
49 |
pos* scout_pos; |
|
50 |
/*int start_side; |
|
51 |
std::vector<uint32_t> readings; |
|
52 |
std::vector<uint32_t> senders; |
|
53 |
|
|
54 |
void reverse_park(); |
|
55 |
int get_side(); |
|
56 |
bool checkBOM(int bNum, int ID); |
|
57 |
void update_readings(); */ |
|
58 |
|
|
59 |
}; |
|
60 |
#endif |
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 |
|
scout/scoutsim/src/sim_frame.h | ||
---|---|---|
157 | 157 |
double aperture, double distance); |
158 | 158 |
void checkBOM(geometry_msgs::Pose2D scout_pos, |
159 | 159 |
M_Scout::iterator it); |
160 |
// std::map<std::string, EmitterPtr>::iterator m_it, |
|
161 |
// std::map<std::string, EmitterPtr>::iterator m_end); |
|
162 |
|
|
160 |
int getActiveEmitter(geometry_msgs::Pose2D bom_pos); |
|
161 |
|
|
163 | 162 |
|
164 | 163 |
ros::Subscriber wireless_send; |
165 | 164 |
ros::Publisher wireless_receive; |
Also available in: Unified diff