├── README.md
├── proto
    ├── grSim_Packet.proto
    ├── grSim_Replacement.proto
    ├── grSim_Commands.proto
    ├── vision_detection.proto
    ├── zss_debug.proto
    ├── grSim_Packet.pb.h
    ├── grSim_Packet.pb.cc
    ├── grSim_Replacement.pb.h
    ├── grSim_Commands.pb.h
    └── vision_detection.pb.h
├── communication
    ├── udpreceiver.h
    ├── udpsender.h
    ├── serialsender.h
    ├── udpsender.cpp
    ├── serialsender.cpp
    └── udpreceiver.cpp
├── utils
    ├── singleton.hpp
    ├── datamanager.cpp
    ├── visualizationmodule.h
    ├── datamanager.h
    ├── mymath.cpp
    ├── mymath.h
    ├── params.h
    └── visualizationmodule.cpp
├── algorithm
    ├── artifical_potential.h
    ├── obstacles.h
    ├── rrt.h
    ├── rrtstar.h
    ├── pathplanner.h
    ├── rrt.cpp
    ├── obstacles.cpp
    ├── rrtstar.cpp
    ├── pathplanner.cpp
    └── artifical_potential.cpp
├── .gitignore
├── LICENSE
├── ssl-homework.pro
└── main.cpp


/README.md:
--------------------------------------------------------------------------------
1 | # ssl-homework
2 | 
3 | ### feature
4 | 
5 | - 实时RRT与人工势场法的结合
6 | - 胶囊膨胀
7 | - 梯形速度规划
8 | - 强大的debug信息模块
9 | 


--------------------------------------------------------------------------------
/proto/grSim_Packet.proto:
--------------------------------------------------------------------------------
1 | import "grSim_Commands.proto";
2 | import "grSim_Replacement.proto";
3 | message grSim_Packet {
4 |     optional grSim_Commands commands = 1;
5 |     optional grSim_Replacement replacement = 2;
6 | }


--------------------------------------------------------------------------------
/communication/udpreceiver.h:
--------------------------------------------------------------------------------
 1 | #ifndef UDPRECEIVER_H
 2 | #define UDPRECEIVER_H
 3 | 
 4 | #include <QtNetwork>
 5 | 
 6 | class UDPReceiver
 7 | {
 8 | public:
 9 |     static UDPReceiver* instance();
10 |     void readDatagrams();
11 | private:
12 |     UDPReceiver();
13 |     ~UDPReceiver();
14 |     static UDPReceiver* _instance;
15 |     QUdpSocket *receiver;
16 | };
17 | 
18 | 
19 | #endif // UDPRECEIVER_H
20 | 


--------------------------------------------------------------------------------
/utils/singleton.hpp:
--------------------------------------------------------------------------------
 1 | #ifndef __SINGLETON_H__
 2 | #define __SINGLETON_H__
 3 | template <class SingletonClass>
 4 | class Singleton{
 5 | public:
 6 |     static SingletonClass * instance(){
 7 |         static SingletonClass instance;
 8 |         return &instance;
 9 |     }
10 |     SingletonClass* operator ->() { return instance(); }
11 |     const SingletonClass* operator ->() const { return instance(); }
12 | private:
13 |     Singleton(){}
14 |     ~Singleton(){}
15 | };
16 | #endif // __SINGLETON_H__
17 | 


--------------------------------------------------------------------------------
/algorithm/artifical_potential.h:
--------------------------------------------------------------------------------
 1 | #ifndef ARTIFICAL_POTENTIAL_H
 2 | #define ARTIFICAL_POTENTIAL_H
 3 | 
 4 | #include "utils/singleton.hpp"
 5 | #include "utils/mymath.h"
 6 | 
 7 | class ArtificalPotential{
 8 | public:
 9 | //    void plan( MyPoint target, MyVector v_target );
10 |     bool plan( MyPoint target );
11 |     float v_x = 0;
12 |     float v_y = 0;
13 |     float v_w = 0;
14 |     bool last_statue = false;
15 | };
16 | 
17 | typedef Singleton<ArtificalPotential> ApPlanner;
18 | 
19 | #endif // ARTIFICAL_POTENTIAL_H
20 | 


--------------------------------------------------------------------------------
/proto/grSim_Replacement.proto:
--------------------------------------------------------------------------------
 1 | message grSim_RobotReplacement {
 2 | required double x=1;
 3 | required double y=2;
 4 | required double dir=3;
 5 | required uint32 id=4;
 6 | required bool yellowteam=5;
 7 | optional bool turnon=6;
 8 | }
 9 | 
10 | message grSim_BallReplacement {
11 | required double x=1;
12 | required double y=2;
13 | required double vx=3;
14 | required double vy=4;
15 | }
16 | 
17 | message grSim_Replacement {
18 | optional grSim_BallReplacement ball = 1;
19 | repeated grSim_RobotReplacement robots = 2;
20 | }
21 | 


--------------------------------------------------------------------------------
/communication/udpsender.h:
--------------------------------------------------------------------------------
 1 | #ifndef UDPSENDER_H
 2 | #define UDPSENDER_H
 3 | 
 4 | #include <QObject>
 5 | #include <QtNetwork>
 6 | #include "utils/singleton.hpp"
 7 | 
 8 | class UDPSender : public QObject
 9 | {
10 |     Q_OBJECT
11 | public:
12 |     explicit UDPSender(QObject *parent = nullptr);
13 |     ~UDPSender();
14 |     void sendToSim(int robot_id=0, double vel_x=0, double vel_y=0, double vel_w=0);
15 | private:
16 |     QUdpSocket* sender;
17 | };
18 | typedef Singleton<UDPSender> CommandSender;
19 | 
20 | #endif // UDPSENDER_H
21 | 


--------------------------------------------------------------------------------
/algorithm/obstacles.h:
--------------------------------------------------------------------------------
 1 | #ifndef OBSTACLES_H
 2 | #define OBSTACLES_H
 3 | #include "utils/singleton.hpp"
 4 | 
 5 | //enum inflationType{
 6 | //    CIRCLE = 1,
 7 | //    CAPSULE = 2,
 8 | //};
 9 | 
10 | class Obstacles
11 | {
12 | public:
13 |     Obstacles(){}
14 |     bool hasObstacle(double x, double y, int type);
15 |     bool hasObstacle(double x1, double y1, double x2, double y2, int type);
16 |     double distanceofPointandLine(double pointx, double pointy, double startx, double starty, double endx, double endy);
17 | };
18 | typedef Singleton<Obstacles> ObstaclesInfo;
19 | 
20 | #endif // OBSTACLES_H
21 | 


--------------------------------------------------------------------------------
/proto/grSim_Commands.proto:
--------------------------------------------------------------------------------
 1 | message grSim_Robot_Command {
 2 | required uint32 id = 1;
 3 | required float kickspeedx = 2;
 4 | required float kickspeedz = 3;
 5 | required float veltangent = 4;
 6 | required float velnormal  = 5;
 7 | required float velangular = 6;
 8 | required bool spinner = 7;
 9 | required bool wheelsspeed = 8;
10 | optional float wheel1 = 9;
11 | optional float wheel2 = 10;
12 | optional float wheel3 = 11;
13 | optional float wheel4 = 12;
14 | }
15 | 
16 | message grSim_Commands {
17 | required double timestamp = 1;
18 | required bool isteamyellow = 2;
19 | repeated grSim_Robot_Command robot_commands = 3;
20 | }
21 | 
22 | 


--------------------------------------------------------------------------------
/algorithm/rrt.h:
--------------------------------------------------------------------------------
 1 | #ifndef RRT_H
 2 | #define RRT_H
 3 | #include "utils/mymath.h"
 4 | #include "utils/singleton.hpp"
 5 | #include <vector>
 6 | 
 7 | class RRT
 8 | {
 9 | public:
10 |     RRT(){}
11 |     void plan(double start_x, double start_y, double end_x, double end_y);
12 |     std::vector<MyPoint> finalPath;
13 |     std::vector<MyPoint> smoothPath;
14 |     std::vector<Node> NodeList;
15 | private:
16 |     int findNearestNode(int x, int y);
17 |     void pathSmooth();
18 |     bool inNodeList(int x, int y);
19 |     Node randomSample(double epsilon, int end_x, int end_y);
20 | };
21 | typedef Singleton<RRT> RRTPlanner;
22 | 
23 | #endif // RRT_H
24 | 


--------------------------------------------------------------------------------
/.gitignore:
--------------------------------------------------------------------------------
 1 | # C++ objects and libs
 2 | *.slo
 3 | *.lo
 4 | *.o
 5 | *.a
 6 | *.la
 7 | *.lai
 8 | *.so
 9 | *.dll
10 | *.dylib
11 | 
12 | # Qt-es
13 | object_script.*.Release
14 | object_script.*.Debug
15 | *_plugin_import.cpp
16 | /.qmake.cache
17 | /.qmake.stash
18 | *.pro.user
19 | *.pro.user.*
20 | *.qbs.user
21 | *.qbs.user.*
22 | *.moc
23 | moc_*.cpp
24 | moc_*.h
25 | qrc_*.cpp
26 | ui_*.h
27 | *.qmlc
28 | *.jsc
29 | Makefile*
30 | *build-*
31 | 
32 | # Qt unit tests
33 | target_wrapper.*
34 | 
35 | # QtCreator
36 | *.autosave
37 | 
38 | # QtCreator Qml
39 | *.qmlproject.user
40 | *.qmlproject.user.*
41 | 
42 | # QtCreator CMake
43 | CMakeLists.txt.user*
44 | 


--------------------------------------------------------------------------------
/utils/datamanager.cpp:
--------------------------------------------------------------------------------
 1 | #include "datamanager.h"
 2 | 
 3 | DataManager::DataManager() : cycle(0)
 4 | {
 5 |     reset();
 6 | }
 7 | 
 8 | void DataManager::reset()
 9 | {
10 |     for (int i=0; i<PARAMS::ROBOT_NUM; i++){
11 |         validBlueRobots[i] = false;
12 |         validYellowRobots[i] = false;
13 |     }
14 | }
15 | 
16 | RobotInfo& DataManager::ourRobot()
17 | {
18 |     if (PARAMS::isBlue)
19 |         return blueRobots[PARAMS::IS_SIMULATION ? PARAMS::our_id-1 : PARAMS::our_id-1];
20 |     else
21 |         return yellowRobots[PARAMS::IS_SIMULATION ? PARAMS::our_id-1 : PARAMS::our_id-1];
22 | }
23 | 
24 | void DataManager::updateCycle()
25 | {
26 |     if(cycle < 4000000000)
27 |         cycle++;
28 |     else
29 |         cycle = 0;
30 | }
31 | 


--------------------------------------------------------------------------------
/communication/serialsender.h:
--------------------------------------------------------------------------------
 1 | #ifndef SERIALSENDER_H
 2 | #define SERIALSENDER_H
 3 | 
 4 | #include <QSerialPort>
 5 | #include "utils/singleton.hpp"
 6 | 
 7 | class serialSender
 8 | {
 9 | public:
10 |     serialSender();
11 |     void openSerialPort();
12 |     void closeSerialPort();
13 |     void sendStartPacket();
14 |     void encode(int robot_id=0, int vel_x=0, int vel_y=0, int vel_w=0, bool ctrl=false, bool shootMode=false, int ctrlPowerLevel=0);
15 |     void sendToReal(int robot_id=0, int vel_x=0, int vel_y=0, int vel_w=0);
16 | private:
17 |     QSerialPort serial;
18 |     QByteArray startPacket1;
19 |     QByteArray startPacket2;
20 |     QByteArray transmitPacket;
21 | };
22 | typedef Singleton<serialSender> RealCommandSender;
23 | 
24 | #endif // SERIALSENDER_H
25 | 


--------------------------------------------------------------------------------
/algorithm/rrtstar.h:
--------------------------------------------------------------------------------
 1 | #ifndef RRTSTAR_H
 2 | #define RRTSTAR_H
 3 | #include <vector>
 4 | #include "utils/mymath.h"
 5 | #include "utils/singleton.hpp"
 6 | 
 7 | class RRTStar
 8 | {
 9 | public:
10 |     RRTStar(){}
11 |     void plan(double start_x, double start_y, double end_x, double end_y);
12 |     std::vector<MyPoint> finalPath;
13 |     std::vector<MyPoint> smoothPath;
14 | private:
15 |     int chooseBestParent(int x, int y, double radius, double old_cost);
16 |     int findNearestNode(int x, int y);
17 | //    void findNearNodes(std::vector<Node> &nearNodes);
18 |     void rewire(Node newNode, double radius);
19 |     void pathSmooth();
20 |     bool inNodeList(int x, int y);
21 |     Node randomSample(double epsilon, int end_x, int end_y);
22 |     std::vector<Node> NodeList;
23 | };
24 | typedef Singleton<RRTStar> RRTStarPlanner;
25 | 
26 | #endif // RRTSTAR_H
27 | 


--------------------------------------------------------------------------------
/utils/visualizationmodule.h:
--------------------------------------------------------------------------------
 1 | #ifndef VISUALIZATIONMODULE_H
 2 | #define VISUALIZATIONMODULE_H
 3 | 
 4 | #include <vector>
 5 | #include <QObject>
 6 | #include <QtNetwork>
 7 | #include "utils/singleton.hpp"
 8 | #include "utils/mymath.h"
 9 | #include "proto/zss_debug.pb.h"
10 | #include "proto/grSim_Packet.pb.h"
11 | #include <deque>
12 | 
13 | class visualizationmodule : public QObject
14 | {
15 |     Q_OBJECT
16 | public:
17 |     explicit visualizationmodule(QObject *parent = nullptr);
18 |     ~visualizationmodule();
19 |     void drawTree(std::vector<Node> &someNodes);
20 |     void drawPoints(std::vector<MyPoint> &somePoints);
21 |     void drawLines(std::vector<MyPoint> &somePoints);
22 |     void drawPoint(MyPoint point);
23 |     void drawAll(std::deque<MyPoint> goals);
24 | private:
25 |     QUdpSocket* sender;
26 | signals:
27 | 
28 | public slots:
29 | };
30 | 
31 | typedef Singleton<visualizationmodule> VisualModule;
32 | #endif // VISUALIZATIONMODULE_H
33 | 


--------------------------------------------------------------------------------
/proto/vision_detection.proto:
--------------------------------------------------------------------------------
 1 | syntax = "proto2";
 2 | 
 3 | message Vision_DetectionBall {
 4 |   optional float  vel_x      =  1;
 5 |   optional float  vel_y      =  2; 
 6 |   optional uint32 area       =  3;
 7 |   required float  x          =  4;
 8 |   required float  y          =  5;
 9 |   optional float  height     =  6;
10 |   optional uint32 ball_state =  7;
11 |   optional uint32 last_touch =  8;
12 |   required bool   valid      =  9;
13 | }
14 | 
15 | message Vision_DetectionRobot {
16 |   required float  confidence  =  1;
17 |   optional uint32 robot_id    =  2;
18 |   required float  x           =  3;
19 |   required float  y           =  4;
20 |   optional float  orientation =  5;
21 |   optional float  vel_x       =  6;
22 |   optional float  vel_y       =  7; 
23 |   optional float  rotate_vel  =  8;
24 | }
25 | 
26 | message Vision_DetectionFrame {
27 |   required Vision_DetectionBall  balls         = 1;
28 |   repeated Vision_DetectionRobot robots_yellow = 2;
29 |   repeated Vision_DetectionRobot robots_blue   = 3;
30 | }
31 | 


--------------------------------------------------------------------------------
/LICENSE:
--------------------------------------------------------------------------------
 1 | MIT License
 2 | 
 3 | Copyright (c) 2019 0AQZ0
 4 | 
 5 | Permission is hereby granted, free of charge, to any person obtaining a copy
 6 | of this software and associated documentation files (the "Software"), to deal
 7 | in the Software without restriction, including without limitation the rights
 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 9 | copies of the Software, and to permit persons to whom the Software is
10 | furnished to do so, subject to the following conditions:
11 | 
12 | The above copyright notice and this permission notice shall be included in all
13 | copies or substantial portions of the Software.
14 | 
15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
21 | SOFTWARE.
22 | 


--------------------------------------------------------------------------------
/utils/datamanager.h:
--------------------------------------------------------------------------------
 1 | #ifndef DATAMANAGER_H
 2 | #define DATAMANAGER_H
 3 | #include "utils/singleton.hpp"
 4 | #include "utils/params.h"
 5 | #include "utils/mymath.h"
 6 | #include <deque>
 7 | 
 8 | struct BallInfo
 9 | {
10 |     float vel_x;
11 |     float vel_y;
12 |     float x;
13 |     float y;
14 |     bool valid;
15 |     BallInfo() : vel_x(9999), vel_y(9999), x(9999), y(9999), valid(false){}
16 | };
17 | 
18 | struct RobotInfo
19 | {
20 |     int robot_id;
21 |     float x;
22 |     float y;
23 |     float orientation;
24 |     float vel_x;
25 |     float vel_y;
26 |     float rotate_vel;
27 |     RobotInfo() : robot_id(9999), x(9999), y(9999), orientation(9999),
28 |     vel_x(9999), vel_y(9999), rotate_vel(9999){}
29 | };
30 | 
31 | class DataManager
32 | {
33 | public:
34 |     DataManager();
35 |     BallInfo ball;
36 |     RobotInfo blueRobots[PARAMS::ROBOT_NUM];
37 |     RobotInfo yellowRobots[PARAMS::ROBOT_NUM];
38 |     bool validBlueRobots[PARAMS::ROBOT_NUM];
39 |     bool validYellowRobots[PARAMS::ROBOT_NUM];
40 |     unsigned long cycle;
41 |     RobotInfo& ourRobot();
42 |     void updateCycle();
43 |     void reset();
44 | };
45 | typedef Singleton<DataManager> MyDataManager;
46 | 
47 | #endif // DATAMANAGER_H
48 | 


--------------------------------------------------------------------------------
/communication/udpsender.cpp:
--------------------------------------------------------------------------------
 1 | #include "udpsender.h"
 2 | #include "proto/grSim_Packet.pb.h"
 3 | #include "proto/zss_debug.pb.h"
 4 | #include "utils/params.h"
 5 | 
 6 | UDPSender::UDPSender(QObject *parent) : QObject(parent)
 7 | {
 8 |     sender = new QUdpSocket();
 9 | }
10 | 
11 | UDPSender::~UDPSender()
12 | {
13 |     sender->abort();
14 | }
15 | 
16 | void UDPSender::sendToSim(int robot_id, double vel_x, double vel_y, double vel_w)
17 | {
18 |     grSim_Packet packet;
19 |     auto* command = packet.mutable_commands();
20 |     command->set_timestamp(0);
21 |     if(PARAMS::isBlue)
22 |         command->set_isteamyellow(false);
23 |     else
24 |         command->set_isteamyellow(true);
25 |     auto* robot_command = command->add_robot_commands();
26 |     robot_command->set_id(robot_id-1);
27 |     robot_command->set_kickspeedx(0);
28 |     robot_command->set_kickspeedz(0);
29 |     robot_command->set_veltangent(vel_x);
30 |     robot_command->set_velnormal(vel_y);
31 |     robot_command->set_velangular(vel_w);
32 |     robot_command->set_spinner(0);
33 |     robot_command->set_wheelsspeed(false);
34 |     int size = packet.ByteSize();
35 |     QByteArray data(size, 0);
36 |     packet.SerializePartialToArray(data.data(), data.size());
37 |     sender->writeDatagram(data, size, PARAMS::simAddress, PARAMS::simPort);
38 | }
39 | 


--------------------------------------------------------------------------------
/algorithm/pathplanner.h:
--------------------------------------------------------------------------------
 1 | #ifndef PATHPLANNER_H
 2 | #define PATHPLANNER_H
 3 | #include <deque>
 4 | #include "communication/udpsender.h"
 5 | #include "communication/serialsender.h"
 6 | #include "utils/datamanager.h"
 7 | #include "utils/mymath.h"
 8 | 
 9 | /**************************************************************/
10 | /*                         速度规划                            */
11 | /*          Description: 作为将路径点转换为具体下发速度的API       */
12 | /**************************************************************/
13 | 
14 | class PathPlanner
15 | {
16 | public:
17 |     PathPlanner() : velX(0), velY(0), velW(0){}
18 |     void plan();
19 |     bool hasArrived(MyPoint target);
20 |     bool moveToNext(MyPoint target);
21 |     void rotateToPoint(MyPoint target);
22 |     void goToPoint(MyPoint target);
23 |     void goToPointTrapezoid(MyPoint target);
24 |     float goToPosition1d( float me2target_dis, float me_vel );
25 |     // return unit is mm, input target unit is cm
26 |     void goToPosition2d( MyPoint target );
27 |     void stopMoving();
28 |     void updatePath(std::vector<Node> &nodePath);
29 |     void updatePath(std::vector<MyPoint> &pointPath);
30 |     void clearPath();
31 |     int pathSize(){ return path.size(); }
32 |     double velX;
33 |     double velY;
34 |     double velW;
35 |     std::deque<MyPoint> path;
36 | };
37 | 
38 | #endif // PATHPLANNER_H
39 | 


--------------------------------------------------------------------------------
/utils/mymath.cpp:
--------------------------------------------------------------------------------
 1 | #include "mymath.h"
 2 | #include <iostream>
 3 | 
 4 | MyVector MyPoint::operator -( const MyPoint& p ) const{
 5 |     float vec_x = _x - p.x();
 6 |     float vec_y = _y - p.y();
 7 |     return MyVector( vec_x, vec_y );
 8 | }
 9 | 
10 | MyVector MyVector::operator *( const float a ) const{
11 |     float vec_x = _x * a;
12 |     float vec_y = _y * a;
13 |     return MyVector( vec_x, vec_y );
14 | }
15 | 
16 | MyVector MyVector::operator -( const MyVector& v ) const{
17 |     float vec_x = _x - v.x();
18 |     float vec_y = _y - v.y();
19 |     return MyVector( vec_x, vec_y );
20 | }
21 | 
22 | MyVector MyVector::operator +( const MyVector& v ) const{
23 |     float vec_x = _x + v.x();
24 |     float vec_y = _y + v.y();
25 |     return MyVector( vec_x, vec_y );
26 | }
27 | 
28 | MyVector MyVector::operator /( const float a ) const{
29 |     float vec_x = _x / a;
30 |     float vec_y = _y / a;
31 |     return MyVector( vec_x, vec_y );
32 | }
33 | 
34 | 
35 | float MyVector::operator *( const MyVector& v ) const{
36 |     float result = v.x() * _x + v.y() * _y;
37 |     return result;
38 | }
39 | 
40 | void MyVector::operator =( const MyVector& v ){
41 |     _x = v.x();
42 |     _y = v.y();
43 | }
44 | 
45 | void MyPoint::operator =( const MyPoint& p ){
46 |     _x = p.x();
47 |     _y = p.y();
48 | }
49 | 
50 | MyVector MyVector::Unitization(){
51 |     float vec_x = _x / this->mod();
52 |     float vec_y = _y / this->mod();
53 |     return MyVector( vec_x, vec_y );
54 | }
55 | 


--------------------------------------------------------------------------------
/utils/mymath.h:
--------------------------------------------------------------------------------
 1 | #ifndef MYMATH_H
 2 | #define MYMATH_H
 3 | 
 4 | #include <cmath>
 5 | 
 6 | class MyVector{
 7 | public:
 8 |     MyVector(): _x(0), _y(0){}
 9 |     MyVector( float x, float y ): _x(x), _y(y){}
10 |     float x ( void ) const { return _x; }
11 |     float y ( void ) const { return _y; }
12 |     void Setx ( float x ) { _x = x; }
13 |     void Sety ( float y ) { _y = y; }
14 |     float mod() { return sqrt( _x * _x + _y * _y ); }
15 |     float dir() { return std::atan2( _y, _x ); }
16 |     MyVector operator*( const float a ) const;
17 |     MyVector operator-( const MyVector& v ) const;
18 |     MyVector operator+( const MyVector& v ) const;
19 |     MyVector operator/( const float a ) const;
20 |     float operator*( const MyVector& v ) const;
21 |     void operator=( const MyVector& v );
22 |     MyVector Unitization();
23 | private:
24 |     float _x;
25 |     float _y;
26 | };
27 | 
28 | class MyPoint{
29 | public:
30 |     MyPoint(): _x(0), _y(0) {}
31 |     MyPoint( float x, float y ): _x(x), _y(y){}
32 |     float x( void ) const { return _x; }
33 |     float y( void ) const { return _y; }
34 |     void Setx ( float x ) { _x = x; }
35 |     void Sety ( float y ) { _y = y; }
36 |     MyVector operator-(const MyPoint& p) const;
37 |     void operator=(const MyPoint& p);
38 | private:
39 |     float _x;
40 |     float _y;
41 | };
42 | 
43 | class Node {
44 | public:
45 |     Node(int x, int y,int parent=-1, double cost=0) {
46 |         this->x = x;
47 |         this->y = y;
48 |         this->parent = parent;
49 |         cost = 0.0;
50 |     }
51 |     int x, y;
52 |     int parent;
53 |     double cost;
54 | };
55 | 
56 | #endif // MYMATH_H
57 | 


--------------------------------------------------------------------------------
/proto/zss_debug.proto:
--------------------------------------------------------------------------------
 1 | syntax = "proto2";
 2 | package ZSS.Protocol;
 3 | 
 4 | message Point{
 5 | 	required float x = 1;
 6 | 	required float y = 2;
 7 | }
 8 | message Rectangle{
 9 | 	required Point point1 = 1;
10 | 	required Point point2 = 2;
11 | }
12 | message Debug_Robot{
13 | 	required Point pos = 1;
14 | 	required float dir = 2;
15 | }
16 | message Debug_Line {
17 | 	required Point start = 1;
18 | 	required Point end = 2;
19 | 	required bool FORWARD = 3;
20 | 	required bool BACK = 4;
21 | }
22 | message Debug_Arc {
23 | 	required Rectangle rectangle = 1;
24 | 	required float start = 2;
25 | 	required float end = 3;
26 | 	required bool FILL = 4;
27 | }
28 | message Debug_Polygon {
29 | 	repeated Point vertex = 1;
30 | 	required bool FILL = 2;
31 | }
32 | message Debug_Text {
33 | 	required Point pos = 1;
34 | 	required string text = 2;
35 | }
36 | message Debug_Curve_ {
37 | 	required float num = 1;
38 | 	required float maxLimit = 2;
39 | 	required float minLimit = 3;
40 | }
41 | message Debug_Curve {
42 | 	required Point start = 1;
43 | 	required Point p1 = 2;
44 | 	required Point p2 = 3;
45 | 	required Point end = 4;
46 | }
47 | message Debug_Msg {
48 | 	enum Debug_Type {
49 | 		ARC 	= 0;
50 | 		LINE 	= 1;		
51 | 		TEXT 	= 2;
52 | 		ROBOT 	= 3;
53 | 		CURVE 	= 4;
54 | 		POLYGON = 5;
55 | 	}
56 | 	required Debug_Type type = 1;
57 | 	enum Color {
58 | 		WHITE = 	0;
59 | 		RED = 		1;
60 | 		ORANGE = 	2;
61 | 		YELLOW = 	3;
62 | 		GREEN = 	4;
63 | 		CYAN = 		5;
64 | 		BLUE = 		6;
65 | 		PURPLE = 	7;
66 | 		GRAY = 		8;
67 | 		BLACK = 	9;
68 | 	}
69 | 	required Color color = 2;
70 | 	// one of the follow >>> need to be tested with TYPE <oneof> 
71 | 	optional Debug_Arc arc = 	3;
72 | 	optional Debug_Line line = 	4;
73 | 	optional Debug_Text text = 	5;
74 | 	optional Debug_Robot robot = 	6;
75 | 	optional Debug_Curve_ curve = 	7;
76 | 	optional Debug_Polygon polygon = 8;
77 | }
78 | message Debug_Msgs{
79 | 	repeated Debug_Msg msgs= 1;
80 | }


--------------------------------------------------------------------------------
/ssl-homework.pro:
--------------------------------------------------------------------------------
 1 | QT += gui network
 2 | QT += serialport
 3 | 
 4 | CONFIG += c++11 console
 5 | CONFIG -= app_bundle
 6 | 
 7 | # The following define makes your compiler emit warnings if you use
 8 | # any feature of Qt which as been marked deprecated (the exact warnings
 9 | # depend on your compiler). Please consult the documentation of the
10 | # deprecated API in order to know how to port your code away from it.
11 | DEFINES += QT_DEPRECATED_WARNINGS
12 | 
13 | # You can also make your code fail to compile if you use deprecated APIs.
14 | # In order to do so, uncomment the following line.
15 | # You can also select to disable deprecated APIs only up to a certain version of Qt.
16 | #DEFINES += QT_DISABLE_DEPRECATED_BEFORE=0x060000    # disables all the APIs deprecated before Qt 6.0.0
17 | 
18 | SOURCES += main.cpp \
19 |     communication\udpreceiver.cpp \
20 |     proto\vision_detection.pb.cc \
21 |     proto\grSim_Commands.pb.cc \
22 |     proto\grSim_Packet.pb.cc \
23 |     proto\grSim_Replacement.pb.cc \
24 |     communication\udpsender.cpp \
25 |     utils/datamanager.cpp \
26 |     algorithm/pathplanner.cpp \
27 |     proto/zss_debug.pb.cc \
28 |     communication\serialsender.cpp \
29 |     algorithm/artifical_potential.cpp \
30 |     utils/mymath.cpp \
31 |     algorithm/rrtstar.cpp \
32 |     algorithm/rrt.cpp \
33 |     utils/visualizationmodule.cpp \
34 |     algorithm/obstacles.cpp
35 | 
36 | HEADERS += \
37 |     communication\udpreceiver.h \
38 |     proto\vision_detection.pb.h \
39 |     proto\grSim_Commands.pb.h \
40 |     proto\grSim_Packet.pb.h \
41 |     proto\grSim_Replacement.pb.h \
42 |     communication\udpsender.h \
43 |     utils/singleton.hpp \
44 |     utils/datamanager.h \
45 |     algorithm/pathplanner.h \
46 |     proto/zss_debug.pb.h \
47 |     communication\serialsender.h \
48 |     algorithm/artifical_potential.h \
49 |     utils/mymath.h \
50 |     utils/params.h \
51 |     algorithm/rrtstar.h \
52 |     algorithm/rrt.h \
53 |     utils/visualizationmodule.h \
54 |     algorithm/obstacles.h
55 | 
56 | INCLUDEPATH += \
57 |     ..\protobuf\include
58 | 
59 | LIBS += \
60 |     ..\protobuf\lib\x86\libprotobuf.lib
61 | 


--------------------------------------------------------------------------------
/utils/params.h:
--------------------------------------------------------------------------------
 1 | #ifndef PARAMS_H
 2 | #define PARAMS_H
 3 | #include <QString>
 4 | #include <QHostAddress>
 5 | 
 6 | namespace PARAMS {
 7 |     const bool IS_SIMULATION = false;
 8 |     const double ACCEPT_RADIUS = 20.0;
 9 | 
10 |     // data manager
11 |     const int ROBOT_NUM = 12;
12 | 
13 |     // our car
14 |     const int our_id = 3;  //从1开始
15 |     const bool isBlue =true;
16 | 
17 |     // field
18 |     namespace FIELD {
19 |         const int LENGTH = 580;
20 |         const int WIDTH = 380;
21 |     }
22 | 
23 |     // serial sender
24 |     const QString serialPort = "COM17";
25 |     const int frequency = 0;
26 |     const int TRANSMIT_PACKET_SIZE = 25;
27 | 
28 |     // UDP receiver
29 |     const int visionPort = 23333;
30 | 
31 |     // UDP sender
32 |     const int simPort = 20011;
33 |     const QHostAddress simAddress = QHostAddress("127.0.0.1");
34 | 
35 |     // path planning
36 |     const double ANGLE_THRESHOLD = 0.1;
37 |     const double FORWARD_ANGLE_THRESHOLD = 0.5;
38 |     const double ROTATE_COFF = 4;
39 |     const double FORWARD_SPEED = 2;//原本是5
40 |     const double FORWARD_ACC = 5;//原本是7.5
41 |     const double FORWARD_ROTATE_COFF = 3.0;
42 | 
43 |     //visualization module
44 |     const QHostAddress visualAddress = QHostAddress("127.0.0.1");
45 |     const int visualPort = 20001;
46 | 
47 |     // RRT
48 |     namespace RRT {
49 |         const int ITERATIONS = 1e3;
50 |         const int STEP_SIZE = 5;
51 |         const double EPSILON = 0.1;
52 |     }
53 | 
54 |     // RRTStar
55 |     namespace RRTStar {
56 |         const int ITERATIONS = 1e4;
57 |         const int STEP_SIZE = 2;
58 |         const double EPSILON = 0.1;
59 |         const double GAMMA = 30.0;
60 |     }
61 | 
62 |     // math constant
63 |     namespace MATH {
64 |         const double PI = 3.14159265359;
65 |     }
66 | 
67 |     // FPS
68 |     const int FRAME = 65;
69 | 
70 |     // obstacle module
71 |     namespace OBSTACLE {
72 |         //1 is CIRCLE , 2 is CAPSULE
73 |         const int OBSTACLETYPE = 2;
74 |         const double INFLATION_RADIUS = 20.0;//CAPSULE
75 |         const double INFLATION_RADIUS_SQUARE = 900;//CIRCLE
76 |         const double STEP_SIZE = 10;
77 |         const double REFRESH_TIME = 0.02;
78 |     }
79 | 
80 |     namespace DEBUG {
81 |         // switch for artifical potential message
82 |         const bool kAPDebugMessage  = false;
83 |         const bool kMainDebug       = false;
84 |         const bool kShowPathPlanner = false;
85 |         const bool kgoToPosition2d  = false;
86 |         const bool kUdpReceiver     = false;
87 | 
88 |         const bool RRTDebug = false;
89 |         const bool pathPlannerDebug = false;
90 |     }
91 | }
92 | 
93 | #endif // PARAMS_H
94 | 


