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