void found_beacon(string top, string bottom, pair<double, double> position) {
double d = position.first;
double theta = position.second;
pair<double, double> bot_pos = bot.get_position();
double beacon_yaw = bot.get_yaw() + theta;
double b_x = bot_pos.first + d*cos(beacon_yaw);
double b_y = bot_pos.second + d*sin(beacon_yaw);
ROS_INFO_STREAM("BOT POS: " << top << " " << bottom << " " << b_x << "," << b_y << "beacon_yaw: " <<
beacon_yaw << " d: " << d << "robot_yaw " << bot.get_yaw() << "theta: " << theta);
for (auto it = beacons.begin(); it != beacons.end(); ++it) {
if (it->top == top && it->bottom == bottom) {
it->known_location = true;
it->x = b_x;
it->y = b_y;
}
}
}