--------------------------------------------------------------------------------
/main.cpp:
--------------------------------------------------------------------------------
  1 | #include <QCoreApplication>
  2 | #include "communication/udpreceiver.h"
  3 | #include "communication/udpsender.h"
  4 | #include "communication/serialsender.h"
  5 | #include "utils/datamanager.h"
  6 | #include "utils/mymath.h"
  7 | #include "algorithm/pathplanner.h"
  8 | #include "algorithm/rrt.h"
  9 | #include "algorithm/rrtstar.h"
 10 | #include "algorithm/artifical_potential.h"
 11 | #include "algorithm/obstacles.h"
 12 | #include "utils/visualizationmodule.h"
 13 | #include <thread>
 14 | #include <iostream>
 15 | 
 16 | serialSender serial;
 17 | PathPlanner localPlanner;
 18 | 
 19 | // set Goals
 20 | std::deque<MyPoint> goals = { MyPoint(-200, -100), MyPoint(200, 100)}; // -300是边界
 21 | 
 22 | bool updateRRT()
 23 | {
 24 |     bool update = false;
 25 |     // have arrived && target change && no path
 26 |     if(localPlanner.pathSize() == 0)
 27 |         update = true;
 28 |     // meet obstacles
 29 |     RobotInfo& me = MyDataManager::instance()->ourRobot();
 30 |     if(localPlanner.pathSize() != 0){
 31 |         MyPoint target = localPlanner.path.front();
 32 |         if(ObstaclesInfo::instance()->hasObstacle(me.x, me.y, target.x(), target.y(), PARAMS::OBSTACLE::OBSTACLETYPE))
 33 |             update = true;
 34 |     }
 35 |     // 到了下一个点重新更新
 36 | 
 37 |     // can reach my goal directly
 38 |     MyPoint goal = goals.front();
 39 |     if(!ObstaclesInfo::instance()->hasObstacle(me.x, me.y, goal.x(), goal.y(), PARAMS::OBSTACLE::OBSTACLETYPE))
 40 |         update = true;
 41 | 
 42 |     return update;
 43 | }
 44 | 
 45 | 
 46 | int main(int argc, char *argv[])
 47 | {
 48 |     QCoreApplication a(argc, argv);
 49 |     UDPReceiver::instance();
 50 | 
 51 |     // open serial
 52 |     if(!PARAMS::IS_SIMULATION){
 53 | //        serial.sendStartPacket();
 54 |         serial.openSerialPort();
 55 |     }
 56 | 
 57 |     // vel sending
 58 |     while(true){
 59 |         if ( PARAMS::DEBUG::kMainDebug ){
 60 |             std::cout << "[main.cpp] send velocity" << std::endl;
 61 |         }
 62 |         std::this_thread::sleep_for(std::chrono::milliseconds(30));
 63 |         bool if_use_artifical = ApPlanner::instance()->plan(goals.front());
 64 |         if ( !if_use_artifical || true){
 65 |             // update RRT
 66 |             if(updateRRT()){
 67 | //                qDebug() << "path planning!";
 68 |                 RRTPlanner::instance()->plan(MyDataManager::instance()->ourRobot().x, MyDataManager::instance()->ourRobot().y, goals.front().x(), goals.front().y());
 69 |                 localPlanner.updatePath(RRTPlanner::instance()->smoothPath);
 70 |             }
 71 |             // change goals
 72 |             if(localPlanner.hasArrived(goals.front())){
 73 |                 goals.push_back(goals.front());
 74 |                 goals.pop_front();
 75 |                 localPlanner.stopMoving();
 76 |                 localPlanner.clearPath();
 77 | //                qDebug() << "Change Goal to " << goals.front().x() << goals.front().y();
 78 |             }
 79 |             localPlanner.plan();
 80 |             if(PARAMS::IS_SIMULATION)
 81 |                 CommandSender::instance()->sendToSim(PARAMS::our_id, localPlanner.velX, -localPlanner.velY, localPlanner.velW);
 82 |             else
 83 |                 // 从1开始
 84 | //                serial.sendToReal(PARAMS::our_id, 180 * localPlanner.velX, 180 * localPlanner.velY, -40*localPlanner.velW);
 85 |                 serial.sendToReal(PARAMS::our_id, 140 * localPlanner.velX, 200 * localPlanner.velY, 0);
 86 | //                 qDebug() << "vel: "<< localPlanner.velX << localPlanner.velY << localPlanner.velW;
 87 |                 // qDebug() << MyDataManager::instance()->ourRobot().x << MyDataManager::instance()->ourRobot().y;
 88 |         }
 89 |         else {
 90 |             if(PARAMS::IS_SIMULATION){
 91 |                  CommandSender::instance()->sendToSim(PARAMS::our_id,
 92 |                                                       ApPlanner::instance()->v_x / 1000,
 93 |                                                       ApPlanner::instance()->v_y / 1000,
 94 |                                                       ApPlanner::instance()->v_w);
 95 |             }
 96 |             else{
 97 |                 serial.sendToReal(PARAMS::our_id,
 98 |                                   ApPlanner::instance()->v_x / 10,
 99 |                                   ApPlanner::instance()->v_y / 10,
100 |                                   ApPlanner::instance()->v_w);
101 |             }
102 |         }
103 |         if ( !if_use_artifical )
104 |         {
105 |             VisualModule::instance()->drawAll(goals);
106 | //            VisualModule::instance()->drawLines(RRTPlanner::instance()->smoothPath);
107 |         }
108 |         else{
109 | //            VisualModule::instance()->drawPoint(MyPoint(0, -0));
110 |             VisualModule::instance()->drawAll(goals);
111 |         }
112 |     }
113 |     return a.exec();
114 | }
115 | 


--------------------------------------------------------------------------------
/algorithm/rrt.cpp:
--------------------------------------------------------------------------------
  1 | #include "rrt.h"
  2 | #include "utils/params.h"
  3 | #include <QRandomGenerator>
  4 | #include "algorithm/obstacles.h"
  5 | 
  6 | void RRT::plan(double start_x, double start_y, double end_x, double end_y)
  7 | {
  8 |     // exploration
  9 |     NodeList.clear();
 10 |     NodeList.push_back(Node(start_x, start_y));
 11 | 
 12 |     for(int iter=0; iter<PARAMS::RRT::ITERATIONS; iter++) {
 13 |         // debug info
 14 |         if(iter > PARAMS::RRT::ITERATIONS - 10 && PARAMS::DEBUG::RRTDebug)
 15 |             qDebug() << "planning fials!!!";
 16 |         // random sampling
 17 |         Node randNode = randomSample(PARAMS::RRT::EPSILON, end_x, end_y);
 18 |         // Find the nearest node
 19 |         int nearestNode = findNearestNode(randNode.x, randNode.y);
 20 |         // expand the tree
 21 |         double theta = atan2(randNode.y - NodeList[nearestNode].y, randNode.x - NodeList[nearestNode].x);
 22 |         int newNode_x = NodeList[nearestNode].x + PARAMS::RRT::STEP_SIZE*cos(theta);
 23 |         int newNode_y = NodeList[nearestNode].y + PARAMS::RRT::STEP_SIZE*sin(theta);
 24 | 
 25 |         if(PARAMS::DEBUG::RRTDebug)
 26 |             qDebug() << "newNode: " << newNode_x << newNode_y << nearestNode;
 27 | 
 28 |         // outside the map
 29 |         if (newNode_x <= -PARAMS::FIELD::LENGTH/2 || newNode_y <= -PARAMS::FIELD::WIDTH/2 || newNode_x >= PARAMS::FIELD::LENGTH/2 || newNode_y >= PARAMS::FIELD::WIDTH/2)
 30 |             continue;
 31 |         // in the closed list
 32 |         if (inNodeList(newNode_x, newNode_y))
 33 |             continue;
 34 |         // obstacles
 35 |         if (ObstaclesInfo::instance()->hasObstacle(newNode_x, newNode_y, PARAMS::OBSTACLE::OBSTACLETYPE))
 36 |         {
 37 | //            qDebug() << "obstacle!!!";
 38 |             continue;
 39 | 
 40 |         }
 41 | 
 42 | 
 43 |         NodeList.push_back(Node(newNode_x, newNode_y, nearestNode));
 44 |         if (sqrt(pow(newNode_x-end_x,2)+pow(newNode_y-end_y,2)) < PARAMS::RRT::STEP_SIZE){
 45 |             if(PARAMS::DEBUG::RRTDebug)
 46 |                 qDebug() <<"final";
 47 |             // generate the final path
 48 |             std::vector<MyPoint> tempPath;
 49 |             tempPath.push_back(MyPoint(end_x, end_y));
 50 |             tempPath.push_back(MyPoint(newNode_x, newNode_y));
 51 |             int currentNode = NodeList.back().parent;
 52 |             while(currentNode>=0){
 53 |                 tempPath.push_back(MyPoint(NodeList[currentNode].x, NodeList[currentNode].y));
 54 |                 currentNode = NodeList[currentNode].parent;
 55 |                 if(PARAMS::DEBUG::RRTDebug)
 56 |                     qDebug() <<"generate temp path";
 57 |             }
 58 |             if(PARAMS::DEBUG::RRTDebug)
 59 |                 qDebug() <<"finish temp path";
 60 |             // invert the path
 61 |             finalPath.clear();
 62 |             while(tempPath.size()){
 63 |                 finalPath.push_back(tempPath.back());
 64 |                 tempPath.pop_back();
 65 |                 if(PARAMS::DEBUG::RRTDebug)
 66 |                     qDebug() <<"generate final path";
 67 |             }
 68 |             if(PARAMS::DEBUG::RRTDebug)
 69 |                 qDebug() <<"finish final path";
 70 |             pathSmooth();
 71 |             if(PARAMS::DEBUG::RRTDebug)
 72 |                 qDebug() <<"finish smooth path";
 73 |             break;
 74 |         }
 75 |     }
 76 | }
 77 | 
 78 | int RRT::findNearestNode(int x, int y)
 79 | {
 80 |     double max_distance = 1e10;
 81 |     int nearest_index = -1;
 82 |     for(int i = 0; i < NodeList.size(); i++){
 83 |         double distance = sqrt(pow(NodeList[i].x-x,2) + pow(NodeList[i].y-y,2));
 84 |         if(distance < max_distance){
 85 |             max_distance = distance;
 86 |             nearest_index = i;
 87 |         }
 88 |     }
 89 |     return nearest_index;
 90 | }
 91 | 
 92 | Node RRT::randomSample(double epsilon, int end_x, int end_y)
 93 | {
 94 |     int rand_x, rand_y;
 95 | 
 96 |     if(QRandomGenerator::global()->generate()%100 >= epsilon*100){
 97 |         rand_x = QRandomGenerator::global()->generate()%PARAMS::FIELD::LENGTH - PARAMS::FIELD::LENGTH/2;
 98 |         rand_y = QRandomGenerator::global()->generate()%PARAMS::FIELD::WIDTH - PARAMS::FIELD::WIDTH/2;
 99 |     }else{
100 |         rand_x = end_x;
101 |         rand_y = end_y;
102 |     }
103 |     return Node(rand_x, rand_y);
104 | }
105 | 
106 | bool RRT::inNodeList(int x, int y)
107 | {
108 |     for(int i=0; i<NodeList.size(); i++){
109 |         if(NodeList[i].x == x && NodeList[i].y == y)
110 |             return true;
111 |     }
112 |     return false;
113 | }
114 | 
115 | void RRT::pathSmooth()
116 | {
117 |     smoothPath.clear();
118 |     smoothPath.push_back(finalPath[0]);
119 |     int nextPoint = 1;
120 |     while(nextPoint < finalPath.size()){
121 |         while(nextPoint < finalPath.size()-1 && !ObstaclesInfo::instance()->hasObstacle(smoothPath.back().x(), smoothPath.back().y(), finalPath[nextPoint+1].x(), finalPath[nextPoint+1].y(), PARAMS::OBSTACLE::OBSTACLETYPE)){
122 |             nextPoint++;
123 |             if(PARAMS::DEBUG::RRTDebug)
124 |                 qDebug() << "jump!!!";
125 |         }
126 |         smoothPath.push_back(finalPath[nextPoint]);
127 |         nextPoint++;
128 |     }
129 | }
130 | 


--------------------------------------------------------------------------------
/communication/serialsender.cpp:
--------------------------------------------------------------------------------
  1 | #include "serialsender.h"
  2 | #include <QDebug>
  3 | #include "utils/params.h"
  4 | 
  5 | serialSender::serialSender()
  6 |     : startPacket1(PARAMS::TRANSMIT_PACKET_SIZE,0)
  7 |     , startPacket2(PARAMS::TRANSMIT_PACKET_SIZE,0)
  8 |     , transmitPacket(PARAMS::TRANSMIT_PACKET_SIZE,0)
  9 | {
 10 |     startPacket1[0] = 0xff;
 11 |     startPacket1[1] = 0xb0;
 12 |     startPacket1[2] = 0x01;
 13 |     startPacket1[3] = 0x02;
 14 |     startPacket1[4] = 0x03;
 15 |     startPacket1[24] = 0x31;
 16 | 
 17 |     startPacket2[0] = 0xff;
 18 |     startPacket2[1] = 0xb0;
 19 |     startPacket2[2] = 0x04;
 20 |     startPacket2[3] = 0x05;
 21 |     startPacket2[4] = 0x06;
 22 |     startPacket2[5] = 0x10 + PARAMS::frequency;
 23 |     startPacket2[24] = 0x85;
 24 | }
 25 | 
 26 | void serialSender::openSerialPort()
 27 | {
 28 |     serial.setPortName(PARAMS::serialPort);
 29 |     serial.setBaudRate(QSerialPort::Baud115200);
 30 |     serial.setDataBits(QSerialPort::Data8);
 31 |     serial.setParity(QSerialPort::NoParity);
 32 |     serial.setStopBits(QSerialPort::OneStop);
 33 | 
 34 |     if (serial.open(QIODevice::ReadWrite))
 35 |         qDebug() << "Serial Port Connected!!!";
 36 |     else
 37 |         qDebug() << "Serial Port connection failed!!!";
 38 | }
 39 | 
 40 | void serialSender::closeSerialPort()
 41 | {
 42 |     if (serial.isOpen()){
 43 |         serial.close();
 44 |         qDebug() << "Serial Port Disconnected!!!";
 45 |     }
 46 | }
 47 | 
 48 | void serialSender::sendStartPacket()
 49 | {
 50 |     serial.write(startPacket1.data(), PARAMS::TRANSMIT_PACKET_SIZE);
 51 |     serial.flush();
 52 |     if (serial.waitForBytesWritten(2000)){
 53 |         if (serial.waitForReadyRead(2000)){
 54 |             QByteArray respenseData = serial.readAll();
 55 |             while (serial.waitForReadyRead(10))
 56 |                 respenseData += serial.readAll();
 57 |         }
 58 |     }
 59 |     else
 60 |         qDebug() << "start packet time out!!!";
 61 |     qDebug() << "0x" << startPacket1.toHex();
 62 |     qDebug() << "0x" << startPacket2.toHex();
 63 |     serial.write(startPacket2.data(), PARAMS::TRANSMIT_PACKET_SIZE);
 64 |     serial.flush();
 65 | }
 66 | 
 67 | void serialSender::encode(int robotID, int velX, int velY, int velR, bool ctrl, bool shootMode, int ctrlPowerLevel)
 68 | {
 69 |     for (int i=0; i<PARAMS::TRANSMIT_PACKET_SIZE; i++){
 70 |         transmitPacket[i] = 0x00;
 71 |     }
 72 |     transmitPacket[0] = 0xff;
 73 |     //RobotID
 74 |     if(robotID >= 8)
 75 |         transmitPacket[1] = 0x01 << (robotID - 8);
 76 |     else
 77 |         transmitPacket[1] = 0x00;
 78 |     transmitPacket[2] =  0x01 << robotID;
 79 |     // misc
 80 |     transmitPacket[3] = transmitPacket[3] | (ctrl << 7);
 81 |     //shoot or chip
 82 |     transmitPacket[3] = transmitPacket[3] | (shootMode << 6);
 83 |     //power level
 84 |     transmitPacket[3] = transmitPacket[3] | (ctrlPowerLevel << 4);
 85 |     // other
 86 |     transmitPacket[3] = transmitPacket[3] & 0xf0;
 87 |     transmitPacket[3] = transmitPacket[3] | 0x01;
 88 | 
 89 |     // velx
 90 |     if(velX < 0) transmitPacket[4] = transmitPacket[4] | (0x20);
 91 |     transmitPacket[4] = transmitPacket[4] | ((abs(velX) & 0x1f0) >> 4);
 92 | //    qDebug() << "ddq debuging: " << (abs(velX) & 0x1f0);
 93 |     transmitPacket[5] = transmitPacket[5] | ((abs(velX) & 0x0f) << 4);
 94 |     // vely
 95 |     if(velY < 0) transmitPacket[5] = transmitPacket[5] | (0x08);
 96 |     transmitPacket[5] = transmitPacket[5] | ((abs(velY) & 0x1c0) >> 6);
 97 |     transmitPacket[6] = transmitPacket[6] | ((abs(velY) & 0x3f) << 2);
 98 |     // w
 99 |     if(velR < 0) transmitPacket[6] = transmitPacket[6] | 0x02;
100 |     transmitPacket[6] = transmitPacket[6] | ((abs(velR) & 0x100) >> 8);
101 |     transmitPacket[7] = transmitPacket[7] | (abs(velR) & 0x0ff);
102 | 
103 |     // shoot power
104 |     transmitPacket[8] = 0x00;
105 | 
106 |     transmitPacket[21] = 0x07;
107 | 
108 | 
109 | //    transmitPacket[0] = 0x48;
110 | //    //RobotID
111 | //    transmitPacket[1] = (robotID) & 0x0f;
112 | //    //shoot or chip
113 | //    transmitPacket[1] = transmitPacket[1] | (shootMode << 6 );
114 | //    //power level
115 | //    transmitPacket[1] = transmitPacket[1] | (ctrl ? (ctrlPowerLevel << 4):0);
116 | //    //速度的低位
117 | //    transmitPacket[2] = ((velX >= 0)?0:0x80) | (abs(velX) & 0x7f);
118 | //    transmitPacket[3] = ((velY >= 0)?0:0x80) | (abs(velY) & 0x7f);
119 | //    transmitPacket[4] = ((velR >= 0)?0:0x80) | (abs(velR) & 0x7f);
120 | //    //Don't understand !
121 | //    if(transmitPacket[2] == char(0xff)) transmitPacket[4] = 0xfe;
122 | //    if(transmitPacket[3] == char(0xff)) transmitPacket[5] = 0xfe;
123 | //    if(transmitPacket[4] == char(0xff)) transmitPacket[6] = 0xfe;
124 | //    //clear Byte[17-24]
125 | //    transmitPacket[17] = transmitPacket[18] = transmitPacket[19] = transmitPacket[20] = transmitPacket[21] = transmitPacket[22] = transmitPacket[23] = transmitPacket[24] = 0;
126 | //    //速度的高位
127 | //    transmitPacket[17] = ((abs(velX) & 0x180) >> 1) | ((abs(velY) & 0x180) >> 3) | ((abs(velR) & 0x780) >> 7);
128 | //    //shoot power
129 | //    transmitPacket[21] = 0 & 0x7f;
130 | }
131 | 
132 | void serialSender::sendToReal(int robot_id, int vel_x, int vel_y, int vel_w)
133 | {
134 |     encode(robot_id-1, vel_x, vel_y, vel_w);
135 | //    qDebug() << "0x" << transmitPacket.toHex();
136 |     serial.write(transmitPacket.data(), PARAMS::TRANSMIT_PACKET_SIZE);
137 | //    serial.flush();
138 |     serial.waitForBytesWritten(-1);
139 | }
140 | 


--------------------------------------------------------------------------------
/communication/udpreceiver.cpp:
--------------------------------------------------------------------------------
  1 | #include "udpreceiver.h"
  2 | #include "proto/vision_detection.pb.h"
  3 | #include "utils/datamanager.h"
  4 | #include "utils/params.h"
  5 | #include "udpsender.h"
  6 | #include "algorithm/rrt.h"
  7 | #include "algorithm/artifical_potential.h"
  8 | #include <thread>
  9 | #include <iostream>
 10 | 
 11 | UDPReceiver* UDPReceiver::_instance = 0;
 12 | 
 13 | namespace {
 14 |     std::thread* _thread = nullptr;
 15 | }
 16 | 
 17 | UDPReceiver::UDPReceiver()
 18 | {
 19 |     receiver = new QUdpSocket();
 20 |     receiver->bind(QHostAddress::AnyIPv4, PARAMS::visionPort, QUdpSocket::ShareAddress);
 21 |     if(!PARAMS::IS_SIMULATION)
 22 |         receiver->joinMulticastGroup(QHostAddress("224.5.23.2"));
 23 |     _thread = new std::thread([ = ] {readDatagrams();});
 24 | }
 25 | 
 26 | UDPReceiver::~UDPReceiver(){
 27 |     receiver->abort();
 28 |     delete _thread;
 29 | }
 30 | 
 31 | UDPReceiver* UDPReceiver::instance(){
 32 |     if(_instance == 0)
 33 |         _instance = new UDPReceiver();
 34 |     return _instance;
 35 | }
 36 | 
 37 | void UDPReceiver::readDatagrams(){
 38 |     while(true){
 39 |         std::this_thread::sleep_for(std::chrono::milliseconds(2));
 40 |         while(receiver->hasPendingDatagrams()){
 41 |             QByteArray datagram;
 42 |             datagram.resize(receiver->pendingDatagramSize());
 43 |             receiver->readDatagram(datagram.data(), datagram.size());
 44 | //            qDebug() << "Receiving data!!!";
 45 | 
 46 |             // Parse from datagram
 47 |             Vision_DetectionFrame vision;
 48 |             vision.ParseFromArray(datagram, datagram.size());
 49 | 
 50 |             // Read ball info
 51 |             auto ball = vision.balls();
 52 |             if (ball.has_vel_x()) MyDataManager::instance()->ball.vel_x = ball.vel_x();
 53 |             else MyDataManager::instance()->ball.vel_x = 9999;
 54 |             if (ball.has_vel_y()) MyDataManager::instance()->ball.vel_y = ball.vel_y();
 55 |             else MyDataManager::instance()->ball.vel_y = 9999;
 56 |             MyDataManager::instance()->ball.x = ball.x();
 57 |             MyDataManager::instance()->ball.y = ball.y();
 58 |             MyDataManager::instance()->ball.valid = ball.valid();
 59 | 
 60 |             // reset
 61 |             MyDataManager::instance()->reset();
 62 | 
 63 |             if ( PARAMS::DEBUG::kUdpReceiver){
 64 |                 std::cout << "[udpreceiver.cpp] receive message" << std::endl;
 65 |             }
 66 |             // Read blue robot info
 67 |             int blue_size = vision.robots_blue_size();
 68 |             for (int i=0; i<blue_size; i++){
 69 |                 int robot_id = vision.robots_blue(i).robot_id();          // need improve!!!
 70 |                 MyDataManager::instance()->validBlueRobots[robot_id] = true;
 71 |                 // qDebug() << "DDQ debuging";
 72 |                 MyDataManager::instance()->blueRobots[robot_id].robot_id = robot_id;
 73 |                 MyDataManager::instance()->blueRobots[robot_id].x = vision.robots_blue(i).x()/10;
 74 |                 MyDataManager::instance()->blueRobots[robot_id].y = -vision.robots_blue(i).y()/10;
 75 |                 if (vision.robots_blue(i).has_orientation())
 76 |                     MyDataManager::instance()->blueRobots[robot_id].orientation = vision.robots_blue(i).orientation();
 77 |                 else
 78 |                     MyDataManager::instance()->blueRobots[robot_id].orientation = 9999;
 79 |                 if (vision.robots_blue(i).has_vel_x())
 80 |                     MyDataManager::instance()->blueRobots[robot_id].vel_x = vision.robots_blue(i).vel_x();
 81 |                 else
 82 |                     MyDataManager::instance()->blueRobots[robot_id].vel_x = 0;
 83 |                 if (vision.robots_blue(i).has_vel_y())
 84 |                     MyDataManager::instance()->blueRobots[robot_id].vel_y = vision.robots_blue(i).vel_y();
 85 |                 else
 86 |                     MyDataManager::instance()->blueRobots[robot_id].vel_y = 0;
 87 |                 if (vision.robots_blue(i).has_rotate_vel())
 88 |                     MyDataManager::instance()->blueRobots[robot_id].rotate_vel = vision.robots_blue(i).rotate_vel();
 89 |                 else
 90 |                     MyDataManager::instance()->blueRobots[robot_id].rotate_vel = 0;
 91 |             }
 92 | 
 93 |             // Read yellow robot info
 94 |             int yellow_size = vision.robots_yellow_size();
 95 |             for (int i=0; i<yellow_size; i++){
 96 |                 int robot_id = vision.robots_yellow(i).robot_id();          // need improve!!!
 97 |                 MyDataManager::instance()->validYellowRobots[robot_id] = true;
 98 |                 MyDataManager::instance()->yellowRobots[robot_id].robot_id = robot_id;
 99 |                 MyDataManager::instance()->yellowRobots[robot_id].x = vision.robots_yellow(i).x()/10;
100 |                 MyDataManager::instance()->yellowRobots[robot_id].y = -vision.robots_yellow(i).y()/10;
101 |                 if (vision.robots_yellow(i).has_orientation())
102 |                     MyDataManager::instance()->yellowRobots[robot_id].orientation = vision.robots_yellow(i).orientation();
103 |                 else
104 |                     MyDataManager::instance()->yellowRobots[robot_id].orientation = 9999;
105 |                 if (vision.robots_yellow(i).has_vel_x())
106 |                     MyDataManager::instance()->yellowRobots[robot_id].vel_x = vision.robots_yellow(i).vel_x();
107 |                 else
108 |                     MyDataManager::instance()->yellowRobots[robot_id].vel_x = 0;
109 |                 if (vision.robots_yellow(i).has_vel_y())
110 |                     MyDataManager::instance()->yellowRobots[robot_id].vel_y = vision.robots_yellow(i).vel_y();
111 |                 else
112 |                     MyDataManager::instance()->yellowRobots[robot_id].vel_y = 0;
113 |                 if (vision.robots_yellow(i).has_rotate_vel())
114 |                     MyDataManager::instance()->yellowRobots[robot_id].rotate_vel = vision.robots_yellow(i).rotate_vel();
115 |                 else
116 |                     MyDataManager::instance()->yellowRobots[robot_id].rotate_vel = 0;
117 |             }
118 | 
119 |             // Update cycle
120 |             MyDataManager::instance()->updateCycle();
121 |         }
122 |     }
123 | }
124 | 


--------------------------------------------------------------------------------
/algorithm/obstacles.cpp:
--------------------------------------------------------------------------------
  1 | #include "obstacles.h"
  2 | #include "utils/params.h"
  3 | #include "utils/datamanager.h"
  4 | 
  5 | bool Obstacles::hasObstacle(double x, double y, int type)
  6 | {
  7 |     switch (type) {
  8 |     case 1:
  9 |         for(int i=0; i<PARAMS::ROBOT_NUM; i++){
 10 |             // blue robot
 11 |             if(MyDataManager::instance()->validBlueRobots[i] && !((PARAMS::IS_SIMULATION ? i==(PARAMS::our_id-1) : i==(PARAMS::our_id-1)) && PARAMS::isBlue)){
 12 |                 RobotInfo& blue = MyDataManager::instance()->blueRobots[i];
 13 |                 if(pow(blue.x - x, 2) + pow(blue.y - y, 2) <= PARAMS::OBSTACLE::INFLATION_RADIUS_SQUARE)
 14 |                     return true;
 15 |             }
 16 |             // yellow robot
 17 |             if(MyDataManager::instance()->validYellowRobots[i] && !((PARAMS::IS_SIMULATION ? i==(PARAMS::our_id-1) : i==(PARAMS::our_id-1)) && !PARAMS::isBlue)){
 18 |                 RobotInfo& yellow = MyDataManager::instance()->yellowRobots[i];
 19 |                 if(pow(yellow.x - x, 2) + pow(yellow.y - y, 2) <= PARAMS::OBSTACLE::INFLATION_RADIUS_SQUARE)
 20 |                     return true;
 21 |             }
 22 |         }
 23 |         break;
 24 |     case 2:
 25 |         for(int i=0; i<PARAMS::ROBOT_NUM; i++){
 26 |             // blue robot
 27 |             //qDebug() << "i" << i << "NUM" << PARAMS::ROBOT_NUM;
 28 |             if(MyDataManager::instance()->validBlueRobots[i] && !((PARAMS::IS_SIMULATION ? i==(PARAMS::our_id-1) : i==(PARAMS::our_id-1)) && PARAMS::isBlue))
 29 |             {
 30 |                 RobotInfo& blue = MyDataManager::instance()->blueRobots[i];
 31 |                 //尤其注意，这里是路子豪写的，是按照正常情况下的X-Y坐标系来做的，对于仿真和实际的环境有点痴迷，详情请询问路子豪
 32 |                 double globalvx = blue.vel_x;
 33 |                 double globalvy = -blue.vel_y;
 34 |                 if(pow(globalvx,2)+pow(globalvy,2) <= 10000)//这是静态的东西
 35 |                 {
 36 | //                    qDebug() << i;
 37 |                     if(pow(blue.x - x, 2) + pow(blue.y - y, 2) <= PARAMS::OBSTACLE::INFLATION_RADIUS_SQUARE)
 38 |                         return true;
 39 |                 }
 40 |                 else
 41 |                 {
 42 |                     double endx = blue.x + PARAMS::OBSTACLE::REFRESH_TIME * globalvx;
 43 |                     double endy = blue.y + PARAMS::OBSTACLE::REFRESH_TIME * globalvy;
 44 |                     //qDebug() << "x" << x << "y" << y << "blue.x" << blue.x << "blue.y" << blue.y << "endx" << endx << "endy" << endy;
 45 |                     //qDebug() << "the dis" <<distanceofPointandLine(x, y, blue.x, blue.y, endx, endy);
 46 |                     //qDebug() << "INFLATION_RADIUS" << PARAMS::OBSTACLE::INFLATION_RADIUS;
 47 |                     if(distanceofPointandLine(x, y, blue.x, blue.y, endx, endy) <= PARAMS::OBSTACLE::INFLATION_RADIUS)
 48 |                     {
 49 |                         //qDebug() << "true";
 50 |                         return true;
 51 |                     }
 52 |                     else
 53 |                     {
 54 |                         //qDebug() << "false";
 55 |                     }
 56 |                 }
 57 |             }
 58 |             // yellow robot
 59 |             if(MyDataManager::instance()->validYellowRobots[i] && !((PARAMS::IS_SIMULATION ? i==(PARAMS::our_id-1) : i==(PARAMS::our_id-1)) && !PARAMS::isBlue))
 60 |             {
 61 |                 RobotInfo& yellow = MyDataManager::instance()->yellowRobots[i];
 62 |                 //尤其注意，这里是路子豪写的，是按照正常情况下的X-Y坐标系来做的，对于仿真和实际的环境有点痴迷，详情请询问路子豪
 63 |                 double globalvx = yellow.vel_x;
 64 |                 double globalvy = -yellow.vel_y;
 65 |                 if(pow(globalvx,2)+pow(globalvy,2) <= 10000)
 66 |                 {
 67 |                     if(pow(yellow.x - x, 2) + pow(yellow.y - y, 2) <= PARAMS::OBSTACLE::INFLATION_RADIUS_SQUARE)
 68 |                         return true;
 69 |                 }
 70 |                 else
 71 |                 {
 72 |                     double endx = yellow.x + PARAMS::OBSTACLE::REFRESH_TIME * globalvx;
 73 |                     double endy = yellow.y + PARAMS::OBSTACLE::REFRESH_TIME * globalvy;
 74 |                     //qDebug() << "x" << x << "y" << y << "yellow.x" << yellow.x << "yellow.y" << yellow.y << "endx" << endx << "endy" << endy;
 75 |                     //qDebug() << "the dis" <<distanceofPointandLine(x, y, yellow.x, yellow.y, endx, endy);
 76 |                     //qDebug() << "INFLATION_RADIUS" << PARAMS::OBSTACLE::INFLATION_RADIUS;
 77 |                     if(distanceofPointandLine(x, y, yellow.x, yellow.y, endx, endy) <= PARAMS::OBSTACLE::INFLATION_RADIUS)
 78 |                     {
 79 |                         //qDebug() << "true";
 80 |                         return true;
 81 |                     }
 82 |                     else
 83 |                     {
 84 |                         //qDebug() << "false";
 85 |                     }
 86 |                 }
 87 |             }
 88 |         }
 89 |         break;
 90 |     default:
 91 |         break;
 92 |     }
 93 |     //qDebug() << "there is false finally!!!!!!";
 94 |     return false;
 95 | }
 96 | 
 97 | bool Obstacles::hasObstacle(double x1, double y1, double x2, double y2, int type)
 98 | {
 99 |     double distance = sqrt(pow(y1 - y2, 2) + pow(x1 - x2, 2));
100 |     double theta = atan2(y2 - y1, x2 - x1);
101 |     double dx = PARAMS::OBSTACLE::STEP_SIZE*cos(theta);
102 |     double dy = PARAMS::OBSTACLE::STEP_SIZE*sin(theta);
103 |     double x = x1;
104 |     double y = y1;
105 |         for(int i=0; i<distance/PARAMS::OBSTACLE::STEP_SIZE; i++){
106 |             if(hasObstacle(x, y, type))
107 |                 return true;
108 |             x += dx;
109 |             y += dy;
110 |         }
111 |     return false;
112 | }
113 | 
114 | double Obstacles::distanceofPointandLine(double pointx, double pointy, double startx, double starty, double endx, double endy)
115 | {
116 |     if (startx == endx && starty == endy)
117 |         return sqrt(pow(endx-pointx,2)+pow(endy-pointy,2));
118 |     double r = ((pointx-startx)*(endx-startx) + (pointy-starty)*(endy-starty))/(pow(endx-startx,2)+pow(endy-starty,2));
119 |     //qDebug() << r;
120 |     if(r <= 0) return sqrt(pow(startx-pointx,2)+pow(starty-pointy,2));
121 |     else if(r >= 1) return sqrt(pow(endx-pointx,2)+pow(endy-pointy,2));
122 |     else{
123 |         //通过斜率求解
124 |         if(endx == startx) return (pointx-startx);
125 |         double k =  (endy-starty) / (endx-startx);
126 |         return fabs(starty-pointy - k*(startx-pointx))/sqrt(k*k+1);
127 |     }
128 | }
129 | 


