Revision 98ed4757
Basic BOM works (only 1 BOM in 'centre' of scout)
Added BOM topic to scouts
Added 'kill' for emitters
scout/scoutsim/src/scout.cpp | ||
---|---|---|
102 | 102 |
pose_pub = node.advertise<Pose>("pose", 1); |
103 | 103 |
color_pub = node.advertise<Color>("color_sensor", 1); |
104 | 104 |
sonar_pub = node.advertise< ::messages::sonar_distance>("sonar_distance", 1); |
105 |
bom_pub = node.advertise<BOM>("BOM", 1); |
|
105 | 106 |
set_pen_srv = node.advertiseService("set_pen", |
106 | 107 |
&Scout::setPenCallback, |
107 | 108 |
this); |
... | ... | |
429 | 430 |
} |
430 | 431 |
} |
431 | 432 |
|
433 |
void Scout::pub_BOM(bool bom_0) { |
|
434 |
BOM b; |
|
435 |
|
|
436 |
b.bom_0 = true; |
|
437 |
bom_pub.publish(b); |
|
438 |
} |
|
439 |
|
|
440 |
|
|
432 | 441 |
/// Sends back the position of this scout so scoutsim can save |
433 | 442 |
/// the world state |
434 | 443 |
/// @todo remove dt param |
scout/scoutsim/src/scout.h | ||
---|---|---|
62 | 62 |
#include <scoutsim/Pose.h> |
63 | 63 |
#include <scoutsim/SetPen.h> |
64 | 64 |
#include <scoutsim/Color.h> |
65 |
#include <scoutsim/BOM.h> |
|
65 | 66 |
|
66 | 67 |
#include <geometry_msgs/Pose2D.h> |
67 | 68 |
|
... | ... | |
121 | 122 |
world_state state); |
122 | 123 |
void paint(wxDC& dc); |
123 | 124 |
void set_sonar_visual(bool on); |
125 |
void pub_BOM(bool bom_0); |
|
124 | 126 |
|
125 | 127 |
private: |
126 | 128 |
float absolute_to_mps(int absolute_speed); |
... | ... | |
206 | 208 |
ros::Publisher pose_pub; |
207 | 209 |
ros::Publisher color_pub; |
208 | 210 |
ros::Publisher sonar_pub; |
211 |
ros::Publisher bom_pub; |
|
209 | 212 |
ros::ServiceServer set_pen_srv; |
210 | 213 |
ros::ServiceServer query_encoders_srv; |
211 | 214 |
ros::ServiceServer query_linesensor_srv; |
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 |
} |
scout/scoutsim/src/sim_frame.h | ||
---|---|---|
148 | 148 |
bool setTeleopCallback(SetTeleop::Request&, SetTeleop::Response&); |
149 | 149 |
|
150 | 150 |
void wirelessCallback(const messages::WirelessPacket::ConstPtr& msg); |
151 |
void readBOM(); |
|
151 |
void readBOM(geometry_msgs::Pose2D scout_pos, |
|
152 |
geometry_msgs::Pose2D emitter_pos, |
|
153 |
std::map<std::string, ScoutPtr>::iterator it); |
|
152 | 154 |
|
153 | 155 |
|
154 | 156 |
ros::Subscriber wireless_send; |
Also available in: Unified diff