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; }