--------------------------------------------------------------------------------
/algorithm/rrtstar.cpp:
--------------------------------------------------------------------------------
  1 | #include "rrtstar.h"
  2 | #include "utils/params.h"
  3 | #include <QRandomGenerator>
  4 | #include "algorithm/obstacles.h"
  5 | 
  6 | void RRTStar::plan(double start_x, double start_y, double end_x, double end_y)
  7 | {
  8 |     // exploration
  9 |     NodeList.clear();
 10 |     NodeList.push_back(Node(start_x, start_y));
 11 | 
 12 |     for(int iter=0; iter<PARAMS::RRTStar::ITERATIONS; iter++) {
 13 |         // random sampling
 14 |         Node randNode = randomSample(PARAMS::RRTStar::EPSILON, end_x, end_y);
 15 |         // Find the nearest node
 16 |         int nearestNode = findNearestNode(randNode.x, randNode.y);
 17 |         // expand the tree
 18 |         double theta = atan2(randNode.y - NodeList[nearestNode].y, randNode.x - NodeList[nearestNode].x);
 19 |         int newNode_x = NodeList[nearestNode].x + PARAMS::RRTStar::STEP_SIZE*cos(theta);
 20 |         int newNode_y = NodeList[nearestNode].y + PARAMS::RRTStar::STEP_SIZE*sin(theta);
 21 |         double newNode_cost = NodeList[nearestNode].cost + sqrt(pow(newNode_x - NodeList[nearestNode].x, 2) + pow(newNode_y - NodeList[nearestNode].y, 2));
 22 |         int newNode_parent = nearestNode;
 23 | 
 24 |         // outside the map
 25 |         if (newNode_x <= -PARAMS::FIELD::LENGTH/2 || newNode_y <= -PARAMS::FIELD::WIDTH/2 || newNode_x >= PARAMS::FIELD::LENGTH/2 || newNode_y >= PARAMS::FIELD::WIDTH/2)
 26 |             continue;
 27 |         // in the closed list
 28 |         if (inNodeList(newNode_x, newNode_y))
 29 |             continue;
 30 |         // obstacles
 31 |         if (ObstaclesInfo::instance()->hasObstacle(newNode_x, newNode_y, PARAMS::OBSTACLE::OBSTACLETYPE))
 32 |             continue;
 33 | 
 34 |         // choose best parent
 35 |         double radius = PARAMS::RRTStar::GAMMA*sqrt(log(NodeList.size())/NodeList.size());
 36 |         int bestParent = chooseBestParent(newNode_x, newNode_y, radius, newNode_cost);
 37 |         if(bestParent>=0){
 38 |             newNode_parent = bestParent;
 39 |             newNode_cost = NodeList[newNode_parent].cost + sqrt(pow(NodeList[newNode_parent].y - newNode_y, 2)+pow(NodeList[newNode_parent].x - newNode_x, 2));
 40 |         }
 41 |         Node newNode = Node(newNode_x, newNode_y, newNode_parent, newNode_cost);
 42 |         NodeList.push_back(newNode);
 43 | 
 44 |         // rewire
 45 |         rewire(newNode, radius);
 46 | 
 47 |         if (sqrt(pow(newNode_x-end_x, 2) + pow(newNode_y-end_y, 2)) < PARAMS::RRTStar::STEP_SIZE)
 48 |             break;
 49 |     }
 50 |     // generate the final path
 51 |     std::vector<MyPoint> tempPath;
 52 |     tempPath.clear();
 53 |     tempPath.push_back(MyPoint(end_x, end_y));
 54 |     int currentNode = NodeList.back().parent;
 55 |     while(currentNode>=0){
 56 |         tempPath.push_back(MyPoint(NodeList[currentNode].x, NodeList[currentNode].y));
 57 |         currentNode = NodeList[currentNode].parent;
 58 |     }
 59 |     // invert the path
 60 |     finalPath.clear();
 61 |     while(tempPath.size()){
 62 |         finalPath.push_back(tempPath.back());
 63 |         tempPath.pop_back();
 64 |     }
 65 |     pathSmooth();
 66 | //    for(int i=0; i<finalPath.size();i++){
 67 | //        qDebug() << finalPath[i].x() << finalPath[i].y();
 68 | //    }
 69 | }
 70 | 
 71 | int RRTStar::chooseBestParent(int x, int y, double radius, double old_cost)
 72 | {
 73 |     // find near nodes
 74 |     std::vector<Node> nearNodes;
 75 |     std::vector<int> nearNodesIndex;
 76 |     nearNodes.clear();
 77 |     nearNodesIndex.clear();
 78 |     int bestParent = -1;
 79 |     for(int i=0; i<NodeList.size(); i++){
 80 |         if(pow(NodeList[i].x - x, 2)+pow(NodeList[i].y - y, 2) < pow(radius, 2)){
 81 |             nearNodes.push_back(NodeList[i]);
 82 |             nearNodesIndex.push_back(i);
 83 |         }
 84 |     }
 85 |     // choose best parent
 86 |     if(nearNodes.size()){
 87 |         for (int i=0; i<nearNodes.size(); i++)
 88 |         {
 89 |             double new_cost = nearNodes[i].cost + sqrt(pow(nearNodes[i].y - y, 2) + pow(nearNodes[i].x - x, 2));
 90 |             // check collision
 91 |             if(ObstaclesInfo::instance()->hasObstacle(nearNodes[i].x, nearNodes[i].y, x, y, PARAMS::OBSTACLE::OBSTACLETYPE))
 92 |                 continue;
 93 | 
 94 |             if(new_cost < old_cost){
 95 |                 bestParent = nearNodesIndex[i];
 96 |                 old_cost = new_cost;
 97 |             }
 98 |         }
 99 |     }
100 |     return bestParent;
101 | }
102 | 
103 | void RRTStar::rewire(Node newNode, double radius)
104 | {
105 |     // find near nodes
106 |     std::vector<Node> nearNodes;
107 |     nearNodes.clear();
108 |     for(int i=0; i<NodeList.size(); i++){
109 |         if(pow(NodeList[i].x - newNode.x, 2)+pow(NodeList[i].y - newNode.y, 2) < pow(radius, 2)){
110 |             nearNodes.push_back(NodeList[i]);
111 |         }
112 |     }
113 |     // rewire
114 |     for(int i=0; i<nearNodes.size(); i++){
115 |         double new_cost = newNode.cost + sqrt(pow(newNode.y - nearNodes[i].y, 2) + pow(newNode.x - nearNodes[i].x, 2));
116 |         // check collision
117 | //        if(ObstaclesInfo::instance()->hasObstacle(nearNodes[i].x, nearNodes[i].y, newNode.x, newNode.y, PARAMS::OBSTACLE::OBSTACLETYPE))
118 | //            continue;
119 |         if(new_cost < nearNodes[i].cost){
120 |             nearNodes[i].cost = new_cost;
121 |             nearNodes[i].parent = NodeList.size()-1;
122 |         }
123 |     }
124 | }
125 | 
126 | int RRTStar::findNearestNode(int x, int y)
127 | {
128 |     double max_distance = 1e10;
129 |     int nearest_index = -1;
130 |     for(int i = 0; i < NodeList.size(); i++){
131 |         double distance = sqrt(pow(NodeList[i].x-x,2) + pow(NodeList[i].y-y,2));
132 |         if(distance < max_distance){
133 |             max_distance = distance;
134 |             nearest_index = i;
135 |         }
136 |     }
137 |     return nearest_index;
138 | }
139 | 
140 | Node RRTStar::randomSample(double epsilon, int end_x, int end_y)
141 | {
142 |     int rand_x, rand_y;
143 | 
144 |     if(QRandomGenerator::global()->generate()%100 >= epsilon*100){
145 |         rand_x = QRandomGenerator::global()->generate()%PARAMS::FIELD::LENGTH - PARAMS::FIELD::LENGTH/2;
146 |         rand_y = QRandomGenerator::global()->generate()%PARAMS::FIELD::WIDTH - PARAMS::FIELD::WIDTH/2;
147 |     }else{
148 |         rand_x = end_x;
149 |         rand_y = end_y;
150 |     }
151 |     return Node(rand_x, rand_y);
152 | }
153 | 
154 | bool RRTStar::inNodeList(int x, int y)
155 | {
156 |     for(int i=0; i<NodeList.size(); i++){
157 |         if(NodeList[i].x == x && NodeList[i].y == y)
158 |             return true;
159 |     }
160 |     return false;
161 | }
162 | 
163 | void RRTStar::pathSmooth()
164 | {
165 |     smoothPath.clear();
166 |     smoothPath.push_back(finalPath[0]);
167 |     int nextPoint = 1;
168 |     while(nextPoint < finalPath.size()){
169 |         while(!ObstaclesInfo::instance()->hasObstacle(smoothPath.back().x(), smoothPath.back().y(), finalPath[nextPoint+1].x(), finalPath[nextPoint+1].y(), PARAMS::OBSTACLE::OBSTACLETYPE) && nextPoint < finalPath.size()-1)
170 |             nextPoint++;
171 |         smoothPath.push_back(finalPath[nextPoint]);
172 |         nextPoint++;
173 |     }
174 | }
175 | 
176 | 
177 | 


--------------------------------------------------------------------------------
/proto/grSim_Packet.pb.h:
--------------------------------------------------------------------------------
  1 | // Generated by the protocol buffer compiler.  DO NOT EDIT!
  2 | // source: grSim_Packet.proto
  3 | 
  4 | #ifndef PROTOBUF_grSim_5fPacket_2eproto__INCLUDED
  5 | #define PROTOBUF_grSim_5fPacket_2eproto__INCLUDED
  6 | 
  7 | #include <string>
  8 | 
  9 | #include <google/protobuf/stubs/common.h>
 10 | 
 11 | #if GOOGLE_PROTOBUF_VERSION < 2006000
 12 | #error This file was generated by a newer version of protoc which is
 13 | #error incompatible with your Protocol Buffer headers.  Please update
 14 | #error your headers.
 15 | #endif
 16 | #if 2006001 < GOOGLE_PROTOBUF_MIN_PROTOC_VERSION
 17 | #error This file was generated by an older version of protoc which is
 18 | #error incompatible with your Protocol Buffer headers.  Please
 19 | #error regenerate this file with a newer version of protoc.
 20 | #endif
 21 | 
 22 | #include <google/protobuf/generated_message_util.h>
 23 | #include <google/protobuf/message.h>
 24 | #include <google/protobuf/repeated_field.h>
 25 | #include <google/protobuf/extension_set.h>
 26 | #include <google/protobuf/unknown_field_set.h>
 27 | #include "grSim_Commands.pb.h"
 28 | #include "grSim_Replacement.pb.h"
 29 | // @@protoc_insertion_point(includes)
 30 | 
 31 | // Internal implementation detail -- do not call these.
 32 | void  protobuf_AddDesc_grSim_5fPacket_2eproto();
 33 | void protobuf_AssignDesc_grSim_5fPacket_2eproto();
 34 | void protobuf_ShutdownFile_grSim_5fPacket_2eproto();
 35 | 
 36 | class grSim_Packet;
 37 | 
 38 | // ===================================================================
 39 | 
 40 | class grSim_Packet : public ::google::protobuf::Message {
 41 |  public:
 42 |   grSim_Packet();
 43 |   virtual ~grSim_Packet();
 44 | 
 45 |   grSim_Packet(const grSim_Packet& from);
 46 | 
 47 |   inline grSim_Packet& operator=(const grSim_Packet& from) {
 48 |     CopyFrom(from);
 49 |     return *this;
 50 |   }
 51 | 
 52 |   inline const ::google::protobuf::UnknownFieldSet& unknown_fields() const {
 53 |     return _unknown_fields_;
 54 |   }
 55 | 
 56 |   inline ::google::protobuf::UnknownFieldSet* mutable_unknown_fields() {
 57 |     return &_unknown_fields_;
 58 |   }
 59 | 
 60 |   static const ::google::protobuf::Descriptor* descriptor();
 61 |   static const grSim_Packet& default_instance();
 62 | 
 63 |   void Swap(grSim_Packet* other);
 64 | 
 65 |   // implements Message ----------------------------------------------
 66 | 
 67 |   grSim_Packet* New() const;
 68 |   void CopyFrom(const ::google::protobuf::Message& from);
 69 |   void MergeFrom(const ::google::protobuf::Message& from);
 70 |   void CopyFrom(const grSim_Packet& from);
 71 |   void MergeFrom(const grSim_Packet& from);
 72 |   void Clear();
 73 |   bool IsInitialized() const;
 74 | 
 75 |   int ByteSize() const;
 76 |   bool MergePartialFromCodedStream(
 77 |       ::google::protobuf::io::CodedInputStream* input);
 78 |   void SerializeWithCachedSizes(
 79 |       ::google::protobuf::io::CodedOutputStream* output) const;
 80 |   ::google::protobuf::uint8* SerializeWithCachedSizesToArray(::google::protobuf::uint8* output) const;
 81 |   int GetCachedSize() const { return _cached_size_; }
 82 |   private:
 83 |   void SharedCtor();
 84 |   void SharedDtor();
 85 |   void SetCachedSize(int size) const;
 86 |   public:
 87 |   ::google::protobuf::Metadata GetMetadata() const;
 88 | 
 89 |   // nested types ----------------------------------------------------
 90 | 
 91 |   // accessors -------------------------------------------------------
 92 | 
 93 |   // optional .grSim_Commands commands = 1;
 94 |   inline bool has_commands() const;
 95 |   inline void clear_commands();
 96 |   static const int kCommandsFieldNumber = 1;
 97 |   inline const ::grSim_Commands& commands() const;
 98 |   inline ::grSim_Commands* mutable_commands();
 99 |   inline ::grSim_Commands* release_commands();
100 |   inline void set_allocated_commands(::grSim_Commands* commands);
101 | 
102 |   // optional .grSim_Replacement replacement = 2;
103 |   inline bool has_replacement() const;
104 |   inline void clear_replacement();
105 |   static const int kReplacementFieldNumber = 2;
106 |   inline const ::grSim_Replacement& replacement() const;
107 |   inline ::grSim_Replacement* mutable_replacement();
108 |   inline ::grSim_Replacement* release_replacement();
109 |   inline void set_allocated_replacement(::grSim_Replacement* replacement);
110 | 
111 |   // @@protoc_insertion_point(class_scope:grSim_Packet)
112 |  private:
113 |   inline void set_has_commands();
114 |   inline void clear_has_commands();
115 |   inline void set_has_replacement();
116 |   inline void clear_has_replacement();
117 | 
118 |   ::google::protobuf::UnknownFieldSet _unknown_fields_;
119 | 
120 |   ::google::protobuf::uint32 _has_bits_[1];
121 |   mutable int _cached_size_;
122 |   ::grSim_Commands* commands_;
123 |   ::grSim_Replacement* replacement_;
124 |   friend void  protobuf_AddDesc_grSim_5fPacket_2eproto();
125 |   friend void protobuf_AssignDesc_grSim_5fPacket_2eproto();
126 |   friend void protobuf_ShutdownFile_grSim_5fPacket_2eproto();
127 | 
128 |   void InitAsDefaultInstance();
129 |   static grSim_Packet* default_instance_;
130 | };
131 | // ===================================================================
132 | 
133 | 
134 | // ===================================================================
135 | 
136 | // grSim_Packet
137 | 
138 | // optional .grSim_Commands commands = 1;
139 | inline bool grSim_Packet::has_commands() const {
140 |   return (_has_bits_[0] & 0x00000001u) != 0;
141 | }
142 | inline void grSim_Packet::set_has_commands() {
143 |   _has_bits_[0] |= 0x00000001u;
144 | }
145 | inline void grSim_Packet::clear_has_commands() {
146 |   _has_bits_[0] &= ~0x00000001u;
147 | }
148 | inline void grSim_Packet::clear_commands() {
149 |   if (commands_ != NULL) commands_->::grSim_Commands::Clear();
150 |   clear_has_commands();
151 | }
152 | inline const ::grSim_Commands& grSim_Packet::commands() const {
153 |   // @@protoc_insertion_point(field_get:grSim_Packet.commands)
154 |   return commands_ != NULL ? *commands_ : *default_instance_->commands_;
155 | }
156 | inline ::grSim_Commands* grSim_Packet::mutable_commands() {
157 |   set_has_commands();
158 |   if (commands_ == NULL) commands_ = new ::grSim_Commands;
159 |   // @@protoc_insertion_point(field_mutable:grSim_Packet.commands)
160 |   return commands_;
161 | }
162 | inline ::grSim_Commands* grSim_Packet::release_commands() {
163 |   clear_has_commands();
164 |   ::grSim_Commands* temp = commands_;
165 |   commands_ = NULL;
166 |   return temp;
167 | }
168 | inline void grSim_Packet::set_allocated_commands(::grSim_Commands* commands) {
169 |   delete commands_;
170 |   commands_ = commands;
171 |   if (commands) {
172 |     set_has_commands();
173 |   } else {
174 |     clear_has_commands();
175 |   }
176 |   // @@protoc_insertion_point(field_set_allocated:grSim_Packet.commands)
177 | }
178 | 
179 | // optional .grSim_Replacement replacement = 2;
180 | inline bool grSim_Packet::has_replacement() const {
181 |   return (_has_bits_[0] & 0x00000002u) != 0;
182 | }
183 | inline void grSim_Packet::set_has_replacement() {
184 |   _has_bits_[0] |= 0x00000002u;
185 | }
186 | inline void grSim_Packet::clear_has_replacement() {
187 |   _has_bits_[0] &= ~0x00000002u;
188 | }
189 | inline void grSim_Packet::clear_replacement() {
190 |   if (replacement_ != NULL) replacement_->::grSim_Replacement::Clear();
191 |   clear_has_replacement();
192 | }
193 | inline const ::grSim_Replacement& grSim_Packet::replacement() const {
194 |   // @@protoc_insertion_point(field_get:grSim_Packet.replacement)
195 |   return replacement_ != NULL ? *replacement_ : *default_instance_->replacement_;
196 | }
197 | inline ::grSim_Replacement* grSim_Packet::mutable_replacement() {
198 |   set_has_replacement();
199 |   if (replacement_ == NULL) replacement_ = new ::grSim_Replacement;
200 |   // @@protoc_insertion_point(field_mutable:grSim_Packet.replacement)
201 |   return replacement_;
202 | }
203 | inline ::grSim_Replacement* grSim_Packet::release_replacement() {
204 |   clear_has_replacement();
205 |   ::grSim_Replacement* temp = replacement_;
206 |   replacement_ = NULL;
207 |   return temp;
208 | }
209 | inline void grSim_Packet::set_allocated_replacement(::grSim_Replacement* replacement) {
210 |   delete replacement_;
211 |   replacement_ = replacement;
212 |   if (replacement) {
213 |     set_has_replacement();
214 |   } else {
215 |     clear_has_replacement();
216 |   }
217 |   // @@protoc_insertion_point(field_set_allocated:grSim_Packet.replacement)
218 | }
219 | 
220 | 
221 | // @@protoc_insertion_point(namespace_scope)
222 | 
223 | #ifndef SWIG
224 | namespace google {
225 | namespace protobuf {
226 | 
227 | 
228 | }  // namespace google
229 | }  // namespace protobuf
230 | #endif  // SWIG
231 | 
232 | // @@protoc_insertion_point(global_scope)
233 | 
234 | #endif  // PROTOBUF_grSim_5fPacket_2eproto__INCLUDED
235 | 


--------------------------------------------------------------------------------
/algorithm/pathplanner.cpp:
--------------------------------------------------------------------------------
  1 | #include "pathplanner.h"
  2 | #include "communication/udpsender.h"
  3 | #include "communication/serialsender.h"
  4 | #include "obstacles.h"
  5 | #include <iostream>
  6 | #include <iomanip>
  7 | 
  8 | namespace {
  9 | const float DIRECTION_ACCURACY = 2 / 180 * PARAMS::MATH::PI;
 10 | const float DELTA_TIME = 0.03; // unit is second
 11 | const float ACC_BUFFER = 1.2;
 12 | const float STOP_BUFFER_UP = 400;
 13 | const float STOP_BUFFER_DOWN = 200;
 14 | const float ROTATE = 5;
 15 | }
 16 | 
 17 | void PathPlanner::plan()
 18 | {
 19 |     while (path.size() > 0 && moveToNext(path.front()))
 20 |     {
 21 |         if(PARAMS::DEBUG::pathPlannerDebug)
 22 |             qDebug() << "has arrived";
 23 |         path.pop_front();
 24 |     }
 25 |     if(PARAMS::DEBUG::pathPlannerDebug)
 26 |         qDebug() << "path size:" << path.size();
 27 |     if(path.size() > 0)
 28 | //        goToPoint(path.front());
 29 | //        goToPointTrapezoid(path.front());
 30 |         goToPosition2d(path.front());
 31 | }
 32 | 
 33 | bool PathPlanner::hasArrived(MyPoint target)
 34 | {
 35 |     if(PARAMS::DEBUG::pathPlannerDebug)
 36 |         qDebug() << "has arrived";
 37 | 
 38 |     return pow(MyDataManager::instance()->ourRobot().x - target.x(), 2)
 39 |             + pow(MyDataManager::instance()->ourRobot().y - target.y(), 2)
 40 |             <= pow(PARAMS::ACCEPT_RADIUS, 2);
 41 | }
 42 | 
 43 | // 判断机器人是否需要走下一个点
 44 | bool PathPlanner::moveToNext(MyPoint target)
 45 | {
 46 |     bool next = false;
 47 |     if(hasArrived(target))
 48 |         next = true;
 49 |     // 解决机器人走过头的问题
 50 |     if(path.size() > 1){
 51 |         MyPoint nextTarget = path[1];
 52 |         RobotInfo& me = MyDataManager::instance()->ourRobot();
 53 |         double distToTarget = sqrt(pow(me.x - target.x(),2) + pow(me.y - target.y(), 2))
 54 |                 + sqrt(pow(target.x() - nextTarget.x(),2) + pow(target.y() - nextTarget.y(),2));
 55 |         double distToNextTarget = sqrt(pow(me.x - nextTarget.x(),2) + pow(me.x - nextTarget.x(),2));
 56 |         if(distToTarget > distToNextTarget && !ObstaclesInfo::instance()->hasObstacle(me.x, me.y, nextTarget.x(), nextTarget.y(), PARAMS::OBSTACLE::OBSTACLETYPE))
 57 |             next = true;
 58 |     }
 59 |     return next;
 60 | }
 61 | 
 62 | void PathPlanner::rotateToPoint(MyPoint target)
 63 | {
 64 |     RobotInfo& me = MyDataManager::instance()->ourRobot();
 65 |     double targetAngle = -atan2(target.y() - me.y, target.x() - me.x);
 66 |     double me_angle = MyDataManager::instance()->ourRobot().orientation;
 67 | 
 68 |     double diffAngle = fabs(me_angle - targetAngle);
 69 |     diffAngle = diffAngle > PARAMS::MATH::PI ? 2*PARAMS::MATH::PI - diffAngle : diffAngle;
 70 |     // 计算旋转速度的绝对值
 71 |     double rotVel = diffAngle * PARAMS::ROTATE_COFF; // need improve!!!
 72 |     // 计算旋转速度的方向
 73 |     if(me_angle > targetAngle)
 74 |         rotVel *= -1;
 75 |     if(fabs(me_angle - targetAngle) > PARAMS::MATH::PI)
 76 |         rotVel *= -1;
 77 | 
 78 |     if(fabs(diffAngle) >= PARAMS::ANGLE_THRESHOLD){
 79 |         velX = 0;
 80 |         velY = 0;
 81 |         velW = rotVel;
 82 |     }
 83 |     else{
 84 |         velX = 0;
 85 |         velY = 0;
 86 |         velW = 0;
 87 |     }
 88 | }
 89 | 
 90 | void PathPlanner::goToPoint(MyPoint target)
 91 | {
 92 |     if(PARAMS::DEBUG::pathPlannerDebug)
 93 |         qDebug() << "go to point";
 94 |     RobotInfo& me = MyDataManager::instance()->ourRobot();
 95 |     double targetAngle = -atan2(target.y() - me.y, target.x() - me.x);
 96 |     double me_angle = MyDataManager::instance()->ourRobot().orientation;
 97 | 
 98 |     double diffAngle = fabs(me_angle - targetAngle);
 99 | //    qDebug() <<"angle" <<me_angle << MyDataManager::instance()->blueRobots[1].orientation << MyDataManager::instance()->blueRobots[2].orientation <<MyDataManager::instance()->blueRobots[0].orientation;
100 |     diffAngle = diffAngle > PARAMS::MATH::PI ? 2*PARAMS::MATH::PI - diffAngle : diffAngle;
101 |     // 计算旋转速度的绝对值
102 |     double rotVel = diffAngle * PARAMS::FORWARD_ROTATE_COFF; // need improve!!!
103 |     // 计算旋转速度的方向
104 |     if(me_angle > targetAngle)
105 |         rotVel *= -1;
106 |     if(fabs(me_angle - targetAngle) > PARAMS::MATH::PI)
107 |         rotVel *= -1;
108 | 
109 |     if(fabs(diffAngle) >= PARAMS::FORWARD_ANGLE_THRESHOLD) {
110 |         velX = 0;
111 |         velY = 0;
112 |         velW = rotVel;
113 |     }
114 |     else{
115 |         velX = PARAMS::FORWARD_SPEED;
116 |         velY = 0;
117 |         velW = rotVel;
118 |     }
119 | }
120 | 
121 | void PathPlanner::goToPointTrapezoid(MyPoint target)
122 | {
123 |     //尤其注意，本函数得出的velx和vely和实际场地是相同的，不需要乘-1，可以乘倍数
124 |     RobotInfo& me = MyDataManager::instance()->ourRobot();
125 |     double targetAngle = atan2(target.y() - me.y, target.x() - me.x);
126 |     double globalvx = PARAMS::FORWARD_SPEED * cos(targetAngle);
127 |     double globalvy = PARAMS::FORWARD_SPEED * sin(targetAngle);
128 |     double localvx = globalvx * cos(me.orientation) - globalvy * sin(me.orientation);
129 |     double localvy = globalvx * sin(me.orientation) + globalvy * cos(me.orientation);
130 | 
131 | 
132 | //    float dis = sqrt(pow(target.x()-me.x,2)+pow(target.y()-me.y,2));
133 | //    float vel_now = sqrt(pow(me.vel_x,2)+pow(me.vel_y,2));
134 | //    float dismax = vel_now/PARAMS::multiple_realToCommand*15;
135 | //    qDebug() << "dis" << dis << "dismax" << dismax;
136 | //    if(dis < dismax && false)
137 | //    {
138 | //        //it's good in reality, but not in simulink.
139 | //        velX = fmax(localvx,2.0);
140 | //        velY = fmax(localvy,2.0);
141 | //    }
142 | //    else{
143 |         velX = localvx;
144 |         velY = localvy;
145 | //    }
146 | //    velW = 0;
147 | //    qDebug() << "now goToPoint-------------ing";
148 | }
149 | 
150 | void PathPlanner::goToPosition2d( MyPoint target ){
151 |     if ( PARAMS::DEBUG::kgoToPosition2d ){
152 |         std::cout << "\n=================  [pathplanner.cpp] debug  =========="
153 |                         "========" << std::endl;
154 |     }
155 |     RobotInfo& me = MyDataManager::instance()->ourRobot();
156 |     MyPoint me_pos( me.x * 10, me.y * 10 );
157 |     target.Setx( target.x() * 10 );
158 |     target.Sety( target.y() * 10 );
159 |     MyVector me2target = target - me_pos;
160 |     MyVector me_vel( me.vel_x, me.vel_y );
161 |     MyVector next_step;
162 | 
163 |     double targetAngle = -atan2(target.y() - me.y, target.x() - me.x);
164 |     double me_angle = MyDataManager::instance()->ourRobot().orientation;
165 | 
166 |     double diffAngle = fabs(me_angle - targetAngle);
167 |     diffAngle = diffAngle > PARAMS::MATH::PI ? 2*PARAMS::MATH::PI - diffAngle : diffAngle;
168 |     // 计算旋转速度的绝对值
169 |     double rotVel = diffAngle * PARAMS::ROTATE_COFF; // need improve!!!
170 |     // 计算旋转速度的方向
171 |     if(me_angle > targetAngle)
172 |         rotVel *= -1;
173 |     if(fabs(me_angle - targetAngle) > PARAMS::MATH::PI)
174 |         rotVel *= -1;
175 | 
176 |     velW = rotVel;
177 | 
178 |     if ( me_vel.mod() < 300 ){
179 |         // our robot stop
180 |         next_step =  me_vel + me2target.Unitization() * PARAMS::FORWARD_ACC *
181 |                 1000 * DELTA_TIME;
182 |     }
183 | 
184 | //    else if ( angular_bias < DIRECTION_ACCURACY ){
185 |     else {
186 |         // the direction of our robot's velocity is toward target
187 |         float next_step_mod = 0;
188 |         next_step_mod = goToPosition1d( me2target.mod(), me_vel.mod() );
189 |         next_step = me2target.Unitization() * next_step_mod;
190 |     }
191 | 
192 | //    else if ( angular_bias >= DIRECTION_ACCURACY &&
193 | //              angular_bias < PARAMS::MATH::PI ) {
194 | //        // The Angle between our vehicle speed and the target direction is
195 | //        // greater than DIRECTION_ACCURACY and is less than 90 degree
196 | 
197 | //    }
198 | 
199 | //    else {
200 | //        // The Angle between our vehicle speed and the target direction is
201 | //        // greater than 90 degree
202 | 
203 | //    }
204 |     velX = next_step.x() * cos(me.orientation) - next_step.y() * sin(me.orientation);
205 |     velY = next_step.x() * sin(me.orientation) + next_step.y() * cos(me.orientation);
206 | //    velX = next_step.x() * cos(me.orientation) + next_step.y() * sin(me.orientation);
207 | //    velY = next_step.x() * sin(me.orientation) - next_step.y() * cos(me.orientation);
208 |     velX = velX / 1000;
209 |     velY = velY / 1000;
210 |     if ( PARAMS::DEBUG::kgoToPosition2d ){
211 |         std::cout /*<< "next_step : " << std::right << std::setw(12) << next_step.x()
212 |                   << ", next_step: " << std::right << std::setw(12) << next_step.y()*/
213 |                   << "\nme_vel_x : " << std::right << std::setw(12) << me_vel.x()
214 |                   << ", me_vel_y : " << std::right << std::setw(12) << me_vel.y()
215 |                   << std::endl;
216 |         std::cout << "\n=========================================================="
217 |                         "=====" << std::endl;
218 |     }
219 | }
220 | 
221 | float PathPlanner::goToPosition1d( float me2target_dis, float me_vel){
222 |     float v_max = PARAMS::FORWARD_SPEED * 1000;
223 |     float a_max = PARAMS::FORWARD_ACC * 1000;
224 |     float stop_dis = me_vel * me_vel / 2 / a_max;
225 | 
226 |     if ( PARAMS::DEBUG::kgoToPosition2d ){
227 | //        std::cout << "stop_dis : " << std::right << std::setw(12) << stop_dis / ACC_BUFFER
228 | //                  << ", me2target_dis: " << std::right << std::setw(12) << me2target_dis
229 | //                  << "\nBUFFER: " << ACC_BUFFER << std::endl;
230 |     }
231 |     float STOP_BUFFER;
232 |     if ( me_vel > 1500 ){
233 |         STOP_BUFFER = STOP_BUFFER_UP;
234 |     }
235 |     else {
236 | //        std::cout << "[pathplanner.cpp] test" << std::endl;
237 |         STOP_BUFFER = STOP_BUFFER_DOWN;
238 |     }
239 |     if ( me2target_dis <= stop_dis + STOP_BUFFER ){
240 |         return me_vel - a_max * DELTA_TIME * ACC_BUFFER;
241 |     }
242 |     else if ( (v_max - me_vel) > a_max * DELTA_TIME ) {
243 |         return me_vel + a_max * DELTA_TIME;
244 |     }
245 |     else {
246 |         return v_max;
247 |     }
248 | }
249 | 
250 | void PathPlanner::stopMoving()
251 | {
252 |     velX = 0;
253 |     velY = 0;
254 |     velW = 0;
255 | }
256 | 
257 | void PathPlanner::updatePath(std::vector<MyPoint> &pointPath)
258 | {
259 |     if(PARAMS::DEBUG::pathPlannerDebug)
260 |         qDebug() << "update path begin";
261 |     path.clear();
262 |     //不要起点
263 |     for(int i=1; i<pointPath.size(); i++)
264 |         path.push_back(pointPath[i]);
265 |     if(PARAMS::DEBUG::pathPlannerDebug)
266 |         qDebug() << "update path finish";
267 | }
268 | 
269 | void PathPlanner::clearPath()
270 | {
271 |     path.clear();
272 | }
273 | 


