diff --git a/Client/src/field.cpp b/Client/src/field.cpp index e1899be..e8c2d62 100644 --- a/Client/src/field.cpp +++ b/Client/src/field.cpp @@ -136,6 +136,7 @@ int robotAmount; int robotID[PARAM::ROBOTNUM]; int robotTeam; auto originRobot = GlobalData::instance()->processRobot[0]; +auto _G = GlobalData::instance(); } namespace LeftEvent { QPoint start; @@ -191,6 +192,15 @@ Field::Field(QQuickItem *parent) ZRecRecorder::instance()->init(); } +void Field::setType(int t){ + this->_type = t; + // check if type in selected_points(type:map>>) + std::scoped_lock lock(_G->selected_points_mutex); + if(_G->selected_points.find(t) == _G->selected_points.end()) { + _G->selected_points[t] = std::vector>(); + } +} + void Field::paint(QPainter* painter) { // painter->drawPixmap(area, *pixmap, QRect(zoomStart, ::size * zoomRatio)); painter->drawPixmap(area, *pixmap); @@ -313,7 +323,6 @@ void Field::leftReleaseEvent(QMouseEvent *e) { leftAltModifierReleaseEvent(e); break; } - } void Field::leftCtrlModifierMoveEvent(QMouseEvent *e) { auto x1 = ::rx(e->x()); @@ -387,7 +396,7 @@ void Field::leftAltModifierReleaseEvent(QMouseEvent *e) { selectRobots = robotAmount > 0 ? true : false; GlobalSettings::instance()->resetSelectCarArea(); } -void Field::rightMoveEvent(QMouseEvent *e) { +void Field::rightNoModifierMoveEvent(QMouseEvent *e) { QLineF line(start, end); if(pressedRobot) { displayData = -line.angle(); @@ -400,15 +409,84 @@ void Field::rightMoveEvent(QMouseEvent *e) { displayData = ballRatio * line.length() / 1000.0; } } -void Field::rightPressEvent(QMouseEvent *e) { - +void Field::rightNoModifierPressEvent(QMouseEvent *e) { } -void Field::rightReleaseEvent(QMouseEvent *e) { +void Field::rightNoModifierReleaseEvent(QMouseEvent *e) { QLineF line(start, end); if(!pressedRobot) { Simulator::instance()->setBall(start.x() / 1000.0, start.y() / 1000.0, ballRatio * line.dx() / 1000.0, ballRatio * line.dy() / 1000.0); } } + +void Field::rightMoveEvent(QMouseEvent *e){ + switch(mouse_modifiers) { + case Qt::NoModifier: + rightNoModifierMoveEvent(e); + break; + case Qt::ControlModifier: + rightCtrlModifierMoveEvent(e); + break; + case Qt::AltModifier: + rightAltModifierMoveEvent(e); + break; + default: + break; + } +} +void Field::rightPressEvent(QMouseEvent *e){ + switch(mouse_modifiers) { + case Qt::NoModifier: + rightNoModifierPressEvent(e); + break; + case Qt::ControlModifier: + rightCtrlModifierPressEvent(e); + break; + case Qt::AltModifier: + rightAltModifierPressEvent(e); + break; + default: + break; + } +} +void Field::rightReleaseEvent(QMouseEvent *e){ + switch(mouse_modifiers) { + case Qt::NoModifier: + rightNoModifierReleaseEvent(e); + break; + case Qt::ControlModifier: + rightCtrlModifierReleaseEvent(e); + break; + case Qt::AltModifier: + rightAltModifierReleaseEvent(e); + break; + default: + break; + } +} +void Field::rightCtrlModifierMoveEvent(QMouseEvent *e){ +} +void Field::rightCtrlModifierPressEvent(QMouseEvent *e){ +} +void Field::rightCtrlModifierReleaseEvent(QMouseEvent *e){ + std::scoped_lock lock(_G->selected_points_mutex); + if(_G->selected_points.find(_type) == _G->selected_points.end()) { + _G->selected_points[_type] = std::vector>(); + } + auto pos = rp(e->pos()); + _G->selected_points[_type].push_back(std::pair(pos.x(), pos.y())); +} +void Field::rightAltModifierMoveEvent(QMouseEvent *e){ +} +void Field::rightAltModifierPressEvent(QMouseEvent *e){ +} +void Field::rightAltModifierReleaseEvent(QMouseEvent *e){ + std::scoped_lock lock(_G->selected_points_mutex); + if(_G->selected_points.find(_type) == _G->selected_points.end()) { + _G->selected_points[_type] = std::vector>(); + } + _G->selected_points[_type].clear(); +} + void Field::middleMoveEvent(QMouseEvent *e) { switch(mouse_modifiers) { case Qt::NoModifier: @@ -552,6 +630,7 @@ void Field::repaint() {//change here!!!!!!! paintInit(); drawMaintainVision(0); if (selectRobots) paintSelectedCar(); + paintSelectedPoints(); drawDebugMessages(PARAM::BLUE); //BLUE break; case 3: @@ -561,6 +640,7 @@ void Field::repaint() {//change here!!!!!!! paintInit(); drawMaintainVision(0); if (selectRobots) paintSelectedCar(); + paintSelectedPoints(); drawDebugMessages(PARAM::YELLOW); //YELLOW break; default: @@ -580,7 +660,7 @@ void Field::draw() { repaint(); } void Field::drawBallLine() { - if(pressed == Qt::RightButton) { + if(pressed == Qt::RightButton && mouse_modifiers == Qt::NoModifier) { pixmapPainter.setBrush(QBrush(FONT_COLOR[0])); pixmapPainter.setPen(QPen(FONT_COLOR[0], ::w(20), Qt::DashLine)); pixmapPainter.drawLine(p(start), p(end)); @@ -686,6 +766,19 @@ void Field::paintSelectedCar() { pixmapPainter.drawChord(QRectF(::x(robot.pos.x() - radius), ::y(robot.pos.y() - radius), ::w(2 * radius), ::h(2 * radius)), ::a(90.0 - chordAngel + 180 / M_PI * robot.angle), ::r(180.0 + 2 * chordAngel)); } } +void Field::paintSelectedPoints(){ + pixmapPainter.setBrush(QBrush(COLOR_GREEN)); + pixmapPainter.setPen(QPen(COLOR_GREEN, ::w(50))); + float size = 20; + std::scoped_lock lock(_G->selected_points_mutex); + if(_G->selected_points.find(_type) == _G->selected_points.end()) { + _G->selected_points[_type] = std::vector>(); + } + auto points = _G->selected_points[_type]; + for (auto& p : points) { + pixmapPainter.drawEllipse(QRectF(::x(p.first-size/2), ::y(p.second+size/2), ::w(size), ::h(-size))); + } +} void Field::paintCarShadow(const QColor& color,qreal x, qreal y, qreal radian) { static qreal radius = carDiameter / 2.0; static qreal chordAngel = qRadiansToDegrees(qAcos(1.0 * carFaceWidth / carDiameter)); diff --git a/Client/src/field.h b/Client/src/field.h index 5b92c31..62cdf6f 100644 --- a/Client/src/field.h +++ b/Client/src/field.h @@ -5,6 +5,7 @@ #include #include #include +#include #include #include "messageformat.h" #include "staticparams.h" @@ -17,7 +18,7 @@ class Field : public QQuickPaintedItem{ void paint(QPainter* painter) override; Field(QQuickItem *parent = 0); inline int type() { return this->_type; } - inline void setType(int t) { this->_type = t; } + void setType(int t); inline bool ifDraw() { return this->_draw; } inline void setDraw(bool t) { this->_draw = t; } void mouseMoveEvent(QMouseEvent *event) override; @@ -47,6 +48,7 @@ private slots: void paintCar(const QColor& color,quint8 num,qreal x,qreal y,qreal radian ,bool ifDrawNum = true,const QColor& textColor = Qt::white,bool needCircle = false); void paintSelectedCar(); + void paintSelectedPoints(); void paintCarShadow(const QColor& color,qreal x, qreal y, qreal radian); void paintBall(const QColor& color,qreal x,qreal y); void paintShadow(const QColor& color,qreal x,qreal y); @@ -66,6 +68,15 @@ private slots: void rightMoveEvent(QMouseEvent *); void rightPressEvent(QMouseEvent *); void rightReleaseEvent(QMouseEvent *); + void rightCtrlModifierMoveEvent(QMouseEvent *); + void rightCtrlModifierPressEvent(QMouseEvent *); + void rightCtrlModifierReleaseEvent(QMouseEvent *); + void rightAltModifierMoveEvent(QMouseEvent *); + void rightAltModifierPressEvent(QMouseEvent *); + void rightAltModifierReleaseEvent(QMouseEvent *); + void rightNoModifierMoveEvent(QMouseEvent *); + void rightNoModifierPressEvent(QMouseEvent *); + void rightNoModifierReleaseEvent(QMouseEvent *); void middleMoveEvent(QMouseEvent *); void middlePressEvent(QMouseEvent *); void middleReleaseEvent(QMouseEvent *); @@ -86,6 +97,7 @@ private slots: int _type; bool _draw; + // QVector selected_points; private: int pressed; bool pressedRobot; diff --git a/Client/src/globaldata.h b/Client/src/globaldata.h index f6531b6..b096594 100644 --- a/Client/src/globaldata.h +++ b/Client/src/globaldata.h @@ -5,7 +5,7 @@ #include "messageformat.h" #include "ballrecords.h" #include "zss_cmd.pb.h" -#include +#include struct RobotInformation { bool infrared; bool flat; @@ -41,7 +41,7 @@ class CGlobalData { int cameraID[PARAM::CAMERA];//show the mapping of cameras id double robotPossible[2][PARAM::ROBOTMAXID]; RobotInformation robotInformation[PARAM::TEAMS][PARAM::ROBOTMAXID]; - QMutex robotInfoMutex; + std::mutex robotInfoMutex; DataQueue robotCommand[PARAM::TEAMS]; int commandMissingFrame[PARAM::TEAMS];//team command VALID --> commandMissingFrame<20 CameraFix cameraFixMatrix[PARAM::CAMERA]; @@ -54,12 +54,14 @@ class CGlobalData { int lastTouch;//Be attention it's id!!! QByteArray debugBlueMessages; QByteArray debugYellowMessages; - QMutex debugMutex;// debugMessages; + std::mutex debugMutex;// debugMessages; bool ctrlC; - QMutex ctrlCMutex; + std::mutex ctrlCMutex; void CameraInit(); + std::mutex selected_points_mutex; + std::map>> selected_points; private: CGeoPoint saoConvert(CGeoPoint); void saoConvertEdge(); diff --git a/Client/src/vision/visionmodule.cpp b/Client/src/vision/visionmodule.cpp index f95e9a8..7db74ae 100644 --- a/Client/src/vision/visionmodule.cpp +++ b/Client/src/vision/visionmodule.cpp @@ -314,6 +314,15 @@ void CVisionModule::udpSend() { robot->set_raw_rotate_vel(result.robot[team][i].rawRotateVel); } } + auto selected_points = GlobalData::instance()->selected_points; + for (auto& it : selected_points) { + auto selected_points_proto = detectionFrame.add_selected_points(); + selected_points_proto->set_id(it.first); + for (auto& xy: it.second) { + selected_points_proto->add_x(xy.first); + selected_points_proto->add_y(xy.second); + } + } int size = detectionFrame.ByteSizeLong(); QByteArray buffer(size, 0); detectionFrame.SerializeToArray(buffer.data(), buffer.size()); @@ -327,6 +336,7 @@ void CVisionModule::udpSend() { } detectionFrame.clear_robots_blue(); detectionFrame.clear_robots_yellow(); + detectionFrame.clear_selected_points(); } /** diff --git a/ZBin/py_playground/rocos/algm/messi.py b/ZBin/py_playground/rocos/algm/messi.py index 72f3daf..0d1c583 100644 --- a/ZBin/py_playground/rocos/algm/messi.py +++ b/ZBin/py_playground/rocos/algm/messi.py @@ -6,42 +6,44 @@ from tzcp.ssl.rocos.zss_debug_pb2 import Debug_Heatmap, Debug_Msgs, Debug_Msg from tzcp.ssl.rocos.zss_geometry_pb2 import Point from threading import Event - +import time HEATMAP_COLORS = ["gray", "rainbow", "jet", "PiYG", "cool", "coolwarm", "seismic", "default"] class DEF: - HEATMAP = "rainbow" + HEATMAP = "coolwarm" FLX = 9000 FLY = 6000 PLX = 1000 PLY = 2000 ROBOT_RADIUS = 90 GL = 1000 - STEP = 100 + STEP = 80 GOAL = np.array((FLX/2, 0)) MAX_ACC = 4000 MAX_VEL = 3500 MAX_BALL_VEL = 6000 + POINTS_MAX_NUM = 2000 + def get_points_and_sizes(robot): points = np.empty((0,2)) sizes = np.empty(0) # resolution of heatmap res = DEF.STEP R = DEF.ROBOT_RADIUS - + # represent points from unimportant to important # points in back field res = 3*DEF.STEP p = np.mgrid[-DEF.FLX/2-R:0:res, -DEF.FLY/2:DEF.FLY/2:res].reshape(2, -1).T points, sizes = np.concatenate((points, p)), np.concatenate((sizes, np.ones(len(p))*res)) # points in front field - res = DEF.STEP + res = DEF.STEP*0.8 p = np.mgrid[0:DEF.FLX/2:res, -DEF.FLY/2:DEF.FLY/2:res].reshape(2, -1).T points, sizes = np.concatenate((points, p)), np.concatenate((sizes, np.ones(len(p))*res)) # points around robot - res = DEF.STEP*0.8 + res = DEF.STEP*0.3 dl = DEF.FLX/10 p = np.mgrid[-dl:dl:res, -dl:dl:res].reshape(2, -1).T circle = np.linalg.norm(p, axis=1) < 1.0*dl @@ -56,7 +58,7 @@ def get_points_and_sizes(robot): ban = np.logical_or(np.logical_or(in_their_penalty, in_our_penalty), out_of_field) points = points[~ban] sizes = sizes[~ban] - print("points", len(points)) + # print("points", len(points)) return points, sizes def dist(pos:np.ndarray, target:np.ndarray): @@ -70,7 +72,7 @@ def max_run_dist(t): w1 = np.maximum(t - 2*h/DEF.MAX_ACC, 0) return 0.5*h*(w1 + t) -def calculate_interception(points, ball, enemy): +def calculate_interception(points, ball, robot, enemy): lines = points - ball enemy_relative = enemy - ball angles = np.arctan2(lines[:,1], lines[:,0]) @@ -82,15 +84,17 @@ def calculate_interception(points, ball, enemy): dist = np.linalg.norm(lines, axis=1) ban1 = projection_x < 0 ban2 = projection_x > dist - need_compare = np.logical_or(ban1, ban2) - projection_y[need_compare] = dist_mx[need_compare] - value = -(1/np.clip(projection_y/max_run_dist(dist / DEF.MAX_BALL_VEL), 0.25, 1.25)) + projection_y[ban1] = np.linalg.norm(enemy_relative, axis=1)[np.argwhere(ban1)[:,0]] + projection_y[ban2] = dist_mx[ban2] + value = -4*(1/np.clip(projection_y/(max_run_dist(dist / DEF.MAX_BALL_VEL)+DEF.ROBOT_RADIUS), 0.5, 1.5) - 1/1.5) return value.min(axis=0) +def calculate_shoot_angle(points, ball, robot, enemy): + pass class Messi: def __init__(self): self.signal = Event() - self.receiver = UDPMultiCastReceiver("233.233.233.233", 23333, callback=self.callback, plugin = ProtobufParser(Vision_DetectionFrame)) + self.receiver = UDPMultiCastReceiver("233.233.233.233", 41001, callback=self.callback, plugin = ProtobufParser(Vision_DetectionFrame)) self.sender = UDPSender(plugin=ProtobufParser(Debug_Heatmap)) self.heatmap_endpoint = ("127.0.0.1", 20003) self.debug_endpoint = ("127.0.0.1", 20001) @@ -132,7 +136,7 @@ def test_heatmap(self): self.signal.clear() def calculate(self): self.signal.wait() - + starttime = time.time() robot = np.array([(robot.x, robot.y) for robot in self.vision.robots_blue]) enemy = np.array([(robot.x, robot.y) for robot in self.vision.robots_yellow]) ball = np.array([self.vision.balls.x, self.vision.balls.y]) @@ -142,19 +146,22 @@ def calculate(self): value = np.zeros(len(points)) # near to goal - value += 0.7*-np.clip(dist(points, DEF.GOAL),2000, 5000) / 3000 + value += 1.0*-np.clip(dist(points, DEF.GOAL),2000, 5000) / 3000 # near to robot value += -0.5*np.clip(distance_matrix(points, robot).min(axis=1) / 3000, 0.3, 1.0) # far from enemy value += 2*(np.clip(distance_matrix(points, enemy).min(axis=1) / 3000, 0.0, 0.3)) # dist to ball - value += 1*np.clip(dist(points, ball) / 3000, 0.0, 0.5) + value += -1/np.clip(dist(points, ball) / 2000, 0.2, 1.0) # intercept by enemy - value += 0.7*calculate_interception(points, ball, enemy) + value += 0.7*calculate_interception(points, ball, robot, enemy) + # value += 0.7*calculate_shoot_angle(points, ball, robot, enemy) self.send_heatmap(points, value, sizes) self.signal.clear() + print("time", time.time()-starttime) + def histogram_equalization(self, values): hist, bins = np.histogram(values, bins=256, range=(0,1)) cdf = hist.cumsum() @@ -162,6 +169,12 @@ def histogram_equalization(self, values): values = np.interp(values, bins[:-1], cdf) return values def send_heatmap(self, points, values, size=[DEF.STEP]): + if len(values) > DEF.POINTS_MAX_NUM: + index = np.argsort(-values)[:DEF.POINTS_MAX_NUM] + if len(size) == len(values): + size = size[index] + values = values[index] + points = points[index] values = norm(values) values = self.histogram_equalization(values) heatmap = Debug_Heatmap() diff --git a/ZBin/py_playground/rocos/robot/model.py b/ZBin/py_playground/rocos/robot/model.py index bbe34dc..b9efd16 100644 --- a/ZBin/py_playground/rocos/robot/model.py +++ b/ZBin/py_playground/rocos/robot/model.py @@ -54,7 +54,9 @@ def updateSpeed(self, speed): assert(speed.shape[-1] == 2) speed_wheel = (speed @ self.wheels_matrix.T) acc_wheel = np.stack([wheel.getAcc(speed_wheel[...,i]) for i, wheel in enumerate(self.wheels)],axis=-2) + print("acc_wheel", acc_wheel, acc_wheel.shape) self.acc_limit = acc_wheel[:2] * 2 + print("acc_limit : ",self.acc_limit) def getAcc(self, _theta, speed = None): # return for polar coordinate theta = np.array(_theta) - 3*np.pi/4 @@ -143,7 +145,7 @@ def getAcc(speed): cood_lim = 26 ax.set_xlim(-cood_lim,cood_lim) ax.set_ylim(-cood_lim,cood_lim) - acc, coods, others = getAcc(np.array([0.01,0.01])) + acc, coods, others = getAcc(np.array([-0.1,-0.1])) line, = ax.plot(acc[coods,0], acc[coods,1], 'g') # draw circle diff --git a/share/proto/vision_detection.proto b/share/proto/vision_detection.proto index 0d5a8cc..3b084c5 100644 --- a/share/proto/vision_detection.proto +++ b/share/proto/vision_detection.proto @@ -35,8 +35,15 @@ message Vision_DetectionRobot { optional float raw_rotate_vel = 16; } +message SelectedPoint{ + repeated float x = 1; + repeated float y = 2; + required int32 id = 3; +} + message Vision_DetectionFrame { required Vision_DetectionBall balls = 1; repeated Vision_DetectionRobot robots_yellow = 2; repeated Vision_DetectionRobot robots_blue = 3; + repeated SelectedPoint selected_points = 4; }