bool Environment::occluded(Geometry2d::Point ball, Geometry2d::Point camera) {
float camZ = 4;
float ballZ = Ball_Radius;
float intZ = Robot_Height;
// FIXME: use actual physics engine to determine line of sight
// Find where the line from the camera to the ball intersects the
// plane at the top of the robots.
//
// intZ = (ballZ - camZ) * t + camZ
float t = (intZ - camZ) / (ballZ - camZ);
Geometry2d::Point intersection;
intersection.x() = (ball.x() - camera.x()) * t + camera.x();
intersection.y() = (ball.y() - camera.y()) * t + camera.y();
// Return true if the intersection point is inside any robot
for (const Robot* r : _blue) {
if (intersection.nearPoint(r->getPosition(), Robot_Radius)) {
return true;
}
}
for (const Robot* r : _yellow) {
if (intersection.nearPoint(r->getPosition(), Robot_Radius)) {
return true;
}
}
return false;
}
static Geometry2d::Point fromOursToTheirs(Geometry2d::Point& pt) {
Geometry2d::Point c;
c.y() = Field_Dimensions::Current_Dimensions.Length() - pt.y();
c.x() = -pt.x();
return c;
}
void Environment::sendVision() {
SSL_WrapperPacket wrapper;
SSL_DetectionFrame* det = wrapper.mutable_detection();
det->set_frame_number(_frameNumber++);
det->set_camera_id(0);
struct timeval tv;
gettimeofday(&tv, nullptr);
det->set_t_capture(tv.tv_sec + (double)tv.tv_usec * 1.0e-6);
det->set_t_sent(det->t_capture());
for (Robot* robot : _yellow) {
if ((rand() % 100) < robot->visibility) {
SSL_DetectionRobot* out = det->add_robots_yellow();
convert_robot(robot, out);
}
}
for (Robot* robot : _blue) {
if ((rand() % 100) < robot->visibility) {
SSL_DetectionRobot* out = det->add_robots_blue();
convert_robot(robot, out);
}
}
for (const Ball* b : _balls) {
Geometry2d::Point ballPos = b->getPosition();
// bool occ;
// if (ballPos.x < 0)
// {
// occ = occluded(ballPos, cam0);
// } else {
// occ = occluded(ballPos, cam1);
// }
if ((rand() % 100) < ballVisibility) {
SSL_DetectionBall* out = det->add_balls();
out->set_confidence(1);
out->set_x(ballPos.x() * 1000);
out->set_y(ballPos.y() * 1000);
out->set_pixel_x(ballPos.x() * 1000);
out->set_pixel_y(ballPos.y() * 1000);
}
}
std::string buf;
wrapper.SerializeToString(&buf);
if (sendShared) {
_visionSocket.writeDatagram(&buf[0], buf.size(), MulticastAddress,
SharedVisionPort);
} else {
_visionSocket.writeDatagram(&buf[0], buf.size(), LocalAddress,
SimVisionPort);
_visionSocket.writeDatagram(&buf[0], buf.size(), LocalAddress,
SimVisionPort + 1);
}
}
void Environment::addBall(Geometry2d::Point pos) {
Ball* b = new Ball(this);
b->initPhysics();
b->position(pos.x(), pos.y());
_balls.append(b);
printf("New Ball: %f %f\n", pos.x(), pos.y());
}
void Environment::convert_robot(const Robot* robot, SSL_DetectionRobot* out) {
Geometry2d::Point pos = robot->getPosition();
out->set_confidence(1);
out->set_robot_id(robot->shell);
out->set_x(pos.x() * 1000);
out->set_y(pos.y() * 1000);
out->set_orientation(robot->getAngle());
out->set_pixel_x(pos.x() * 1000);
out->set_pixel_y(pos.y() * 1000);
}
void OurRobot::move(Geometry2d::Point goal, Geometry2d::Point endVelocity) {
if (!visible) return;
// sets flags for future movement
if (verbose)
cout << " in OurRobot::move(goal): adding a goal (" << goal.x() << ", "
<< goal.y() << ")" << std::endl;
_motionCommand = std::make_unique<Planning::PathTargetCommand>(
MotionInstant(goal, endVelocity));
*_cmdText << "move(" << goal.x() << ", " << goal.y() << ")" << endl;
*_cmdText << "endVelocity(" << endVelocity.x() << ", " << endVelocity.y()
<< ")" << endl;
}
TEST(WorldRobot, two_robot) {
RJ::Time t = RJ::now();
Geometry2d::Point p1 = Geometry2d::Point(1,1);
Geometry2d::Point p2 = Geometry2d::Point(2,2);
double th1 = 1;
double th2 = 2;
int rID = 1;
CameraRobot b1 = CameraRobot(t, p1, th1, rID);
CameraRobot b2 = CameraRobot(t, p2, th2, rID);
int cID = 1;
WorldRobot w;
KalmanRobot kb1 = KalmanRobot(cID, t, b1, w);
KalmanRobot kb2 = KalmanRobot(cID, t, b2, w);
std::list<KalmanRobot> kbl;
kbl.push_back(kb1);
kbl.push_back(kb2);
WorldRobot wb = WorldRobot(t, WorldRobot::Team::BLUE, rID, kbl);
Geometry2d::Point rp = wb.getPos();
double rt = wb.getTheta();
Geometry2d::Point rv = wb.getVel();
double ro = wb.getOmega();
double rpc = wb.getPosCov();
double rvc = wb.getVelCov();
std::list<KalmanRobot> list = wb.getRobotComponents();
EXPECT_TRUE(wb.getIsValid());
EXPECT_EQ(rp.x(), (p1.x() + p2.x()) / 2);
EXPECT_EQ(rp.y(), (p1.y() + p2.y()) / 2);
EXPECT_EQ(rt, (th1 + th2) / 2);
EXPECT_EQ(rv.x(), 0);
EXPECT_EQ(rv.y(), 0);
EXPECT_EQ(ro, 0);
EXPECT_GT(rpc, 0);
EXPECT_GT(rvc, 0);
EXPECT_LT(rpc, 10000);
EXPECT_LT(rvc, 10000);
EXPECT_EQ(list.size(), 2);
}
Geometry2d::Point gaussianPoint(int n, float scale) {
Geometry2d::Point pt;
for (int i = 0; i < n; ++i) {
pt.x() += drand48() - 0.5;
pt.y() += drand48() - 0.5;
}
pt *= scale / n;
return pt;
}
/** Checks whether or not the given ball is in the defense area. */
static inline bool ballIsInGoalieBox(Geometry2d::Point point) {
if (std::abs(point.x()) <
Field_Dimensions::Current_Dimensions.GoalFlat() / 2.0f) {
// Ball is in center (rectangular) portion of defensive bubble
return point.y() > 0 &&
point.y() < Field_Dimensions::Current_Dimensions.ArcRadius();
} else if (std::abs(point.x()) <
(Field_Dimensions::Current_Dimensions.ArcRadius() +
Field_Dimensions::Current_Dimensions.GoalFlat() / 2.0f)) {
// Ball is in one of the side (arc) portions of defensive bubble
double adjusted_x =
std::abs(point.x()) -
(Field_Dimensions::Current_Dimensions.GoalFlat() / 2.0f);
double max_y = sqrt((Field_Dimensions::Current_Dimensions.ArcRadius() *
Field_Dimensions::Current_Dimensions.ArcRadius()) -
(adjusted_x * adjusted_x));
return point.y() > 0 && point.y() <= max_y;
}
return false;
}
TEST(WorldRobot, one_robot) {
RJ::Time t = RJ::now();
Geometry2d::Point p = Geometry2d::Point(1,1);
double th = 1;
int rID = 1;
CameraRobot b = CameraRobot(t, p, th, rID);
int cID = 1;
WorldRobot w;
KalmanRobot kb = KalmanRobot(cID, t, b, w);
std::list<KalmanRobot> kbl;
kbl.push_back(kb);
WorldRobot wb = WorldRobot(t, WorldRobot::Team::BLUE, rID, kbl);
Geometry2d::Point rp = wb.getPos();
double rt = wb.getTheta();
Geometry2d::Point rv = wb.getVel();
double ro = wb.getOmega();
double rpc = wb.getPosCov();
double rvc = wb.getVelCov();
std::list<KalmanRobot> list = wb.getRobotComponents();
EXPECT_TRUE(wb.getIsValid());
EXPECT_EQ(wb.getRobotID(), rID);
EXPECT_EQ(rp.x(), p.x());
EXPECT_EQ(rp.y(), p.y());
EXPECT_EQ(rt, th);
EXPECT_EQ(rv.x(), 0);
EXPECT_EQ(rv.y(), 0);
EXPECT_EQ(ro, 0);
EXPECT_GT(rpc, 0);
EXPECT_GT(rvc, 0);
EXPECT_LT(rpc, 10000);
EXPECT_LT(rvc, 10000);
EXPECT_EQ(list.size(), 1);
}
void OurRobot::pivot(Geometry2d::Point pivotTarget) {
_rotationCommand = std::make_unique<Planning::EmptyAngleCommand>();
const float radius = Robot_Radius * 1;
Geometry2d::Point pivotPoint = _state->ball.pos;
// reset other conflicting motion commands
_motionCommand = std::make_unique<Planning::PivotCommand>(
pivotPoint, pivotTarget, radius);
*_cmdText << "pivot(" << pivotTarget.x() << ", " << pivotTarget.y() << ")"
<< endl;
}
void OurRobot::moveDirect(Geometry2d::Point goal, float endSpeed) {
if (!visible) return;
// sets flags for future movement
if (verbose)
cout << " in OurRobot::moveDirect(goal): adding a goal (" << goal.x()
<< ", " << goal.y() << ")" << endl;
_motionCommand = std::make_unique<Planning::DirectPathTargetCommand>(
MotionInstant(goal, (goal - pos).normalized() * endSpeed));
*_cmdText << "moveDirect(" << goal << ")" << endl;
*_cmdText << "endSpeed(" << endSpeed << ")" << endl;
}
void Environment::addRobot(bool blue, int id, Geometry2d::Point pos,
Robot::RobotRevision rev) {
Robot* r = new Robot(this, id, rev, pos);
r->initPhysics(blue);
if (blue) {
_blue.insert(id, r);
} else {
_yellow.insert(id, r);
}
Geometry2d::Point actPos = r->getPosition();
switch (rev) {
case Robot::rev2008:
printf("New 2008 Robot: %d : %f %f\n", id, actPos.x(), actPos.y());
break;
case Robot::rev2011:
printf("New 2011 Robot: %d : %f %f\n", id, actPos.x(), actPos.y());
}
QVector3D pos3(pos.x(), pos.y(), 0.0);
QVector3D axis(0.0, 0.0, 1.0);
}
Robot::Robot(Environment* env, unsigned int id, Robot::RobotRevision rev,
Geometry2d::Point startPos)
: Entity(env),
shell(id),
_rev(rev),
_robotChassis(nullptr),
_robotVehicle(nullptr),
_wheelShape(nullptr),
_controller(nullptr),
_brakingForce(0),
_targetVel(0, 0, 0),
_targetRot(0),
_simEngine(env->getSimEngine()) {
visibility = 100;
_startTransform.setIdentity();
_startTransform.setOrigin(btVector3(startPos.y(), 0, startPos.x()) *
scaling);
setEngineForce(0);
}
void OurRobot::worldVelocity(Geometry2d::Point v) {
_motionCommand = std::make_unique<Planning::WorldVelTargetCommand>(v);
setPath(nullptr);
*_cmdText << "worldVel(" << v.x() << ", " << v.y() << ")" << endl;
}
void OurRobot::face(Geometry2d::Point pt) {
_rotationCommand = std::make_unique<Planning::FacePointCommand>(pt);
*_cmdText << "face(" << pt.x() << ", " << pt.y() << ")" << endl;
}
WorldBall::WorldBall(RJ::Time calcTime, std::list<KalmanBall> kalmanBalls)
: isValid(true), time(calcTime){
Geometry2d::Point posAvg = Geometry2d::Point(0, 0);
Geometry2d::Point velAvg = Geometry2d::Point(0, 0);
double totalPosWeight = 0;
double totalVelWeight = 0;
// Below 1 would invert the ratio of scaling
// Above 2 would just be super noisy
if (*ball_merger_power < 1 || *ball_merger_power > 2) {
std::cout
<< "WARN: ball_merger_power should be between 1 and 2"
<< std::endl;
}
if (kalmanBalls.size() == 0) {
std::cout
<< "ERROR: Zero balls are given to the WorldBall constructor"
<< std::endl;
isValid = false;
pos = posAvg;
vel = velAvg;
posCov = 0;
velCov = 0;
return;
}
for (KalmanBall& ball : kalmanBalls) {
// Get the covariance of everything
// AKA how well we can predict the next measurement
Geometry2d::Point posCov = ball.getPosCov();
Geometry2d::Point velCov = ball.getVelCov();
// Std dev of each state
// Lower std dev gives better idea of true values
Geometry2d::Point posStdDev;
Geometry2d::Point velStdDev;
posStdDev.x() = std::sqrt(posCov.x());
posStdDev.y() = std::sqrt(posCov.y());
velStdDev.x() = std::sqrt(velCov.x());
velStdDev.y() = std::sqrt(velCov.y());
// Inversely proportional to how much the filter has been updated
double filterUncertantity = 1.0 / ball.getHealth();
// How good of pos/vel estimation in total
// (This is less efficient than just doing the sqrt(x_cov + y_cov),
// but it's a little more clear math-wise)
double posUncertantity = posStdDev.mag();
double velUncertantity = velStdDev.mag();
// Weight better estimates higher
double filterPosWeight = std::pow(posUncertantity * filterUncertantity,
-*ball_merger_power);
double filterVelWeight = std::pow(velUncertantity * filterUncertantity,
-*ball_merger_power);
posAvg += filterPosWeight * ball.getPos();
velAvg += filterVelWeight * ball.getVel();
totalPosWeight += filterPosWeight;
totalVelWeight += filterVelWeight;
}
posAvg /= totalPosWeight;
velAvg /= totalVelWeight;
pos = posAvg;
vel = velAvg;
posCov = totalPosWeight / kalmanBalls.size();
velCov = totalVelWeight / kalmanBalls.size();
ballComponents = kalmanBalls;
}