--------------------------------------------------------------------------------
/proto/grSim_Packet.pb.cc:
--------------------------------------------------------------------------------
  1 | // Generated by the protocol buffer compiler.  DO NOT EDIT!
  2 | // source: grSim_Packet.proto
  3 | 
  4 | #define INTERNAL_SUPPRESS_PROTOBUF_FIELD_DEPRECATION
  5 | #include "grSim_Packet.pb.h"
  6 | 
  7 | #include <algorithm>
  8 | 
  9 | #include <google/protobuf/stubs/common.h>
 10 | #include <google/protobuf/stubs/once.h>
 11 | #include <google/protobuf/io/coded_stream.h>
 12 | #include <google/protobuf/wire_format_lite_inl.h>
 13 | #include <google/protobuf/descriptor.h>
 14 | #include <google/protobuf/generated_message_reflection.h>
 15 | #include <google/protobuf/reflection_ops.h>
 16 | #include <google/protobuf/wire_format.h>
 17 | // @@protoc_insertion_point(includes)
 18 | 
 19 | namespace {
 20 | 
 21 | const ::google::protobuf::Descriptor* grSim_Packet_descriptor_ = NULL;
 22 | const ::google::protobuf::internal::GeneratedMessageReflection*
 23 |   grSim_Packet_reflection_ = NULL;
 24 | 
 25 | }  // namespace
 26 | 
 27 | 
 28 | void protobuf_AssignDesc_grSim_5fPacket_2eproto() {
 29 |   protobuf_AddDesc_grSim_5fPacket_2eproto();
 30 |   const ::google::protobuf::FileDescriptor* file =
 31 |     ::google::protobuf::DescriptorPool::generated_pool()->FindFileByName(
 32 |       "grSim_Packet.proto");
 33 |   GOOGLE_CHECK(file != NULL);
 34 |   grSim_Packet_descriptor_ = file->message_type(0);
 35 |   static const int grSim_Packet_offsets_[2] = {
 36 |     GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(grSim_Packet, commands_),
 37 |     GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(grSim_Packet, replacement_),
 38 |   };
 39 |   grSim_Packet_reflection_ =
 40 |     new ::google::protobuf::internal::GeneratedMessageReflection(
 41 |       grSim_Packet_descriptor_,
 42 |       grSim_Packet::default_instance_,
 43 |       grSim_Packet_offsets_,
 44 |       GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(grSim_Packet, _has_bits_[0]),
 45 |       GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(grSim_Packet, _unknown_fields_),
 46 |       -1,
 47 |       ::google::protobuf::DescriptorPool::generated_pool(),
 48 |       ::google::protobuf::MessageFactory::generated_factory(),
 49 |       sizeof(grSim_Packet));
 50 | }
 51 | 
 52 | namespace {
 53 | 
 54 | GOOGLE_PROTOBUF_DECLARE_ONCE(protobuf_AssignDescriptors_once_);
 55 | inline void protobuf_AssignDescriptorsOnce() {
 56 |   ::google::protobuf::GoogleOnceInit(&protobuf_AssignDescriptors_once_,
 57 |                  &protobuf_AssignDesc_grSim_5fPacket_2eproto);
 58 | }
 59 | 
 60 | void protobuf_RegisterTypes(const ::std::string&) {
 61 |   protobuf_AssignDescriptorsOnce();
 62 |   ::google::protobuf::MessageFactory::InternalRegisterGeneratedMessage(
 63 |     grSim_Packet_descriptor_, &grSim_Packet::default_instance());
 64 | }
 65 | 
 66 | }  // namespace
 67 | 
 68 | void protobuf_ShutdownFile_grSim_5fPacket_2eproto() {
 69 |   delete grSim_Packet::default_instance_;
 70 |   delete grSim_Packet_reflection_;
 71 | }
 72 | 
 73 | void protobuf_AddDesc_grSim_5fPacket_2eproto() {
 74 |   static bool already_here = false;
 75 |   if (already_here) return;
 76 |   already_here = true;
 77 |   GOOGLE_PROTOBUF_VERIFY_VERSION;
 78 | 
 79 |   ::protobuf_AddDesc_grSim_5fCommands_2eproto();
 80 |   ::protobuf_AddDesc_grSim_5fReplacement_2eproto();
 81 |   ::google::protobuf::DescriptorPool::InternalAddGeneratedFile(
 82 |     "\n\022grSim_Packet.proto\032\024grSim_Commands.pro"
 83 |     "to\032\027grSim_Replacement.proto\"Z\n\014grSim_Pac"
 84 |     "ket\022!\n\010commands\030\001 \001(\0132\017.grSim_Commands\022\'"
 85 |     "\n\013replacement\030\002 \001(\0132\022.grSim_Replacement", 159);
 86 |   ::google::protobuf::MessageFactory::InternalRegisterGeneratedFile(
 87 |     "grSim_Packet.proto", &protobuf_RegisterTypes);
 88 |   grSim_Packet::default_instance_ = new grSim_Packet();
 89 |   grSim_Packet::default_instance_->InitAsDefaultInstance();
 90 |   ::google::protobuf::internal::OnShutdown(&protobuf_ShutdownFile_grSim_5fPacket_2eproto);
 91 | }
 92 | 
 93 | // Force AddDescriptors() to be called at static initialization time.
 94 | struct StaticDescriptorInitializer_grSim_5fPacket_2eproto {
 95 |   StaticDescriptorInitializer_grSim_5fPacket_2eproto() {
 96 |     protobuf_AddDesc_grSim_5fPacket_2eproto();
 97 |   }
 98 | } static_descriptor_initializer_grSim_5fPacket_2eproto_;
 99 | 
100 | // ===================================================================
101 | 
102 | #ifndef _MSC_VER
103 | const int grSim_Packet::kCommandsFieldNumber;
104 | const int grSim_Packet::kReplacementFieldNumber;
105 | #endif  // !_MSC_VER
106 | 
107 | grSim_Packet::grSim_Packet()
108 |   : ::google::protobuf::Message() {
109 |   SharedCtor();
110 |   // @@protoc_insertion_point(constructor:grSim_Packet)
111 | }
112 | 
113 | void grSim_Packet::InitAsDefaultInstance() {
114 |   commands_ = const_cast< ::grSim_Commands*>(&::grSim_Commands::default_instance());
115 |   replacement_ = const_cast< ::grSim_Replacement*>(&::grSim_Replacement::default_instance());
116 | }
117 | 
118 | grSim_Packet::grSim_Packet(const grSim_Packet& from)
119 |   : ::google::protobuf::Message() {
120 |   SharedCtor();
121 |   MergeFrom(from);
122 |   // @@protoc_insertion_point(copy_constructor:grSim_Packet)
123 | }
124 | 
125 | void grSim_Packet::SharedCtor() {
126 |   _cached_size_ = 0;
127 |   commands_ = NULL;
128 |   replacement_ = NULL;
129 |   ::memset(_has_bits_, 0, sizeof(_has_bits_));
130 | }
131 | 
132 | grSim_Packet::~grSim_Packet() {
133 |   // @@protoc_insertion_point(destructor:grSim_Packet)
134 |   SharedDtor();
135 | }
136 | 
137 | void grSim_Packet::SharedDtor() {
138 |   if (this != default_instance_) {
139 |     delete commands_;
140 |     delete replacement_;
141 |   }
142 | }
143 | 
144 | void grSim_Packet::SetCachedSize(int size) const {
145 |   GOOGLE_SAFE_CONCURRENT_WRITES_BEGIN();
146 |   _cached_size_ = size;
147 |   GOOGLE_SAFE_CONCURRENT_WRITES_END();
148 | }
149 | const ::google::protobuf::Descriptor* grSim_Packet::descriptor() {
150 |   protobuf_AssignDescriptorsOnce();
151 |   return grSim_Packet_descriptor_;
152 | }
153 | 
154 | const grSim_Packet& grSim_Packet::default_instance() {
155 |   if (default_instance_ == NULL) protobuf_AddDesc_grSim_5fPacket_2eproto();
156 |   return *default_instance_;
157 | }
158 | 
159 | grSim_Packet* grSim_Packet::default_instance_ = NULL;
160 | 
161 | grSim_Packet* grSim_Packet::New() const {
162 |   return new grSim_Packet;
163 | }
164 | 
165 | void grSim_Packet::Clear() {
166 |   if (_has_bits_[0 / 32] & 3) {
167 |     if (has_commands()) {
168 |       if (commands_ != NULL) commands_->::grSim_Commands::Clear();
169 |     }
170 |     if (has_replacement()) {
171 |       if (replacement_ != NULL) replacement_->::grSim_Replacement::Clear();
172 |     }
173 |   }
174 |   ::memset(_has_bits_, 0, sizeof(_has_bits_));
175 |   mutable_unknown_fields()->Clear();
176 | }
177 | 
178 | bool grSim_Packet::MergePartialFromCodedStream(
179 |     ::google::protobuf::io::CodedInputStream* input) {
180 | #define DO_(EXPRESSION) if (!(EXPRESSION)) goto failure
181 |   ::google::protobuf::uint32 tag;
182 |   // @@protoc_insertion_point(parse_start:grSim_Packet)
183 |   for (;;) {
184 |     ::std::pair< ::google::protobuf::uint32, bool> p = input->ReadTagWithCutoff(127);
185 |     tag = p.first;
186 |     if (!p.second) goto handle_unusual;
187 |     switch (::google::protobuf::internal::WireFormatLite::GetTagFieldNumber(tag)) {
188 |       // optional .grSim_Commands commands = 1;
189 |       case 1: {
190 |         if (tag == 10) {
191 |           DO_(::google::protobuf::internal::WireFormatLite::ReadMessageNoVirtual(
192 |                input, mutable_commands()));
193 |         } else {
194 |           goto handle_unusual;
195 |         }
196 |         if (input->ExpectTag(18)) goto parse_replacement;
197 |         break;
198 |       }
199 | 
200 |       // optional .grSim_Replacement replacement = 2;
201 |       case 2: {
202 |         if (tag == 18) {
203 |          parse_replacement:
204 |           DO_(::google::protobuf::internal::WireFormatLite::ReadMessageNoVirtual(
205 |                input, mutable_replacement()));
206 |         } else {
207 |           goto handle_unusual;
208 |         }
209 |         if (input->ExpectAtEnd()) goto success;
210 |         break;
211 |       }
212 | 
213 |       default: {
214 |       handle_unusual:
215 |         if (tag == 0 ||
216 |             ::google::protobuf::internal::WireFormatLite::GetTagWireType(tag) ==
217 |             ::google::protobuf::internal::WireFormatLite::WIRETYPE_END_GROUP) {
218 |           goto success;
219 |         }
220 |         DO_(::google::protobuf::internal::WireFormat::SkipField(
221 |               input, tag, mutable_unknown_fields()));
222 |         break;
223 |       }
224 |     }
225 |   }
226 | success:
227 |   // @@protoc_insertion_point(parse_success:grSim_Packet)
228 |   return true;
229 | failure:
230 |   // @@protoc_insertion_point(parse_failure:grSim_Packet)
231 |   return false;
232 | #undef DO_
233 | }
234 | 
235 | void grSim_Packet::SerializeWithCachedSizes(
236 |     ::google::protobuf::io::CodedOutputStream* output) const {
237 |   // @@protoc_insertion_point(serialize_start:grSim_Packet)
238 |   // optional .grSim_Commands commands = 1;
239 |   if (has_commands()) {
240 |     ::google::protobuf::internal::WireFormatLite::WriteMessageMaybeToArray(
241 |       1, this->commands(), output);
242 |   }
243 | 
244 |   // optional .grSim_Replacement replacement = 2;
245 |   if (has_replacement()) {
246 |     ::google::protobuf::internal::WireFormatLite::WriteMessageMaybeToArray(
247 |       2, this->replacement(), output);
248 |   }
249 | 
250 |   if (!unknown_fields().empty()) {
251 |     ::google::protobuf::internal::WireFormat::SerializeUnknownFields(
252 |         unknown_fields(), output);
253 |   }
254 |   // @@protoc_insertion_point(serialize_end:grSim_Packet)
255 | }
256 | 
257 | ::google::protobuf::uint8* grSim_Packet::SerializeWithCachedSizesToArray(
258 |     ::google::protobuf::uint8* target) const {
259 |   // @@protoc_insertion_point(serialize_to_array_start:grSim_Packet)
260 |   // optional .grSim_Commands commands = 1;
261 |   if (has_commands()) {
262 |     target = ::google::protobuf::internal::WireFormatLite::
263 |       WriteMessageNoVirtualToArray(
264 |         1, this->commands(), target);
265 |   }
266 | 
267 |   // optional .grSim_Replacement replacement = 2;
268 |   if (has_replacement()) {
269 |     target = ::google::protobuf::internal::WireFormatLite::
270 |       WriteMessageNoVirtualToArray(
271 |         2, this->replacement(), target);
272 |   }
273 | 
274 |   if (!unknown_fields().empty()) {
275 |     target = ::google::protobuf::internal::WireFormat::SerializeUnknownFieldsToArray(
276 |         unknown_fields(), target);
277 |   }
278 |   // @@protoc_insertion_point(serialize_to_array_end:grSim_Packet)
279 |   return target;
280 | }
281 | 
282 | int grSim_Packet::ByteSize() const {
283 |   int total_size = 0;
284 | 
285 |   if (_has_bits_[0 / 32] & (0xffu << (0 % 32))) {
286 |     // optional .grSim_Commands commands = 1;
287 |     if (has_commands()) {
288 |       total_size += 1 +
289 |         ::google::protobuf::internal::WireFormatLite::MessageSizeNoVirtual(
290 |           this->commands());
291 |     }
292 | 
293 |     // optional .grSim_Replacement replacement = 2;
294 |     if (has_replacement()) {
295 |       total_size += 1 +
296 |         ::google::protobuf::internal::WireFormatLite::MessageSizeNoVirtual(
297 |           this->replacement());
298 |     }
299 | 
300 |   }
301 |   if (!unknown_fields().empty()) {
302 |     total_size +=
303 |       ::google::protobuf::internal::WireFormat::ComputeUnknownFieldsSize(
304 |         unknown_fields());
305 |   }
306 |   GOOGLE_SAFE_CONCURRENT_WRITES_BEGIN();
307 |   _cached_size_ = total_size;
308 |   GOOGLE_SAFE_CONCURRENT_WRITES_END();
309 |   return total_size;
310 | }
311 | 
312 | void grSim_Packet::MergeFrom(const ::google::protobuf::Message& from) {
313 |   GOOGLE_CHECK_NE(&from, this);
314 |   const grSim_Packet* source =
315 |     ::google::protobuf::internal::dynamic_cast_if_available<const grSim_Packet*>(
316 |       &from);
317 |   if (source == NULL) {
318 |     ::google::protobuf::internal::ReflectionOps::Merge(from, this);
319 |   } else {
320 |     MergeFrom(*source);
321 |   }
322 | }
323 | 
324 | void grSim_Packet::MergeFrom(const grSim_Packet& from) {
325 |   GOOGLE_CHECK_NE(&from, this);
326 |   if (from._has_bits_[0 / 32] & (0xffu << (0 % 32))) {
327 |     if (from.has_commands()) {
328 |       mutable_commands()->::grSim_Commands::MergeFrom(from.commands());
329 |     }
330 |     if (from.has_replacement()) {
331 |       mutable_replacement()->::grSim_Replacement::MergeFrom(from.replacement());
332 |     }
333 |   }
334 |   mutable_unknown_fields()->MergeFrom(from.unknown_fields());
335 | }
336 | 
337 | void grSim_Packet::CopyFrom(const ::google::protobuf::Message& from) {
338 |   if (&from == this) return;
339 |   Clear();
340 |   MergeFrom(from);
341 | }
342 | 
343 | void grSim_Packet::CopyFrom(const grSim_Packet& from) {
344 |   if (&from == this) return;
345 |   Clear();
346 |   MergeFrom(from);
347 | }
348 | 
349 | bool grSim_Packet::IsInitialized() const {
350 | 
351 |   if (has_commands()) {
352 |     if (!this->commands().IsInitialized()) return false;
353 |   }
354 |   if (has_replacement()) {
355 |     if (!this->replacement().IsInitialized()) return false;
356 |   }
357 |   return true;
358 | }
359 | 
360 | void grSim_Packet::Swap(grSim_Packet* other) {
361 |   if (other != this) {
362 |     std::swap(commands_, other->commands_);
363 |     std::swap(replacement_, other->replacement_);
364 |     std::swap(_has_bits_[0], other->_has_bits_[0]);
365 |     _unknown_fields_.Swap(&other->_unknown_fields_);
366 |     std::swap(_cached_size_, other->_cached_size_);
367 |   }
368 | }
369 | 
370 | ::google::protobuf::Metadata grSim_Packet::GetMetadata() const {
371 |   protobuf_AssignDescriptorsOnce();
372 |   ::google::protobuf::Metadata metadata;
373 |   metadata.descriptor = grSim_Packet_descriptor_;
374 |   metadata.reflection = grSim_Packet_reflection_;
375 |   return metadata;
376 | }
377 | 
378 | 
379 | // @@protoc_insertion_point(namespace_scope)
380 | 
381 | // @@protoc_insertion_point(global_scope)
382 | 


--------------------------------------------------------------------------------
/algorithm/artifical_potential.cpp:
--------------------------------------------------------------------------------
  1 | #include "artifical_potential.h"
  2 | #include "utils/datamanager.h"
  3 | #include "utils/params.h"
  4 | #include "communication/udpsender.h"
  5 | #include <cmath>
  6 | #include <iostream>
  7 | #include <iomanip>
  8 | 
  9 | // Athena     output unit mm
 10 | // simulation output unit m
 11 | // serial     output unit cm
 12 | 
 13 | namespace{
 14 | const int m = 3;
 15 | const int n = 2;
 16 | const float alpha_p = 1.0;
 17 | const float alpha_v = 5;
 18 | // virtual mass of attractive potential field
 19 | // for sim
 20 | //const float m_attractive = 1200;
 21 | // for real
 22 | const float m_attractive = 12;
 23 | // virtual mass of repulsive potential field
 24 | // for sim
 25 | //const float m_repulsive = 0.05;
 26 | // for real
 27 | const float m_repulsive = 0.00005;
 28 | //const float MAX_ACC = 1000;
 29 | //const float MAX_SPEED = 1500;
 30 | const float MAX_ACC = 3500;
 31 | const float MAX_SPEED = 5500;
 32 | const float ACC_BUFFER = 2;
 33 | const float RATE = 0.5;
 34 | const float REP_RATE = 0.8;
 35 | //const float eta = 7500.0;  // positive constant
 36 | //const float rou_0 = 110;  // a positive constant describing the influence range of
 37 | //                        // the obstacle
 38 | const float eta = 11000.0;  // positive constant
 39 | const float rou_0 = 110;  // a positive constant describing the influence range of
 40 |                         // the obstacle
 41 | const float DELTA_TIME  = 0.03;  // unit is second
 42 | }
 43 | 
 44 | // change Athena axis y up
 45 | //void ArtificalPotential::plan( MyPoint target, MyVector v_target ){
 46 | bool ArtificalPotential::plan( MyPoint target ){
 47 |     target.Sety(-target.y());
 48 |     bool if_use_artifical_potential = false;
 49 |     if ( PARAMS::DEBUG::kAPDebugMessage ){
 50 |         std::cout << "\n==============  [artifical_potenial.cpp] debug  ======="
 51 |                         "========" << std::endl;
 52 |     }
 53 | 
 54 |     RobotInfo& me = MyDataManager::instance()->ourRobot();
 55 |     MyVector v_target( 0, 0 );
 56 |     MyPoint me_pos( me.x, -me.y );
 57 |     MyVector me_vel;
 58 |     me_vel.Setx(me.vel_x);
 59 |     me_vel.Sety(me.vel_y);
 60 | 
 61 |     if ( PARAMS::DEBUG::kAPDebugMessage ) {
 62 | //        std::cout << "me_vel.x                      : "
 63 | //                  << std::right << std::setw(12)
 64 | //                  << me_vel.x()
 65 | //                  << ", \nme_vel.y                      : "
 66 | //                  << std::right << std::setw(12)
 67 | //                  << me_vel.y()
 68 | //                  << std::endl;
 69 | //        std::cout << "me.vel_x                      : "
 70 | //                  << std::right << std::setw(12)
 71 | //                  << me.vel_x
 72 | //                  << ", \nme.vel_y                      : "
 73 | //                  << std::right << std::setw(12)
 74 | //                  << me.vel_y
 75 | //                  << std::endl;
 76 |     }
 77 | 
 78 |     MyVector f_attractive =
 79 |             ( target - me_pos ) * m * alpha_p *
 80 |             pow( (target - me_pos).mod(), m - 2 )
 81 |              + ( v_target - me_vel ) * n * alpha_v *
 82 |             pow( ( v_target - me_vel ).mod(), n - 2 );
 83 |     MyVector acc_attractive = f_attractive / m_attractive ;
 84 |     float rou_s[PARAMS::ROBOT_NUM];
 85 |     float rou_m[PARAMS::ROBOT_NUM];
 86 |     MyVector f_rep1[PARAMS::ROBOT_NUM];
 87 |     MyVector f_rep2[PARAMS::ROBOT_NUM];
 88 |     MyVector f_rep( 0, 0 );
 89 |     for ( int i = 0; i < PARAMS::ROBOT_NUM; i++ ){
 90 |         if ( MyDataManager::instance()->validYellowRobots[i] ){
 91 |             MyPoint yellow_pos( MyDataManager::instance()->yellowRobots[i].x,
 92 |                                 -MyDataManager::instance()->yellowRobots[i].y );
 93 | 
 94 |             if ( PARAMS::DEBUG::kAPDebugMessage ) {
 95 |                 std::cout << "Yellow id: " << i << std::endl;
 96 |             }
 97 | 
 98 |             MyVector yellow_vel(
 99 |                         MyDataManager::instance()->yellowRobots[i].vel_x,
100 |                         MyDataManager::instance()->yellowRobots[i].vel_y);
101 |             rou_s[i] = ( me_pos - yellow_pos ).mod();
102 |             MyVector n_ro = ( yellow_pos - me_pos ).Unitization();
103 |             MyVector v_ro = n_ro * ( ( me_vel - yellow_vel ) * n_ro );
104 |             MyVector v_ro_vertical = me_vel - yellow_vel - v_ro;
105 |             MyVector n_ro_vertical = v_ro_vertical.Unitization();
106 |             rou_m[i] = v_ro.mod() * v_ro.mod() / 2 / MAX_ACC;
107 | 
108 |             bool if_rep;
109 |             if ( !last_statue )
110 |                 if_rep = rou_s[i] - rou_m[i] < rou_0 &&
111 |                     rou_s[i] - rou_m[i] > 0 &&
112 |                     ( me_vel - yellow_vel ) * n_ro > 0;
113 |             else
114 |                 if_rep = rou_s[i] - rou_m[i] < rou_0 / REP_RATE &&
115 |                         rou_s[i] - rou_m[i] > 0 &&
116 |                         ( me_vel - yellow_vel ) * n_ro > 0;
117 |             last_statue = false;
118 | 
119 |             if ( PARAMS::DEBUG::kAPDebugMessage ) {
120 | //                std::cout << "rou_s[i] - rou_m[i]           : "
121 | //                          << std::right << std::setw(12)
122 | //                          << rou_s[i] - rou_m[i]
123 | //                          << "\n"
124 | //                          << std::endl;
125 | //                std::cout << "( me_vel - yellow_vel ) * n_ro: "
126 | //                          << std::right << std::setw(12)
127 | //                          << ( me_vel - yellow_vel ) * n_ro
128 | //                          << std::endl;
129 | //                std::cout << "(me_vel - yellow_vel) x       : "
130 | //                          << std::right << std::setw(12)
131 | //                          << (me_vel - yellow_vel).x()
132 | //                          << "\n(me_vel - yellow_vel) y       : "
133 | //                          << std::right << std::setw(12)
134 | //                          << (me_vel - yellow_vel).y()
135 | //                          << std::endl;
136 | //                std::cout << "n_ro x                        : "
137 | //                          << std::right << std::setw(12) << n_ro.x()
138 | //                          << "\nn_ro y                        : "
139 | //                          << std::right << std::setw(12) << n_ro.y()
140 | //                          << std::endl
141 | //                          << std::endl;
142 |             }
143 | 
144 |             if ( rou_s[i] - rou_m[i] >= rou_0 ||
145 |                  ( me_vel - yellow_vel ) * n_ro <= 0 ){
146 | 
147 |             }
148 |             else if ( if_rep ){
149 |                 f_rep1[i] = n_ro * (-1) * eta * ( 1 + v_ro.mod() / MAX_ACC ) /
150 |                         ( rou_s[i] - rou_m[i] ) / ( rou_s[i] - rou_m[i] );
151 |                 f_rep2[i] = n_ro_vertical *
152 |                         eta * v_ro.mod() * v_ro_vertical.mod() / rou_s[i]
153 |                         / MAX_ACC / ( rou_s[i] - rou_m[i] ) /
154 |                         ( rou_s[i] - rou_m[i] );
155 |                 f_rep = f_rep + f_rep1[i] + f_rep2[i];
156 |                 if ( yellow_vel.mod() > 300 ){
157 |                     if_use_artifical_potential = true;
158 |                     if (PARAMS::DEBUG::kAPDebugMessage){
159 |                         std::cout << "[artifical_potential.cpp]"
160 |                                   << "\nyellow_vel.mod: " << yellow_vel.mod()
161 |                                   << std::endl;
162 |                     }
163 |                 }
164 |             }
165 |             else{
166 |                 if ( PARAMS::DEBUG::kAPDebugMessage ){
167 |                     std::cout << "[artifical_potential.cpp] not defined!\n"
168 |                               << std::endl;
169 |                 }
170 |             }
171 |         }
172 |     }
173 |     for ( int i = 0; i < PARAMS::ROBOT_NUM; i++ ){
174 |         if ( MyDataManager::instance()->validBlueRobots[i] &&
175 |              i != PARAMS::our_id - 1 ){
176 |             MyPoint blue_pos( MyDataManager::instance()->blueRobots[i].x,
177 |                                -MyDataManager::instance()->blueRobots[i].y );
178 | 
179 |             if ( PARAMS::DEBUG::kAPDebugMessage ) {
180 |                 std::cout << "Blue id: " << i << std::endl;
181 |             }
182 | 
183 |             MyVector blue_vel(
184 |                         MyDataManager::instance()->blueRobots[i].vel_x,
185 |                         MyDataManager::instance()->blueRobots[i].vel_y);
186 |             rou_s[i] = ( me_pos - blue_pos ).mod();
187 |             MyVector n_ro = ( blue_pos - me_pos ).Unitization();
188 |             MyVector v_ro = n_ro * ( ( me_vel - blue_vel ) * n_ro );
189 |             MyVector v_ro_vertical = me_vel - blue_vel - v_ro;
190 |             MyVector n_ro_vertical = v_ro_vertical.Unitization();
191 |             rou_m[i] = v_ro.mod() * v_ro.mod() / 2 / MAX_ACC;
192 | 
193 |             bool if_rep;
194 |             if ( !last_statue )
195 |                 if_rep = rou_s[i] - rou_m[i] < rou_0 &&
196 |                     rou_s[i] - rou_m[i] > 0 &&
197 |                     ( me_vel - blue_vel ) * n_ro > 0;
198 |             else
199 |                 if_rep = rou_s[i] - rou_m[i] < rou_0 * REP_RATE &&
200 |                         rou_s[i] - rou_m[i] > 0 &&
201 |                         ( me_vel - blue_vel ) * n_ro > 0;
202 |             last_statue = false;
203 |             if ( PARAMS::DEBUG::kAPDebugMessage ) {
204 | //                std::cout << "rou_s[i] - rou_m[i]           : "
205 | //                          << std::right << std::setw(12)
206 | //                          << rou_s[i] - rou_m[i]
207 | //                          << "\n"
208 | //                          << std::endl;
209 | //                std::cout << "( me_vel - blue_vel ) * n_ro  : "
210 | //                          << std::right << std::setw(12)
211 | //                          << ( me_vel - blue_vel ) * n_ro
212 | //                          << std::endl;
213 | //                std::cout << "(me_vel - blue_vel) x         : "
214 | //                          << std::right << std::setw(12)
215 | //                          << (me_vel - blue_vel).x()
216 | //                          << "\n(me_vel - blue_vel) y         : "
217 | //                          << std::right << std::setw(12)
218 | //                          << (me_vel - blue_vel).y()
219 | //                          << std::endl;
220 | //                std::cout << "n_ro x                        : "
221 | //                          << std::right << std::setw(12) << n_ro.x()
222 | //                          << "\nn_ro y                        : "
223 | //                          << std::right << std::setw(12) << n_ro.y()
224 | //                          << std::endl
225 | //                          << std::endl;
226 |             }
227 | 
228 |             if ( rou_s[i] - rou_m[i] >= rou_0 ||
229 |                  ( me_vel - blue_vel ) * n_ro <= 0 ){
230 |             }
231 |             else if ( if_rep ){
232 |                 f_rep1[i] = n_ro * (-1) * eta * ( 1 + v_ro.mod() / MAX_ACC ) /
233 |                         ( rou_s[i] - rou_m[i] ) / ( rou_s[i] - rou_m[i] );
234 |                 f_rep2[i] = n_ro_vertical *
235 |                         eta * v_ro.mod() * v_ro_vertical.mod() / rou_s[i]
236 |                         / MAX_ACC / ( rou_s[i] - rou_m[i] ) /
237 |                         ( rou_s[i] - rou_m[i] );
238 |                 f_rep = f_rep + f_rep1[i] + f_rep2[i];
239 |                 if ( blue_vel.mod() > 300 ){
240 |                     if_use_artifical_potential = true;
241 |                     if (PARAMS::DEBUG::kAPDebugMessage){
242 |                         std::cout << "[artifical_potential.cpp]"
243 |                                   << "\nblue_vel.mod: " << blue_vel.mod()
244 |                                   << std::endl;
245 |                     }
246 |                 }
247 |             }
248 |             else{
249 |                 if ( PARAMS::DEBUG::kAPDebugMessage ){
250 |                     std::cout << "[artifical_potential.cpp] not defined!\n"
251 |                               << std::endl;
252 |                 }
253 |             }
254 |         }
255 |     }
256 | 
257 |     MyVector acc_repulsive = f_rep / m_repulsive;
258 | 
259 | //    MyVector acc_tol = acc_attractive + acc_repulsive;
260 |     MyVector acc_tol;
261 |     if ( me_vel.mod() > MAX_SPEED * RATE ){
262 |         acc_tol = acc_repulsive;
263 |     }
264 |     else{
265 |         acc_tol = acc_attractive + acc_repulsive;
266 |     }
267 | 
268 |     if ( acc_tol.mod() > MAX_ACC ){
269 |         acc_tol = acc_tol.Unitization() * MAX_ACC;
270 |     }
271 | 
272 |     MyVector next_step_vel;
273 |     next_step_vel = me_vel + acc_tol * DELTA_TIME * ACC_BUFFER;
274 | 
275 |     if ( next_step_vel.mod() > MAX_SPEED ){
276 |         next_step_vel = next_step_vel.Unitization() * MAX_SPEED;
277 |     }
278 | 
279 |     float dir = me.orientation;
280 |     if ( PARAMS::IS_SIMULATION ){
281 |         v_x = next_step_vel.x() * cos(dir) + next_step_vel.y() * sin(dir);
282 |         v_y = - next_step_vel.x() * sin(dir) + next_step_vel.y() * cos(dir);
283 |     }
284 |     else {
285 |         v_x = next_step_vel.x() * cos(dir) + next_step_vel.y() * sin(dir);
286 |         v_y = next_step_vel.x() * sin(dir) - next_step_vel.y() * cos(dir);
287 |     }
288 | 
289 |     if ( PARAMS::DEBUG::kAPDebugMessage ) {
290 | //        std::cout << "[ap.cpp] \nacc_attractive_x: " << std::right << std::setw(12) << acc_attractive.x()
291 | //                  << ", acc_attractive_y: " << std::right << std::setw(12) << acc_attractive.y()
292 | //                  << "\nacc_repulsive_x : " << std::right << std::setw(12) << acc_repulsive.x()
293 | //                  << ", acc_repulsive_y : " << std::right << std::setw(12) << acc_repulsive.y()
294 | //                  << "\nacc_tol_x       : " << std::right << std::setw(12) << acc_tol.x()
295 | //                  << ", acc_tol_y       : " << std::right << std::setw(12) <<  acc_tol.y()
296 | //                  << "\nme_vel_x        : " << std::right << std::setw(12) << me.vel_x
297 | //                  << ", me_vel_y        : " << std::right << std::setw(12) << me.vel_y
298 | //                  << "\nnext_step_vel_x : " << std::right << std::setw(12) << next_step_vel.x()
299 | //                  << ", next_step_vel_y : " << std::right << std::setw(12) << next_step_vel.y()
300 | //                  << "\nv_x             : " << std::right << std::setw(12) << v_x
301 | //                  << ", v_y             : " << std::right << std::setw(12) << v_y
302 | //                  << "\ndir             : " << std::right << std::setw(12) << dir
303 | //                  << std::endl;
304 |         std::cout << "\n=========================================================="
305 |                         "=====" << std::endl;
306 |     }
307 | 
308 |     return if_use_artifical_potential;
309 | }
310 | 


