Revision 11bb78c2
BOM now works with rect. scout shape
scout/scoutsim/src/sim_frame.cpp | ||
---|---|---|
136 | 136 |
wireless_send = nh.subscribe("/wireless/send", 1000, |
137 | 137 |
&SimFrame::wirelessCallback, this); |
138 | 138 |
|
139 |
//Emitter default values |
|
140 |
em_aperture = PI / 3.0; |
|
141 |
em_distance = 1; |
|
142 |
|
|
143 |
|
|
144 | 139 |
// Teleop |
145 | 140 |
teleop_type = TELEOP_OFF; |
146 | 141 |
teleop_l_speed = 0; |
147 | 142 |
teleop_r_speed = 0; |
148 | 143 |
teleop_scoutname = "scout1"; |
149 | 144 |
|
145 |
//Emitter default values |
|
146 |
em_aperture = PI / 3.0; |
|
147 |
em_distance = 1.0; |
|
148 |
|
|
149 |
|
|
150 | 150 |
teleop_pub = nh.advertise< ::messages::set_motors>("/scout1/set_motors", 1000); |
151 | 151 |
|
152 | 152 |
ROS_INFO("Starting scoutsim with node name %s", |
... | ... | |
718 | 718 |
M_Emitter::iterator m_it = emitters.begin(); |
719 | 719 |
M_Emitter::iterator m_end = emitters.end(); |
720 | 720 |
|
721 |
//Emitter default values |
|
722 |
double bom_aperture = PI / 3.0; |
|
723 |
double bom_distance = 1.0; |
|
724 |
|
|
721 | 725 |
//iterate over Emitters: |
722 | 726 |
for (; m_it != m_end; ++m_it) |
723 | 727 |
{ |
724 | 728 |
geometry_msgs::Pose2D emitter_pos; |
725 | 729 |
emitter_pos = m_it->second->get_pos(); |
726 |
if (is_inrange(scout_pos, emitter_pos)) { //if scout is within emitter range |
|
730 |
if (is_inrange(scout_pos, emitter_pos, em_aperture, em_distance)) { |
|
731 |
//if scout is within emitter range |
|
727 | 732 |
//check if emitter is within each BOM range |
728 |
|
|
733 |
double x_offset; |
|
734 |
double y_offset; |
|
735 |
double offset_angle; |
|
736 |
double diag_dis = pow((pow((SCOUT_W/2.0),2) + |
|
737 |
pow((SCOUT_H/2.0),2)),(0.5)); |
|
738 |
|
|
729 | 739 |
for (int i = 0; i<10; i++) { |
730 | 740 |
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); |
|
741 |
|
|
742 |
if (i == 2) { //middle left |
|
743 |
offset_angle = PI/2.0; |
|
744 |
bom_pos.theta = scout_pos.theta + PI/2.0; |
|
745 |
} |
|
746 |
else if (i == 7) { //middle right |
|
747 |
offset_angle = 3*PI/2.0; |
|
748 |
bom_pos.theta = scout_pos.theta + 3.0*PI/2.0; |
|
749 |
} |
|
750 |
else if (i < 2) { //top left |
|
751 |
offset_angle = atan(abs((SCOUT_W/2.0)/(SCOUT_H/2.0))); |
|
752 |
bom_pos.theta = scout_pos.theta + PI/2.0 * i; |
|
753 |
} |
|
754 |
else if (i >= 2 && i <5) { //bottom left |
|
755 |
offset_angle = PI - atan(abs((SCOUT_W/2.0)/(SCOUT_H/2.0))); |
|
756 |
bom_pos.theta = scout_pos.theta + PI/2.0 + PI/2.0 * (i/4); |
|
757 |
} |
|
758 |
else if (i >= 5 && i < 8) { //bottom right |
|
759 |
offset_angle = PI + atan(abs((SCOUT_W/2.0)/(SCOUT_H/2.0))); |
|
760 |
bom_pos.theta = scout_pos.theta + PI + PI/2.0 * (i/6); |
|
761 |
} |
|
762 |
else { //top right |
|
763 |
offset_angle = 2*PI - atan(abs((SCOUT_W/2.0)/(SCOUT_H/2.0))); |
|
764 |
bom_pos.theta = scout_pos.theta + 3*PI/2.0 + PI/2.0 * (i/9); |
|
765 |
} |
|
766 |
offset_angle += scout_pos.theta; |
|
767 |
|
|
768 |
if (i == 2 || i == 7) { |
|
769 |
x_offset = (SCOUT_W/2.0)*cos(offset_angle); |
|
770 |
y_offset = (SCOUT_W/2.0)*sin(offset_angle); |
|
771 |
} |
|
772 |
else { |
|
773 |
x_offset = diag_dis*cos(offset_angle); |
|
774 |
y_offset = diag_dis*sin(offset_angle); |
|
775 |
} |
|
776 |
|
|
777 |
bom_pos.x = scout_pos.x + x_offset; |
|
778 |
bom_pos.y = scout_pos.y - y_offset; |
|
734 | 779 |
|
735 | 780 |
if (bom_pos.theta > 2*PI) { |
736 | 781 |
bom_pos.theta -= 2*PI; |
... | ... | |
739 | 784 |
bom_pos.theta += 2*PI; |
740 | 785 |
} |
741 | 786 |
|
742 |
if(is_inrange(emitter_pos, bom_pos)) { |
|
743 |
//ROS_INFO("BOM%d %f: true", i, bom_pos.theta); |
|
744 |
|
|
787 |
if(is_inrange(emitter_pos, bom_pos, bom_aperture, bom_distance)) { |
|
788 |
|
|
745 | 789 |
it->second->update_BOM(i, true); |
790 |
|
|
746 | 791 |
} |
747 | 792 |
else { |
748 |
//ROS_INFO("BOM%d %f: false", i, bom_pos.theta); |
|
749 |
|
|
793 |
|
|
750 | 794 |
it->second->update_BOM(i, false); |
795 |
|
|
751 | 796 |
} |
752 | 797 |
} |
753 | 798 |
|
... | ... | |
783 | 828 |
|
784 | 829 |
|
785 | 830 |
bool SimFrame::is_inrange(geometry_msgs::Pose2D scout_pos, |
786 |
geometry_msgs::Pose2D emitter_pos) |
|
831 |
geometry_msgs::Pose2D emitter_pos, |
|
832 |
double aperture, double distance) |
|
787 | 833 |
|
788 | 834 |
{ |
789 | 835 |
//ROS_INFO("SCOUT: %f %f %f ", scout_pos.x, scout_pos.x, scout_pos.theta); |
... | ... | |
803 | 849 |
|
804 | 850 |
//ROS_INFO("distance: %f, angle: %f, orient: %f", se_distance, se_angle, emitter_orient); |
805 | 851 |
|
806 |
if ((se_distance <= em_distance)) { //distance check
|
|
852 |
if ((se_distance <= distance)) { //distance check |
|
807 | 853 |
|
808 |
double upper_limit = (emitter_orient + em_aperture/2);
|
|
809 |
double lower_limit = (emitter_orient - em_aperture/2);
|
|
854 |
double upper_limit = (emitter_orient + aperture/2); |
|
855 |
double lower_limit = (emitter_orient - aperture/2); |
|
810 | 856 |
|
811 | 857 |
//ROS_INFO("upper: %f, lower: %f", upper_limit, lower_limit+2*PI); |
812 | 858 |
|
scout/scoutsim/src/sim_frame.h | ||
---|---|---|
93 | 93 |
#define TELEOP_PRECISE 1 |
94 | 94 |
#define TELEOP_FLUID 2 |
95 | 95 |
|
96 |
//Scout DImensions |
|
97 |
#define SCOUT_H 0.25 |
|
98 |
#define SCOUT_W 0.125 |
|
99 |
|
|
96 | 100 |
namespace scoutsim |
97 | 101 |
{ |
98 | 102 |
class SimFrame : public wxFrame |
... | ... | |
149 | 153 |
|
150 | 154 |
void wirelessCallback(const messages::WirelessPacket::ConstPtr& msg); |
151 | 155 |
bool is_inrange(geometry_msgs::Pose2D scout_pos, |
152 |
geometry_msgs::Pose2D emitter_pos); |
|
156 |
geometry_msgs::Pose2D emitter_pos, |
|
157 |
double aperture, double distance); |
|
153 | 158 |
void checkBOM(geometry_msgs::Pose2D scout_pos, |
154 | 159 |
M_Scout::iterator it); |
155 | 160 |
// std::map<std::string, EmitterPtr>::iterator m_it, |
... | ... | |
159 | 164 |
ros::Subscriber wireless_send; |
160 | 165 |
ros::Publisher wireless_receive; |
161 | 166 |
|
162 |
//emitter |
|
163 |
float em_aperture; |
|
164 |
int em_distance; |
|
165 |
|
|
166 | 167 |
// Teleop |
167 | 168 |
short teleop_l_speed; |
168 | 169 |
short teleop_r_speed; |
... | ... | |
205 | 206 |
float width_in_meters; |
206 | 207 |
float height_in_meters; |
207 | 208 |
|
209 |
//Emitter default values |
|
210 |
double em_aperture; |
|
211 |
double em_distance; |
|
212 |
|
|
213 |
|
|
208 | 214 |
std::string map_base_name; |
209 | 215 |
std::string map_lines_name; |
210 | 216 |
std::string map_walls_name; |
Also available in: Unified diff