From 4ee2a9f95d5769a717d643011eecacb2336a9faf Mon Sep 17 00:00:00 2001 From: SYLG <2330180633@qq.com> Date: Mon, 5 Feb 2024 15:13:42 +0800 Subject: [PATCH] feat: add visual blind zone --- CMakeLists.txt | 2 + include/configwidget.h | 1 + include/geometry.h | 343 +++++++++++++++++++++++++++++++++++++++++ include/sslworld.h | 7 + src/configwidget.cpp | 1 + src/geometry.cpp | 98 ++++++++++++ src/sslworld.cpp | 113 ++++++++++---- 7 files changed, 535 insertions(+), 30 deletions(-) create mode 100644 include/geometry.h create mode 100644 src/geometry.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 833c5cce..6c568ba4 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -152,6 +152,7 @@ set(SOURCES src/physics/pray.cpp src/net/robocup_ssl_server.cpp src/net/robocup_ssl_client.cpp + src/geometry.cpp src/sslworld.cpp src/robot.cpp src/configwidget.cpp @@ -175,6 +176,7 @@ set(HEADERS include/physics/pray.h include/net/robocup_ssl_server.h include/net/robocup_ssl_client.h + include/geometry.h include/sslworld.h include/robot.h include/configwidget.h diff --git a/include/configwidget.h b/include/configwidget.h index df7cece2..57077918 100644 --- a/include/configwidget.h +++ b/include/configwidget.h @@ -192,6 +192,7 @@ class ConfigWidget : public VarTreeView DEF_VALUE(int,Int,sendGeometryEvery) DEF_VALUE(double,Double,Gravity) DEF_VALUE(bool,Bool,ResetTurnOver) + DEF_VALUE(bool,Bool,BlindZone) DEF_VALUE(std::string,String,VisionMulticastAddr) DEF_VALUE(int,Int,VisionMulticastPort) DEF_VALUE(int,Int,CommandListenPort) diff --git a/include/geometry.h b/include/geometry.h new file mode 100644 index 00000000..799aa0e4 --- /dev/null +++ b/include/geometry.h @@ -0,0 +1,343 @@ +#ifndef _GEOMETRY_H_ +#define _GEOMETRY_H_ + +#define _USE_MATH_DEFINES +#include +#include +#include + +/************************************************************************/ +/* CVector */ +/************************************************************************/ +class CVector { +public: + CVector() : _x(0), _y(0) {} + CVector(double x, double y) : _x(x), _y(y) {} + CVector(const CVector& v) : _x(v.x()), _y(v.y()) {} + bool setVector(double x, double y) { + _x = x; + _y = y; + return true; + } + double mod() const { + return std::sqrt(_x * _x + _y * _y); + } + double mod2() const { + return (_x * _x + _y * _y); + } + double dir() const { + return std::atan2(y(), x()); + } + double theta(const CVector& v) { + // the angle between this vector and vector v + double _theta; + _theta = std::atan2(_y, _x) - std::atan2(v.y(), v.x()); + if (_theta > M_PI) return _theta - 2 * M_PI; + if (_theta < -M_PI) return _theta + 2 * M_PI; + return _theta; + } + CVector rotate(double angle) const; + CVector unit() const { + CVector vector(_x, _y); + if (vector.mod() < 1e-8) { + return CVector(1, 0); + } + return CVector(vector.x() / vector.mod(), + vector.y() / vector.mod()); + } + double x() const { + return _x; + } + double y() const { + return _y; + } + double value(double angle) const { + return mod() * std::cos(dir() - angle); + } + CVector operator +(const CVector& v) const { + return CVector(_x + v.x(), _y + v.y()); + } + CVector operator -(const CVector& v) const { + return CVector(_x - v.x(), _y - v.y()); + } + CVector operator *(double a) const { + return CVector(_x * a, _y * a); + } + double operator *(CVector b) const { + // dot product + return double(_x * b.x() + _y * b.y()); + } + double operator ^(CVector b) const { + // length of cross product, signed + return double(_x * b.y() - _y * b.x()); + } + CVector operator ^(double b) const { + // cross product with a verticle vector of length b + return CVector(copysign(_y, b), -copysign(_x, b)); + } + CVector operator /(double a) const { + return CVector(_x / a, _y / a); + } + CVector operator -() const { + return CVector(-1 * _x, -1 * _y); + } + friend std::ostream& operator <<(std::ostream& os, const CVector& v) { + return os << "(" << v.x() << ":" << v.y() << ")"; + } + +private: + double _x, _y; +}; + +/************************************************************************/ +/* CGeoPoint */ +/************************************************************************/ +class CGeoPoint { +public: + CGeoPoint() : _x(0), _y(0) {} + ~CGeoPoint() {} + CGeoPoint(double x, double y) : _x(x), _y(y) {} + CGeoPoint(const CGeoPoint& p) : _x(p.x()), _y(p.y()) {} + bool operator==(const CGeoPoint& rhs) { + return ((this->x() == rhs.x()) && (this->y() == rhs.y())); + } + double x() const { + return _x; + } + double y() const { + return _y; + } + void setX(double x) { + _x = x; + } + void setY(double y) { + _y = y; + } + bool fill(double x, double y) { + _x = x; + _y = y; + return true; + } + double dist(const CGeoPoint& p) const { + return CVector(p - CGeoPoint(_x, _y)).mod(); + } + double dist2(const CGeoPoint& p) const { + return CVector(p - CGeoPoint(_x, _y)).mod2(); + } + CGeoPoint operator+(const CVector& v) const { + return CGeoPoint(_x + v.x(), _y + v.y()); + } + CGeoPoint operator*(const double& a) const { + return CGeoPoint(_x * a, _y * a); + } + CVector operator-(const CGeoPoint& p) const { + return CVector(_x - p.x(), _y - p.y()); + } + CGeoPoint midPoint(const CGeoPoint& p) const { + return CGeoPoint((_x + p.x()) / 2, (_y + p.y()) / 2); + } + friend std::ostream& operator <<(std::ostream& os, const CGeoPoint& v) { + return os << "(" << v.x() << ":" << v.y() << ")"; + } + +private: + double _x, _y; +}; + +/************************************************************************/ +/* CGeoLine */ +/************************************************************************/ +class CGeoLine { +public: + CGeoLine() {} + CGeoLine(const CGeoPoint& p1, const CGeoPoint& p2) : _p1(p1), _p2(p2) { + calABC(); + } + CGeoLine(const CGeoPoint& p, double angle) : _p1(p), _p2(p.x() + std::cos(angle), p.y() + std::sin(angle)) { + calABC(); + } + void calABC() { + if (_p1.y() == _p2.y()) { + _a = 0; + _b = 1; + _c = -1.0 * _p1.y(); + } else { + _a = 1; + _b = -1.0 * (_p1.x() - _p2.x()) / (_p1.y() - _p2.y()); + _c = (_p1.x() * _p2.y() - _p1.y() * _p2.x()) / (_p1.y() - _p2.y()); + } + } + + CGeoPoint projection(const CGeoPoint& p) const { + if (_p2.x() == _p1.x()) { + return CGeoPoint(_p1.x(), p.y()); + } else { + double k = (_p2.y() - _p1.y()) / (_p2.x() - _p1.x()); + // equation: y = k* ( x - pt1.x) + pt1.y + // equation of perpendicular line: y = (-1/k) * (x - p.x) + p.y + // solve the two equations + double x = (k * k * _p1.x() + k * (p.y() - _p1.y()) + p.x()) / (k * k + 1); + double y = k * (x - _p1.x()) + _p1.y(); + return CGeoPoint(x, y); + } + } + CGeoPoint point1() const { + return _p1; + } + CGeoPoint point2() const { + return _p2; + } + bool operator==(const CGeoLine& rhs) { + return ((this->point1().x() == rhs.point1().x()) && (this->point1().y() == rhs.point1().y()) + && (this->point2().x() == rhs.point2().x()) && (this->point2().y() == rhs.point2().y())); + } + const double& a() const { + return _a; + } + const double& b() const { + return _b; + } + const double& c() const { + return _c; + } +private: + CGeoPoint _p1; + CGeoPoint _p2; + + // a*x+b*y+c=0, a>=0 + double _a; + double _b; + double _c; +}; + +class CGeoLineLineIntersection { +public: + CGeoLineLineIntersection(const CGeoLine& line_1, const CGeoLine& line_2); + bool Intersectant() const { + return _intersectant; + } + const CGeoPoint& IntersectPoint() const { + return _point; + } +private: + bool _intersectant; + CGeoPoint _point; +}; + +/************************************************************************/ +/* CGeoSegment */ +/************************************************************************/ +class CGeoSegment : public CGeoLine { +public: + CGeoSegment() {} + CGeoSegment(const CGeoPoint& p1, const CGeoPoint& p2) : CGeoLine(p1, p2), _start(p1), _end(p2) { + _compareX = std::abs(p1.x() - p2.x()) > std::abs(p1.y() - p2.y()); + _center = CGeoPoint((p1.x() + p2.x()) / 2, (p1.y() + p2.y()) / 2); + } + bool IsPointOnLineOnSegment(const CGeoPoint& p) const { + if (_compareX) { + return p.x() > (std::min)(_start.x(), _end.x()) && p.x() < (std::max)(_start.x(), _end.x()); + } + return p.y() > (std::min)(_start.y(), _end.y()) && p.y() < (std::max)(_start.y(), _end.y()); + } + bool IsSegmentsIntersect(const CGeoSegment& p) const { + CGeoLineLineIntersection tmpInter(*this, p); + CGeoPoint interPoint = tmpInter.IntersectPoint(); + return (IsPointOnLineOnSegment(interPoint) && p.IsPointOnLineOnSegment(interPoint)); + } + CGeoPoint segmentsIntersectPoint(const CGeoSegment& p) const { + CGeoLineLineIntersection tmpInter(*this, p); + CGeoPoint interPoint = tmpInter.IntersectPoint(); + if (IsPointOnLineOnSegment(interPoint) && p.IsPointOnLineOnSegment(interPoint)) + return interPoint; + else return CGeoPoint(9999, 9999); + } + double dist2Point(const CGeoPoint& p) { + CGeoPoint tmpProj = projection(p); + if (IsPointOnLineOnSegment(tmpProj)) return p.dist(tmpProj); + else return std::min(_start.dist(p), _end.dist(p)); + } + double dist2Segment(const CGeoSegment& s) { + if (IsSegmentsIntersect(s)) return 0; + else return std::min(dist2Point(s.point1()), dist2Point(s.point2())); + } + const CGeoPoint& start() const { + return _start; + } + const CGeoPoint& end() const { + return _end; + } + const CGeoPoint& center() { + return _center; + } + +private: + CGeoPoint _start; + CGeoPoint _end; + CGeoPoint _center; + bool _compareX; +}; + +/************************************************************************/ +/* CGeoShape */ +/************************************************************************/ +class CGeoShape { +public: + virtual ~CGeoShape() { } + virtual bool HasPoint(const CGeoPoint& p) const = 0; +}; + +/************************************************************************/ +/* CGeoQuadrilateral */ +/************************************************************************/ +class CGeoQuadrilateral : public CGeoShape { +public: + CGeoQuadrilateral() :CGeoQuadrilateral(CGeoPoint(), CGeoPoint(), CGeoPoint(), CGeoPoint()) {} + CGeoQuadrilateral(const CGeoPoint p1, const CGeoPoint p2, const CGeoPoint p3, const CGeoPoint p4); + virtual bool HasPoint(const CGeoPoint& p) const; + CGeoPoint _point[4]; + CGeoPoint _center; +}; + +/************************************************************************/ +/* CGeoCircle */ +/************************************************************************/ +class CGeoCirlce : public CGeoShape { +public: + CGeoCirlce() { } + CGeoCirlce(const CGeoPoint& c, double r) : _radius(r), _center(c) { } + virtual bool HasPoint(const CGeoPoint& p) const; + CGeoPoint Center() const { + return _center; + } + double Radius() const { + return _radius; + } + double Radius2() const { + return _radius * _radius; + } +private: + double _radius; + CGeoPoint _center; +}; +/************************************************************************/ +/* CGeoCircleTangent */ +/************************************************************************/ +class CGeoCircleTangent { +public: + CGeoCircleTangent(const CGeoCirlce& circle, const CGeoPoint& p); + const CGeoPoint& point1() const { + return _point1; + } + const CGeoPoint& point2() const { + return _point2; + } + int size() { + return tangent_size; + } +private: + int tangent_size; + CGeoPoint _point1; + CGeoPoint _point2; +}; +#endif diff --git a/include/sslworld.h b/include/sslworld.h index 60915835..02cbd9d3 100644 --- a/include/sslworld.h +++ b/include/sslworld.h @@ -35,6 +35,8 @@ Copyright (C) 2011, Parsian Robotic Center (eew.aut.ac.ir/~parsian/grsim) #include "net/robocup_ssl_server.h" +#include "geometry.h" + #include "robot.h" #include "configwidget.h" @@ -56,6 +58,7 @@ class SendingPacket { SSL_WrapperPacket* packet; int t; }; +typedef QList blindZoneList; class SSLWorld : public QObject { @@ -68,6 +71,7 @@ class SSLWorld : public QObject QList sendQueue; bool lastInfraredState[TEAM_COUNT][MAX_ROBOT_COUNT]{}; KickStatus lastKickState[TEAM_COUNT][MAX_ROBOT_COUNT]{}; + QList blindZones; void processSimControl(const SimulatorCommand &simulatorCommand, SimulatorResponse &simulatorResponse); void processRobotControl(const RobotControl &robotControl, RobotControlResponse &robotControlResponse, Team team); void processRobotSpec(const RobotSpecs &robotSpec) const; @@ -89,6 +93,9 @@ class SSLWorld : public QObject static void addFieldArc(SSL_GeometryFieldSize *field, const string &name, float c_x, float c_y, float radius, float a1, float a2, float thickness); void sendVisionBuffer(); static bool visibleInCam(int id, double x, double y); + void clearBlindZone(int cam_id); + void addBlindZone(int cam_id, int robot_index); + bool inBlindZone(int cam_id, dReal x, dReal y, dReal z); QPair cameraPosition(int id); int robotIndex(int robot,int team); static void addRobotStatus(Robots_Status& robotsPacket, int robotID, bool infrared, KickStatus kickStatus); diff --git a/src/configwidget.cpp b/src/configwidget.cpp index c88d732e..446ce512 100644 --- a/src/configwidget.cpp +++ b/src/configwidget.cpp @@ -111,6 +111,7 @@ ConfigWidget::ConfigWidget() { ADD_VALUE(worldp_vars,Double,DeltaTime,1.0/60,"ODE time step") ADD_VALUE(worldp_vars,Double,Gravity,9.81,"Gravity") ADD_VALUE(worldp_vars,Bool,ResetTurnOver,true,"Auto reset turn-over") + ADD_VALUE(worldp_vars,Bool,BlindZone,false,"Enable blind zone") VarListPtr ballp_vars(new VarList("Ball")); phys_vars->addChild(ballp_vars); ADD_VALUE(ballp_vars,Double,BallMass,0.043,"Ball mass"); diff --git a/src/geometry.cpp b/src/geometry.cpp new file mode 100644 index 00000000..b9a8ccc4 --- /dev/null +++ b/src/geometry.cpp @@ -0,0 +1,98 @@ +#include +#include + +/************************************************************************/ +/* CVector */ +/************************************************************************/ +CVector CVector::rotate(double angle) const +{ + double nowdir = dir() + angle; + double nowx = mod() * cos(nowdir); + double nowy = mod() * sin(nowdir); + return CVector(nowx, nowy); +} + +/************************************************************************/ +/* CGeoLineLineIntersection */ +/************************************************************************/ +CGeoLineLineIntersection::CGeoLineLineIntersection(const CGeoLine& line_1, const CGeoLine& line_2) +{ + double d = line_1.a() * line_2.b() - line_1.b() * line_2.a(); + if (std::abs(d) < 0.0001) { + _intersectant = false; + } else { + double px = (line_1.b() * line_2.c() - line_1.c() * line_2.b()) / d; + double py = (line_1.c() * line_2.a() - line_1.a() * line_2.c()) / d; + _point = CGeoPoint(px, py); + _intersectant = true; + } +} + +/************************************************************************/ +/* CGeoQuadrilateral */ +/************************************************************************/ +CGeoQuadrilateral::CGeoQuadrilateral(const CGeoPoint p1, const CGeoPoint p2, const CGeoPoint p3, const CGeoPoint p4) + :_point{ p1,p2,p3,p4 } +{ + double px = 0, py = 0; + // find a point p inside polygon: the average of the vertexes + for (int i = 0; i < 4; i++) { + px += _point[i].x(); + py += _point[i].y(); + } + px /= 4.0; + py /= 4.0; + _center = CGeoPoint(px, py); + std::sort(_point, _point + 4, [&](const CGeoPoint& p1, const CGeoPoint& p2)->bool {return (p1 - _center).dir() < (p2 - _center).dir(); }); +} +bool CGeoQuadrilateral::HasPoint(const CGeoPoint& p) const +{ + CGeoSegment line_1(p, _center); + for (int i = 0; i < 4; i++) { + CGeoSegment line_2(_point[i], _point[(i + 1) % 4]); + CGeoLineLineIntersection inter(line_1, line_2); + if (inter.Intersectant() + && line_1.IsPointOnLineOnSegment(inter.IntersectPoint()) + && line_2.IsPointOnLineOnSegment(inter.IntersectPoint())) { + return false; + } + } + return true; +} + +/************************************************************************/ +/* CGeoCirlce */ +/************************************************************************/ +bool CGeoCirlce::HasPoint(const CGeoPoint& p) const +{ + double d = (p - _center).mod(); + if (d < _radius) { + return true; + } + return false; +} + +/************************************************************************/ +/* CGeoCircleTangent */ +/************************************************************************/ +CGeoCircleTangent::CGeoCircleTangent(const CGeoCirlce& circle, const CGeoPoint& p) +{ + double d = (p - circle.Center()).mod(); + if (fabs(d - circle.Radius()) < 1e-10) { + tangent_size = 1; + _point1 = p; + _point2 = CGeoPoint(); + } else if (d < circle.Radius()) { + tangent_size = 0; + _point1 = CGeoPoint(); + _point2 = CGeoPoint(); + } else { + tangent_size = 2; + + double angle = acos(circle.Radius() / d); + double dir_p = (p - circle.Center()).dir(); + + _point1 = circle.Center() + CVector(circle.Radius() * cos(dir_p + angle), circle.Radius() * sin(dir_p + angle)); + _point2 = circle.Center() + CVector(circle.Radius() * cos(dir_p - angle), circle.Radius() * sin(dir_p - angle)); + } +} \ No newline at end of file diff --git a/src/sslworld.cpp b/src/sslworld.cpp index 5cc33755..f9182c94 100644 --- a/src/sslworld.cpp +++ b/src/sslworld.cpp @@ -312,6 +312,11 @@ SSLWorld::SSLWorld(QGLWidget* parent, ConfigWidget* _cfg, RobotsFormation *form1 } } + // initialize blind zone list + for (int i = 0; i < 4; i++) { + blindZones.append(blindZoneList()); + } + restartRequired = false; elapsedLastPackageBlue.start(); @@ -962,6 +967,53 @@ bool SSLWorld::visibleInCam(int id, double x, double y) { return false; } +void SSLWorld::clearBlindZone(int cam_id) { + for (auto& blindZone : blindZones[cam_id]) { + delete blindZone; + } + blindZones[cam_id].clear(); +} + +void SSLWorld::addBlindZone(int cam_id, int robot_id) { + if (cfg->BlindZone()) { + dReal robot_x, robot_y; + robots[robot_id]->getXY(robot_x, robot_y); + double robot_r = robots[robot_id]->cfg->robotSettings.RobotRadius; + double robot_z = robots[robot_id]->cfg->robotSettings.RobotHeight; + if (visibleInCam(cam_id, robot_x, robot_y)) { + double cam_x = cameraPosition(cam_id).first; + double cam_y = cameraPosition(cam_id).second; + double cam_z = cfg->Camera_Height(); + // the blind zone of a robot (cylinder) to a camera can be devided into three parts + // the bottom of the cylinder, which can be omitted + // the projection of the top of the cylinder, proj + // the quadrilateral formed by two external common tangent lines of the two circles + double proj_x = (robot_z * cam_x - robot_x * cam_z) / (robot_z - cam_z); + double proj_y = (robot_z * cam_y - robot_y * cam_z) / (robot_z - cam_z); + double proj_r = robot_r * cam_z / (cam_z - robot_z); + blindZones[cam_id].append(new CGeoCirlce(CGeoPoint(proj_x, proj_y), proj_r)); + // first calculate the intersect of the two external tangent lines using math + // then try to draw tangent lines to the two circles + // if there are 4 tangent lines, then the quadrilateral exists + CGeoPoint intersect((robot_r * proj_x - proj_r * robot_x) / (robot_r - proj_r), (robot_r * proj_y - proj_r * robot_y) / (robot_r - proj_r)); + CGeoCircleTangent robot_tangent(CGeoCirlce(CGeoPoint(robot_x, robot_y), robot_r), intersect); + CGeoCircleTangent proj_tangent(CGeoCirlce(CGeoPoint(proj_x, proj_y), proj_r), intersect); + if (robot_tangent.size() + proj_tangent.size() == 4) { + blindZones[cam_id].append(new CGeoQuadrilateral(robot_tangent.point1(), robot_tangent.point2(), proj_tangent.point1(), proj_tangent.point2())); + } + } + } +} + +bool SSLWorld::inBlindZone(int cam_id, dReal x, dReal y, dReal z) { + for (auto& blindZone : blindZones[cam_id]) { + if (blindZone->HasPoint(CGeoPoint(x, y))) { + return true; + } + } + return false; +} + QPair SSLWorld::cameraPosition(int id) { Q_ASSERT(id >= 0 && id < 4); @@ -979,8 +1031,6 @@ QPair SSLWorld::cameraPosition(int id) { #define CONVUNIT(x) ((int)(1000*(x))) SSL_WrapperPacket* SSLWorld::generatePacket(int cam_id) { auto* pPacket = new SSL_WrapperPacket(); - dReal x,y,z,dir,k; - ball->getBodyPosition(x,y,z); pPacket->mutable_detection()->set_camera_id(cam_id); pPacket->mutable_detection()->set_frame_number(frame_num); pPacket->mutable_detection()->set_t_capture(sim_time); @@ -1042,8 +1092,38 @@ SSL_WrapperPacket* SSLWorld::generatePacket(int cam_id) { } if (!cfg->noise()) { dev_x = 0;dev_y = 0;dev_a = 0;} + clearBlindZone(cam_id); + dReal x, y, z, dir, k; + for (int i = 0; i < cfg->Robots_Count() * 2; i++) { + if (!robots[i]->on) continue; + addBlindZone(cam_id, i); + bool is_blue = i < cfg->Robots_Count(); + const double vanishing = (is_blue) ? cfg->blue_team_vanishing() : cfg->yellow_team_vanishing(); + const int id = (is_blue) ? i : i - cfg->Robots_Count(); + + if (!cfg->vanishing() || (rand0_1() > vanishing)) { + robots[i]->getXY(x, y); + dir = robots[i]->getDir(k); + + // reset when the robot has turned over + if (cfg->ResetTurnOver() && k < 0.9) robots[i]->resetRobot(); + + if (visibleInCam(cam_id, x, y)) { + SSL_DetectionRobot* rob = (is_blue) ? pPacket->mutable_detection()->add_robots_blue() + : pPacket->mutable_detection()->add_robots_yellow(); + rob->set_robot_id(id); + rob->set_pixel_x(x * 1000.0f); + rob->set_pixel_y(y * 1000.0f); + rob->set_confidence(1); + rob->set_x(randn_notrig(x * 1000.0f, dev_x)); + rob->set_y(randn_notrig(y * 1000.0f, dev_y)); + rob->set_orientation(normalizeAngle(randn_notrig(dir, dev_a)) * M_PI / 180.0f); + } + } + } if (!cfg->vanishing() || (rand0_1() > cfg->ball_vanishing())) { - if (visibleInCam(cam_id, x, y)) { + ball->getBodyPosition(x, y, z); + if (visibleInCam(cam_id, x, y) && !inBlindZone(cam_id, x, y, z)) { const double BALL_RADIUS = cfg->BallRadius() * 1000.f; const double SCALING_LIMIT = cfg->Camera_Scaling_Limit(); SSL_DetectionBall* vball = pPacket->mutable_detection()->add_balls(); @@ -1073,33 +1153,6 @@ SSL_WrapperPacket* SSLWorld::generatePacket(int cam_id) { vball->set_confidence(0.9 + rand0_1()*0.1); } } - - for(int i = 0; i < cfg->Robots_Count() * 2; i++) { - if (!robots[i]->on) continue; - bool is_blue = i < cfg->Robots_Count(); - const double vanishing = (is_blue) ? cfg->blue_team_vanishing() : cfg->yellow_team_vanishing(); - const int id = (is_blue) ? i : i - cfg->Robots_Count(); - - if (!cfg->vanishing() || (rand0_1() > vanishing)) { - robots[i]->getXY(x,y); - dir = robots[i]->getDir(k); - - // reset when the robot has turned over - if (cfg->ResetTurnOver() && k < 0.9) robots[i]->resetRobot(); - - if (visibleInCam(cam_id, x, y)) { - SSL_DetectionRobot* rob = (is_blue) ? pPacket->mutable_detection()->add_robots_blue() - : pPacket->mutable_detection()->add_robots_yellow(); - rob->set_robot_id(id); - rob->set_pixel_x(x*1000.0f); - rob->set_pixel_y(y*1000.0f); - rob->set_confidence(1); - rob->set_x(randn_notrig(x*1000.0f,dev_x)); - rob->set_y(randn_notrig(y*1000.0f,dev_y)); - rob->set_orientation(normalizeAngle(randn_notrig(dir,dev_a))*M_PI/180.0f); - } - } - } return pPacket; }