--------------------------------------------------------------------------------
/proto/grSim_Replacement.pb.h:
--------------------------------------------------------------------------------
  1 | // Generated by the protocol buffer compiler.  DO NOT EDIT!
  2 | // source: grSim_Replacement.proto
  3 | 
  4 | #ifndef PROTOBUF_grSim_5fReplacement_2eproto__INCLUDED
  5 | #define PROTOBUF_grSim_5fReplacement_2eproto__INCLUDED
  6 | 
  7 | #include <string>
  8 | 
  9 | #include <google/protobuf/stubs/common.h>
 10 | 
 11 | #if GOOGLE_PROTOBUF_VERSION < 2006000
 12 | #error This file was generated by a newer version of protoc which is
 13 | #error incompatible with your Protocol Buffer headers.  Please update
 14 | #error your headers.
 15 | #endif
 16 | #if 2006001 < GOOGLE_PROTOBUF_MIN_PROTOC_VERSION
 17 | #error This file was generated by an older version of protoc which is
 18 | #error incompatible with your Protocol Buffer headers.  Please
 19 | #error regenerate this file with a newer version of protoc.
 20 | #endif
 21 | 
 22 | #include <google/protobuf/generated_message_util.h>
 23 | #include <google/protobuf/message.h>
 24 | #include <google/protobuf/repeated_field.h>
 25 | #include <google/protobuf/extension_set.h>
 26 | #include <google/protobuf/unknown_field_set.h>
 27 | // @@protoc_insertion_point(includes)
 28 | 
 29 | // Internal implementation detail -- do not call these.
 30 | void  protobuf_AddDesc_grSim_5fReplacement_2eproto();
 31 | void protobuf_AssignDesc_grSim_5fReplacement_2eproto();
 32 | void protobuf_ShutdownFile_grSim_5fReplacement_2eproto();
 33 | 
 34 | class grSim_RobotReplacement;
 35 | class grSim_BallReplacement;
 36 | class grSim_Replacement;
 37 | 
 38 | // ===================================================================
 39 | 
 40 | class grSim_RobotReplacement : public ::google::protobuf::Message {
 41 |  public:
 42 |   grSim_RobotReplacement();
 43 |   virtual ~grSim_RobotReplacement();
 44 | 
 45 |   grSim_RobotReplacement(const grSim_RobotReplacement& from);
 46 | 
 47 |   inline grSim_RobotReplacement& operator=(const grSim_RobotReplacement& from) {
 48 |     CopyFrom(from);
 49 |     return *this;
 50 |   }
 51 | 
 52 |   inline const ::google::protobuf::UnknownFieldSet& unknown_fields() const {
 53 |     return _unknown_fields_;
 54 |   }
 55 | 
 56 |   inline ::google::protobuf::UnknownFieldSet* mutable_unknown_fields() {
 57 |     return &_unknown_fields_;
 58 |   }
 59 | 
 60 |   static const ::google::protobuf::Descriptor* descriptor();
 61 |   static const grSim_RobotReplacement& default_instance();
 62 | 
 63 |   void Swap(grSim_RobotReplacement* other);
 64 | 
 65 |   // implements Message ----------------------------------------------
 66 | 
 67 |   grSim_RobotReplacement* New() const;
 68 |   void CopyFrom(const ::google::protobuf::Message& from);
 69 |   void MergeFrom(const ::google::protobuf::Message& from);
 70 |   void CopyFrom(const grSim_RobotReplacement& from);
 71 |   void MergeFrom(const grSim_RobotReplacement& from);
 72 |   void Clear();
 73 |   bool IsInitialized() const;
 74 | 
 75 |   int ByteSize() const;
 76 |   bool MergePartialFromCodedStream(
 77 |       ::google::protobuf::io::CodedInputStream* input);
 78 |   void SerializeWithCachedSizes(
 79 |       ::google::protobuf::io::CodedOutputStream* output) const;
 80 |   ::google::protobuf::uint8* SerializeWithCachedSizesToArray(::google::protobuf::uint8* output) const;
 81 |   int GetCachedSize() const { return _cached_size_; }
 82 |   private:
 83 |   void SharedCtor();
 84 |   void SharedDtor();
 85 |   void SetCachedSize(int size) const;
 86 |   public:
 87 |   ::google::protobuf::Metadata GetMetadata() const;
 88 | 
 89 |   // nested types ----------------------------------------------------
 90 | 
 91 |   // accessors -------------------------------------------------------
 92 | 
 93 |   // required double x = 1;
 94 |   inline bool has_x() const;
 95 |   inline void clear_x();
 96 |   static const int kXFieldNumber = 1;
 97 |   inline double x() const;
 98 |   inline void set_x(double value);
 99 | 
100 |   // required double y = 2;
101 |   inline bool has_y() const;
102 |   inline void clear_y();
103 |   static const int kYFieldNumber = 2;
104 |   inline double y() const;
105 |   inline void set_y(double value);
106 | 
107 |   // required double dir = 3;
108 |   inline bool has_dir() const;
109 |   inline void clear_dir();
110 |   static const int kDirFieldNumber = 3;
111 |   inline double dir() const;
112 |   inline void set_dir(double value);
113 | 
114 |   // required uint32 id = 4;
115 |   inline bool has_id() const;
116 |   inline void clear_id();
117 |   static const int kIdFieldNumber = 4;
118 |   inline ::google::protobuf::uint32 id() const;
119 |   inline void set_id(::google::protobuf::uint32 value);
120 | 
121 |   // required bool yellowteam = 5;
122 |   inline bool has_yellowteam() const;
123 |   inline void clear_yellowteam();
124 |   static const int kYellowteamFieldNumber = 5;
125 |   inline bool yellowteam() const;
126 |   inline void set_yellowteam(bool value);
127 | 
128 |   // optional bool turnon = 6;
129 |   inline bool has_turnon() const;
130 |   inline void clear_turnon();
131 |   static const int kTurnonFieldNumber = 6;
132 |   inline bool turnon() const;
133 |   inline void set_turnon(bool value);
134 | 
135 |   // @@protoc_insertion_point(class_scope:grSim_RobotReplacement)
136 |  private:
137 |   inline void set_has_x();
138 |   inline void clear_has_x();
139 |   inline void set_has_y();
140 |   inline void clear_has_y();
141 |   inline void set_has_dir();
142 |   inline void clear_has_dir();
143 |   inline void set_has_id();
144 |   inline void clear_has_id();
145 |   inline void set_has_yellowteam();
146 |   inline void clear_has_yellowteam();
147 |   inline void set_has_turnon();
148 |   inline void clear_has_turnon();
149 | 
150 |   ::google::protobuf::UnknownFieldSet _unknown_fields_;
151 | 
152 |   ::google::protobuf::uint32 _has_bits_[1];
153 |   mutable int _cached_size_;
154 |   double x_;
155 |   double y_;
156 |   double dir_;
157 |   ::google::protobuf::uint32 id_;
158 |   bool yellowteam_;
159 |   bool turnon_;
160 |   friend void  protobuf_AddDesc_grSim_5fReplacement_2eproto();
161 |   friend void protobuf_AssignDesc_grSim_5fReplacement_2eproto();
162 |   friend void protobuf_ShutdownFile_grSim_5fReplacement_2eproto();
163 | 
164 |   void InitAsDefaultInstance();
165 |   static grSim_RobotReplacement* default_instance_;
166 | };
167 | // -------------------------------------------------------------------
168 | 
169 | class grSim_BallReplacement : public ::google::protobuf::Message {
170 |  public:
171 |   grSim_BallReplacement();
172 |   virtual ~grSim_BallReplacement();
173 | 
174 |   grSim_BallReplacement(const grSim_BallReplacement& from);
175 | 
176 |   inline grSim_BallReplacement& operator=(const grSim_BallReplacement& from) {
177 |     CopyFrom(from);
178 |     return *this;
179 |   }
180 | 
181 |   inline const ::google::protobuf::UnknownFieldSet& unknown_fields() const {
182 |     return _unknown_fields_;
183 |   }
184 | 
185 |   inline ::google::protobuf::UnknownFieldSet* mutable_unknown_fields() {
186 |     return &_unknown_fields_;
187 |   }
188 | 
189 |   static const ::google::protobuf::Descriptor* descriptor();
190 |   static const grSim_BallReplacement& default_instance();
191 | 
192 |   void Swap(grSim_BallReplacement* other);
193 | 
194 |   // implements Message ----------------------------------------------
195 | 
196 |   grSim_BallReplacement* New() const;
197 |   void CopyFrom(const ::google::protobuf::Message& from);
198 |   void MergeFrom(const ::google::protobuf::Message& from);
199 |   void CopyFrom(const grSim_BallReplacement& from);
200 |   void MergeFrom(const grSim_BallReplacement& from);
201 |   void Clear();
202 |   bool IsInitialized() const;
203 | 
204 |   int ByteSize() const;
205 |   bool MergePartialFromCodedStream(
206 |       ::google::protobuf::io::CodedInputStream* input);
207 |   void SerializeWithCachedSizes(
208 |       ::google::protobuf::io::CodedOutputStream* output) const;
209 |   ::google::protobuf::uint8* SerializeWithCachedSizesToArray(::google::protobuf::uint8* output) const;
210 |   int GetCachedSize() const { return _cached_size_; }
211 |   private:
212 |   void SharedCtor();
213 |   void SharedDtor();
214 |   void SetCachedSize(int size) const;
215 |   public:
216 |   ::google::protobuf::Metadata GetMetadata() const;
217 | 
218 |   // nested types ----------------------------------------------------
219 | 
220 |   // accessors -------------------------------------------------------
221 | 
222 |   // required double x = 1;
223 |   inline bool has_x() const;
224 |   inline void clear_x();
225 |   static const int kXFieldNumber = 1;
226 |   inline double x() const;
227 |   inline void set_x(double value);
228 | 
229 |   // required double y = 2;
230 |   inline bool has_y() const;
231 |   inline void clear_y();
232 |   static const int kYFieldNumber = 2;
233 |   inline double y() const;
234 |   inline void set_y(double value);
235 | 
236 |   // required double vx = 3;
237 |   inline bool has_vx() const;
238 |   inline void clear_vx();
239 |   static const int kVxFieldNumber = 3;
240 |   inline double vx() const;
241 |   inline void set_vx(double value);
242 | 
243 |   // required double vy = 4;
244 |   inline bool has_vy() const;
245 |   inline void clear_vy();
246 |   static const int kVyFieldNumber = 4;
247 |   inline double vy() const;
248 |   inline void set_vy(double value);
249 | 
250 |   // @@protoc_insertion_point(class_scope:grSim_BallReplacement)
251 |  private:
252 |   inline void set_has_x();
253 |   inline void clear_has_x();
254 |   inline void set_has_y();
255 |   inline void clear_has_y();
256 |   inline void set_has_vx();
257 |   inline void clear_has_vx();
258 |   inline void set_has_vy();
259 |   inline void clear_has_vy();
260 | 
261 |   ::google::protobuf::UnknownFieldSet _unknown_fields_;
262 | 
263 |   ::google::protobuf::uint32 _has_bits_[1];
264 |   mutable int _cached_size_;
265 |   double x_;
266 |   double y_;
267 |   double vx_;
268 |   double vy_;
269 |   friend void  protobuf_AddDesc_grSim_5fReplacement_2eproto();
270 |   friend void protobuf_AssignDesc_grSim_5fReplacement_2eproto();
271 |   friend void protobuf_ShutdownFile_grSim_5fReplacement_2eproto();
272 | 
273 |   void InitAsDefaultInstance();
274 |   static grSim_BallReplacement* default_instance_;
275 | };
276 | // -------------------------------------------------------------------
277 | 
278 | class grSim_Replacement : public ::google::protobuf::Message {
279 |  public:
280 |   grSim_Replacement();
281 |   virtual ~grSim_Replacement();
282 | 
283 |   grSim_Replacement(const grSim_Replacement& from);
284 | 
285 |   inline grSim_Replacement& operator=(const grSim_Replacement& from) {
286 |     CopyFrom(from);
287 |     return *this;
288 |   }
289 | 
290 |   inline const ::google::protobuf::UnknownFieldSet& unknown_fields() const {
291 |     return _unknown_fields_;
292 |   }
293 | 
294 |   inline ::google::protobuf::UnknownFieldSet* mutable_unknown_fields() {
295 |     return &_unknown_fields_;
296 |   }
297 | 
298 |   static const ::google::protobuf::Descriptor* descriptor();
299 |   static const grSim_Replacement& default_instance();
300 | 
301 |   void Swap(grSim_Replacement* other);
302 | 
303 |   // implements Message ----------------------------------------------
304 | 
305 |   grSim_Replacement* New() const;
306 |   void CopyFrom(const ::google::protobuf::Message& from);
307 |   void MergeFrom(const ::google::protobuf::Message& from);
308 |   void CopyFrom(const grSim_Replacement& from);
309 |   void MergeFrom(const grSim_Replacement& from);
310 |   void Clear();
311 |   bool IsInitialized() const;
312 | 
313 |   int ByteSize() const;
314 |   bool MergePartialFromCodedStream(
315 |       ::google::protobuf::io::CodedInputStream* input);
316 |   void SerializeWithCachedSizes(
317 |       ::google::protobuf::io::CodedOutputStream* output) const;
318 |   ::google::protobuf::uint8* SerializeWithCachedSizesToArray(::google::protobuf::uint8* output) const;
319 |   int GetCachedSize() const { return _cached_size_; }
320 |   private:
321 |   void SharedCtor();
322 |   void SharedDtor();
323 |   void SetCachedSize(int size) const;
324 |   public:
325 |   ::google::protobuf::Metadata GetMetadata() const;
326 | 
327 |   // nested types ----------------------------------------------------
328 | 
329 |   // accessors -------------------------------------------------------
330 | 
331 |   // optional .grSim_BallReplacement ball = 1;
332 |   inline bool has_ball() const;
333 |   inline void clear_ball();
334 |   static const int kBallFieldNumber = 1;
335 |   inline const ::grSim_BallReplacement& ball() const;
336 |   inline ::grSim_BallReplacement* mutable_ball();
337 |   inline ::grSim_BallReplacement* release_ball();
338 |   inline void set_allocated_ball(::grSim_BallReplacement* ball);
339 | 
340 |   // repeated .grSim_RobotReplacement robots = 2;
341 |   inline int robots_size() const;
342 |   inline void clear_robots();
343 |   static const int kRobotsFieldNumber = 2;
344 |   inline const ::grSim_RobotReplacement& robots(int index) const;
345 |   inline ::grSim_RobotReplacement* mutable_robots(int index);
346 |   inline ::grSim_RobotReplacement* add_robots();
347 |   inline const ::google::protobuf::RepeatedPtrField< ::grSim_RobotReplacement >&
348 |       robots() const;
349 |   inline ::google::protobuf::RepeatedPtrField< ::grSim_RobotReplacement >*
350 |       mutable_robots();
351 | 
352 |   // @@protoc_insertion_point(class_scope:grSim_Replacement)
353 |  private:
354 |   inline void set_has_ball();
355 |   inline void clear_has_ball();
356 | 
357 |   ::google::protobuf::UnknownFieldSet _unknown_fields_;
358 | 
359 |   ::google::protobuf::uint32 _has_bits_[1];
360 |   mutable int _cached_size_;
361 |   ::grSim_BallReplacement* ball_;
362 |   ::google::protobuf::RepeatedPtrField< ::grSim_RobotReplacement > robots_;
363 |   friend void  protobuf_AddDesc_grSim_5fReplacement_2eproto();
364 |   friend void protobuf_AssignDesc_grSim_5fReplacement_2eproto();
365 |   friend void protobuf_ShutdownFile_grSim_5fReplacement_2eproto();
366 | 
367 |   void InitAsDefaultInstance();
368 |   static grSim_Replacement* default_instance_;
369 | };
370 | // ===================================================================
371 | 
372 | 
373 | // ===================================================================
374 | 
375 | // grSim_RobotReplacement
376 | 
377 | // required double x = 1;
378 | inline bool grSim_RobotReplacement::has_x() const {
379 |   return (_has_bits_[0] & 0x00000001u) != 0;
380 | }
381 | inline void grSim_RobotReplacement::set_has_x() {
382 |   _has_bits_[0] |= 0x00000001u;
383 | }
384 | inline void grSim_RobotReplacement::clear_has_x() {
385 |   _has_bits_[0] &= ~0x00000001u;
386 | }
387 | inline void grSim_RobotReplacement::clear_x() {
388 |   x_ = 0;
389 |   clear_has_x();
390 | }
391 | inline double grSim_RobotReplacement::x() const {
392 |   // @@protoc_insertion_point(field_get:grSim_RobotReplacement.x)
393 |   return x_;
394 | }
395 | inline void grSim_RobotReplacement::set_x(double value) {
396 |   set_has_x();
397 |   x_ = value;
398 |   // @@protoc_insertion_point(field_set:grSim_RobotReplacement.x)
399 | }
400 | 
401 | // required double y = 2;
402 | inline bool grSim_RobotReplacement::has_y() const {
403 |   return (_has_bits_[0] & 0x00000002u) != 0;
404 | }
405 | inline void grSim_RobotReplacement::set_has_y() {
406 |   _has_bits_[0] |= 0x00000002u;
407 | }
408 | inline void grSim_RobotReplacement::clear_has_y() {
409 |   _has_bits_[0] &= ~0x00000002u;
410 | }
411 | inline void grSim_RobotReplacement::clear_y() {
412 |   y_ = 0;
413 |   clear_has_y();
414 | }
415 | inline double grSim_RobotReplacement::y() const {
416 |   // @@protoc_insertion_point(field_get:grSim_RobotReplacement.y)
417 |   return y_;
418 | }
419 | inline void grSim_RobotReplacement::set_y(double value) {
420 |   set_has_y();
421 |   y_ = value;
422 |   // @@protoc_insertion_point(field_set:grSim_RobotReplacement.y)
423 | }
424 | 
425 | // required double dir = 3;
426 | inline bool grSim_RobotReplacement::has_dir() const {
427 |   return (_has_bits_[0] & 0x00000004u) != 0;
428 | }
429 | inline void grSim_RobotReplacement::set_has_dir() {
430 |   _has_bits_[0] |= 0x00000004u;
431 | }
432 | inline void grSim_RobotReplacement::clear_has_dir() {
433 |   _has_bits_[0] &= ~0x00000004u;
434 | }
435 | inline void grSim_RobotReplacement::clear_dir() {
436 |   dir_ = 0;
437 |   clear_has_dir();
438 | }
439 | inline double grSim_RobotReplacement::dir() const {
440 |   // @@protoc_insertion_point(field_get:grSim_RobotReplacement.dir)
441 |   return dir_;
442 | }
443 | inline void grSim_RobotReplacement::set_dir(double value) {
444 |   set_has_dir();
445 |   dir_ = value;
446 |   // @@protoc_insertion_point(field_set:grSim_RobotReplacement.dir)
447 | }
448 | 
449 | // required uint32 id = 4;
450 | inline bool grSim_RobotReplacement::has_id() const {
451 |   return (_has_bits_[0] & 0x00000008u) != 0;
452 | }
453 | inline void grSim_RobotReplacement::set_has_id() {
454 |   _has_bits_[0] |= 0x00000008u;
455 | }
456 | inline void grSim_RobotReplacement::clear_has_id() {
457 |   _has_bits_[0] &= ~0x00000008u;
458 | }
459 | inline void grSim_RobotReplacement::clear_id() {
460 |   id_ = 0u;
461 |   clear_has_id();
462 | }
463 | inline ::google::protobuf::uint32 grSim_RobotReplacement::id() const {
464 |   // @@protoc_insertion_point(field_get:grSim_RobotReplacement.id)
465 |   return id_;
466 | }
467 | inline void grSim_RobotReplacement::set_id(::google::protobuf::uint32 value) {
468 |   set_has_id();
469 |   id_ = value;
470 |   // @@protoc_insertion_point(field_set:grSim_RobotReplacement.id)
471 | }
472 | 
473 | // required bool yellowteam = 5;
474 | inline bool grSim_RobotReplacement::has_yellowteam() const {
475 |   return (_has_bits_[0] & 0x00000010u) != 0;
476 | }
477 | inline void grSim_RobotReplacement::set_has_yellowteam() {
478 |   _has_bits_[0] |= 0x00000010u;
479 | }
480 | inline void grSim_RobotReplacement::clear_has_yellowteam() {
481 |   _has_bits_[0] &= ~0x00000010u;
482 | }
483 | inline void grSim_RobotReplacement::clear_yellowteam() {
484 |   yellowteam_ = false;
485 |   clear_has_yellowteam();
486 | }
487 | inline bool grSim_RobotReplacement::yellowteam() const {
488 |   // @@protoc_insertion_point(field_get:grSim_RobotReplacement.yellowteam)
489 |   return yellowteam_;
490 | }
491 | inline void grSim_RobotReplacement::set_yellowteam(bool value) {
492 |   set_has_yellowteam();
493 |   yellowteam_ = value;
494 |   // @@protoc_insertion_point(field_set:grSim_RobotReplacement.yellowteam)
495 | }
496 | 
497 | // optional bool turnon = 6;
498 | inline bool grSim_RobotReplacement::has_turnon() const {
499 |   return (_has_bits_[0] & 0x00000020u) != 0;
500 | }
501 | inline void grSim_RobotReplacement::set_has_turnon() {
502 |   _has_bits_[0] |= 0x00000020u;
503 | }
504 | inline void grSim_RobotReplacement::clear_has_turnon() {
505 |   _has_bits_[0] &= ~0x00000020u;
506 | }
507 | inline void grSim_RobotReplacement::clear_turnon() {
508 |   turnon_ = false;
509 |   clear_has_turnon();
510 | }
511 | inline bool grSim_RobotReplacement::turnon() const {
512 |   // @@protoc_insertion_point(field_get:grSim_RobotReplacement.turnon)
513 |   return turnon_;
514 | }
515 | inline void grSim_RobotReplacement::set_turnon(bool value) {
516 |   set_has_turnon();
517 |   turnon_ = value;
518 |   // @@protoc_insertion_point(field_set:grSim_RobotReplacement.turnon)
519 | }
520 | 
521 | // -------------------------------------------------------------------
522 | 
523 | // grSim_BallReplacement
524 | 
525 | // required double x = 1;
526 | inline bool grSim_BallReplacement::has_x() const {
527 |   return (_has_bits_[0] & 0x00000001u) != 0;
528 | }
529 | inline void grSim_BallReplacement::set_has_x() {
530 |   _has_bits_[0] |= 0x00000001u;
531 | }
532 | inline void grSim_BallReplacement::clear_has_x() {
533 |   _has_bits_[0] &= ~0x00000001u;
534 | }
535 | inline void grSim_BallReplacement::clear_x() {
536 |   x_ = 0;
537 |   clear_has_x();
538 | }
539 | inline double grSim_BallReplacement::x() const {
540 |   // @@protoc_insertion_point(field_get:grSim_BallReplacement.x)
541 |   return x_;
542 | }
543 | inline void grSim_BallReplacement::set_x(double value) {
544 |   set_has_x();
545 |   x_ = value;
546 |   // @@protoc_insertion_point(field_set:grSim_BallReplacement.x)
547 | }
548 | 
549 | // required double y = 2;
550 | inline bool grSim_BallReplacement::has_y() const {
551 |   return (_has_bits_[0] & 0x00000002u) != 0;
552 | }
553 | inline void grSim_BallReplacement::set_has_y() {
554 |   _has_bits_[0] |= 0x00000002u;
555 | }
556 | inline void grSim_BallReplacement::clear_has_y() {
557 |   _has_bits_[0] &= ~0x00000002u;
558 | }
559 | inline void grSim_BallReplacement::clear_y() {
560 |   y_ = 0;
561 |   clear_has_y();
562 | }
563 | inline double grSim_BallReplacement::y() const {
564 |   // @@protoc_insertion_point(field_get:grSim_BallReplacement.y)
565 |   return y_;
566 | }
567 | inline void grSim_BallReplacement::set_y(double value) {
568 |   set_has_y();
569 |   y_ = value;
570 |   // @@protoc_insertion_point(field_set:grSim_BallReplacement.y)
571 | }
572 | 
573 | // required double vx = 3;
574 | inline bool grSim_BallReplacement::has_vx() const {
575 |   return (_has_bits_[0] & 0x00000004u) != 0;
576 | }
577 | inline void grSim_BallReplacement::set_has_vx() {
578 |   _has_bits_[0] |= 0x00000004u;
579 | }
580 | inline void grSim_BallReplacement::clear_has_vx() {
581 |   _has_bits_[0] &= ~0x00000004u;
582 | }
583 | inline void grSim_BallReplacement::clear_vx() {
584 |   vx_ = 0;
585 |   clear_has_vx();
586 | }
587 | inline double grSim_BallReplacement::vx() const {
588 |   // @@protoc_insertion_point(field_get:grSim_BallReplacement.vx)
589 |   return vx_;
590 | }
591 | inline void grSim_BallReplacement::set_vx(double value) {
592 |   set_has_vx();
593 |   vx_ = value;
594 |   // @@protoc_insertion_point(field_set:grSim_BallReplacement.vx)
595 | }
596 | 
597 | // required double vy = 4;
598 | inline bool grSim_BallReplacement::has_vy() const {
599 |   return (_has_bits_[0] & 0x00000008u) != 0;
600 | }
601 | inline void grSim_BallReplacement::set_has_vy() {
602 |   _has_bits_[0] |= 0x00000008u;
603 | }
604 | inline void grSim_BallReplacement::clear_has_vy() {
605 |   _has_bits_[0] &= ~0x00000008u;
606 | }
607 | inline void grSim_BallReplacement::clear_vy() {
608 |   vy_ = 0;
609 |   clear_has_vy();
610 | }
611 | inline double grSim_BallReplacement::vy() const {
612 |   // @@protoc_insertion_point(field_get:grSim_BallReplacement.vy)
613 |   return vy_;
614 | }
615 | inline void grSim_BallReplacement::set_vy(double value) {
616 |   set_has_vy();
617 |   vy_ = value;
618 |   // @@protoc_insertion_point(field_set:grSim_BallReplacement.vy)
619 | }
620 | 
621 | // -------------------------------------------------------------------
622 | 
623 | // grSim_Replacement
624 | 
625 | // optional .grSim_BallReplacement ball = 1;
626 | inline bool grSim_Replacement::has_ball() const {
627 |   return (_has_bits_[0] & 0x00000001u) != 0;
628 | }
629 | inline void grSim_Replacement::set_has_ball() {
630 |   _has_bits_[0] |= 0x00000001u;
631 | }
632 | inline void grSim_Replacement::clear_has_ball() {
633 |   _has_bits_[0] &= ~0x00000001u;
634 | }
635 | inline void grSim_Replacement::clear_ball() {
636 |   if (ball_ != NULL) ball_->::grSim_BallReplacement::Clear();
637 |   clear_has_ball();
638 | }
639 | inline const ::grSim_BallReplacement& grSim_Replacement::ball() const {
640 |   // @@protoc_insertion_point(field_get:grSim_Replacement.ball)
641 |   return ball_ != NULL ? *ball_ : *default_instance_->ball_;
642 | }
643 | inline ::grSim_BallReplacement* grSim_Replacement::mutable_ball() {
644 |   set_has_ball();
645 |   if (ball_ == NULL) ball_ = new ::grSim_BallReplacement;
646 |   // @@protoc_insertion_point(field_mutable:grSim_Replacement.ball)
647 |   return ball_;
648 | }
649 | inline ::grSim_BallReplacement* grSim_Replacement::release_ball() {
650 |   clear_has_ball();
651 |   ::grSim_BallReplacement* temp = ball_;
652 |   ball_ = NULL;
653 |   return temp;
654 | }
655 | inline void grSim_Replacement::set_allocated_ball(::grSim_BallReplacement* ball) {
656 |   delete ball_;
657 |   ball_ = ball;
658 |   if (ball) {
659 |     set_has_ball();
660 |   } else {
661 |     clear_has_ball();
662 |   }
663 |   // @@protoc_insertion_point(field_set_allocated:grSim_Replacement.ball)
664 | }
665 | 
666 | // repeated .grSim_RobotReplacement robots = 2;
667 | inline int grSim_Replacement::robots_size() const {
668 |   return robots_.size();
669 | }
670 | inline void grSim_Replacement::clear_robots() {
671 |   robots_.Clear();
672 | }
673 | inline const ::grSim_RobotReplacement& grSim_Replacement::robots(int index) const {
674 |   // @@protoc_insertion_point(field_get:grSim_Replacement.robots)
675 |   return robots_.Get(index);
676 | }
677 | inline ::grSim_RobotReplacement* grSim_Replacement::mutable_robots(int index) {
678 |   // @@protoc_insertion_point(field_mutable:grSim_Replacement.robots)
679 |   return robots_.Mutable(index);
680 | }
681 | inline ::grSim_RobotReplacement* grSim_Replacement::add_robots() {
682 |   // @@protoc_insertion_point(field_add:grSim_Replacement.robots)
683 |   return robots_.Add();
684 | }
685 | inline const ::google::protobuf::RepeatedPtrField< ::grSim_RobotReplacement >&
686 | grSim_Replacement::robots() const {
687 |   // @@protoc_insertion_point(field_list:grSim_Replacement.robots)
688 |   return robots_;
689 | }
690 | inline ::google::protobuf::RepeatedPtrField< ::grSim_RobotReplacement >*
691 | grSim_Replacement::mutable_robots() {
692 |   // @@protoc_insertion_point(field_mutable_list:grSim_Replacement.robots)
693 |   return &robots_;
694 | }
695 | 
696 | 
697 | // @@protoc_insertion_point(namespace_scope)
698 | 
699 | #ifndef SWIG
700 | namespace google {
701 | namespace protobuf {
702 | 
703 | 
704 | }  // namespace google
705 | }  // namespace protobuf
706 | #endif  // SWIG
707 | 
708 | // @@protoc_insertion_point(global_scope)
709 | 
710 | #endif  // PROTOBUF_grSim_5fReplacement_2eproto__INCLUDED
711 | 


--------------------------------------------------------------------------------
/proto/grSim_Commands.pb.h:
--------------------------------------------------------------------------------
  1 | // Generated by the protocol buffer compiler.  DO NOT EDIT!
  2 | // source: grSim_Commands.proto
  3 | 
  4 | #ifndef PROTOBUF_grSim_5fCommands_2eproto__INCLUDED
  5 | #define PROTOBUF_grSim_5fCommands_2eproto__INCLUDED
  6 | 
  7 | #include <string>
  8 | 
  9 | #include <google/protobuf/stubs/common.h>
 10 | 
 11 | #if GOOGLE_PROTOBUF_VERSION < 2006000
 12 | #error This file was generated by a newer version of protoc which is
 13 | #error incompatible with your Protocol Buffer headers.  Please update
 14 | #error your headers.
 15 | #endif
 16 | #if 2006001 < GOOGLE_PROTOBUF_MIN_PROTOC_VERSION
 17 | #error This file was generated by an older version of protoc which is
 18 | #error incompatible with your Protocol Buffer headers.  Please
 19 | #error regenerate this file with a newer version of protoc.
 20 | #endif
 21 | 
 22 | #include <google/protobuf/generated_message_util.h>
 23 | #include <google/protobuf/message.h>
 24 | #include <google/protobuf/repeated_field.h>
 25 | #include <google/protobuf/extension_set.h>
 26 | #include <google/protobuf/unknown_field_set.h>
 27 | // @@protoc_insertion_point(includes)
 28 | 
 29 | // Internal implementation detail -- do not call these.
 30 | void  protobuf_AddDesc_grSim_5fCommands_2eproto();
 31 | void protobuf_AssignDesc_grSim_5fCommands_2eproto();
 32 | void protobuf_ShutdownFile_grSim_5fCommands_2eproto();
 33 | 
 34 | class grSim_Robot_Command;
 35 | class grSim_Commands;
 36 | 
 37 | // ===================================================================
 38 | 
 39 | class grSim_Robot_Command : public ::google::protobuf::Message {
 40 |  public:
 41 |   grSim_Robot_Command();
 42 |   virtual ~grSim_Robot_Command();
 43 | 
 44 |   grSim_Robot_Command(const grSim_Robot_Command& from);
 45 | 
 46 |   inline grSim_Robot_Command& operator=(const grSim_Robot_Command& from) {
 47 |     CopyFrom(from);
 48 |     return *this;
 49 |   }
 50 | 
 51 |   inline const ::google::protobuf::UnknownFieldSet& unknown_fields() const {
 52 |     return _unknown_fields_;
 53 |   }
 54 | 
 55 |   inline ::google::protobuf::UnknownFieldSet* mutable_unknown_fields() {
 56 |     return &_unknown_fields_;
 57 |   }
 58 | 
 59 |   static const ::google::protobuf::Descriptor* descriptor();
 60 |   static const grSim_Robot_Command& default_instance();
 61 | 
 62 |   void Swap(grSim_Robot_Command* other);
 63 | 
 64 |   // implements Message ----------------------------------------------
 65 | 
 66 |   grSim_Robot_Command* New() const;
 67 |   void CopyFrom(const ::google::protobuf::Message& from);
 68 |   void MergeFrom(const ::google::protobuf::Message& from);
 69 |   void CopyFrom(const grSim_Robot_Command& from);
 70 |   void MergeFrom(const grSim_Robot_Command& from);
 71 |   void Clear();
 72 |   bool IsInitialized() const;
 73 | 
 74 |   int ByteSize() const;
 75 |   bool MergePartialFromCodedStream(
 76 |       ::google::protobuf::io::CodedInputStream* input);
 77 |   void SerializeWithCachedSizes(
 78 |       ::google::protobuf::io::CodedOutputStream* output) const;
 79 |   ::google::protobuf::uint8* SerializeWithCachedSizesToArray(::google::protobuf::uint8* output) const;
 80 |   int GetCachedSize() const { return _cached_size_; }
 81 |   private:
 82 |   void SharedCtor();
 83 |   void SharedDtor();
 84 |   void SetCachedSize(int size) const;
 85 |   public:
 86 |   ::google::protobuf::Metadata GetMetadata() const;
 87 | 
 88 |   // nested types ----------------------------------------------------
 89 | 
 90 |   // accessors -------------------------------------------------------
 91 | 
 92 |   // required uint32 id = 1;
 93 |   inline bool has_id() const;
 94 |   inline void clear_id();
 95 |   static const int kIdFieldNumber = 1;
 96 |   inline ::google::protobuf::uint32 id() const;
 97 |   inline void set_id(::google::protobuf::uint32 value);
 98 | 
 99 |   // required float kickspeedx = 2;
100 |   inline bool has_kickspeedx() const;
101 |   inline void clear_kickspeedx();
102 |   static const int kKickspeedxFieldNumber = 2;
103 |   inline float kickspeedx() const;
104 |   inline void set_kickspeedx(float value);
105 | 
106 |   // required float kickspeedz = 3;
107 |   inline bool has_kickspeedz() const;
108 |   inline void clear_kickspeedz();
109 |   static const int kKickspeedzFieldNumber = 3;
110 |   inline float kickspeedz() const;
111 |   inline void set_kickspeedz(float value);
112 | 
113 |   // required float veltangent = 4;
114 |   inline bool has_veltangent() const;
115 |   inline void clear_veltangent();
116 |   static const int kVeltangentFieldNumber = 4;
117 |   inline float veltangent() const;
118 |   inline void set_veltangent(float value);
119 | 
120 |   // required float velnormal = 5;
121 |   inline bool has_velnormal() const;
122 |   inline void clear_velnormal();
123 |   static const int kVelnormalFieldNumber = 5;
124 |   inline float velnormal() const;
125 |   inline void set_velnormal(float value);
126 | 
127 |   // required float velangular = 6;
128 |   inline bool has_velangular() const;
129 |   inline void clear_velangular();
130 |   static const int kVelangularFieldNumber = 6;
131 |   inline float velangular() const;
132 |   inline void set_velangular(float value);
133 | 
134 |   // required bool spinner = 7;
135 |   inline bool has_spinner() const;
136 |   inline void clear_spinner();
137 |   static const int kSpinnerFieldNumber = 7;
138 |   inline bool spinner() const;
139 |   inline void set_spinner(bool value);
140 | 
141 |   // required bool wheelsspeed = 8;
142 |   inline bool has_wheelsspeed() const;
143 |   inline void clear_wheelsspeed();
144 |   static const int kWheelsspeedFieldNumber = 8;
145 |   inline bool wheelsspeed() const;
146 |   inline void set_wheelsspeed(bool value);
147 | 
148 |   // optional float wheel1 = 9;
149 |   inline bool has_wheel1() const;
150 |   inline void clear_wheel1();
151 |   static const int kWheel1FieldNumber = 9;
152 |   inline float wheel1() const;
153 |   inline void set_wheel1(float value);
154 | 
155 |   // optional float wheel2 = 10;
156 |   inline bool has_wheel2() const;
157 |   inline void clear_wheel2();
158 |   static const int kWheel2FieldNumber = 10;
159 |   inline float wheel2() const;
160 |   inline void set_wheel2(float value);
161 | 
162 |   // optional float wheel3 = 11;
163 |   inline bool has_wheel3() const;
164 |   inline void clear_wheel3();
165 |   static const int kWheel3FieldNumber = 11;
166 |   inline float wheel3() const;
167 |   inline void set_wheel3(float value);
168 | 
169 |   // optional float wheel4 = 12;
170 |   inline bool has_wheel4() const;
171 |   inline void clear_wheel4();
172 |   static const int kWheel4FieldNumber = 12;
173 |   inline float wheel4() const;
174 |   inline void set_wheel4(float value);
175 | 
176 |   // @@protoc_insertion_point(class_scope:grSim_Robot_Command)
177 |  private:
178 |   inline void set_has_id();
179 |   inline void clear_has_id();
180 |   inline void set_has_kickspeedx();
181 |   inline void clear_has_kickspeedx();
182 |   inline void set_has_kickspeedz();
183 |   inline void clear_has_kickspeedz();
184 |   inline void set_has_veltangent();
185 |   inline void clear_has_veltangent();
186 |   inline void set_has_velnormal();
187 |   inline void clear_has_velnormal();
188 |   inline void set_has_velangular();
189 |   inline void clear_has_velangular();
190 |   inline void set_has_spinner();
191 |   inline void clear_has_spinner();
192 |   inline void set_has_wheelsspeed();
193 |   inline void clear_has_wheelsspeed();
194 |   inline void set_has_wheel1();
195 |   inline void clear_has_wheel1();
196 |   inline void set_has_wheel2();
197 |   inline void clear_has_wheel2();
198 |   inline void set_has_wheel3();
199 |   inline void clear_has_wheel3();
200 |   inline void set_has_wheel4();
201 |   inline void clear_has_wheel4();
202 | 
203 |   ::google::protobuf::UnknownFieldSet _unknown_fields_;
204 | 
205 |   ::google::protobuf::uint32 _has_bits_[1];
206 |   mutable int _cached_size_;
207 |   ::google::protobuf::uint32 id_;
208 |   float kickspeedx_;
209 |   float kickspeedz_;
210 |   float veltangent_;
211 |   float velnormal_;
212 |   float velangular_;
213 |   bool spinner_;
214 |   bool wheelsspeed_;
215 |   float wheel1_;
216 |   float wheel2_;
217 |   float wheel3_;
218 |   float wheel4_;
219 |   friend void  protobuf_AddDesc_grSim_5fCommands_2eproto();
220 |   friend void protobuf_AssignDesc_grSim_5fCommands_2eproto();
221 |   friend void protobuf_ShutdownFile_grSim_5fCommands_2eproto();
222 | 
223 |   void InitAsDefaultInstance();
224 |   static grSim_Robot_Command* default_instance_;
225 | };
226 | // -------------------------------------------------------------------
227 | 
228 | class grSim_Commands : public ::google::protobuf::Message {
229 |  public:
230 |   grSim_Commands();
231 |   virtual ~grSim_Commands();
232 | 
233 |   grSim_Commands(const grSim_Commands& from);
234 | 
235 |   inline grSim_Commands& operator=(const grSim_Commands& from) {
236 |     CopyFrom(from);
237 |     return *this;
238 |   }
239 | 
240 |   inline const ::google::protobuf::UnknownFieldSet& unknown_fields() const {
241 |     return _unknown_fields_;
242 |   }
243 | 
244 |   inline ::google::protobuf::UnknownFieldSet* mutable_unknown_fields() {
245 |     return &_unknown_fields_;
246 |   }
247 | 
248 |   static const ::google::protobuf::Descriptor* descriptor();
249 |   static const grSim_Commands& default_instance();
250 | 
251 |   void Swap(grSim_Commands* other);
252 | 
253 |   // implements Message ----------------------------------------------
254 | 
255 |   grSim_Commands* New() const;
256 |   void CopyFrom(const ::google::protobuf::Message& from);
257 |   void MergeFrom(const ::google::protobuf::Message& from);
258 |   void CopyFrom(const grSim_Commands& from);
259 |   void MergeFrom(const grSim_Commands& from);
260 |   void Clear();
261 |   bool IsInitialized() const;
262 | 
263 |   int ByteSize() const;
264 |   bool MergePartialFromCodedStream(
265 |       ::google::protobuf::io::CodedInputStream* input);
266 |   void SerializeWithCachedSizes(
267 |       ::google::protobuf::io::CodedOutputStream* output) const;
268 |   ::google::protobuf::uint8* SerializeWithCachedSizesToArray(::google::protobuf::uint8* output) const;
269 |   int GetCachedSize() const { return _cached_size_; }
270 |   private:
271 |   void SharedCtor();
272 |   void SharedDtor();
273 |   void SetCachedSize(int size) const;
274 |   public:
275 |   ::google::protobuf::Metadata GetMetadata() const;
276 | 
277 |   // nested types ----------------------------------------------------
278 | 
279 |   // accessors -------------------------------------------------------
280 | 
281 |   // required double timestamp = 1;
282 |   inline bool has_timestamp() const;
283 |   inline void clear_timestamp();
284 |   static const int kTimestampFieldNumber = 1;
285 |   inline double timestamp() const;
286 |   inline void set_timestamp(double value);
287 | 
288 |   // required bool isteamyellow = 2;
289 |   inline bool has_isteamyellow() const;
290 |   inline void clear_isteamyellow();
291 |   static const int kIsteamyellowFieldNumber = 2;
292 |   inline bool isteamyellow() const;
293 |   inline void set_isteamyellow(bool value);
294 | 
295 |   // repeated .grSim_Robot_Command robot_commands = 3;
296 |   inline int robot_commands_size() const;
297 |   inline void clear_robot_commands();
298 |   static const int kRobotCommandsFieldNumber = 3;
299 |   inline const ::grSim_Robot_Command& robot_commands(int index) const;
300 |   inline ::grSim_Robot_Command* mutable_robot_commands(int index);
301 |   inline ::grSim_Robot_Command* add_robot_commands();
302 |   inline const ::google::protobuf::RepeatedPtrField< ::grSim_Robot_Command >&
303 |       robot_commands() const;
304 |   inline ::google::protobuf::RepeatedPtrField< ::grSim_Robot_Command >*
305 |       mutable_robot_commands();
306 | 
307 |   // @@protoc_insertion_point(class_scope:grSim_Commands)
308 |  private:
309 |   inline void set_has_timestamp();
310 |   inline void clear_has_timestamp();
311 |   inline void set_has_isteamyellow();
312 |   inline void clear_has_isteamyellow();
313 | 
314 |   ::google::protobuf::UnknownFieldSet _unknown_fields_;
315 | 
316 |   ::google::protobuf::uint32 _has_bits_[1];
317 |   mutable int _cached_size_;
318 |   double timestamp_;
319 |   ::google::protobuf::RepeatedPtrField< ::grSim_Robot_Command > robot_commands_;
320 |   bool isteamyellow_;
321 |   friend void  protobuf_AddDesc_grSim_5fCommands_2eproto();
322 |   friend void protobuf_AssignDesc_grSim_5fCommands_2eproto();
323 |   friend void protobuf_ShutdownFile_grSim_5fCommands_2eproto();
324 | 
325 |   void InitAsDefaultInstance();
326 |   static grSim_Commands* default_instance_;
327 | };
328 | // ===================================================================
329 | 
330 | 
331 | // ===================================================================
332 | 
333 | // grSim_Robot_Command
334 | 
335 | // required uint32 id = 1;
336 | inline bool grSim_Robot_Command::has_id() const {
337 |   return (_has_bits_[0] & 0x00000001u) != 0;
338 | }
339 | inline void grSim_Robot_Command::set_has_id() {
340 |   _has_bits_[0] |= 0x00000001u;
341 | }
342 | inline void grSim_Robot_Command::clear_has_id() {
343 |   _has_bits_[0] &= ~0x00000001u;
344 | }
345 | inline void grSim_Robot_Command::clear_id() {
346 |   id_ = 0u;
347 |   clear_has_id();
348 | }
349 | inline ::google::protobuf::uint32 grSim_Robot_Command::id() const {
350 |   // @@protoc_insertion_point(field_get:grSim_Robot_Command.id)
351 |   return id_;
352 | }
353 | inline void grSim_Robot_Command::set_id(::google::protobuf::uint32 value) {
354 |   set_has_id();
355 |   id_ = value;
356 |   // @@protoc_insertion_point(field_set:grSim_Robot_Command.id)
357 | }
358 | 
359 | // required float kickspeedx = 2;
360 | inline bool grSim_Robot_Command::has_kickspeedx() const {
361 |   return (_has_bits_[0] & 0x00000002u) != 0;
362 | }
363 | inline void grSim_Robot_Command::set_has_kickspeedx() {
364 |   _has_bits_[0] |= 0x00000002u;
365 | }
366 | inline void grSim_Robot_Command::clear_has_kickspeedx() {
367 |   _has_bits_[0] &= ~0x00000002u;
368 | }
369 | inline void grSim_Robot_Command::clear_kickspeedx() {
370 |   kickspeedx_ = 0;
371 |   clear_has_kickspeedx();
372 | }
373 | inline float grSim_Robot_Command::kickspeedx() const {
374 |   // @@protoc_insertion_point(field_get:grSim_Robot_Command.kickspeedx)
375 |   return kickspeedx_;
376 | }
377 | inline void grSim_Robot_Command::set_kickspeedx(float value) {
378 |   set_has_kickspeedx();
379 |   kickspeedx_ = value;
380 |   // @@protoc_insertion_point(field_set:grSim_Robot_Command.kickspeedx)
381 | }
382 | 
383 | // required float kickspeedz = 3;
384 | inline bool grSim_Robot_Command::has_kickspeedz() const {
385 |   return (_has_bits_[0] & 0x00000004u) != 0;
386 | }
387 | inline void grSim_Robot_Command::set_has_kickspeedz() {
388 |   _has_bits_[0] |= 0x00000004u;
389 | }
390 | inline void grSim_Robot_Command::clear_has_kickspeedz() {
391 |   _has_bits_[0] &= ~0x00000004u;
392 | }
393 | inline void grSim_Robot_Command::clear_kickspeedz() {
394 |   kickspeedz_ = 0;
395 |   clear_has_kickspeedz();
396 | }
397 | inline float grSim_Robot_Command::kickspeedz() const {
398 |   // @@protoc_insertion_point(field_get:grSim_Robot_Command.kickspeedz)
399 |   return kickspeedz_;
400 | }
401 | inline void grSim_Robot_Command::set_kickspeedz(float value) {
402 |   set_has_kickspeedz();
403 |   kickspeedz_ = value;
404 |   // @@protoc_insertion_point(field_set:grSim_Robot_Command.kickspeedz)
405 | }
406 | 
407 | // required float veltangent = 4;
408 | inline bool grSim_Robot_Command::has_veltangent() const {
409 |   return (_has_bits_[0] & 0x00000008u) != 0;
410 | }
411 | inline void grSim_Robot_Command::set_has_veltangent() {
412 |   _has_bits_[0] |= 0x00000008u;
413 | }
414 | inline void grSim_Robot_Command::clear_has_veltangent() {
415 |   _has_bits_[0] &= ~0x00000008u;
416 | }
417 | inline void grSim_Robot_Command::clear_veltangent() {
418 |   veltangent_ = 0;
419 |   clear_has_veltangent();
420 | }
421 | inline float grSim_Robot_Command::veltangent() const {
422 |   // @@protoc_insertion_point(field_get:grSim_Robot_Command.veltangent)
423 |   return veltangent_;
424 | }
425 | inline void grSim_Robot_Command::set_veltangent(float value) {
426 |   set_has_veltangent();
427 |   veltangent_ = value;
428 |   // @@protoc_insertion_point(field_set:grSim_Robot_Command.veltangent)
429 | }
430 | 
431 | // required float velnormal = 5;
432 | inline bool grSim_Robot_Command::has_velnormal() const {
433 |   return (_has_bits_[0] & 0x00000010u) != 0;
434 | }
435 | inline void grSim_Robot_Command::set_has_velnormal() {
436 |   _has_bits_[0] |= 0x00000010u;
437 | }
438 | inline void grSim_Robot_Command::clear_has_velnormal() {
439 |   _has_bits_[0] &= ~0x00000010u;
440 | }
441 | inline void grSim_Robot_Command::clear_velnormal() {
442 |   velnormal_ = 0;
443 |   clear_has_velnormal();
444 | }
445 | inline float grSim_Robot_Command::velnormal() const {
446 |   // @@protoc_insertion_point(field_get:grSim_Robot_Command.velnormal)
447 |   return velnormal_;
448 | }
449 | inline void grSim_Robot_Command::set_velnormal(float value) {
450 |   set_has_velnormal();
451 |   velnormal_ = value;
452 |   // @@protoc_insertion_point(field_set:grSim_Robot_Command.velnormal)
453 | }
454 | 
455 | // required float velangular = 6;
456 | inline bool grSim_Robot_Command::has_velangular() const {
457 |   return (_has_bits_[0] & 0x00000020u) != 0;
458 | }
459 | inline void grSim_Robot_Command::set_has_velangular() {
460 |   _has_bits_[0] |= 0x00000020u;
461 | }
462 | inline void grSim_Robot_Command::clear_has_velangular() {
463 |   _has_bits_[0] &= ~0x00000020u;
464 | }
465 | inline void grSim_Robot_Command::clear_velangular() {
466 |   velangular_ = 0;
467 |   clear_has_velangular();
468 | }
469 | inline float grSim_Robot_Command::velangular() const {
470 |   // @@protoc_insertion_point(field_get:grSim_Robot_Command.velangular)
471 |   return velangular_;
472 | }
473 | inline void grSim_Robot_Command::set_velangular(float value) {
474 |   set_has_velangular();
475 |   velangular_ = value;
476 |   // @@protoc_insertion_point(field_set:grSim_Robot_Command.velangular)
477 | }
478 | 
479 | // required bool spinner = 7;
480 | inline bool grSim_Robot_Command::has_spinner() const {
481 |   return (_has_bits_[0] & 0x00000040u) != 0;
482 | }
483 | inline void grSim_Robot_Command::set_has_spinner() {
484 |   _has_bits_[0] |= 0x00000040u;
485 | }
486 | inline void grSim_Robot_Command::clear_has_spinner() {
487 |   _has_bits_[0] &= ~0x00000040u;
488 | }
489 | inline void grSim_Robot_Command::clear_spinner() {
490 |   spinner_ = false;
491 |   clear_has_spinner();
492 | }
493 | inline bool grSim_Robot_Command::spinner() const {
494 |   // @@protoc_insertion_point(field_get:grSim_Robot_Command.spinner)
495 |   return spinner_;
496 | }
497 | inline void grSim_Robot_Command::set_spinner(bool value) {
498 |   set_has_spinner();
499 |   spinner_ = value;
500 |   // @@protoc_insertion_point(field_set:grSim_Robot_Command.spinner)
501 | }
502 | 
503 | // required bool wheelsspeed = 8;
504 | inline bool grSim_Robot_Command::has_wheelsspeed() const {
505 |   return (_has_bits_[0] & 0x00000080u) != 0;
506 | }
507 | inline void grSim_Robot_Command::set_has_wheelsspeed() {
508 |   _has_bits_[0] |= 0x00000080u;
509 | }
510 | inline void grSim_Robot_Command::clear_has_wheelsspeed() {
511 |   _has_bits_[0] &= ~0x00000080u;
512 | }
513 | inline void grSim_Robot_Command::clear_wheelsspeed() {
514 |   wheelsspeed_ = false;
515 |   clear_has_wheelsspeed();
516 | }
517 | inline bool grSim_Robot_Command::wheelsspeed() const {
518 |   // @@protoc_insertion_point(field_get:grSim_Robot_Command.wheelsspeed)
519 |   return wheelsspeed_;
520 | }
521 | inline void grSim_Robot_Command::set_wheelsspeed(bool value) {
522 |   set_has_wheelsspeed();
523 |   wheelsspeed_ = value;
524 |   // @@protoc_insertion_point(field_set:grSim_Robot_Command.wheelsspeed)
525 | }
526 | 
527 | // optional float wheel1 = 9;
528 | inline bool grSim_Robot_Command::has_wheel1() const {
529 |   return (_has_bits_[0] & 0x00000100u) != 0;
530 | }
531 | inline void grSim_Robot_Command::set_has_wheel1() {
532 |   _has_bits_[0] |= 0x00000100u;
533 | }
534 | inline void grSim_Robot_Command::clear_has_wheel1() {
535 |   _has_bits_[0] &= ~0x00000100u;
536 | }
537 | inline void grSim_Robot_Command::clear_wheel1() {
538 |   wheel1_ = 0;
539 |   clear_has_wheel1();
540 | }
541 | inline float grSim_Robot_Command::wheel1() const {
542 |   // @@protoc_insertion_point(field_get:grSim_Robot_Command.wheel1)
543 |   return wheel1_;
544 | }
545 | inline void grSim_Robot_Command::set_wheel1(float value) {
546 |   set_has_wheel1();
547 |   wheel1_ = value;
548 |   // @@protoc_insertion_point(field_set:grSim_Robot_Command.wheel1)
549 | }
550 | 
551 | // optional float wheel2 = 10;
552 | inline bool grSim_Robot_Command::has_wheel2() const {
553 |   return (_has_bits_[0] & 0x00000200u) != 0;
554 | }
555 | inline void grSim_Robot_Command::set_has_wheel2() {
556 |   _has_bits_[0] |= 0x00000200u;
557 | }
558 | inline void grSim_Robot_Command::clear_has_wheel2() {
559 |   _has_bits_[0] &= ~0x00000200u;
560 | }
561 | inline void grSim_Robot_Command::clear_wheel2() {
562 |   wheel2_ = 0;
563 |   clear_has_wheel2();
564 | }
565 | inline float grSim_Robot_Command::wheel2() const {
566 |   // @@protoc_insertion_point(field_get:grSim_Robot_Command.wheel2)
567 |   return wheel2_;
568 | }
569 | inline void grSim_Robot_Command::set_wheel2(float value) {
570 |   set_has_wheel2();
571 |   wheel2_ = value;
572 |   // @@protoc_insertion_point(field_set:grSim_Robot_Command.wheel2)
573 | }
574 | 
575 | // optional float wheel3 = 11;
576 | inline bool grSim_Robot_Command::has_wheel3() const {
577 |   return (_has_bits_[0] & 0x00000400u) != 0;
578 | }
579 | inline void grSim_Robot_Command::set_has_wheel3() {
580 |   _has_bits_[0] |= 0x00000400u;
581 | }
582 | inline void grSim_Robot_Command::clear_has_wheel3() {
583 |   _has_bits_[0] &= ~0x00000400u;
584 | }
585 | inline void grSim_Robot_Command::clear_wheel3() {
586 |   wheel3_ = 0;
587 |   clear_has_wheel3();
588 | }
589 | inline float grSim_Robot_Command::wheel3() const {
590 |   // @@protoc_insertion_point(field_get:grSim_Robot_Command.wheel3)
591 |   return wheel3_;
592 | }
593 | inline void grSim_Robot_Command::set_wheel3(float value) {
594 |   set_has_wheel3();
595 |   wheel3_ = value;
596 |   // @@protoc_insertion_point(field_set:grSim_Robot_Command.wheel3)
597 | }
598 | 
599 | // optional float wheel4 = 12;
600 | inline bool grSim_Robot_Command::has_wheel4() const {
601 |   return (_has_bits_[0] & 0x00000800u) != 0;
602 | }
603 | inline void grSim_Robot_Command::set_has_wheel4() {
604 |   _has_bits_[0] |= 0x00000800u;
605 | }
606 | inline void grSim_Robot_Command::clear_has_wheel4() {
607 |   _has_bits_[0] &= ~0x00000800u;
608 | }
609 | inline void grSim_Robot_Command::clear_wheel4() {
610 |   wheel4_ = 0;
611 |   clear_has_wheel4();
612 | }
613 | inline float grSim_Robot_Command::wheel4() const {
614 |   // @@protoc_insertion_point(field_get:grSim_Robot_Command.wheel4)
615 |   return wheel4_;
616 | }
617 | inline void grSim_Robot_Command::set_wheel4(float value) {
618 |   set_has_wheel4();
619 |   wheel4_ = value;
620 |   // @@protoc_insertion_point(field_set:grSim_Robot_Command.wheel4)
621 | }
622 | 
623 | // -------------------------------------------------------------------
624 | 
625 | // grSim_Commands
626 | 
627 | // required double timestamp = 1;
628 | inline bool grSim_Commands::has_timestamp() const {
629 |   return (_has_bits_[0] & 0x00000001u) != 0;
630 | }
631 | inline void grSim_Commands::set_has_timestamp() {
632 |   _has_bits_[0] |= 0x00000001u;
633 | }
634 | inline void grSim_Commands::clear_has_timestamp() {
635 |   _has_bits_[0] &= ~0x00000001u;
636 | }
637 | inline void grSim_Commands::clear_timestamp() {
638 |   timestamp_ = 0;
639 |   clear_has_timestamp();
640 | }
641 | inline double grSim_Commands::timestamp() const {
642 |   // @@protoc_insertion_point(field_get:grSim_Commands.timestamp)
643 |   return timestamp_;
644 | }
645 | inline void grSim_Commands::set_timestamp(double value) {
646 |   set_has_timestamp();
647 |   timestamp_ = value;
648 |   // @@protoc_insertion_point(field_set:grSim_Commands.timestamp)
649 | }
650 | 
651 | // required bool isteamyellow = 2;
652 | inline bool grSim_Commands::has_isteamyellow() const {
653 |   return (_has_bits_[0] & 0x00000002u) != 0;
654 | }
655 | inline void grSim_Commands::set_has_isteamyellow() {
656 |   _has_bits_[0] |= 0x00000002u;
657 | }
658 | inline void grSim_Commands::clear_has_isteamyellow() {
659 |   _has_bits_[0] &= ~0x00000002u;
660 | }
661 | inline void grSim_Commands::clear_isteamyellow() {
662 |   isteamyellow_ = false;
663 |   clear_has_isteamyellow();
664 | }
665 | inline bool grSim_Commands::isteamyellow() const {
666 |   // @@protoc_insertion_point(field_get:grSim_Commands.isteamyellow)
667 |   return isteamyellow_;
668 | }
669 | inline void grSim_Commands::set_isteamyellow(bool value) {
670 |   set_has_isteamyellow();
671 |   isteamyellow_ = value;
672 |   // @@protoc_insertion_point(field_set:grSim_Commands.isteamyellow)
673 | }
674 | 
675 | // repeated .grSim_Robot_Command robot_commands = 3;
676 | inline int grSim_Commands::robot_commands_size() const {
677 |   return robot_commands_.size();
678 | }
679 | inline void grSim_Commands::clear_robot_commands() {
680 |   robot_commands_.Clear();
681 | }
682 | inline const ::grSim_Robot_Command& grSim_Commands::robot_commands(int index) const {
683 |   // @@protoc_insertion_point(field_get:grSim_Commands.robot_commands)
684 |   return robot_commands_.Get(index);
685 | }
686 | inline ::grSim_Robot_Command* grSim_Commands::mutable_robot_commands(int index) {
687 |   // @@protoc_insertion_point(field_mutable:grSim_Commands.robot_commands)
688 |   return robot_commands_.Mutable(index);
689 | }
690 | inline ::grSim_Robot_Command* grSim_Commands::add_robot_commands() {
691 |   // @@protoc_insertion_point(field_add:grSim_Commands.robot_commands)
692 |   return robot_commands_.Add();
693 | }
694 | inline const ::google::protobuf::RepeatedPtrField< ::grSim_Robot_Command >&
695 | grSim_Commands::robot_commands() const {
696 |   // @@protoc_insertion_point(field_list:grSim_Commands.robot_commands)
697 |   return robot_commands_;
698 | }
699 | inline ::google::protobuf::RepeatedPtrField< ::grSim_Robot_Command >*
700 | grSim_Commands::mutable_robot_commands() {
701 |   // @@protoc_insertion_point(field_mutable_list:grSim_Commands.robot_commands)
702 |   return &robot_commands_;
703 | }
704 | 
705 | 
706 | // @@protoc_insertion_point(namespace_scope)
707 | 
708 | #ifndef SWIG
709 | namespace google {
710 | namespace protobuf {
711 | 
712 | 
713 | }  // namespace google
714 | }  // namespace protobuf
715 | #endif  // SWIG
716 | 
717 | // @@protoc_insertion_point(global_scope)
718 | 
719 | #endif  // PROTOBUF_grSim_5fCommands_2eproto__INCLUDED
720 | 


--------------------------------------------------------------------------------
/utils/visualizationmodule.cpp:
--------------------------------------------------------------------------------
  1 | #include "visualizationmodule.h"
  2 | #include "proto/zss_debug.pb.h"
  3 | #include "proto/grSim_Packet.pb.h"
  4 | #include "utils/params.h"
  5 | #include "utils/mymath.h"
  6 | #include "utils/datamanager.h"
  7 | #include "algorithm/rrt.h"
  8 | 
  9 | 
 10 | visualizationmodule::visualizationmodule(QObject *parent) : QObject(parent)
 11 | {
 12 |     sender = new QUdpSocket();
 13 | }
 14 | 
 15 | visualizationmodule::~visualizationmodule()
 16 | {
 17 |     sender->abort();
 18 | }
 19 | 
 20 | void visualizationmodule::drawPoint(MyPoint point)
 21 | {
 22 |     ZSS::Protocol::Debug_Msgs msgs;
 23 |     ZSS::Protocol::Debug_Msg* msg;
 24 |     ZSS::Protocol::Debug_Line* line;
 25 |     msg = msgs.add_msgs();
 26 |     msg->set_type(ZSS::Protocol::Debug_Msg_Debug_Type_LINE);
 27 |     msg->set_color(ZSS::Protocol::Debug_Msg_Color_RED);
 28 |     line = msg->mutable_line();
 29 |     line->mutable_start()->set_x(point.x()-5);
 30 |     line->mutable_start()->set_y(point.y()-5);
 31 |     line->mutable_end()->set_x(point.x()+5);
 32 |     line->mutable_end()->set_y(point.y()+5);
 33 |     line->set_forward(false);
 34 |     line->set_back(false);
 35 |     msg = msgs.add_msgs();
 36 |     msg->set_type(ZSS::Protocol::Debug_Msg_Debug_Type_LINE);
 37 |     msg->set_color(ZSS::Protocol::Debug_Msg_Color_RED);
 38 |     line = msg->mutable_line();
 39 |     line->mutable_start()->set_x(point.x()-5);
 40 |     line->mutable_start()->set_y(point.y()+5);
 41 |     line->mutable_end()->set_x(point.x()+5);
 42 |     line->mutable_end()->set_y(point.y()-5);
 43 |     line->set_forward(false);
 44 |     line->set_back(false);
 45 | 
 46 |     int msgs_size = msgs.ByteSize();
 47 |     QByteArray msgs_data(msgs_size, 0);
 48 |     msgs.SerializeToArray(msgs_data.data(), msgs_data.size());
 49 |     sender->writeDatagram(msgs_data, msgs_size, PARAMS::visualAddress, PARAMS::visualPort);
 50 | }
 51 | 
 52 | void visualizationmodule::drawPoints(std::vector<MyPoint> &somePoints)
 53 | {
 54 |     ZSS::Protocol::Debug_Msgs msgs;
 55 |     ZSS::Protocol::Debug_Msg* msg;
 56 |     ZSS::Protocol::Debug_Line* line;
 57 |     for(unsigned i = 0; i < somePoints.size(); i++)
 58 |     {
 59 |         msg = msgs.add_msgs();
 60 |         msg->set_type(ZSS::Protocol::Debug_Msg_Debug_Type_LINE);
 61 |         msg->set_color(ZSS::Protocol::Debug_Msg_Color_RED);
 62 |         line = msg->mutable_line();
 63 |         line->mutable_start()->set_x(somePoints[i].x()-5);
 64 |         line->mutable_start()->set_y(somePoints[i].y()-5);
 65 |         line->mutable_end()->set_x(somePoints[i].x()+5);
 66 |         line->mutable_end()->set_y(somePoints[i].y()+5);
 67 |         line->set_forward(false);
 68 |         line->set_back(false);
 69 |         msg = msgs.add_msgs();
 70 |         msg->set_type(ZSS::Protocol::Debug_Msg_Debug_Type_LINE);
 71 |         msg->set_color(ZSS::Protocol::Debug_Msg_Color_RED);
 72 |         line = msg->mutable_line();
 73 |         line->mutable_start()->set_x(somePoints[i].x()-5);
 74 |         line->mutable_start()->set_y(somePoints[i].y()+5);
 75 |         line->mutable_end()->set_x(somePoints[i].x()+5);
 76 |         line->mutable_end()->set_y(somePoints[i].y()-5);
 77 |         line->set_forward(false);
 78 |         line->set_back(false);
 79 |     }
 80 | 
 81 |     int msgs_size = msgs.ByteSize();
 82 |     QByteArray msgs_data(msgs_size, 0);
 83 |     msgs.SerializeToArray(msgs_data.data(), msgs_data.size());
 84 |     sender->writeDatagram(msgs_data, msgs_size, PARAMS::visualAddress, PARAMS::visualPort);
 85 | }
 86 | 
 87 | void visualizationmodule::drawLines(std::vector<MyPoint> &somePoints)
 88 | {
 89 |     ZSS::Protocol::Debug_Msgs msgs;
 90 |     ZSS::Protocol::Debug_Msg* msg;
 91 |     ZSS::Protocol::Debug_Line* line;
 92 |     for(unsigned i = 1; i < somePoints.size(); i++)
 93 |     {
 94 |         msg = msgs.add_msgs();
 95 |         msg->set_type(ZSS::Protocol::Debug_Msg_Debug_Type_LINE);
 96 |         msg->set_color(ZSS::Protocol::Debug_Msg_Color_RED);
 97 |         line = msg->mutable_line();
 98 |         line->mutable_start()->set_x(somePoints[i-1].x());
 99 |         line->mutable_start()->set_y(somePoints[i-1].y());
100 |         line->mutable_end()->set_x(somePoints[i].x());
101 |         line->mutable_end()->set_y(somePoints[i].y());
102 |         line->set_forward(false);
103 |         line->set_back(false);
104 |     }
105 | 
106 |     int msgs_size = msgs.ByteSize();
107 |     QByteArray msgs_data(msgs_size, 0);
108 |     msgs.SerializeToArray(msgs_data.data(), msgs_data.size());
109 |     sender->writeDatagram(msgs_data, msgs_size, PARAMS::visualAddress, PARAMS::visualPort);
110 | //    sender->writeDatagram(msgs_data, msgs_size,  QHostAddress("127.0.0.1"), 20001);
111 | }
112 | 
113 | void visualizationmodule::drawTree(std::vector<Node> &someNodes)
114 | {
115 |     ZSS::Protocol::Debug_Msgs msgs;
116 |     ZSS::Protocol::Debug_Msg* msg;
117 |     ZSS::Protocol::Debug_Line* line;
118 |     for(unsigned i = 0; i < someNodes.size(); i++)
119 |     {
120 |         //画出点来
121 |         msg = msgs.add_msgs();
122 |         msg->set_type(ZSS::Protocol::Debug_Msg_Debug_Type_LINE);
123 |         msg->set_color(ZSS::Protocol::Debug_Msg_Color_RED);
124 |         line = msg->mutable_line();
125 |         line->mutable_start()->set_x(someNodes[i].x-5);
126 |         line->mutable_start()->set_y(someNodes[i].y-5);
127 |         line->mutable_end()->set_x(someNodes[i].x+5);
128 |         line->mutable_end()->set_y(someNodes[i].y+5);
129 |         line->set_forward(false);
130 |         line->set_back(false);
131 |         msg = msgs.add_msgs();
132 |         msg->set_type(ZSS::Protocol::Debug_Msg_Debug_Type_LINE);
133 |         msg->set_color(ZSS::Protocol::Debug_Msg_Color_RED);
134 |         line = msg->mutable_line();
135 |         line->mutable_start()->set_x(someNodes[i].x+5);
136 |         line->mutable_start()->set_y(someNodes[i].y-5);
137 |         line->mutable_end()->set_x(someNodes[i].x-5);
138 |         line->mutable_end()->set_y(someNodes[i].y+5);
139 |         line->set_forward(false);
140 |         line->set_back(false);
141 |         //画线
142 |         if(someNodes[i].parent == -1) continue;
143 |         msg = msgs.add_msgs();
144 |         msg->set_type(ZSS::Protocol::Debug_Msg_Debug_Type_LINE);
145 |         msg->set_color(ZSS::Protocol::Debug_Msg_Color_BLUE);
146 |         line = msg->mutable_line();
147 |         line->mutable_start()->set_x(someNodes[i].x);
148 |         line->mutable_start()->set_y(someNodes[i].y);
149 |         line->mutable_end()->set_x(someNodes[someNodes[i].parent].x);
150 |         line->mutable_end()->set_y(someNodes[someNodes[i].parent].y);
151 |         line->set_forward(false);
152 |         line->set_back(false);
153 |     }
154 |     //画一个特别的终点
155 |     for(int j = 0; j < 15; j++)
156 |     {
157 |         int i = someNodes.size()-1;
158 |         msg = msgs.add_msgs();
159 |         msg->set_type(ZSS::Protocol::Debug_Msg_Debug_Type_LINE);
160 |         msg->set_color(ZSS::Protocol::Debug_Msg_Color_RED);
161 |         line = msg->mutable_line();
162 |         line->mutable_start()->set_x(someNodes[i].x-j);
163 |         line->mutable_start()->set_y(someNodes[i].y-15+j);
164 |         line->mutable_end()->set_x(someNodes[i].x+j);
165 |         line->mutable_end()->set_y(someNodes[i].y+15-j);
166 |         line->set_forward(false);
167 |         line->set_back(false);
168 |         msg = msgs.add_msgs();
169 |         msg->set_type(ZSS::Protocol::Debug_Msg_Debug_Type_LINE);
170 |         msg->set_color(ZSS::Protocol::Debug_Msg_Color_RED);
171 |         line = msg->mutable_line();
172 |         line->mutable_start()->set_x(someNodes[i].x+15-j);
173 |         line->mutable_start()->set_y(someNodes[i].y-j);
174 |         line->mutable_end()->set_x(someNodes[i].x-15+j);
175 |         line->mutable_end()->set_y(someNodes[i].y+j);
176 |         line->set_forward(false);
177 |         line->set_back(false);
178 |     }
179 |     //发送信息
180 |     int msgs_size = msgs.ByteSize();
181 |     QByteArray msgs_data(msgs_size, 0);
182 |     msgs.SerializeToArray(msgs_data.data(), msgs_data.size());
183 |     sender->writeDatagram(msgs_data, msgs_size, PARAMS::visualAddress, PARAMS::visualPort);
184 | //    sender->writeDatagram(msgs_data, msgs_size,  QHostAddress("127.0.0.1"), 20001);
185 | }
186 | 
187 | void visualizationmodule::drawAll(std::deque<MyPoint> goals)
188 | {
189 |     ZSS::Protocol::Debug_Msgs msgs;
190 |     ZSS::Protocol::Debug_Msg* msg;
191 |     ZSS::Protocol::Debug_Line* line;
192 |     std::vector<Node> someNodes = RRTPlanner::instance()->NodeList;
193 | 
194 |     for(unsigned i = 0; i < someNodes.size(); i++)
195 |     {
196 |         //画出点来
197 |         msg = msgs.add_msgs();
198 |         msg->set_type(ZSS::Protocol::Debug_Msg_Debug_Type_LINE);
199 |         msg->set_color(ZSS::Protocol::Debug_Msg_Color_RED);
200 |         line = msg->mutable_line();
201 |         line->mutable_start()->set_x(someNodes[i].x-5);
202 |         line->mutable_start()->set_y(someNodes[i].y-5);
203 |         line->mutable_end()->set_x(someNodes[i].x+5);
204 |         line->mutable_end()->set_y(someNodes[i].y+5);
205 |         line->set_forward(false);
206 |         line->set_back(false);
207 |         msg = msgs.add_msgs();
208 |         msg->set_type(ZSS::Protocol::Debug_Msg_Debug_Type_LINE);
209 |         msg->set_color(ZSS::Protocol::Debug_Msg_Color_RED);
210 |         line = msg->mutable_line();
211 |         line->mutable_start()->set_x(someNodes[i].x+5);
212 |         line->mutable_start()->set_y(someNodes[i].y-5);
213 |         line->mutable_end()->set_x(someNodes[i].x-5);
214 |         line->mutable_end()->set_y(someNodes[i].y+5);
215 |         line->set_forward(false);
216 |         line->set_back(false);
217 |         //画线
218 |         if(someNodes[i].parent == -1) continue;
219 |         msg = msgs.add_msgs();
220 |         msg->set_type(ZSS::Protocol::Debug_Msg_Debug_Type_LINE);
221 |         msg->set_color(ZSS::Protocol::Debug_Msg_Color_BLUE);
222 |         line = msg->mutable_line();
223 |         line->mutable_start()->set_x(someNodes[i].x);
224 |         line->mutable_start()->set_y(someNodes[i].y);
225 |         line->mutable_end()->set_x(someNodes[someNodes[i].parent].x);
226 |         line->mutable_end()->set_y(someNodes[someNodes[i].parent].y);
227 |         line->set_forward(false);
228 |         line->set_back(false);
229 |     }
230 |     //画几个特别的终点
231 |     for(unsigned i = 0; i < goals.size(); i++)
232 |     {
233 |         for(int j = 0; j < 15; j++)
234 |         {
235 |             msg = msgs.add_msgs();
236 |             msg->set_type(ZSS::Protocol::Debug_Msg_Debug_Type_LINE);
237 |             msg->set_color(ZSS::Protocol::Debug_Msg_Color_RED);
238 |             line = msg->mutable_line();
239 |             line->mutable_start()->set_x(goals[i].x()-j);
240 |             line->mutable_start()->set_y(goals[i].y()-15+j);
241 |             line->mutable_end()->set_x(goals[i].x()+j);
242 |             line->mutable_end()->set_y(goals[i].y()+15-j);
243 |             line->set_forward(false);
244 |             line->set_back(false);
245 |             msg = msgs.add_msgs();
246 |             msg->set_type(ZSS::Protocol::Debug_Msg_Debug_Type_LINE);
247 |             msg->set_color(ZSS::Protocol::Debug_Msg_Color_RED);
248 |             line = msg->mutable_line();
249 |             line->mutable_start()->set_x(goals[i].x()+15-j);
250 |             line->mutable_start()->set_y(goals[i].y()-j);
251 |             line->mutable_end()->set_x(goals[i].x()-15+j);
252 |             line->mutable_end()->set_y(goals[i].y()+j);
253 |             line->set_forward(false);
254 |             line->set_back(false);
255 |             msg = msgs.add_msgs();
256 |         }
257 |     }
258 |     //画平滑的线
259 |     std::vector<MyPoint> somePoints = RRTPlanner::instance()->smoothPath;
260 |     for(unsigned i = 1; i < somePoints.size(); i++)
261 |     {
262 |         msg = msgs.add_msgs();
263 |         msg->set_type(ZSS::Protocol::Debug_Msg_Debug_Type_LINE);
264 |         msg->set_color(ZSS::Protocol::Debug_Msg_Color_RED);
265 |         line = msg->mutable_line();
266 |         line->mutable_start()->set_x(somePoints[i-1].x());
267 |         line->mutable_start()->set_y(somePoints[i-1].y());
268 |         line->mutable_end()->set_x(somePoints[i].x());
269 |         line->mutable_end()->set_y(somePoints[i].y());
270 |         line->set_forward(false);
271 |         line->set_back(false);
272 |     }
273 | 
274 |     //画出膨胀之后的障碍物来，这里只写了capsule，如果需要circle再说吧
275 | 
276 |     int x[8], y[8], maxdistance, mydis, first, second, third, forth;
277 |     double globalvx, globalvy, theta, endx, endy;
278 |     for(int i=0; i<PARAMS::ROBOT_NUM; i++){
279 |         //为了debug
280 |     //for(int i=7; i<8; i++){
281 |         // blue robot
282 |         //for(int k = 0; k < 8; k++) x[k] = y[k] = 0;
283 |         if(MyDataManager::instance()->validBlueRobots[i] && !((PARAMS::IS_SIMULATION ? i==(PARAMS::our_id-1) : i==(PARAMS::our_id-1)) && PARAMS::isBlue)){
284 |             RobotInfo& blue = MyDataManager::instance()->blueRobots[i];
285 |             //尤其注意，这里是路子豪写的，是按照正常情况下的X-Y坐标系来做的，对于仿真和实际的环境有点痴迷，详情请询问路子豪
286 |             //globalvx = blue.vel_x * cos(blue.orientation) + blue.vel_y * sin(blue.orientation);
287 |             //globalvy = blue.vel_x * sin(blue.orientation) - blue.vel_y * cos(blue.orientation);
288 |             //使用之前场地计算的模块
289 |             //globalvx = blue.vel_x * cos(blue.orientation) - blue.vel_y * sin(blue.orientation);
290 |             //globalvy = blue.vel_x * sin(blue.orientation) + blue.vel_y * cos(blue.orientation);
291 | 
292 |             globalvx = blue.vel_x;
293 |             globalvy = -blue.vel_y;
294 |             //theta = 3.14159*(-1.0/3);
295 |             theta = atan2(-blue.vel_y, blue.vel_x);
296 |             endx = blue.x + PARAMS::OBSTACLE::REFRESH_TIME *globalvx;//*30* cos(theta);//;
297 |             endy = blue.y + PARAMS::OBSTACLE::REFRESH_TIME *globalvy;//*30* sin(theta);//
298 | 
299 |             x[0] = blue.x - sqrt(2)*PARAMS::OBSTACLE::INFLATION_RADIUS*cos(PARAMS::MATH::PI/4-theta);
300 |             y[0] = blue.y + sqrt(2)*PARAMS::OBSTACLE::INFLATION_RADIUS*sin(PARAMS::MATH::PI/4-theta);
301 | 
302 |             x[1] = blue.x - sqrt(2)*PARAMS::OBSTACLE::INFLATION_RADIUS*cos(PARAMS::MATH::PI/4+theta);
303 |             y[1] = blue.y - sqrt(2)*PARAMS::OBSTACLE::INFLATION_RADIUS*sin(PARAMS::MATH::PI/4+theta);
304 | 
305 |             x[2] = blue.x + sqrt(2)*PARAMS::OBSTACLE::INFLATION_RADIUS*cos(PARAMS::MATH::PI/4+theta);
306 |             y[2] = blue.y + sqrt(2)*PARAMS::OBSTACLE::INFLATION_RADIUS*sin(PARAMS::MATH::PI/4+theta);
307 | 
308 |             x[3] = blue.x + sqrt(2)*PARAMS::OBSTACLE::INFLATION_RADIUS*cos(PARAMS::MATH::PI/4-theta);
309 |             y[3] = blue.y - sqrt(2)*PARAMS::OBSTACLE::INFLATION_RADIUS*sin(PARAMS::MATH::PI/4-theta);
310 | 
311 |             //qDebug() << "blue.id" << blue.robot_id << "V^2" << (pow(blue.vel_x,2)+pow(blue.vel_y,2));
312 |             if(pow(blue.vel_x,2)+pow(blue.vel_y,2) <= 1000)
313 |             {
314 |                 first = 0;
315 |                 second = 3;
316 |                 third = 1;
317 |                 forth = 2;
318 |             }
319 |             else
320 |             {
321 |                 x[4] = endx - sqrt(2)*PARAMS::OBSTACLE::INFLATION_RADIUS*cos(PARAMS::MATH::PI/4-theta);
322 |                 y[4] = endy + sqrt(2)*PARAMS::OBSTACLE::INFLATION_RADIUS*sin(PARAMS::MATH::PI/4-theta);
323 | 
324 |                 x[5] = endx - sqrt(2)*PARAMS::OBSTACLE::INFLATION_RADIUS*cos(PARAMS::MATH::PI/4+theta);
325 |                 y[5] = endy - sqrt(2)*PARAMS::OBSTACLE::INFLATION_RADIUS*sin(PARAMS::MATH::PI/4+theta);
326 | 
327 |                 x[6] = endx + sqrt(2)*PARAMS::OBSTACLE::INFLATION_RADIUS*cos(PARAMS::MATH::PI/4+theta);
328 |                 y[6] = endy + sqrt(2)*PARAMS::OBSTACLE::INFLATION_RADIUS*sin(PARAMS::MATH::PI/4+theta);
329 | 
330 |                 x[7] = endx + sqrt(2)*PARAMS::OBSTACLE::INFLATION_RADIUS*cos(PARAMS::MATH::PI/4-theta);
331 |                 y[7] = endy - sqrt(2)*PARAMS::OBSTACLE::INFLATION_RADIUS*sin(PARAMS::MATH::PI/4-theta);
332 | 
333 |                 //找到最边上的四个点
334 |                 maxdistance = -1;
335 |                 for(int i = 0; i < 8; i++)
336 |                 {
337 |                     for(int j = i+1; j < 8; j++)
338 |                     {
339 |                         mydis = pow(x[j]-x[i],2)+pow(y[j]-y[i],2);
340 |     //                    qDebug() << "i" << i << "j" << j << "mydis" << mydis << "maxdis" << maxdistance;
341 |                         if(mydis > maxdistance)
342 |                         {
343 |                             first = i;
344 |                             second = j;
345 |                             maxdistance = mydis;
346 | 
347 |                         }
348 |                     }
349 |                 }
350 |                 maxdistance = -1;
351 |                 for(int i = 0; i < 8; i++)
352 |                 {
353 |                     for(int j = i+1; j < 8; j++)
354 |                     {
355 |                         if(i == first && j == second) continue;
356 |                         mydis = pow(x[j]-x[i],2)+pow(y[j]-y[i],2);
357 |                         if(mydis > maxdistance)
358 |                         {
359 |                             third = i;
360 |                             forth = j;
361 |                             maxdistance = mydis;
362 |                         }
363 |                     }
364 |                 }
365 |             }
366 | 
367 | 
368 |             //绘制直线
369 |             msg = msgs.add_msgs();
370 |             msg->set_type(ZSS::Protocol::Debug_Msg_Debug_Type_LINE);
371 |             msg->set_color(ZSS::Protocol::Debug_Msg_Color_WHITE);
372 |             line = msg->mutable_line();
373 |             line->mutable_start()->set_x(x[first]);
374 |             line->mutable_start()->set_y(y[first]);
375 |             line->mutable_end()->set_x(x[third]);
376 |             line->mutable_end()->set_y(y[third]);
377 |             line->set_forward(false);
378 |             line->set_back(false);
379 | 
380 |             msg = msgs.add_msgs();
381 |             msg->set_type(ZSS::Protocol::Debug_Msg_Debug_Type_LINE);
382 |             msg->set_color(ZSS::Protocol::Debug_Msg_Color_WHITE);
383 |             line = msg->mutable_line();
384 |             line->mutable_start()->set_x(x[first]);
385 |             line->mutable_start()->set_y(y[first]);
386 |             line->mutable_end()->set_x(x[forth]);
387 |             line->mutable_end()->set_y(y[forth]);
388 |             line->set_forward(false);
389 |             line->set_back(false);
390 | 
391 |             msg = msgs.add_msgs();
392 |             msg->set_type(ZSS::Protocol::Debug_Msg_Debug_Type_LINE);
393 |             msg->set_color(ZSS::Protocol::Debug_Msg_Color_WHITE);
394 |             line = msg->mutable_line();
395 |             line->mutable_start()->set_x(x[second]);
396 |             line->mutable_start()->set_y(y[second]);
397 |             line->mutable_end()->set_x(x[third]);
398 |             line->mutable_end()->set_y(y[third]);
399 |             line->set_forward(false);
400 |             line->set_back(false);
401 | 
402 |             msg = msgs.add_msgs();
403 |             msg->set_type(ZSS::Protocol::Debug_Msg_Debug_Type_LINE);
404 |             msg->set_color(ZSS::Protocol::Debug_Msg_Color_WHITE);
405 |             line = msg->mutable_line();
406 |             line->mutable_start()->set_x(x[second]);
407 |             line->mutable_start()->set_y(y[second]);
408 |             line->mutable_end()->set_x(x[forth]);
409 |             line->mutable_end()->set_y(y[forth]);
410 |             line->set_forward(false);
411 |             line->set_back(false);
412 |         }
413 |         // yellow robot
414 |         if(MyDataManager::instance()->validYellowRobots[i] && !((PARAMS::IS_SIMULATION ? i==(PARAMS::our_id-1) : i==(PARAMS::our_id-1)) && !PARAMS::isBlue)){
415 |             RobotInfo& yellow = MyDataManager::instance()->yellowRobots[i];
416 |             //尤其注意，这里是路子豪写的，是按照正常情况下的X-Y坐标系来做的，对于仿真和实际的环境有点痴迷，详情请询问路子豪
417 |             //globalvx = yellow.vel_x * cos(yellow.orientation) + yellow.vel_y * sin(yellow.orientation);
418 |             //globalvy = yellow.vel_x * sin(yellow.orientation) - yellow.vel_y * cos(yellow.orientation);
419 | //            theta = atan2(globalvy, globalvx);
420 |             globalvx = yellow.vel_x;
421 |             globalvy = -yellow.vel_y;
422 |             //theta = 3.14159*(-1.0/3);
423 |             theta = atan2(-yellow.vel_y, yellow.vel_x);
424 |             endx = yellow.x + PARAMS::OBSTACLE::REFRESH_TIME *globalvx;//*30* cos(theta);//
425 |             endy = yellow.y + PARAMS::OBSTACLE::REFRESH_TIME *globalvy;//*30* sin(theta);//
426 | 
427 | 
428 |             x[0] = yellow.x - sqrt(2)*PARAMS::OBSTACLE::INFLATION_RADIUS*cos(PARAMS::MATH::PI/4-theta);
429 |             y[0] = yellow.y + sqrt(2)*PARAMS::OBSTACLE::INFLATION_RADIUS*sin(PARAMS::MATH::PI/4-theta);
430 | 
431 |             x[1] = yellow.x - sqrt(2)*PARAMS::OBSTACLE::INFLATION_RADIUS*cos(PARAMS::MATH::PI/4+theta);
432 |             y[1] = yellow.y - sqrt(2)*PARAMS::OBSTACLE::INFLATION_RADIUS*sin(PARAMS::MATH::PI/4+theta);
433 | 
434 |             x[2] = yellow.x + sqrt(2)*PARAMS::OBSTACLE::INFLATION_RADIUS*cos(PARAMS::MATH::PI/4+theta);
435 |             y[2] = yellow.y + sqrt(2)*PARAMS::OBSTACLE::INFLATION_RADIUS*sin(PARAMS::MATH::PI/4+theta);
436 | 
437 |             x[3] = yellow.x + sqrt(2)*PARAMS::OBSTACLE::INFLATION_RADIUS*cos(PARAMS::MATH::PI/4-theta);
438 |             y[3] = yellow.y - sqrt(2)*PARAMS::OBSTACLE::INFLATION_RADIUS*sin(PARAMS::MATH::PI/4-theta);
439 | 
440 |             if(pow(yellow.vel_x,2)+pow(yellow.vel_y,2) <= 1000)
441 |             {
442 |                 first = 0;
443 |                 second = 3;
444 |                 third = 1;
445 |                 forth = 2;
446 |             }
447 |             else
448 |             {
449 |                 x[4] = endx - sqrt(2)*PARAMS::OBSTACLE::INFLATION_RADIUS*cos(PARAMS::MATH::PI/4-theta);
450 |                 y[4] = endy + sqrt(2)*PARAMS::OBSTACLE::INFLATION_RADIUS*sin(PARAMS::MATH::PI/4-theta);
451 | 
452 |                 x[5] = endx - sqrt(2)*PARAMS::OBSTACLE::INFLATION_RADIUS*cos(PARAMS::MATH::PI/4+theta);
453 |                 y[5] = endy - sqrt(2)*PARAMS::OBSTACLE::INFLATION_RADIUS*sin(PARAMS::MATH::PI/4+theta);
454 | 
455 |                 x[6] = endx + sqrt(2)*PARAMS::OBSTACLE::INFLATION_RADIUS*cos(PARAMS::MATH::PI/4+theta);
456 |                 y[6] = endy + sqrt(2)*PARAMS::OBSTACLE::INFLATION_RADIUS*sin(PARAMS::MATH::PI/4+theta);
457 | 
458 |                 x[7] = endx + sqrt(2)*PARAMS::OBSTACLE::INFLATION_RADIUS*cos(PARAMS::MATH::PI/4-theta);
459 |                 y[7] = endy - sqrt(2)*PARAMS::OBSTACLE::INFLATION_RADIUS*sin(PARAMS::MATH::PI/4-theta);
460 | 
461 |                 //找到最边上的四个点
462 |                 maxdistance = -1;
463 |                 for(int i = 0; i < 8; i++)
464 |                 {
465 |                     for(int j = i+1; j < 8; j++)
466 |                     {
467 |                         //debug,把线全部画出来
468 |     //                    msg = msgs.add_msgs();
469 |     //                    msg->set_type(ZSS::Protocol::Debug_Msg_Debug_Type_LINE);
470 |     //                    msg->set_color(ZSS::Protocol::Debug_Msg_Color_RED);
471 |     //                    line = msg->mutable_line();
472 |     //                    line->mutable_start()->set_x(x[i]);
473 |     //                    line->mutable_start()->set_y(y[i]);
474 |     //                    line->mutable_end()->set_x(x[j]);
475 |     //                    line->mutable_end()->set_y(y[j]);
476 |     //                    line->set_forward(false);
477 |     //                    line->set_back(false);
478 | 
479 | 
480 |                         mydis = pow(x[j]-x[i],2)+pow(y[j]-y[i],2);
481 |     //                    qDebug() << "i" << i << "j" << j << "mydis" << mydis << "maxdis" << maxdistance;
482 |                         if(mydis > maxdistance)
483 |                         {
484 |                             first = i;
485 |                             second = j;
486 |                             maxdistance = mydis;
487 | 
488 |                         }
489 |                     }
490 |                 }
491 |                 maxdistance = -1;
492 |                 for(int i = 0; i < 8; i++)
493 |                 {
494 |                     for(int j = i+1; j < 8; j++)
495 |                     {
496 |                         if(i == first && j == second) continue;
497 |                         mydis = pow(x[j]-x[i],2)+pow(y[j]-y[i],2);
498 |                         if(mydis > maxdistance)
499 |                         {
500 |                             third = i;
501 |                             forth = j;
502 |                             maxdistance = mydis;
503 |                         }
504 |                     }
505 |                 }
506 |             }
507 | 
508 | 
509 | //            qDebug() << x[0] << x[1] << x[2] << x[3] << x[4] << x[5] << x[6] << x[7];
510 | //            qDebug() << y[0] << y[1] << y[2] << y[3] << y[4] << y[5] << y[6] << y[7];
511 | //            qDebug() << x[first] << x[second] << x[third] << x[forth];
512 | //            qDebug() << y[first] << y[second] << y[third] << y[forth];
513 | //            qDebug() << first << second << third << forth;
514 | 
515 |             msg = msgs.add_msgs();
516 |             msg->set_type(ZSS::Protocol::Debug_Msg_Debug_Type_LINE);
517 |             msg->set_color(ZSS::Protocol::Debug_Msg_Color_WHITE);
518 |             line = msg->mutable_line();
519 |             line->mutable_start()->set_x(x[first]);
520 |             line->mutable_start()->set_y(y[first]);
521 |             line->mutable_end()->set_x(x[third]);
522 |             line->mutable_end()->set_y(y[third]);
523 |             line->set_forward(false);
524 |             line->set_back(false);
525 | 
526 |             msg = msgs.add_msgs();
527 |             msg->set_type(ZSS::Protocol::Debug_Msg_Debug_Type_LINE);
528 |             msg->set_color(ZSS::Protocol::Debug_Msg_Color_WHITE);
529 |             line = msg->mutable_line();
530 |             line->mutable_start()->set_x(x[first]);
531 |             line->mutable_start()->set_y(y[first]);
532 |             line->mutable_end()->set_x(x[forth]);
533 |             line->mutable_end()->set_y(y[forth]);
534 |             line->set_forward(false);
535 |             line->set_back(false);
536 | 
537 |             msg = msgs.add_msgs();
538 |             msg->set_type(ZSS::Protocol::Debug_Msg_Debug_Type_LINE);
539 |             msg->set_color(ZSS::Protocol::Debug_Msg_Color_WHITE);
540 |             line = msg->mutable_line();
541 |             line->mutable_start()->set_x(x[second]);
542 |             line->mutable_start()->set_y(y[second]);
543 |             line->mutable_end()->set_x(x[third]);
544 |             line->mutable_end()->set_y(y[third]);
545 |             line->set_forward(false);
546 |             line->set_back(false);
547 | 
548 |             msg = msgs.add_msgs();
549 |             msg->set_type(ZSS::Protocol::Debug_Msg_Debug_Type_LINE);
550 |             msg->set_color(ZSS::Protocol::Debug_Msg_Color_WHITE);
551 |             line = msg->mutable_line();
552 |             line->mutable_start()->set_x(x[second]);
553 |             line->mutable_start()->set_y(y[second]);
554 |             line->mutable_end()->set_x(x[forth]);
555 |             line->mutable_end()->set_y(y[forth]);
556 |             line->set_forward(false);
557 |             line->set_back(false);
558 |         }
559 |     }
560 |     //终尾
561 | 
562 |     int msgs_size = msgs.ByteSize();
563 |     QByteArray msgs_data(msgs_size, 0);
564 |     msgs.SerializeToArray(msgs_data.data(), msgs_data.size());
565 |     sender->writeDatagram(msgs_data, msgs_size, PARAMS::visualAddress, PARAMS::visualPort);
566 | }
567 | 


--------------------------------------------------------------------------------
/proto/vision_detection.pb.h:
--------------------------------------------------------------------------------
  1 | // Generated by the protocol buffer compiler.  DO NOT EDIT!
  2 | // source: vision_detection.proto
  3 | 
  4 | #ifndef PROTOBUF_vision_5fdetection_2eproto__INCLUDED
  5 | #define PROTOBUF_vision_5fdetection_2eproto__INCLUDED
  6 | 
  7 | #include <string>
  8 | 
  9 | #include <google/protobuf/stubs/common.h>
 10 | 
 11 | #if GOOGLE_PROTOBUF_VERSION < 2006000
 12 | #error This file was generated by a newer version of protoc which is
 13 | #error incompatible with your Protocol Buffer headers.  Please update
 14 | #error your headers.
 15 | #endif
 16 | #if 2006001 < GOOGLE_PROTOBUF_MIN_PROTOC_VERSION
 17 | #error This file was generated by an older version of protoc which is
 18 | #error incompatible with your Protocol Buffer headers.  Please
 19 | #error regenerate this file with a newer version of protoc.
 20 | #endif
 21 | 
 22 | #include <google/protobuf/generated_message_util.h>
 23 | #include <google/protobuf/message.h>
 24 | #include <google/protobuf/repeated_field.h>
 25 | #include <google/protobuf/extension_set.h>
 26 | #include <google/protobuf/unknown_field_set.h>
 27 | // @@protoc_insertion_point(includes)
 28 | 
 29 | // Internal implementation detail -- do not call these.
 30 | void  protobuf_AddDesc_vision_5fdetection_2eproto();
 31 | void protobuf_AssignDesc_vision_5fdetection_2eproto();
 32 | void protobuf_ShutdownFile_vision_5fdetection_2eproto();
 33 | 
 34 | class Vision_DetectionBall;
 35 | class Vision_DetectionRobot;
 36 | class Vision_DetectionFrame;
 37 | 
 38 | // ===================================================================
 39 | 
 40 | class Vision_DetectionBall : public ::google::protobuf::Message {
 41 |  public:
 42 |   Vision_DetectionBall();
 43 |   virtual ~Vision_DetectionBall();
 44 | 
 45 |   Vision_DetectionBall(const Vision_DetectionBall& from);
 46 | 
 47 |   inline Vision_DetectionBall& operator=(const Vision_DetectionBall& from) {
 48 |     CopyFrom(from);
 49 |     return *this;
 50 |   }
 51 | 
 52 |   inline const ::google::protobuf::UnknownFieldSet& unknown_fields() const {
 53 |     return _unknown_fields_;
 54 |   }
 55 | 
 56 |   inline ::google::protobuf::UnknownFieldSet* mutable_unknown_fields() {
 57 |     return &_unknown_fields_;
 58 |   }
 59 | 
 60 |   static const ::google::protobuf::Descriptor* descriptor();
 61 |   static const Vision_DetectionBall& default_instance();
 62 | 
 63 |   void Swap(Vision_DetectionBall* other);
 64 | 
 65 |   // implements Message ----------------------------------------------
 66 | 
 67 |   Vision_DetectionBall* New() const;
 68 |   void CopyFrom(const ::google::protobuf::Message& from);
 69 |   void MergeFrom(const ::google::protobuf::Message& from);
 70 |   void CopyFrom(const Vision_DetectionBall& from);
 71 |   void MergeFrom(const Vision_DetectionBall& from);
 72 |   void Clear();
 73 |   bool IsInitialized() const;
 74 | 
 75 |   int ByteSize() const;
 76 |   bool MergePartialFromCodedStream(
 77 |       ::google::protobuf::io::CodedInputStream* input);
 78 |   void SerializeWithCachedSizes(
 79 |       ::google::protobuf::io::CodedOutputStream* output) const;
 80 |   ::google::protobuf::uint8* SerializeWithCachedSizesToArray(::google::protobuf::uint8* output) const;
 81 |   int GetCachedSize() const { return _cached_size_; }
 82 |   private:
 83 |   void SharedCtor();
 84 |   void SharedDtor();
 85 |   void SetCachedSize(int size) const;
 86 |   public:
 87 |   ::google::protobuf::Metadata GetMetadata() const;
 88 | 
 89 |   // nested types ----------------------------------------------------
 90 | 
 91 |   // accessors -------------------------------------------------------
 92 | 
 93 |   // optional float vel_x = 1;
 94 |   inline bool has_vel_x() const;
 95 |   inline void clear_vel_x();
 96 |   static const int kVelXFieldNumber = 1;
 97 |   inline float vel_x() const;
 98 |   inline void set_vel_x(float value);
 99 | 
100 |   // optional float vel_y = 2;
101 |   inline bool has_vel_y() const;
102 |   inline void clear_vel_y();
103 |   static const int kVelYFieldNumber = 2;
104 |   inline float vel_y() const;
105 |   inline void set_vel_y(float value);
106 | 
107 |   // optional uint32 area = 3;
108 |   inline bool has_area() const;
109 |   inline void clear_area();
110 |   static const int kAreaFieldNumber = 3;
111 |   inline ::google::protobuf::uint32 area() const;
112 |   inline void set_area(::google::protobuf::uint32 value);
113 | 
114 |   // required float x = 4;
115 |   inline bool has_x() const;
116 |   inline void clear_x();
117 |   static const int kXFieldNumber = 4;
118 |   inline float x() const;
119 |   inline void set_x(float value);
120 | 
121 |   // required float y = 5;
122 |   inline bool has_y() const;
123 |   inline void clear_y();
124 |   static const int kYFieldNumber = 5;
125 |   inline float y() const;
126 |   inline void set_y(float value);
127 | 
128 |   // optional float height = 6;
129 |   inline bool has_height() const;
130 |   inline void clear_height();
131 |   static const int kHeightFieldNumber = 6;
132 |   inline float height() const;
133 |   inline void set_height(float value);
134 | 
135 |   // optional uint32 ball_state = 7;
136 |   inline bool has_ball_state() const;
137 |   inline void clear_ball_state();
138 |   static const int kBallStateFieldNumber = 7;
139 |   inline ::google::protobuf::uint32 ball_state() const;
140 |   inline void set_ball_state(::google::protobuf::uint32 value);
141 | 
142 |   // optional uint32 last_touch = 8;
143 |   inline bool has_last_touch() const;
144 |   inline void clear_last_touch();
145 |   static const int kLastTouchFieldNumber = 8;
146 |   inline ::google::protobuf::uint32 last_touch() const;
147 |   inline void set_last_touch(::google::protobuf::uint32 value);
148 | 
149 |   // required bool valid = 9;
150 |   inline bool has_valid() const;
151 |   inline void clear_valid();
152 |   static const int kValidFieldNumber = 9;
153 |   inline bool valid() const;
154 |   inline void set_valid(bool value);
155 | 
156 |   // @@protoc_insertion_point(class_scope:Vision_DetectionBall)
157 |  private:
158 |   inline void set_has_vel_x();
159 |   inline void clear_has_vel_x();
160 |   inline void set_has_vel_y();
161 |   inline void clear_has_vel_y();
162 |   inline void set_has_area();
163 |   inline void clear_has_area();
164 |   inline void set_has_x();
165 |   inline void clear_has_x();
166 |   inline void set_has_y();
167 |   inline void clear_has_y();
168 |   inline void set_has_height();
169 |   inline void clear_has_height();
170 |   inline void set_has_ball_state();
171 |   inline void clear_has_ball_state();
172 |   inline void set_has_last_touch();
173 |   inline void clear_has_last_touch();
174 |   inline void set_has_valid();
175 |   inline void clear_has_valid();
176 | 
177 |   ::google::protobuf::UnknownFieldSet _unknown_fields_;
178 | 
179 |   ::google::protobuf::uint32 _has_bits_[1];
180 |   mutable int _cached_size_;
181 |   float vel_x_;
182 |   float vel_y_;
183 |   ::google::protobuf::uint32 area_;
184 |   float x_;
185 |   float y_;
186 |   float height_;
187 |   ::google::protobuf::uint32 ball_state_;
188 |   ::google::protobuf::uint32 last_touch_;
189 |   bool valid_;
190 |   friend void  protobuf_AddDesc_vision_5fdetection_2eproto();
191 |   friend void protobuf_AssignDesc_vision_5fdetection_2eproto();
192 |   friend void protobuf_ShutdownFile_vision_5fdetection_2eproto();
193 | 
194 |   void InitAsDefaultInstance();
195 |   static Vision_DetectionBall* default_instance_;
196 | };
197 | // -------------------------------------------------------------------
198 | 
199 | class Vision_DetectionRobot : public ::google::protobuf::Message {
200 |  public:
201 |   Vision_DetectionRobot();
202 |   virtual ~Vision_DetectionRobot();
203 | 
204 |   Vision_DetectionRobot(const Vision_DetectionRobot& from);
205 | 
206 |   inline Vision_DetectionRobot& operator=(const Vision_DetectionRobot& from) {
207 |     CopyFrom(from);
208 |     return *this;
209 |   }
210 | 
211 |   inline const ::google::protobuf::UnknownFieldSet& unknown_fields() const {
212 |     return _unknown_fields_;
213 |   }
214 | 
215 |   inline ::google::protobuf::UnknownFieldSet* mutable_unknown_fields() {
216 |     return &_unknown_fields_;
217 |   }
218 | 
219 |   static const ::google::protobuf::Descriptor* descriptor();
220 |   static const Vision_DetectionRobot& default_instance();
221 | 
222 |   void Swap(Vision_DetectionRobot* other);
223 | 
224 |   // implements Message ----------------------------------------------
225 | 
226 |   Vision_DetectionRobot* New() const;
227 |   void CopyFrom(const ::google::protobuf::Message& from);
228 |   void MergeFrom(const ::google::protobuf::Message& from);
229 |   void CopyFrom(const Vision_DetectionRobot& from);
230 |   void MergeFrom(const Vision_DetectionRobot& from);
231 |   void Clear();
232 |   bool IsInitialized() const;
233 | 
234 |   int ByteSize() const;
235 |   bool MergePartialFromCodedStream(
236 |       ::google::protobuf::io::CodedInputStream* input);
237 |   void SerializeWithCachedSizes(
238 |       ::google::protobuf::io::CodedOutputStream* output) const;
239 |   ::google::protobuf::uint8* SerializeWithCachedSizesToArray(::google::protobuf::uint8* output) const;
240 |   int GetCachedSize() const { return _cached_size_; }
241 |   private:
242 |   void SharedCtor();
243 |   void SharedDtor();
244 |   void SetCachedSize(int size) const;
245 |   public:
246 |   ::google::protobuf::Metadata GetMetadata() const;
247 | 
248 |   // nested types ----------------------------------------------------
249 | 
250 |   // accessors -------------------------------------------------------
251 | 
252 |   // required float confidence = 1;
253 |   inline bool has_confidence() const;
254 |   inline void clear_confidence();
255 |   static const int kConfidenceFieldNumber = 1;
256 |   inline float confidence() const;
257 |   inline void set_confidence(float value);
258 | 
259 |   // optional uint32 robot_id = 2;
260 |   inline bool has_robot_id() const;
261 |   inline void clear_robot_id();
262 |   static const int kRobotIdFieldNumber = 2;
263 |   inline ::google::protobuf::uint32 robot_id() const;
264 |   inline void set_robot_id(::google::protobuf::uint32 value);
265 | 
266 |   // required float x = 3;
267 |   inline bool has_x() const;
268 |   inline void clear_x();
269 |   static const int kXFieldNumber = 3;
270 |   inline float x() const;
271 |   inline void set_x(float value);
272 | 
273 |   // required float y = 4;
274 |   inline bool has_y() const;
275 |   inline void clear_y();
276 |   static const int kYFieldNumber = 4;
277 |   inline float y() const;
278 |   inline void set_y(float value);
279 | 
280 |   // optional float orientation = 5;
281 |   inline bool has_orientation() const;
282 |   inline void clear_orientation();
283 |   static const int kOrientationFieldNumber = 5;
284 |   inline float orientation() const;
285 |   inline void set_orientation(float value);
286 | 
287 |   // optional float vel_x = 6;
288 |   inline bool has_vel_x() const;
289 |   inline void clear_vel_x();
290 |   static const int kVelXFieldNumber = 6;
291 |   inline float vel_x() const;
292 |   inline void set_vel_x(float value);
293 | 
294 |   // optional float vel_y = 7;
295 |   inline bool has_vel_y() const;
296 |   inline void clear_vel_y();
297 |   static const int kVelYFieldNumber = 7;
298 |   inline float vel_y() const;
299 |   inline void set_vel_y(float value);
300 | 
301 |   // optional float rotate_vel = 8;
302 |   inline bool has_rotate_vel() const;
303 |   inline void clear_rotate_vel();
304 |   static const int kRotateVelFieldNumber = 8;
305 |   inline float rotate_vel() const;
306 |   inline void set_rotate_vel(float value);
307 | 
308 |   // @@protoc_insertion_point(class_scope:Vision_DetectionRobot)
309 |  private:
310 |   inline void set_has_confidence();
311 |   inline void clear_has_confidence();
312 |   inline void set_has_robot_id();
313 |   inline void clear_has_robot_id();
314 |   inline void set_has_x();
315 |   inline void clear_has_x();
316 |   inline void set_has_y();
317 |   inline void clear_has_y();
318 |   inline void set_has_orientation();
319 |   inline void clear_has_orientation();
320 |   inline void set_has_vel_x();
321 |   inline void clear_has_vel_x();
322 |   inline void set_has_vel_y();
323 |   inline void clear_has_vel_y();
324 |   inline void set_has_rotate_vel();
325 |   inline void clear_has_rotate_vel();
326 | 
327 |   ::google::protobuf::UnknownFieldSet _unknown_fields_;
328 | 
329 |   ::google::protobuf::uint32 _has_bits_[1];
330 |   mutable int _cached_size_;
331 |   float confidence_;
332 |   ::google::protobuf::uint32 robot_id_;
333 |   float x_;
334 |   float y_;
335 |   float orientation_;
336 |   float vel_x_;
337 |   float vel_y_;
338 |   float rotate_vel_;
339 |   friend void  protobuf_AddDesc_vision_5fdetection_2eproto();
340 |   friend void protobuf_AssignDesc_vision_5fdetection_2eproto();
341 |   friend void protobuf_ShutdownFile_vision_5fdetection_2eproto();
342 | 
343 |   void InitAsDefaultInstance();
344 |   static Vision_DetectionRobot* default_instance_;
345 | };
346 | // -------------------------------------------------------------------
347 | 
348 | class Vision_DetectionFrame : public ::google::protobuf::Message {
349 |  public:
350 |   Vision_DetectionFrame();
351 |   virtual ~Vision_DetectionFrame();
352 | 
353 |   Vision_DetectionFrame(const Vision_DetectionFrame& from);
354 | 
355 |   inline Vision_DetectionFrame& operator=(const Vision_DetectionFrame& from) {
356 |     CopyFrom(from);
357 |     return *this;
358 |   }
359 | 
360 |   inline const ::google::protobuf::UnknownFieldSet& unknown_fields() const {
361 |     return _unknown_fields_;
362 |   }
363 | 
364 |   inline ::google::protobuf::UnknownFieldSet* mutable_unknown_fields() {
365 |     return &_unknown_fields_;
366 |   }
367 | 
368 |   static const ::google::protobuf::Descriptor* descriptor();
369 |   static const Vision_DetectionFrame& default_instance();
370 | 
371 |   void Swap(Vision_DetectionFrame* other);
372 | 
373 |   // implements Message ----------------------------------------------
374 | 
375 |   Vision_DetectionFrame* New() const;
376 |   void CopyFrom(const ::google::protobuf::Message& from);
377 |   void MergeFrom(const ::google::protobuf::Message& from);
378 |   void CopyFrom(const Vision_DetectionFrame& from);
379 |   void MergeFrom(const Vision_DetectionFrame& from);
380 |   void Clear();
381 |   bool IsInitialized() const;
382 | 
383 |   int ByteSize() const;
384 |   bool MergePartialFromCodedStream(
385 |       ::google::protobuf::io::CodedInputStream* input);
386 |   void SerializeWithCachedSizes(
387 |       ::google::protobuf::io::CodedOutputStream* output) const;
388 |   ::google::protobuf::uint8* SerializeWithCachedSizesToArray(::google::protobuf::uint8* output) const;
389 |   int GetCachedSize() const { return _cached_size_; }
390 |   private:
391 |   void SharedCtor();
392 |   void SharedDtor();
393 |   void SetCachedSize(int size) const;
394 |   public:
395 |   ::google::protobuf::Metadata GetMetadata() const;
396 | 
397 |   // nested types ----------------------------------------------------
398 | 
399 |   // accessors -------------------------------------------------------
400 | 
401 |   // required .Vision_DetectionBall balls = 1;
402 |   inline bool has_balls() const;
403 |   inline void clear_balls();
404 |   static const int kBallsFieldNumber = 1;
405 |   inline const ::Vision_DetectionBall& balls() const;
406 |   inline ::Vision_DetectionBall* mutable_balls();
407 |   inline ::Vision_DetectionBall* release_balls();
408 |   inline void set_allocated_balls(::Vision_DetectionBall* balls);
409 | 
410 |   // repeated .Vision_DetectionRobot robots_yellow = 2;
411 |   inline int robots_yellow_size() const;
412 |   inline void clear_robots_yellow();
413 |   static const int kRobotsYellowFieldNumber = 2;
414 |   inline const ::Vision_DetectionRobot& robots_yellow(int index) const;
415 |   inline ::Vision_DetectionRobot* mutable_robots_yellow(int index);
416 |   inline ::Vision_DetectionRobot* add_robots_yellow();
417 |   inline const ::google::protobuf::RepeatedPtrField< ::Vision_DetectionRobot >&
418 |       robots_yellow() const;
419 |   inline ::google::protobuf::RepeatedPtrField< ::Vision_DetectionRobot >*
420 |       mutable_robots_yellow();
421 | 
422 |   // repeated .Vision_DetectionRobot robots_blue = 3;
423 |   inline int robots_blue_size() const;
424 |   inline void clear_robots_blue();
425 |   static const int kRobotsBlueFieldNumber = 3;
426 |   inline const ::Vision_DetectionRobot& robots_blue(int index) const;
427 |   inline ::Vision_DetectionRobot* mutable_robots_blue(int index);
428 |   inline ::Vision_DetectionRobot* add_robots_blue();
429 |   inline const ::google::protobuf::RepeatedPtrField< ::Vision_DetectionRobot >&
430 |       robots_blue() const;
431 |   inline ::google::protobuf::RepeatedPtrField< ::Vision_DetectionRobot >*
432 |       mutable_robots_blue();
433 | 
434 |   // @@protoc_insertion_point(class_scope:Vision_DetectionFrame)
435 |  private:
436 |   inline void set_has_balls();
437 |   inline void clear_has_balls();
438 | 
439 |   ::google::protobuf::UnknownFieldSet _unknown_fields_;
440 | 
441 |   ::google::protobuf::uint32 _has_bits_[1];
442 |   mutable int _cached_size_;
443 |   ::Vision_DetectionBall* balls_;
444 |   ::google::protobuf::RepeatedPtrField< ::Vision_DetectionRobot > robots_yellow_;
445 |   ::google::protobuf::RepeatedPtrField< ::Vision_DetectionRobot > robots_blue_;
446 |   friend void  protobuf_AddDesc_vision_5fdetection_2eproto();
447 |   friend void protobuf_AssignDesc_vision_5fdetection_2eproto();
448 |   friend void protobuf_ShutdownFile_vision_5fdetection_2eproto();
449 | 
450 |   void InitAsDefaultInstance();
451 |   static Vision_DetectionFrame* default_instance_;
452 | };
453 | // ===================================================================
454 | 
455 | 
456 | // ===================================================================
457 | 
458 | // Vision_DetectionBall
459 | 
460 | // optional float vel_x = 1;
461 | inline bool Vision_DetectionBall::has_vel_x() const {
462 |   return (_has_bits_[0] & 0x00000001u) != 0;
463 | }
464 | inline void Vision_DetectionBall::set_has_vel_x() {
465 |   _has_bits_[0] |= 0x00000001u;
466 | }
467 | inline void Vision_DetectionBall::clear_has_vel_x() {
468 |   _has_bits_[0] &= ~0x00000001u;
469 | }
470 | inline void Vision_DetectionBall::clear_vel_x() {
471 |   vel_x_ = 0;
472 |   clear_has_vel_x();
473 | }
474 | inline float Vision_DetectionBall::vel_x() const {
475 |   // @@protoc_insertion_point(field_get:Vision_DetectionBall.vel_x)
476 |   return vel_x_;
477 | }
478 | inline void Vision_DetectionBall::set_vel_x(float value) {
479 |   set_has_vel_x();
480 |   vel_x_ = value;
481 |   // @@protoc_insertion_point(field_set:Vision_DetectionBall.vel_x)
482 | }
483 | 
484 | // optional float vel_y = 2;
485 | inline bool Vision_DetectionBall::has_vel_y() const {
486 |   return (_has_bits_[0] & 0x00000002u) != 0;
487 | }
488 | inline void Vision_DetectionBall::set_has_vel_y() {
489 |   _has_bits_[0] |= 0x00000002u;
490 | }
491 | inline void Vision_DetectionBall::clear_has_vel_y() {
492 |   _has_bits_[0] &= ~0x00000002u;
493 | }
494 | inline void Vision_DetectionBall::clear_vel_y() {
495 |   vel_y_ = 0;
496 |   clear_has_vel_y();
497 | }
498 | inline float Vision_DetectionBall::vel_y() const {
499 |   // @@protoc_insertion_point(field_get:Vision_DetectionBall.vel_y)
500 |   return vel_y_;
501 | }
502 | inline void Vision_DetectionBall::set_vel_y(float value) {
503 |   set_has_vel_y();
504 |   vel_y_ = value;
505 |   // @@protoc_insertion_point(field_set:Vision_DetectionBall.vel_y)
506 | }
507 | 
508 | // optional uint32 area = 3;
509 | inline bool Vision_DetectionBall::has_area() const {
510 |   return (_has_bits_[0] & 0x00000004u) != 0;
511 | }
512 | inline void Vision_DetectionBall::set_has_area() {
513 |   _has_bits_[0] |= 0x00000004u;
514 | }
515 | inline void Vision_DetectionBall::clear_has_area() {
516 |   _has_bits_[0] &= ~0x00000004u;
517 | }
518 | inline void Vision_DetectionBall::clear_area() {
519 |   area_ = 0u;
520 |   clear_has_area();
521 | }
522 | inline ::google::protobuf::uint32 Vision_DetectionBall::area() const {
523 |   // @@protoc_insertion_point(field_get:Vision_DetectionBall.area)
524 |   return area_;
525 | }
526 | inline void Vision_DetectionBall::set_area(::google::protobuf::uint32 value) {
527 |   set_has_area();
528 |   area_ = value;
529 |   // @@protoc_insertion_point(field_set:Vision_DetectionBall.area)
530 | }
531 | 
532 | // required float x = 4;
533 | inline bool Vision_DetectionBall::has_x() const {
534 |   return (_has_bits_[0] & 0x00000008u) != 0;
535 | }
536 | inline void Vision_DetectionBall::set_has_x() {
537 |   _has_bits_[0] |= 0x00000008u;
538 | }
539 | inline void Vision_DetectionBall::clear_has_x() {
540 |   _has_bits_[0] &= ~0x00000008u;
541 | }
542 | inline void Vision_DetectionBall::clear_x() {
543 |   x_ = 0;
544 |   clear_has_x();
545 | }
546 | inline float Vision_DetectionBall::x() const {
547 |   // @@protoc_insertion_point(field_get:Vision_DetectionBall.x)
548 |   return x_;
549 | }
550 | inline void Vision_DetectionBall::set_x(float value) {
551 |   set_has_x();
552 |   x_ = value;
553 |   // @@protoc_insertion_point(field_set:Vision_DetectionBall.x)
554 | }
555 | 
556 | // required float y = 5;
557 | inline bool Vision_DetectionBall::has_y() const {
558 |   return (_has_bits_[0] & 0x00000010u) != 0;
559 | }
560 | inline void Vision_DetectionBall::set_has_y() {
561 |   _has_bits_[0] |= 0x00000010u;
562 | }
563 | inline void Vision_DetectionBall::clear_has_y() {
564 |   _has_bits_[0] &= ~0x00000010u;
565 | }
566 | inline void Vision_DetectionBall::clear_y() {
567 |   y_ = 0;
568 |   clear_has_y();
569 | }
570 | inline float Vision_DetectionBall::y() const {
571 |   // @@protoc_insertion_point(field_get:Vision_DetectionBall.y)
572 |   return y_;
573 | }
574 | inline void Vision_DetectionBall::set_y(float value) {
575 |   set_has_y();
576 |   y_ = value;
577 |   // @@protoc_insertion_point(field_set:Vision_DetectionBall.y)
578 | }
579 | 
580 | // optional float height = 6;
581 | inline bool Vision_DetectionBall::has_height() const {
582 |   return (_has_bits_[0] & 0x00000020u) != 0;
583 | }
584 | inline void Vision_DetectionBall::set_has_height() {
585 |   _has_bits_[0] |= 0x00000020u;
586 | }
587 | inline void Vision_DetectionBall::clear_has_height() {
588 |   _has_bits_[0] &= ~0x00000020u;
589 | }
590 | inline void Vision_DetectionBall::clear_height() {
591 |   height_ = 0;
592 |   clear_has_height();
593 | }
594 | inline float Vision_DetectionBall::height() const {
595 |   // @@protoc_insertion_point(field_get:Vision_DetectionBall.height)
596 |   return height_;
597 | }
598 | inline void Vision_DetectionBall::set_height(float value) {
599 |   set_has_height();
600 |   height_ = value;
601 |   // @@protoc_insertion_point(field_set:Vision_DetectionBall.height)
602 | }
603 | 
604 | // optional uint32 ball_state = 7;
605 | inline bool Vision_DetectionBall::has_ball_state() const {
606 |   return (_has_bits_[0] & 0x00000040u) != 0;
607 | }
608 | inline void Vision_DetectionBall::set_has_ball_state() {
609 |   _has_bits_[0] |= 0x00000040u;
610 | }
611 | inline void Vision_DetectionBall::clear_has_ball_state() {
612 |   _has_bits_[0] &= ~0x00000040u;
613 | }
614 | inline void Vision_DetectionBall::clear_ball_state() {
615 |   ball_state_ = 0u;
616 |   clear_has_ball_state();
617 | }
618 | inline ::google::protobuf::uint32 Vision_DetectionBall::ball_state() const {
619 |   // @@protoc_insertion_point(field_get:Vision_DetectionBall.ball_state)
620 |   return ball_state_;
621 | }
622 | inline void Vision_DetectionBall::set_ball_state(::google::protobuf::uint32 value) {
623 |   set_has_ball_state();
624 |   ball_state_ = value;
625 |   // @@protoc_insertion_point(field_set:Vision_DetectionBall.ball_state)
626 | }
627 | 
628 | // optional uint32 last_touch = 8;
629 | inline bool Vision_DetectionBall::has_last_touch() const {
630 |   return (_has_bits_[0] & 0x00000080u) != 0;
631 | }
632 | inline void Vision_DetectionBall::set_has_last_touch() {
633 |   _has_bits_[0] |= 0x00000080u;
634 | }
635 | inline void Vision_DetectionBall::clear_has_last_touch() {
636 |   _has_bits_[0] &= ~0x00000080u;
637 | }
638 | inline void Vision_DetectionBall::clear_last_touch() {
639 |   last_touch_ = 0u;
640 |   clear_has_last_touch();
641 | }
642 | inline ::google::protobuf::uint32 Vision_DetectionBall::last_touch() const {
643 |   // @@protoc_insertion_point(field_get:Vision_DetectionBall.last_touch)
644 |   return last_touch_;
645 | }
646 | inline void Vision_DetectionBall::set_last_touch(::google::protobuf::uint32 value) {
647 |   set_has_last_touch();
648 |   last_touch_ = value;
649 |   // @@protoc_insertion_point(field_set:Vision_DetectionBall.last_touch)
650 | }
651 | 
652 | // required bool valid = 9;
653 | inline bool Vision_DetectionBall::has_valid() const {
654 |   return (_has_bits_[0] & 0x00000100u) != 0;
655 | }
656 | inline void Vision_DetectionBall::set_has_valid() {
657 |   _has_bits_[0] |= 0x00000100u;
658 | }
659 | inline void Vision_DetectionBall::clear_has_valid() {
660 |   _has_bits_[0] &= ~0x00000100u;
661 | }
662 | inline void Vision_DetectionBall::clear_valid() {
663 |   valid_ = false;
664 |   clear_has_valid();
665 | }
666 | inline bool Vision_DetectionBall::valid() const {
667 |   // @@protoc_insertion_point(field_get:Vision_DetectionBall.valid)
668 |   return valid_;
669 | }
670 | inline void Vision_DetectionBall::set_valid(bool value) {
671 |   set_has_valid();
672 |   valid_ = value;
673 |   // @@protoc_insertion_point(field_set:Vision_DetectionBall.valid)
674 | }
675 | 
676 | // -------------------------------------------------------------------
677 | 
678 | // Vision_DetectionRobot
679 | 
680 | // required float confidence = 1;
681 | inline bool Vision_DetectionRobot::has_confidence() const {
682 |   return (_has_bits_[0] & 0x00000001u) != 0;
683 | }
684 | inline void Vision_DetectionRobot::set_has_confidence() {
685 |   _has_bits_[0] |= 0x00000001u;
686 | }
687 | inline void Vision_DetectionRobot::clear_has_confidence() {
688 |   _has_bits_[0] &= ~0x00000001u;
689 | }
690 | inline void Vision_DetectionRobot::clear_confidence() {
691 |   confidence_ = 0;
692 |   clear_has_confidence();
693 | }
694 | inline float Vision_DetectionRobot::confidence() const {
695 |   // @@protoc_insertion_point(field_get:Vision_DetectionRobot.confidence)
696 |   return confidence_;
697 | }
698 | inline void Vision_DetectionRobot::set_confidence(float value) {
699 |   set_has_confidence();
700 |   confidence_ = value;
701 |   // @@protoc_insertion_point(field_set:Vision_DetectionRobot.confidence)
702 | }
703 | 
704 | // optional uint32 robot_id = 2;
705 | inline bool Vision_DetectionRobot::has_robot_id() const {
706 |   return (_has_bits_[0] & 0x00000002u) != 0;
707 | }
708 | inline void Vision_DetectionRobot::set_has_robot_id() {
709 |   _has_bits_[0] |= 0x00000002u;
710 | }
711 | inline void Vision_DetectionRobot::clear_has_robot_id() {
712 |   _has_bits_[0] &= ~0x00000002u;
713 | }
714 | inline void Vision_DetectionRobot::clear_robot_id() {
715 |   robot_id_ = 0u;
716 |   clear_has_robot_id();
717 | }
718 | inline ::google::protobuf::uint32 Vision_DetectionRobot::robot_id() const {
719 |   // @@protoc_insertion_point(field_get:Vision_DetectionRobot.robot_id)
720 |   return robot_id_;
721 | }
722 | inline void Vision_DetectionRobot::set_robot_id(::google::protobuf::uint32 value) {
723 |   set_has_robot_id();
724 |   robot_id_ = value;
725 |   // @@protoc_insertion_point(field_set:Vision_DetectionRobot.robot_id)
726 | }
727 | 
728 | // required float x = 3;
729 | inline bool Vision_DetectionRobot::has_x() const {
730 |   return (_has_bits_[0] & 0x00000004u) != 0;
731 | }
732 | inline void Vision_DetectionRobot::set_has_x() {
733 |   _has_bits_[0] |= 0x00000004u;
734 | }
735 | inline void Vision_DetectionRobot::clear_has_x() {
736 |   _has_bits_[0] &= ~0x00000004u;
737 | }
738 | inline void Vision_DetectionRobot::clear_x() {
739 |   x_ = 0;
740 |   clear_has_x();
741 | }
742 | inline float Vision_DetectionRobot::x() const {
743 |   // @@protoc_insertion_point(field_get:Vision_DetectionRobot.x)
744 |   return x_;
745 | }
746 | inline void Vision_DetectionRobot::set_x(float value) {
747 |   set_has_x();
748 |   x_ = value;
749 |   // @@protoc_insertion_point(field_set:Vision_DetectionRobot.x)
750 | }
751 | 
752 | // required float y = 4;
753 | inline bool Vision_DetectionRobot::has_y() const {
754 |   return (_has_bits_[0] & 0x00000008u) != 0;
755 | }
756 | inline void Vision_DetectionRobot::set_has_y() {
757 |   _has_bits_[0] |= 0x00000008u;
758 | }
759 | inline void Vision_DetectionRobot::clear_has_y() {
760 |   _has_bits_[0] &= ~0x00000008u;
761 | }
762 | inline void Vision_DetectionRobot::clear_y() {
763 |   y_ = 0;
764 |   clear_has_y();
765 | }
766 | inline float Vision_DetectionRobot::y() const {
767 |   // @@protoc_insertion_point(field_get:Vision_DetectionRobot.y)
768 |   return y_;
769 | }
770 | inline void Vision_DetectionRobot::set_y(float value) {
771 |   set_has_y();
772 |   y_ = value;
773 |   // @@protoc_insertion_point(field_set:Vision_DetectionRobot.y)
774 | }
775 | 
776 | // optional float orientation = 5;
777 | inline bool Vision_DetectionRobot::has_orientation() const {
778 |   return (_has_bits_[0] & 0x00000010u) != 0;
779 | }
780 | inline void Vision_DetectionRobot::set_has_orientation() {
781 |   _has_bits_[0] |= 0x00000010u;
782 | }
783 | inline void Vision_DetectionRobot::clear_has_orientation() {
784 |   _has_bits_[0] &= ~0x00000010u;
785 | }
786 | inline void Vision_DetectionRobot::clear_orientation() {
787 |   orientation_ = 0;
788 |   clear_has_orientation();
789 | }
790 | inline float Vision_DetectionRobot::orientation() const {
791 |   // @@protoc_insertion_point(field_get:Vision_DetectionRobot.orientation)
792 |   return orientation_;
793 | }
794 | inline void Vision_DetectionRobot::set_orientation(float value) {
795 |   set_has_orientation();
796 |   orientation_ = value;
797 |   // @@protoc_insertion_point(field_set:Vision_DetectionRobot.orientation)
798 | }
799 | 
800 | // optional float vel_x = 6;
801 | inline bool Vision_DetectionRobot::has_vel_x() const {
802 |   return (_has_bits_[0] & 0x00000020u) != 0;
803 | }
804 | inline void Vision_DetectionRobot::set_has_vel_x() {
805 |   _has_bits_[0] |= 0x00000020u;
806 | }
807 | inline void Vision_DetectionRobot::clear_has_vel_x() {
808 |   _has_bits_[0] &= ~0x00000020u;
809 | }
810 | inline void Vision_DetectionRobot::clear_vel_x() {
811 |   vel_x_ = 0;
812 |   clear_has_vel_x();
813 | }
814 | inline float Vision_DetectionRobot::vel_x() const {
815 |   // @@protoc_insertion_point(field_get:Vision_DetectionRobot.vel_x)
816 |   return vel_x_;
817 | }
818 | inline void Vision_DetectionRobot::set_vel_x(float value) {
819 |   set_has_vel_x();
820 |   vel_x_ = value;
821 |   // @@protoc_insertion_point(field_set:Vision_DetectionRobot.vel_x)
822 | }
823 | 
824 | // optional float vel_y = 7;
825 | inline bool Vision_DetectionRobot::has_vel_y() const {
826 |   return (_has_bits_[0] & 0x00000040u) != 0;
827 | }
828 | inline void Vision_DetectionRobot::set_has_vel_y() {
829 |   _has_bits_[0] |= 0x00000040u;
830 | }
831 | inline void Vision_DetectionRobot::clear_has_vel_y() {
832 |   _has_bits_[0] &= ~0x00000040u;
833 | }
834 | inline void Vision_DetectionRobot::clear_vel_y() {
835 |   vel_y_ = 0;
836 |   clear_has_vel_y();
837 | }
838 | inline float Vision_DetectionRobot::vel_y() const {
839 |   // @@protoc_insertion_point(field_get:Vision_DetectionRobot.vel_y)
840 |   return vel_y_;
841 | }
842 | inline void Vision_DetectionRobot::set_vel_y(float value) {
843 |   set_has_vel_y();
844 |   vel_y_ = value;
845 |   // @@protoc_insertion_point(field_set:Vision_DetectionRobot.vel_y)
846 | }
847 | 
848 | // optional float rotate_vel = 8;
849 | inline bool Vision_DetectionRobot::has_rotate_vel() const {
850 |   return (_has_bits_[0] & 0x00000080u) != 0;
851 | }
852 | inline void Vision_DetectionRobot::set_has_rotate_vel() {
853 |   _has_bits_[0] |= 0x00000080u;
854 | }
855 | inline void Vision_DetectionRobot::clear_has_rotate_vel() {
856 |   _has_bits_[0] &= ~0x00000080u;
857 | }
858 | inline void Vision_DetectionRobot::clear_rotate_vel() {
859 |   rotate_vel_ = 0;
860 |   clear_has_rotate_vel();
861 | }
862 | inline float Vision_DetectionRobot::rotate_vel() const {
863 |   // @@protoc_insertion_point(field_get:Vision_DetectionRobot.rotate_vel)
864 |   return rotate_vel_;
865 | }
866 | inline void Vision_DetectionRobot::set_rotate_vel(float value) {
867 |   set_has_rotate_vel();
868 |   rotate_vel_ = value;
869 |   // @@protoc_insertion_point(field_set:Vision_DetectionRobot.rotate_vel)
870 | }
871 | 
872 | // -------------------------------------------------------------------
873 | 
874 | // Vision_DetectionFrame
875 | 
876 | // required .Vision_DetectionBall balls = 1;
877 | inline bool Vision_DetectionFrame::has_balls() const {
878 |   return (_has_bits_[0] & 0x00000001u) != 0;
879 | }
880 | inline void Vision_DetectionFrame::set_has_balls() {
881 |   _has_bits_[0] |= 0x00000001u;
882 | }
883 | inline void Vision_DetectionFrame::clear_has_balls() {
884 |   _has_bits_[0] &= ~0x00000001u;
885 | }
886 | inline void Vision_DetectionFrame::clear_balls() {
887 |   if (balls_ != NULL) balls_->::Vision_DetectionBall::Clear();
888 |   clear_has_balls();
889 | }
890 | inline const ::Vision_DetectionBall& Vision_DetectionFrame::balls() const {
891 |   // @@protoc_insertion_point(field_get:Vision_DetectionFrame.balls)
892 |   return balls_ != NULL ? *balls_ : *default_instance_->balls_;
893 | }
894 | inline ::Vision_DetectionBall* Vision_DetectionFrame::mutable_balls() {
895 |   set_has_balls();
896 |   if (balls_ == NULL) balls_ = new ::Vision_DetectionBall;
897 |   // @@protoc_insertion_point(field_mutable:Vision_DetectionFrame.balls)
898 |   return balls_;
899 | }
900 | inline ::Vision_DetectionBall* Vision_DetectionFrame::release_balls() {
901 |   clear_has_balls();
902 |   ::Vision_DetectionBall* temp = balls_;
903 |   balls_ = NULL;
904 |   return temp;
905 | }
906 | inline void Vision_DetectionFrame::set_allocated_balls(::Vision_DetectionBall* balls) {
907 |   delete balls_;
908 |   balls_ = balls;
909 |   if (balls) {
910 |     set_has_balls();
911 |   } else {
912 |     clear_has_balls();
913 |   }
914 |   // @@protoc_insertion_point(field_set_allocated:Vision_DetectionFrame.balls)
915 | }
916 | 
917 | // repeated .Vision_DetectionRobot robots_yellow = 2;
918 | inline int Vision_DetectionFrame::robots_yellow_size() const {
919 |   return robots_yellow_.size();
920 | }
921 | inline void Vision_DetectionFrame::clear_robots_yellow() {
922 |   robots_yellow_.Clear();
923 | }
924 | inline const ::Vision_DetectionRobot& Vision_DetectionFrame::robots_yellow(int index) const {
925 |   // @@protoc_insertion_point(field_get:Vision_DetectionFrame.robots_yellow)
926 |   return robots_yellow_.Get(index);
927 | }
928 | inline ::Vision_DetectionRobot* Vision_DetectionFrame::mutable_robots_yellow(int index) {
929 |   // @@protoc_insertion_point(field_mutable:Vision_DetectionFrame.robots_yellow)
930 |   return robots_yellow_.Mutable(index);
931 | }
932 | inline ::Vision_DetectionRobot* Vision_DetectionFrame::add_robots_yellow() {
933 |   // @@protoc_insertion_point(field_add:Vision_DetectionFrame.robots_yellow)
934 |   return robots_yellow_.Add();
935 | }
936 | inline const ::google::protobuf::RepeatedPtrField< ::Vision_DetectionRobot >&
937 | Vision_DetectionFrame::robots_yellow() const {
938 |   // @@protoc_insertion_point(field_list:Vision_DetectionFrame.robots_yellow)
939 |   return robots_yellow_;
940 | }
941 | inline ::google::protobuf::RepeatedPtrField< ::Vision_DetectionRobot >*
942 | Vision_DetectionFrame::mutable_robots_yellow() {
943 |   // @@protoc_insertion_point(field_mutable_list:Vision_DetectionFrame.robots_yellow)
944 |   return &robots_yellow_;
945 | }
946 | 
947 | // repeated .Vision_DetectionRobot robots_blue = 3;
948 | inline int Vision_DetectionFrame::robots_blue_size() const {
949 |   return robots_blue_.size();
950 | }
951 | inline void Vision_DetectionFrame::clear_robots_blue() {
952 |   robots_blue_.Clear();
953 | }
954 | inline const ::Vision_DetectionRobot& Vision_DetectionFrame::robots_blue(int index) const {
955 |   // @@protoc_insertion_point(field_get:Vision_DetectionFrame.robots_blue)
956 |   return robots_blue_.Get(index);
957 | }
958 | inline ::Vision_DetectionRobot* Vision_DetectionFrame::mutable_robots_blue(int index) {
959 |   // @@protoc_insertion_point(field_mutable:Vision_DetectionFrame.robots_blue)
960 |   return robots_blue_.Mutable(index);
961 | }
962 | inline ::Vision_DetectionRobot* Vision_DetectionFrame::add_robots_blue() {
963 |   // @@protoc_insertion_point(field_add:Vision_DetectionFrame.robots_blue)
964 |   return robots_blue_.Add();
965 | }
966 | inline const ::google::protobuf::RepeatedPtrField< ::Vision_DetectionRobot >&
967 | Vision_DetectionFrame::robots_blue() const {
968 |   // @@protoc_insertion_point(field_list:Vision_DetectionFrame.robots_blue)
969 |   return robots_blue_;
970 | }
971 | inline ::google::protobuf::RepeatedPtrField< ::Vision_DetectionRobot >*
972 | Vision_DetectionFrame::mutable_robots_blue() {
973 |   // @@protoc_insertion_point(field_mutable_list:Vision_DetectionFrame.robots_blue)
974 |   return &robots_blue_;
975 | }
976 | 
977 | 
978 | // @@protoc_insertion_point(namespace_scope)
979 | 
980 | #ifndef SWIG
981 | namespace google {
982 | namespace protobuf {
983 | 
984 | 
985 | }  // namespace google
986 | }  // namespace protobuf
987 | #endif  // SWIG
988 | 
989 | // @@protoc_insertion_point(global_scope)
990 | 
991 | #endif  // PROTOBUF_vision_5fdetection_2eproto__INCLUDED
992 | 


--------------------------------------------------------------------------------