diff --git a/.github/config.yml b/.github/config.yml new file mode 100644 index 00000000..1ec00c9a --- /dev/null +++ b/.github/config.yml @@ -0,0 +1,8 @@ +# These are the available config settings. +keyword: ["@todo", "TODO:", "TODO", "TO DO"] # string|string[] +bodyKeyword: ["@body", "BODY", "BODY:"] # string|string[] +blobLines: 5 # number|boolean, 0 or false to disable +autoAssign: true # string|string[]|boolean +label: true # boolean|string|string[] +reopenClosed: true # boolean +caseSensitive: false diff --git a/.github/stale.yml b/.github/stale.yml new file mode 100644 index 00000000..a5d7ec5a --- /dev/null +++ b/.github/stale.yml @@ -0,0 +1,17 @@ +# Number of days of inactivity before an issue becomes stale +daysUntilStale: 60 +# Number of days of inactivity before a stale issue is closed +daysUntilClose: 7 +# Issues with these labels will never be considered stale +exemptLabels: + - pinned + - no_stale +# Label to use when marking an issue as stale +staleLabel: wontfix +# Comment to post when marking an issue as stale. Set to `false` to disable +markComment: > + This issue has been automatically marked as stale because it has not had + recent activity. It will be closed if no further activity occurs. Thank you + for your contributions. +# Comment to post when closing a stale issue. Set to `false` to disable +closeComment: false diff --git a/README.md b/README.md index 981774c7..447f4b41 100644 --- a/README.md +++ b/README.md @@ -1,9 +1,147 @@ -## Parsian SSL -[![CircleCI](https://circleci.com/gh/ParsianRoboticLab/ssl/tree/develop.svg?style=svg)](https://circleci.com/gh/ParsianRoboticLab/ssl/tree/develop) -[![CodeFactor](https://www.codefactor.io/repository/github/parsianroboticlab/ssl/badge/develop)](https://www.codefactor.io/repository/github/parsianroboticlab/ssl/overview/develop) +# Parsian SSL -# Install Stuff +[![CircleCI](https://circleci.com/gh/ParsianRoboticLab/ssl/tree/develop.svg?style=svg)](https://circleci.com/gh/ParsianRoboticLab/ssl/tree/develop) +[![CodeFactor](https://www.codefactor.io/repository/github/parsianroboticlab/ssl/badge?s=ae124388adc531f2cb4c8fc1621e1f251f0c9747)](https://www.codefactor.io/repository/github/parsianroboticlab/ssl) +[![License: LGPL v3](https://img.shields.io/badge/License-LGPL%20v3-blue.svg)](https://www.gnu.org/licenses/lgpl-3.0) + +The **Parsian SSL** stack is the full software framework used by [Parsian Robotic Lab](https://github.com/ParsianRoboticLab) to compete in the [RoboCup Small Size League (SSL)](https://ssl.robocup.org/). It is built on top of [ROS](https://www.ros.org/) and provides a complete pipeline from raw vision and referee data all the way down to individual robot motion commands. + +--- + +## Table of Contents + +- [Architecture](#architecture) +- [Prerequisites](#prerequisites) +- [Installation](#installation) +- [Building](#building) +- [Usage](#usage) +- [Contributing](#contributing) +- [License](#license) +- [Acknowledgements](#acknowledgements) + +--- + +## Architecture + +The stack is organized as a collection of ROS packages, each responsible for a distinct layer of the pipeline: + +``` +SSL Vision / Referee / grSim + │ + ▼ +parsian_protobuf_wrapper ← decodes UDP protobuf packets into ROS messages + │ + ▼ +parsian_world_model ← sensor fusion & Kalman filtering → world model + │ + ▼ +parsian_ai ← multi-agent behavioral AI & play execution + │ + ▼ +parsian_agent ← per-robot path planning & motion control + │ + ▼ +parsian_communication ← serializes commands & sends to robots via radio ``` + +| Package | Description | +|---|---| +| [`parsian_protobuf_wrapper`](parsian_protobuf_wrapper/) | Wraps SSL Vision, grSim, and SSL Referee protobuf UDP streams into ROS topics | +| [`parsian_world_model`](parsian_world_model/) | Reads vision data, runs a Kalman-filter based tracker, and publishes a fused world-model | +| [`parsian_ai`](parsian_ai/) | Behavioral AI layer: evaluates game state, selects plays, and assigns per-robot tasks | +| [`parsian_agent`](parsian_agent/) | Per-robot node: consumes task messages and produces low-level motion commands via path planning | +| [`parsian_communication`](parsian_communication/) | Encodes robot commands into binary packets and transmits them over a serial/radio link | +| [`parsian_msgs`](parsian_msgs/) | Shared ROS message and service definitions used across all packages | +| [`parsian_util`](parsian_util/) | Shared utility library: geometry primitives, action helpers, and math utilities | +| [`parsian_tools`](parsian_tools/) | Miscellaneous tooling and helper nodes | +| [`rqt_parsian_gui`](rqt_parsian_gui/) | rqt-based graphical monitor and operator interface | + +--- + +## Prerequisites + +- **OS**: Ubuntu 16.04 (Xenial) or later +- **ROS**: [ROS Kinetic](http://wiki.ros.org/kinetic/Installation) (or a compatible distro) +- **Build tools**: `catkin_tools` (`catkin build`) +- **System libraries**: + - `libqt4-dev`, `libqjson-dev` + - `protobuf-compiler`, `libprotobuf-dev` + - `libeigen3-dev` + - `curl` +- **ROS packages**: `dynamic-reconfigure`, `nodelet`, `roslint`, `rqt-gui-cpp`, `rqt-gui` + +--- + +## Installation + +A bootstrapping script handles cloning all required repositories and installing dependencies: + +```bash sh -c "$(curl -fsSL https://gist.githubusercontent.com/mahi97/60295c82e21215701d42d4c1e679ac1f/raw/66662032274aa55888099138884748cd2a47f092/install.sh clone)" ``` +> **Note:** Review the script before running it. It will clone this repository into a catkin workspace and install the necessary system and ROS dependencies. + +--- + +## Building + +After installation, build the entire workspace with: + +```bash +cd ~/catkin_ws # or wherever the workspace was created +catkin build +source devel/setup.bash +``` + +--- + +## Usage + +Launch the full stack (vision receiver → world model → AI → agent → communication): + +```bash +# Start the protobuf wrappers (SSL Vision, Referee, grSim) +roslaunch parsian_protobuf_wrapper protos.launch + +# Start the world model +roslaunch parsian_world_model parsian_world_model.launch + +# Start the AI +roslaunch parsian_ai ai.launch + +# Open the operator GUI +rqt --perspective-file $(rospack find rqt_parsian_gui)/perspectives/Main.perspective +``` + +For simulation with **grSim**, make sure grSim is running and listening on the default ports before launching the wrappers. + +--- + +## Contributing + +Please read [CONTRIBUTING.md](CONTRIBUTING.md) for guidelines on naming conventions, coding style, and how to submit changes. + +--- + +## License + +This project is licensed under the **GNU Lesser General Public License v3.0**. See [LICENSE](LICENSE) for the full text. + +--- + +## Acknowledgements + +Parsian SSL is built and maintained by the Parsian Robotic Lab team at Amirkabir University of Technology (Tehran Polytechnic). Core contributors include: + +- Mohammad Mahdi Rahimi +- Ali Gavahi +- Mohammad Mahdi Shirazi +- Kian Behzad +- Hamidreza Roodabeh +- Fateme Hashemi +- Nadia Moradi + +See [CHANGELOG](CHANGELOG) for a full history of contributions. + + diff --git a/parsian_agent/include/parsian_agent/kick.h b/parsian_agent/include/parsian_agent/kick.h index 695723e3..6b0053d8 100644 --- a/parsian_agent/include/parsian_agent/kick.h +++ b/parsian_agent/include/parsian_agent/kick.h @@ -18,8 +18,6 @@ enum class KMode { DONTKICK = 4, JTurn = 5, TurnForKick = 6 - - }; class CSkillKick : public CSkill, public KickAction { diff --git a/parsian_agent/include/planner/planner.h b/parsian_agent/include/planner/planner.h index eb091e5b..38aa928f 100644 --- a/parsian_agent/include/planner/planner.h +++ b/parsian_agent/include/planner/planner.h @@ -8,7 +8,7 @@ #include #include #include -#include "parsian_util/tools/drawer.h" //TODO must be new in node +#include "parsian_util/tools/drawer.h" // TODO must be new in node #include "parsian_util/tools/blackboard.h" #include "parsian_util/core/agent.h" #include "parsian_util/geom/geom.h" @@ -21,7 +21,7 @@ #define _PLANNER_EXTEND_POINT_LIMIT 150 #define _PLANNER_EXTEND_MAX_ATTEMPT 200 -#define _MAX_NUM_PLAYERS 12 //TODO must be get from somewhere else +#define _MAX_NUM_PLAYERS 12 // TODO must be get from somewhere else diff --git a/parsian_agent/src/auto_decider/auto_decider_nodelet.cpp b/parsian_agent/src/auto_decider/auto_decider_nodelet.cpp index 5cb0cd8b..bdcdd852 100755 --- a/parsian_agent/src/auto_decider/auto_decider_nodelet.cpp +++ b/parsian_agent/src/auto_decider/auto_decider_nodelet.cpp @@ -5,74 +5,104 @@ #include #include #include -#include +#include #include #include +#include using namespace rcsc; # define buffer_size 200 # define threshold 0.25 +#define _MAX_NUM_PLAYERS 12 +#define status_timeout 10000 + +// about this node +//this node detects robots faults based on wm and robot_status data, +//and publishes the result on /autofualt topic +//the result contains a list of robot_fualt message for all robots +//if the robots dont send their status for more than 'status_timeout' ms +//the node decides to not to detect faults for any robots -//TODOS -//1)select what state the damaged robot is -//2)what kick(or what ever)fault = true means (dameged or not) -//3)whats the best number f0or threshold and buffersize namespace auto_decider { + + struct RobotInfo{ + parsian_msgs::parsian_robot_status status; + bool ballIsNear; + QList fault; + }; + class Decider : public nodelet::Nodelet { public: - parsian_msgs::parsian_robot_faultPtr res; + parsian_msgs::parsian_robots_faultPtr robotsFault; ros::Publisher pub; ros::Subscriber robo_sub,wm_sub; - bool shootSensors[12]; - bool isNear[12]; - QList faults[12]; + QList robotInfos; + QTime timer; private: virtual void onInit() { ros::NodeHandle &private_nh = getPrivateNodeHandle(); ros::NodeHandle &nh = getNodeHandle(); - pub = private_nh.advertise("/autofault", 5); + pub = private_nh.advertise("/autofault", 5); robo_sub = nh.subscribe("/robots_status", 100, &Decider::statusCb, this); wm_sub = nh.subscribe("/world_model", 100, &Decider::wmCb, this); + for(int i{}; i < _MAX_NUM_PLAYERS; i++) + { + RobotInfo tmp; + robotInfos.push_back(tmp); + } } void statusCb(const parsian_msgs::parsian_robots_status msg) { - for (int i = 0; i < 12; i++) - shootSensors[i] = msg.status[i].shootSensor; + timer.start(); + for(int i{}; i < robotInfos.size(); i++) + { + robotInfos[i].status = parsian_msgs::parsian_robot_status(); + robotInfos[i].status.id = i; + } + for (auto stat: msg.status) + robotInfos[stat.id].status = stat; } void wmCb(const parsian_msgs::parsian_world_model msg) { - for (int i = 0; i < 12; i++) - isNear[i] = Vector2D(msg.our[i].pos).dist(msg.ball.pos) < threshold; + for (auto robotinfo: robotInfos) + robotinfo.ballIsNear = Vector2D(msg.our[robotinfo.status.id].pos).dist(msg.ball.pos) < threshold; faultdetect(); } void faultdetect() { + robotsFault.reset(new parsian_msgs::parsian_robots_fault); + for (int i = 0; i < robotInfos.size(); i++){ + parsian_msgs::parsian_robot_faultPtr tmp; + tmp.reset(new parsian_msgs::parsian_robot_fault); + robotsFault->robots.push_back(*tmp); + } + for (auto robotinfo: robotInfos) { + parsian_msgs::parsian_robot_faultPtr tmp; + tmp.reset(new parsian_msgs::parsian_robot_fault); + tmp->robot_id = robotinfo.status.id; + robotinfo.fault.append(!robotinfo.ballIsNear && robotinfo.status.shootSensor); - for (int i = 0; i < 12; i++) { - res.reset(new parsian_msgs::parsian_robot_fault); - res->robot_id=i; - faults[i].append(!isNear[i] && shootSensors[i]); - - if (faults[i].size() > buffer_size) - faults[i].removeFirst(); + if (robotinfo.fault.size() > buffer_size) + robotinfo.fault.removeFirst(); int sum = 0; - for (auto fault : faults[i]) + for (auto fault : robotinfo.fault) sum+= fault; - if(sum > faults[i].size() * .7) - res->select = 2; + if(sum > robotinfo.fault.size() * .7 && timer.elapsed() < status_timeout) + tmp->select = 2; else - res->select = 0; + tmp->select = 0; - PDEBUG(QString("faults %1 = ").arg(i).toStdString(),sum,D_ALI); - PDEBUG(QString("select %2 = ").arg(i).toStdString(),res->select,D_ALI); - pub.publish(res); + //PDEBUG(QString("faults %1 = ").arg(i).toStdString(),sum,D_ALI); + //PDEBUG(QString("select %2 = ").arg(i).toStdString(),tmp->select,D_ALI); + robotsFault->robots[tmp->robot_id] = *tmp; } + pub.publish(robotsFault); } }; diff --git a/parsian_agent/src/parsian_agent/.kick.cpp.swp b/parsian_agent/src/parsian_agent/.kick.cpp.swp new file mode 100644 index 00000000..0c68d39c Binary files /dev/null and b/parsian_agent/src/parsian_agent/.kick.cpp.swp differ diff --git a/parsian_agent/src/parsian_agent/gotopointavoid.cpp b/parsian_agent/src/parsian_agent/gotopointavoid.cpp index e1f8ff2a..db425a5e 100644 --- a/parsian_agent/src/parsian_agent/gotopointavoid.cpp +++ b/parsian_agent/src/parsian_agent/gotopointavoid.cpp @@ -32,12 +32,16 @@ void CSkillGotoPointAvoid::execute() { bangBang->setDecMax(conf->DecMax); bangBang->setOneTouch(oneTouchMode); bangBang->setDiveMode(diveMode); + double effectiveVelMax = conf->VelMax; + if (maxVelocity > 0.0f) { + effectiveVelMax = min(effectiveVelMax, static_cast(maxVelocity)); + } if (slowMode) { bangBang->setVelMax(1.4); bangBang->setSlow(true); } else { bangBang->setSlow(false); - bangBang->setVelMax(conf->VelMax); + bangBang->setVelMax(effectiveVelMax); } if (!Vector2D(targetPos).valid()) { @@ -89,9 +93,20 @@ void CSkillGotoPointAvoid::execute() { if (!noAvoid) { /*********** PLANNER ***************/ + ourRelaxList.clear(); + for (auto id : ourrelax) { + ourRelaxList.append(static_cast(id)); + } + oppRelaxList.clear(); + for (auto id : theirrelax) { + oppRelaxList.append(static_cast(id)); + } agent->initPlanner(targetPos , ourRelaxList , oppRelaxList , avoidPenaltyArea , avoidCenterCircle , ballObstacleRadius); - for (long i = agent->pathPlannerResult.size() - 1 ; i >= 0 ; i--) { - result.append(agent->pathPlannerResult[i]); + auto plannerSize = agent->pathPlannerResult.size(); + if (plannerSize > 0) { + for (long i = static_cast(plannerSize) - 1; i >= 0; --i) { + result.append(agent->pathPlannerResult[static_cast(i)]); + } } } diff --git a/parsian_agent/src/parsian_agent/kick.cpp b/parsian_agent/src/parsian_agent/kick.cpp index 90050c68..96e93146 100644 --- a/parsian_agent/src/parsian_agent/kick.cpp +++ b/parsian_agent/src/parsian_agent/kick.cpp @@ -129,7 +129,7 @@ void CSkillKick::jTurn() { AngleDeg kickFinalDir = (target - wm->ball->pos).th(); double movementDir = ((wm->ball->pos - agent->pos()).th() - kickFinalDir).degree(); double shift = 0; - double distCoef = 0.15; + double distCoef = 0.10; Vector2D idealPass = (wm->ball->pos - agent->pos()).norm() * distCoef; @@ -178,7 +178,7 @@ void CSkillKick::jTurn() { dirReduce -= 1; } - speedPid->kp = 6 + 4 * agent->pos().dist(wm->ball->pos) + dirReduce*2 ; + speedPid->kp = 5 + 4 * agent->pos().dist(wm->ball->pos) + dirReduce*2 ; if (penaltyKick) { speedPid->kp = 4; @@ -321,12 +321,14 @@ void CSkillKick::direct() { distThr = 0; finalPos = wm->ball->pos - (target - finalPos).norm() * 0.15; finalDir = Vector2D(cos(kickFinalDir.radian()), sin(kickFinalDir.radian())); + } Vector2D temp = finalPos; - CSkillReceivePass::validatePoint(finalPos, agent->pos()); + //CSkillReceivePass::validatePoint(finalPos, agent->pos()); if (temp != finalPos) finalDir = wm->ball->pos - finalPos; + Vector2D s1, s2; Circle2D finalPosArea; Segment2D directPath(agent->pos(), finalPos); @@ -344,7 +346,6 @@ void CSkillKick::direct() { } drawer->draw(Segment2D(agent->pos(), finalPos), QColor(Qt::red)); - drawer->draw(finalPos); gpa->init(finalPos, finalDir); gpa->setNoavoid(false); @@ -352,7 +353,6 @@ void CSkillKick::direct() { gpa->setBallobstacleradius(0); gpa->setSlowmode(slow); gpa->setDivemode(false); - gpa->setAvoidpenaltyarea(true); gpa->execute(); } diff --git a/parsian_agent/src/parsian_agent/receivepass.cpp b/parsian_agent/src/parsian_agent/receivepass.cpp index 2596e75b..cb233201 100644 --- a/parsian_agent/src/parsian_agent/receivepass.cpp +++ b/parsian_agent/src/parsian_agent/receivepass.cpp @@ -64,18 +64,22 @@ void CSkillReceivePass::waitPos() { void CSkillReceivePass::intersect() { Vector2D bestPoint = bestPointToIntersect(); + drawer -> draw(bestPoint,QColor(Qt::black), 0.07); Segment2D ballPath(wm->ball->pos, wm->ball->pos + wm->ball->vel.norm() * 20); if (!bestPoint.valid() || Circle2D(agent->pos(), 0.15).intersection(Segment2D(wm->ball->pos, wm->ball->getPosInFuture(0.5)))) { bestPoint = ballPath.nearestPoint(agent->pos()); + drawer -> draw(bestPoint,QColor(Qt::blue), 0.07); + } - validatePoint(bestPoint); + //validatePoint(bestPoint); agent->setRoller(1); gotopointavoid->setOnetouchmode(false); if(agent->pos().dist(bestPoint) < 0.5) gotopointavoid->setOnetouchmode(true); gotopointavoid->init(bestPoint, wm->ball->pos - bestPoint); gotopointavoid->setSlowmode(false); - drawer -> draw(bestPoint,QColor(Qt::red)); + + drawer -> draw(bestPoint,QColor(Qt::red), 0.07); } void CSkillReceivePass::receive() { @@ -91,7 +95,7 @@ void CSkillReceivePass::validatePoint(Vector2D& _point) { } Vector2D CSkillReceivePass::bestPointToIntersect() { - bestPointToIntersect(agent); + return bestPointToIntersect(agent); } void CSkillReceivePass::validatePointFromPenalty(Vector2D &_point, const Rect2D& _penalty) { diff --git a/parsian_ai/CMakeLists.txt b/parsian_ai/CMakeLists.txt index d712b388..858257d4 100644 --- a/parsian_ai/CMakeLists.txt +++ b/parsian_ai/CMakeLists.txt @@ -108,6 +108,7 @@ add_library(${PROJECT_NAME} src/${PROJECT_NAME}/plays/ourpenaltyshootout.cpp src/${PROJECT_NAME}/plays/stopplay.cpp src/${PROJECT_NAME}/plays/halftimelineup.cpp + src/${PROJECT_NAME}/plays/substitution.cpp src/${PROJECT_NAME}/plans/defenseplan.cpp src/${PROJECT_NAME}/plans/markplan.cpp src/${PROJECT_NAME}/plans/plan.cpp @@ -221,7 +222,9 @@ install(PROGRAMS ) ## Mark executables and/or libraries for installation -install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_nodelet ${PROJECT_NAME}_node +install(TARGETS ${PROJECT_NAME} + ${PROJECT_NAME}_nodelet + ${PROJECT_NAME}_node ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} diff --git a/parsian_ai/cfg/ai.cfg b/parsian_ai/cfg/ai.cfg index 1189f6b5..7544744b 100755 --- a/parsian_ai/cfg/ai.cfg +++ b/parsian_ai/cfg/ai.cfg @@ -19,11 +19,13 @@ formation.add("GoalieFromGUI", bool_t, 0, "GoalieFromGUI", False) formation.add("Goalie", int_t, 0, "Goalie ID", 11, 0, 11) formation.add("Defense", int_t, 0, "Defense Count", 2, 0, 11) formation.add("ThreeDefenseMode", bool_t, 0, "Three Defense Mode", False) +formation.add("PenaltyKickerFromGUI", bool_t, 0, "PenaltyKickerFromGUI", False) +formation.add("PenaltyKicker", int_t, 0, "PenaltyKicker ID", 11, 0, 11) -defense = generator.add_group("Defense", state = True, type = 'tab') -defense.add("NoClear", bool_t, 0, "NoClear", False) -defense.add("DownLimit", double_t, 0, "DownLimit",1.786056275,0.0,2.0) -defense.add("UpLimit", double_t, 0, "UpLimit", 3, 0.0, 4.5) +defenses = generator.add_group("Defenses", state = True, type = 'tab') +defenses.add("NoClear", bool_t, 0, "NoClear", False) +defenses.add("DownLimit", double_t, 0, "DownLimit",1.786056275,0.0,2.0) +defenses.add("UpLimit", double_t, 0, "UpLimit", 3, 0.0, 4.5) playoff = generator.add_group("PlayOff", state = True, type = 'tab') playoff.add("IDBasePasser", bool_t, 0, "ID Base Passer", False) @@ -35,6 +37,11 @@ playoff.add("UseFirstPlay", bool_t, 0, "Use First Play", False) playoff.add("UseBlockBlocker", bool_t, 0, "Use Blocker Block", False) playoff.add("UseForcedBlock", bool_t, 0, "Forced Blocker Block", False) playoff.add("MaxOnetouchAngle", int_t, 0, "Max Onetouch Angle (degree)", 70, 0, 180) +playoff.add("UseKhafan", bool_t, 0, "Use Khafan", True) +playoff.add("StaticTimeOver", double_t, 0, "Static Time Over(per pass)", 4.5, 0, 10) +playoff.add("StaticFirstKickFailed", double_t, 0, "Static First Kick Failed threshold radius", 0.6, 0, 10) +playoff.add("StaticBallDirChanged", double_t, 0, "Static Ball Dir Changed(Receiver circle radius)", 1, 0, 10) +playoff.add("khafandist", double_t, 0, " khafan dist", 0.3, 0, 2) dynamicplay = generator.add_group("DynamicPlay", state = True, type = 'tab') dynamicplay.add("LowSpeedPass", double_t, 0, "Low Speed Pass", 1, 0, 10) @@ -92,6 +99,7 @@ coach.add("playMakeStopThr", double_t, 0, "Threshold Plamake for stopped ball", coach.add("playMakeMoveThr", double_t, 0, "Threshold Plamake for moving ball", 0.2, 0, 2) coach.add("playMakeIntention", int_t, 0, "Intention of Plamake for moving ball", 500, 0, 2000) coach.add("penaltyMargin", double_t, 0, "margin for pushing penalty area", 0.3, 0, 1) +coach.add("parsianWorkshop", bool_t, 0, "parsian workshop", False) badkickers = generator.add_group("badkickers", state = True, type = 'tab') diff --git a/parsian_ai/include/parsian_ai/ai.h b/parsian_ai/include/parsian_ai/ai.h index 2a6bf2b8..77b86e87 100644 --- a/parsian_ai/include/parsian_ai/ai.h +++ b/parsian_ai/include/parsian_ai/ai.h @@ -15,7 +15,7 @@ #include #include #include -#include +#include @@ -28,7 +28,7 @@ class AI { void updateWM(const parsian_msgs::parsian_world_modelConstPtr&); void updateRobotStatus(const parsian_msgs::parsian_robotConstPtr&); - void updateRobotFaults(const parsian_msgs::parsian_robot_fault &); + void updateRobotSubstitutes(const parsian_msgs::parsian_robot_substitution &); void updateReferee(const parsian_msgs::ssl_refree_wrapperConstPtr&); void forceUpdateReferee(const parsian_msgs::ssl_force_refereeConstPtr & _command); CSoccer* getSoccer(); @@ -37,6 +37,7 @@ class AI { private: parsian_msgs::parsian_robot_task robotsTask[_MAX_NUM_PLAYERS]; + void validateRobotTask(parsian_msgs::parsian_robot_task* task); }; diff --git a/parsian_ai/include/parsian_ai/ai_nodelet.h b/parsian_ai/include/parsian_ai/ai_nodelet.h index acaf6bfb..d3ad26b8 100644 --- a/parsian_ai/include/parsian_ai/ai_nodelet.h +++ b/parsian_ai/include/parsian_ai/ai_nodelet.h @@ -17,6 +17,7 @@ #include #include #include +#include #include #include @@ -29,7 +30,7 @@ class AINodelet : public nodelet::Nodelet { private: boost::shared_ptr ai; - ros::Subscriber worldModelSub, robotStatusSub, refereeSub, teamConfSub, mousePosSub,forceRefereeSub, robotfaultSub; + ros::Subscriber worldModelSub, robotStatusSub, refereeSub, teamConfSub, mousePosSub,forceRefereeSub, robotSubstituteSub; ros::Publisher drawPub; ros::Publisher *robTask; @@ -49,7 +50,7 @@ class AINodelet : public nodelet::Nodelet { void robotStatusCallBack(const parsian_msgs::parsian_robotConstPtr & _rs); void teamConfCb(const parsian_msgs::parsian_team_configConstPtr& _conf); void mousePosCb(const parsian_msgs::vector2DConstPtr& _mousePos); - void faultdetectionCallBack(const parsian_msgs::parsian_robot_fault & _rs); + void substitutedetectionCallBack(const parsian_msgs::parsian_robot_substitution &_rs); }; } diff --git a/parsian_ai/include/parsian_ai/coach.h b/parsian_ai/include/parsian_ai/coach.h index 35663a6d..4caed00e 100644 --- a/parsian_ai/include/parsian_ai/coach.h +++ b/parsian_ai/include/parsian_ai/coach.h @@ -78,8 +78,9 @@ class CCoach { static int findGoalie(); static bool useGoalieInPlayOff(); - void generateWorkingRobotIds(); - QList workingIDs; + void seperateHealthyAndDamagedRobots(); + QList healthyIDs; + QList damagedIDs; void replaceFaultedRobots(); CRoleFault *faultRoles[_MAX_NUM_PLAYERS]; void resetNonVisibleAgents(); @@ -115,6 +116,7 @@ class CCoach { CDynamicAttack *dynamicAttack; CStopPlay *stopPlay; CHalftimeLineup *halftimeLineup; + CSubstitution *substitution; public: CRoleStop *stopRoles[_MAX_NUM_PLAYERS]; @@ -129,6 +131,7 @@ class CCoach { QList robotsIdHist; bool first; QList missMatchIds; + bool firsttime_forsubstitution; /////////////////////////////////////// int cyclesWaitAfterballMoved; @@ -224,7 +227,6 @@ class CCoach { int faultDetectionCounter[_MAX_NUM_PLAYERS]; double kickTimeEstimation(Agent * _agent, const Vector2D& target); - double timeNeeded(Agent *_agentT,const Vector2D& posT, double vMax); NoAction* haltAction; diff --git a/parsian_ai/include/parsian_ai/gamestate.h b/parsian_ai/include/parsian_ai/gamestate.h index 1e0ec96f..670a2f34 100644 --- a/parsian_ai/include/parsian_ai/gamestate.h +++ b/parsian_ai/include/parsian_ai/gamestate.h @@ -55,11 +55,13 @@ class GameState { GameState(); - bool ready(); void setRefree(ssl_refree_wrapperConstPtr ref_wrapper); void setForceRefree(ssl_force_refereeConstPtr command); void updateCommand(ssl_refree_command command); + + bool isStart(); + bool ready(); bool isStop(); bool playOffKick(); bool ourPlayOffKick(); diff --git a/parsian_ai/include/parsian_ai/plans/defenseplan.h b/parsian_ai/include/parsian_ai/plans/defenseplan.h index 3b2b1c3f..ee844afa 100644 --- a/parsian_ai/include/parsian_ai/plans/defenseplan.h +++ b/parsian_ai/include/parsian_ai/plans/defenseplan.h @@ -14,15 +14,34 @@ #include #include #include - +#include "parsian_util/tools/blackboard.h" +#include "parsian_util/geom/polygon_2d.h" #define LOOP_TIME_BYKK 0.016 #define MIN_TWO_ROBOTS_DIST 0.02 #define MIN_MORE_ROBOTS_DIST 0.05 -struct velAndAccByKK { - double vel; - double acc; + +enum class shootOutMode { + beforeTouch, + shootOutClear, + ballBisector, + skyDive + +}; + +enum class GKState{ + GKReciveBallInTS, + GKPredictInTs, + playoff, + Stop, + ballIsOutOfField, + ballIsBesidePoles, + clearMode, + clearSlowBall, + oneTouch, + dangerForClear, + strictFollow }; -enum { OneTouchState , ClearState , NoState }; + class DefensePlan : public Plan { protected: @@ -31,18 +50,13 @@ class DefensePlan : public Plan { GotopointAction* gps[_MAX_NUM_PLAYERS]; GotopointavoidAction *gpa[_MAX_NUM_PLAYERS]; KickAction* kickSkill; - NoAction* noSkill; Action* AHZSkills; - CDefPos defPos; Vector2D pointForKick, oneToucherDir; - Vector2D goalKeeperTarget,lastTarget, goalieDirection , defensePoints[12], defenseTargets[12]; + Vector2D goalKeeperTarget , defensePoints[12], defenseTargets[12]; void setPointToKick(); - void setGoalKeeperState(); - void setGoalKeeperTargetPoint(); - bool goalKeeperOneTouch, goalKeeperClearMode, ballIsOutOfField, ballIsBesidePoles; - double strictfollowThr; + GKState setGoalKeeperState(); + Vector2D setGoalKeeperTargetPoint(GKState); double differentialTime = 0; - bool dangerForGoalKeeperClear; int oneTouchCnt; ////////////////////////////// AHZ /////////////////// Line2D getBisectorLine(Vector2D firstPoint , Vector2D originPoint , Vector2D secondPoint); @@ -74,80 +88,35 @@ class DefensePlan : public Plan { Vector2D oneDefenseFormationForRecatngularPositioning(double downLimit , double upLimit); Vector2D oneDefenseFormationForCircularPositioning(double downLimit , double upLimit); Vector2D getGKPositionInOneDefense(Vector2D firstPoint , Vector2D originPoint , Vector2D secondPoint , double downLimit , double upLimit); - Vector2D getGKPositionInThreeDefense(Vector2D firstPoint , Vector2D originPoint , Vector2D secondPoint , double downLimit , double upLimit); Vector2D getGKPositionWithoutDefense(double downLimit , double upLimit); Vector2D getGKPositionAccordingToTheDefense(int numberOfDefenders , Vector2D firstPoint , Vector2D originPoint , Vector2D secondPoint); - Vector2D getGKPositionInTwoDefense(Vector2D firstPoint , Vector2D originPoint , Vector2D secondPoint , double downLimit , double upLimit); - Segment2D getBestSegmentWithTallesForGK(int defenseCount , Vector2D firstPoint , Vector2D originPoint , Vector2D secondPoint); + Vector2D getGKPositionInMoreThanTwoDefense(Vector2D firstPoint , Vector2D originPoint , Vector2D secondPoint , double downLimit , double upLimit); Line2D getBestLineWithTallesForGK(int defenseCount , Vector2D firstPoint , Vector2D originPoint , Vector2D secondPoint); QList defenseFormation(QList circularPositions, QList rectangularPositions); - double timeNeeded(Agent *_agentT, Vector2D posT, double vMax, QList _ourRelax, QList _oppRelax , bool avoidPenalty, double ballObstacleReduce, bool _noAvoid); double findBestRadiusForGK(Line2D bestLineWithTalles ,Vector2D firstPoint , Vector2D originPoint , Vector2D secondPoint , double downLimit , double upLimit); /// \brief angleDegreeThrNotStop Vector2D lastTargetForStrictFollow; double AHZDegThreshOld = 0; - int angleDegreeThrNotStop = 0; - double besideCounter; - int lastOpponentAgentsToBeMarkSize; - double threshOld = 0.0; double ballCircleR = 0.5; double xLimitForblockingPass; double suitableRadius; - bool isPermissionToKick; - bool isCrowdedInFrontOfPenaltyAreaByOurAgents; - bool isCrowdedInFrontOfPenaltyAreaByOppAgents; - bool ballISInpenaltyAreaAndDangerCircle; - bool ballIsNotInPenaltyAreaAndIsInDangerCircle; - bool ballIsInPenaltyAreaAndIsNotInDangerCircle; - bool dangerForGoalKeeperClearByOurAgents; - bool dangerForGoalKeeperClearByOppAgents; - bool dangerForInsideOfThePenaltyArea; - bool stopMode; - bool playOffMode; - bool playOnMode; - bool dangerModeThresholdForClear; - bool dangerModeThresholdForDanger; bool manToManMarkBlockPassFlag; - bool goalKeeperPredictionModeInPlayOff; - bool GKReciveBallInTS; - bool ballIntersectOurPenaltyArea; QList markRoles; QList lastMarkRoles; - Vector2D opponentPasserDirection; - Vector2D tempBallRectanglePoint; - Vector2D oppNearestToBallPossition; - Vector2D tempAHZ; QString lastStateForGoalKeeper; QList AHZDefPoints; /////////////////////////////////////////////////// - void executeGoalKeeper(); Vector2D strictFollowBall(Vector2D _ballPos); Vector2D avoidCircularPenaltyAreaByMasoud(Agent* agent, const Vector2D& point); int decideNumOfMarks(); - DefPos tempDefPos; void matchingDefPos(int _defenseNum); bool defenseOneTouchOrNot(); - bool agentEffectOnBallProbability(Vector2D ballPos, Vector2D ballVel, Vector2D agentPos, Vector2D agentVel, bool isTowardOurgoal); - Vector2D getGoalieShootOutTarget(bool isSkyDive); - bool canReachToBall(int agentId, int theirAgentId); - int decideShootOutMode(); - QList lastBallPos; - int penaltyShootoutMode = beforeTouch; - void penaltyShootOutMode(); - GotopointavoidAction* striker_Robot; - void penaltyMode(); enum exepMode { defOneTouch = 1, defClear = 2, NoneExep = 3 }; - enum shootOutMode { - beforeTouch, - shootOutClear, - ballBisector, - skyDive - }; bool shootOutClearModeSelected = false; bool agentEffectOnBallProbabilityRes; @@ -158,9 +127,7 @@ class DefensePlan : public Plan { exepMode exeptionMode; int exepAgentId; }; - Vector2D ballPrediction(bool _isGoalie); - Vector2D lastBallPosition; public: DefensePlan(); void execute() override; @@ -172,20 +139,29 @@ class DefensePlan : public Plan { //////////////////HMD///////////////// QList markPoses; QList markAngs; - double markRadius; double segmentpershoot; double segmentperpass; - bool MantoManAllTransientFlag; Vector2D dir; /// ALI GAVAHI bool ballIsBounced; Vector2D ballBouncePos, playOffStartBallPos, playOffPassDir,beforeTransientPassDir; /////////////////////////////////// - bool DangerOppLessThan(const int &, const int &); - - private: + ///////////////////////Lhum checked them////////////// + void drawGameState(); + void penaltyMode(); + bool canReachToBall(const int& agentId, const int& theirAgentId); + void penaltyShootOutMode(); + Vector2D getGoalieShootOutTarget(bool isSkyDive); + bool agentEffectOnBallProbability(const Vector2D& agentPos); + shootOutMode decideShootOutMode(); + bool dangerForGK(); + Vector2D movePointToPenaltyArea(const Vector2D&); + Vector2D ballIsBesidePoles(); + void executeGoalKeeper(const Vector2D& , const GKState&); + int stateBallBesidepoles; ///////////////////////HMD/////////////// + Vector2D ballPrediction(const bool); void findPos(int _markAgentSize); void findOppAgentsToMark(); bool isInTheIndirectAreaShoot(Vector2D); @@ -194,7 +170,6 @@ class DefensePlan : public Plan { QList PassBlockRatio(double, Vector2D); QList indirectAvoidShoot(Vector2D); QList indirectAvoidPass(Vector2D); - int numberOfMarkers; QList oppAgentsToMarkPos; QList oppmarkedpos; QList oppAgentsToMark; @@ -209,76 +184,30 @@ class DefensePlan : public Plan { rcsc::Vector2D* getIntersectWithDefenseArea(const Segment2D& segment, const Vector2D& blockPoint); rcsc::Vector2D* getIntersectWithDefenseArea(const Circle2D& circle, bool upperPoint); void assignSkill(Agent *_agent , Action *_skill); - void initVars(float goalCircleRad = 0.9); // default is 0.8 - bool defenderForMark; - bool doubleMarking; - bool isDefenseFastest; - bool clearflag; Agent *goalKeeperAgent; QList defenseAgents; - int oneDefUpOrDown; - int twoDefCurState; - int lastStateOffPlay; int lastMarker[10]; - int markPointNum; int oneToucher; Vector2D defenseDirs[_MAX_NUM_PLAYERS]; - Vector2D ballPos; bool doOneTouch; - bool doClear; - int lastClearID; - int defenseClearIndex; - double lastClearDist; - int clearFrameCnt; - int lastTouchTheGoalie; - bool distClearHysteresis; - int lastOneTouchClearState; - int histOneTouchClearCnt; - double GOTThresh; - int GOTCounter; double thr; - double noDefThr; - QList ballPosHistory; void calcPointForOneTouch(); bool isInOneTouch; - bool isOnetouch; int oneTouchCycleTest; bool checkStillBeingInOneTouch(); int cycleCounter; - Vector2D oneTouchPoint[2]; bool oneTouchPointFlag; - bool oneTouchPointFlagG; - bool doBlockPass; - double timeToReach; - Vector2D blockPassPoint; - QList dangerousOpp; - double goalieAreaHis; - Vector2D goalieTargetDir; - int isBallGoingToOppAreaCnt; - double pushBallHist; - int failureAtempCnt; - int clearCnt; - double savedClearDist; - int goaliePassBlockCnt; - Vector2D gBassBlockTargetSave; - double predictThresh; - bool inPenaltyAreaFlag; int predictMostDangrousOppToBall(); Vector2D NearestDistanceToBallSegment(Vector2D point); - DefPos defPosDecision; defenseExeptions defExceptions; void checkDefenseExeptions(); void runDefenseExeptions(); Vector2D runDefenseOneTouch(); - double defClearThr; bool defenseCheckBallDangerForOneTouch(); - bool defClearFlag; - double overDefThr; - bool FlagBesidePoles; - int f = 0 , counterBallWasBesidePoles = 0; - bool firstTimeGoalKeeperOneTouch = false; - Vector2D oneTouchDir; - Vector2D playoffMarkPredictPos; -}; + int counterBallWasBesidePoles = 0; + +};//tavabei ke vabaste be vorodi and static she +// const & +// moteghayer ha kam she #endif // DEFENSE_H diff --git a/parsian_ai/include/parsian_ai/plays/dynamicattack.h b/parsian_ai/include/parsian_ai/plays/dynamicattack.h index e0ecde3b..d9fc4ec1 100644 --- a/parsian_ai/include/parsian_ai/plays/dynamicattack.h +++ b/parsian_ai/include/parsian_ai/plays/dynamicattack.h @@ -1,6 +1,7 @@ #ifndef DYNAMICATTACK_H #define DYNAMICATTACK_H +#include "algorithm" #include #include @@ -18,12 +19,29 @@ enum class DynamicAttackState { PositioningControl = 2 }; -struct FieldRegion +class FieldRegion { +public: Rect2D rectangle; QList points; int id; + double angle; + double theirNearestRobot; + double chance; + + int oppInside; + int ourInside; + int oppInNeighbor; + int pointPriority; + + bool goodForOneTouch; + //bool chipOrPass; // 1 for pass and 0 for chip + bool passIsForMe; //when the point is getting pass + //Vector2D passReciever; + + + FieldRegion(){}; FieldRegion(Rect2D r, QList p) @@ -32,6 +50,38 @@ struct FieldRegion for(auto& point : p) points.push_back(point); } + + bool operator< (const FieldRegion& a) { + return this->pointPriority < a.pointPriority; + } +}; + + +class passPoint +{ +public: + passPoint(vector2D); + passPoint(); + vector2D point; + bool amIReciever; + bool stay; + bool inPostion; + bool oneTouch; + bool chipOrPass; //0 for pass. 1 for chip. + int region; + bool finalPassReciever; + int chance; + int ID; + bool operator< (passPoint& a){ return (this->chance < a.chance);} + +}; + + +class RecievePoint{ +public: + RecievePoint(); + int ID; + Vector2D point; }; @@ -143,6 +193,10 @@ class CDynamicAttack : public CMasterPlay { SDynamicPlan currentPlan; + + CRobot *findOppGoalKeaper(); + + private: // NEW PASS ZONE static const int REGION_NUM; @@ -174,12 +228,14 @@ class CDynamicAttack : public CMasterPlay { bool shotInPass; void playMake(); - void positioning(QList _points); + void positioning(QList& );//QList _points); void globalExecute(int agentSize); void dynamicPlanner(int agentSize); void makePlan(int agentSize); void assignId(); + void passPositions(const QList&, MWBM&); + void passDecision(); void assignTasks(); void updateAttackState(); bool passDone(); @@ -194,12 +250,19 @@ class CDynamicAttack : public CMasterPlay { double angleOfTwoSegment(const Segment2D &xp, const Segment2D &yp); double findmax(const QList &list); + QList passPoints; + QList finalPassPoints; QList semiDynamicPosition; + QList amirSemiDynamicPosition; QList markPositions; + RecievePoint recievePoint; + bool isRightTimeToPass(); void chooseReceiverAndBestPosForPass(); void chooseBestPositons(); + void regionByBall(int); + void oppInregion(); double getDynamicValue(const Vector2D& _dynamicPos) const; void checkPoints(QList& _points); @@ -210,6 +273,10 @@ class CDynamicAttack : public CMasterPlay { bool isPathClear(Vector2D _pos1, Vector2D _pos2, double rad, double t); bool isPathClearFromOpp(Vector2D _pos1, Vector2D _pos2, double rad, double t); + + bool isClear(Vector2D _pos1, Vector2D _pos2, double rad, double t, QString str, Vector2D point = Vector2D(0,0)); + + inline bool chipOrNot(Vector2D target, double _radius = 1, double _treshold = .5); int appropriatePassSpeed(); @@ -219,6 +286,24 @@ class CDynamicAttack : public CMasterPlay { QString getString(const DynamicMode& _mode) const; + + //bool isPositionInOurWay(Vector2D _pos1, Vector2D _pos2, double rad, double t, Vector2D); + //bool isPassPathOpen(Vector2D _pos1, Vector2D _pos2, double rad, double t); + //bool isPositionClear(Vector2D _pos1, Vector2D _pos2, double rad, double t); + + void bestPos(const QList&, MWBM&); + void checkPositions(); + void isChipOrPass(QList&); + void findOneTouch(QList&); + void isInPosition(QList& ); + void toPassOrNotToPass(QList&); + void passPriority(QList&); + void showPasser(QList&, MWBM&); + //void stayPassReciever(const QList&, MWBM&); + void stayPassReciever(QList& ); + void finalPassReciever(); + + CRoleDynamic *roleAgents[8]; CRoleDynamic *roleAgentPM; @@ -264,6 +349,8 @@ class CDynamicAttack : public CMasterPlay { bool inTimePlan(); protected: void reset() override; + + double calcRegionProperties(int robot_id,int region_index); }; #endif // DYNAMICATTACK_H diff --git a/parsian_ai/include/parsian_ai/plays/ourpenalty.h b/parsian_ai/include/parsian_ai/plays/ourpenalty.h index dd647f39..d92b5ffd 100644 --- a/parsian_ai/include/parsian_ai/plays/ourpenalty.h +++ b/parsian_ai/include/parsian_ai/plays/ourpenalty.h @@ -6,6 +6,7 @@ #include #include #include +#include enum class PenaltyState{ Positioning, @@ -24,9 +25,9 @@ class COurPenalty : public CMasterPlay { void execute_x(); void init(QList& _agents); void setPlaymake(Agent* _playmakeAgent); - void setState(PenaltyState _state){penaltyState = _state;}; - void executeShootoutPositioning(); + void setState(PenaltyState _state){penaltyState = _state;} void executeNormalPositioning(); + void generatePositions(); Vector2D getEmptyTarget(Vector2D _position, double _radius); void assignSkills(); void playmakePositioning(); @@ -38,12 +39,9 @@ class COurPenalty : public CMasterPlay { private: void reset(); - bool isPenaltyShootOut; - CRolePlayMake* playmakeRole; QList moveSkills; GotopointavoidAction* PMgotopoint; KickAction* PMkick; - void generatePositions(); QList positions; PenaltyState penaltyState; KickState kickState; diff --git a/parsian_ai/include/parsian_ai/plays/playoff/dynamicplayoff.h b/parsian_ai/include/parsian_ai/plays/playoff/dynamicplayoff.h index 434a36e0..50687524 100644 --- a/parsian_ai/include/parsian_ai/plays/playoff/dynamicplayoff.h +++ b/parsian_ai/include/parsian_ai/plays/playoff/dynamicplayoff.h @@ -17,6 +17,7 @@ enum class DynamicSelect { Khafan = 1, Chip = 2, Kick = 3 + }; enum class DynamicState { @@ -37,16 +38,28 @@ class CDynamicPlayOff : public CAbstractPlayOff { private: int dynamicMatch[_NUM_PLAYERS]; DynamicSelect dynamicSelect; + DynamicSelect lastselect; + void dynamicPlayKhafan(); void dynamicPlayChipToGoal(bool isChip); void checkEndKhafan(); void checkEndChipToGoal(); + int EvalPlayKhafan(); + void Setposition(); int dynamicAgentSize; DynamicState state; unsigned int dynamicStartTime; Vector2D dummyPositions[_NUM_PLAYERS]; + Vector2D theirpos; + Polygon2D POLYGON; + float theirdist; + int sum; + int eval; + int n; + + }; diff --git a/parsian_ai/include/parsian_ai/plays/plays.h b/parsian_ai/include/parsian_ai/plays/plays.h index a7db1d17..f1033a04 100644 --- a/parsian_ai/include/parsian_ai/plays/plays.h +++ b/parsian_ai/include/parsian_ai/plays/plays.h @@ -10,6 +10,7 @@ #include "parsian_ai/plays/dynamicattack.h" #include "parsian_ai/plays/ourballplacement.h" #include "parsian_ai/plays/theirballplacement.h" +#include "parsian_ai/plays/substitution.h" #include "parsian_ai/plays/stopplay.h" #include "parsian_ai/plays/halftimelineup.h" #include "parsian_ai/plays/ourpenaltyshootout.h" diff --git a/parsian_ai/include/parsian_ai/plays/substitution.h b/parsian_ai/include/parsian_ai/plays/substitution.h new file mode 100644 index 00000000..18403439 --- /dev/null +++ b/parsian_ai/include/parsian_ai/plays/substitution.h @@ -0,0 +1,19 @@ +#ifndef SUBSTITUTION_H +#define SUBSTITUTION_H + +#include "masterplay.h" + +class CSubstitution : public CMasterPlay{ +public: + CSubstitution(); + ~CSubstitution(); + void execute_x(); + void init(QList& _agents); +private: + void reset(); + QList generatepositions(int count); + QList gpa; + +}; + +#endif // SUBSTITUTION_H diff --git a/parsian_ai/include/parsian_ai/util/agent.h b/parsian_ai/include/parsian_ai/util/agent.h index 9dc4d26e..3bb6fc36 100644 --- a/parsian_ai/include/parsian_ai/util/agent.h +++ b/parsian_ai/include/parsian_ai/util/agent.h @@ -12,17 +12,10 @@ class Agent : public CAgent { explicit Agent(const parsian_msgs::parsian_agent& _agent); explicit Agent(int id); Vector2D oneTouchCheck(Vector2D positioningPos, Vector2D* oneTouchDirection); - enum class FaultState{ - HEALTHY = 0, - DISREPAIRED = 1, - DAMEGED = 2, - DESTROYED = 3, - }; QString roleName; bool changeIsNeeded = false; bool shootSensor = false; - bool fault = false; - FaultState faultstate = FaultState::HEALTHY; + bool substitutePermission = false; protected: private: diff --git a/parsian_ai/include/parsian_ai/util/knowledge.h b/parsian_ai/include/parsian_ai/util/knowledge.h index d0dcff00..11b21a1b 100644 --- a/parsian_ai/include/parsian_ai/util/knowledge.h +++ b/parsian_ai/include/parsian_ai/util/knowledge.h @@ -86,7 +86,7 @@ class Knowledge { QVariantMap variables; NewFastestToBall newFastestToBall(double timeStep, QList ourList, QList oppList, const CWorldModel*& wm); FastestToBall findFastestToBall(QList ourList, QList oppList, const CWorldModel*& wm); - + double timeNeeded(Agent *_agentT,const Vector2D& posT, double vMax); NewFastestToBall newFastestToBall(double timeStep = 0.1, QList ourList = wm->our.data->activeAgents, QList oppList = wm->opp.data->activeAgents); int Matching(QList robots, QList pointsToMatch, QList &matchPoints); @@ -98,7 +98,6 @@ class Knowledge { Vector2D getEmptyPosOnGoalForPenalty(double n, bool oppGoal, double th, Agent* ourAgent = NULL); bool isPointClear(Vector2D point, Vector2D from, double rad = 0.0795, bool considerRelaxedIDs = false, QList ourRelaxedIDs = QList(), QList oppRelaxedIDs = QList()); bool isPointClear(Vector2D point, Vector2D from, double radBig, double radSmall, bool considerRelaxedIDs, QListourRelaxedIDs, QListoppRelaxedIDs, QListourSmallIDs, QListoppSmallIDs); - double chipGoalPropability(bool isOurChip, Vector2D _goaliePos); int getNearestOppToPoint(Vector2D point); int nearestOppToBall(); double chipGoalPropability(bool isOurChip); diff --git a/parsian_ai/plans/jolo.json b/parsian_ai/plans/jolo.json new file mode 100644 index 00000000..2aaa066c --- /dev/null +++ b/parsian_ai/plans/jolo.json @@ -0,0 +1,448 @@ +{ + "apiVersion": 1.2, + "id": 1, + "plans": [ + { + "agentInitPos": [ + { + "x": -100, + "y": -100 + }, + { + "x": 1.0386740331491708, + "y": 3.4874999999999998 + }, + { + "x": 2.2099447513812152, + "y": 3.915 + }, + { + "x": 1.8232044198895032, + "y": 3.8812500000000001 + }, + { + "x": 1.8232044198895032, + "y": 2.0812500000000003 + }, + { + "x": -0.97237569060773499, + "y": 3.0149999999999997 + }, + { + "x": -1.182320441988951, + "y": -3.2962499999999997 + }, + { + "x": -0.95027624309392245, + "y": -2.4524999999999997 + } + ], + "agents": [ + { + "ID": 0, + "positions": [ + { + "angel": 0, + "pos-x": 0.95027624309392245, + "pos-y": -3.6787499999999991, + "skills": [ + { + "flag": false, + "name": "MoveSkill", + "primary": 0, + "secondary": 0 + } + ], + "tolerance": 30 + }, + { + "angel": 0, + "pos-x": 4.6850828729281773, + "pos-y": -3.7125000000000004, + "skills": [ + { + "flag": false, + "name": "MoveSkill", + "primary": 0, + "secondary": 0 + }, + { + "flag": false, + "name": "PassSkill", + "primary": 500, + "secondary": 500, + "target": { + "agent": 1, + "index": 2 + } + } + ], + "tolerance": 30 + } + ] + }, + { + "ID": 1, + "positions": [ + { + "angel": 0, + "pos-x": 1.0386740331491708, + "pos-y": 3.4874999999999998, + "skills": [ + { + "flag": false, + "name": "MoveSkill", + "primary": 0, + "secondary": 0 + } + ], + "tolerance": 30 + }, + { + "angel": 0, + "pos-x": 1.0828729281767959, + "pos-y": -1.7549999999999999, + "skills": [ + { + "flag": false, + "name": "MoveSkill", + "primary": 0, + "secondary": 0 + } + ], + "tolerance": 30 + }, + { + "angel": 0, + "pos-x": 2.6740331491712706, + "pos-y": -1.2487500000000002, + "skills": [ + { + "flag": false, + "name": "MoveSkill", + "primary": 0, + "secondary": 0 + }, + { + "flag": false, + "name": "ShotToGoalSkill", + "primary": 500, + "secondary": 500 + } + ], + "tolerance": 30 + }, + { + "angel": 0, + "pos-x": 1.9889502762430942, + "pos-y": 0.64124999999999988, + "skills": [ + { + "flag": false, + "name": "MoveSkill", + "primary": 0, + "secondary": 0 + } + ], + "tolerance": 30 + } + ] + }, + { + "ID": 2, + "positions": [ + { + "angel": 0, + "pos-x": 2.2099447513812152, + "pos-y": 3.915, + "skills": [ + { + "flag": false, + "name": "MoveSkill", + "primary": 0, + "secondary": 0 + } + ], + "tolerance": 30 + }, + { + "angel": 0, + "pos-x": 5.3591160220994478, + "pos-y": 3.915, + "skills": [ + { + "flag": false, + "name": "MoveSkill", + "primary": 0, + "secondary": 0 + } + ], + "tolerance": 30 + }, + { + "angel": 0, + "pos-x": 5.3701657458563528, + "pos-y": 2.55375, + "skills": [ + { + "flag": false, + "name": "MoveSkill", + "primary": 0, + "secondary": 0 + } + ], + "tolerance": 30 + } + ] + }, + { + "ID": 3, + "positions": [ + { + "angel": 0, + "pos-x": 1.8232044198895032, + "pos-y": 3.8812500000000001, + "skills": [ + { + "flag": false, + "name": "MoveSkill", + "primary": 0, + "secondary": 0 + } + ], + "tolerance": 30 + }, + { + "angel": 0, + "pos-x": 1.790055248618784, + "pos-y": 2.5987499999999999, + "skills": [ + { + "flag": false, + "name": "MoveSkill", + "primary": 0, + "secondary": 0 + } + ], + "tolerance": 30 + }, + { + "angel": 0, + "pos-x": 5.0497237569060776, + "pos-y": 2.55375, + "skills": [ + { + "flag": false, + "name": "MoveSkill", + "primary": 0, + "secondary": 0 + } + ], + "tolerance": 30 + } + ] + }, + { + "ID": 4, + "positions": [ + { + "angel": 0, + "pos-x": 1.8232044198895032, + "pos-y": 2.0812500000000003, + "skills": [ + { + "flag": false, + "name": "MoveSkill", + "primary": 0, + "secondary": 0 + } + ], + "tolerance": 30 + }, + { + "angel": 0, + "pos-x": 3.2707182320441994, + "pos-y": 1.8225000000000002, + "skills": [ + { + "flag": false, + "name": "MoveSkill", + "primary": 0, + "secondary": 0 + } + ], + "tolerance": 30 + }, + { + "angel": 0, + "pos-x": 4.751381215469614, + "pos-y": 1.99125, + "skills": [ + { + "flag": false, + "name": "MoveSkill", + "primary": 0, + "secondary": 0 + } + ], + "tolerance": 30 + } + ] + }, + { + "ID": 5, + "positions": [ + { + "angel": 0, + "pos-x": -0.97237569060773499, + "pos-y": 3.0149999999999997, + "skills": [ + { + "flag": false, + "name": "MoveSkill", + "primary": 0, + "secondary": 0 + } + ], + "tolerance": 30 + }, + { + "angel": 0, + "pos-x": -1.2044198895027627, + "pos-y": -1.6312500000000005, + "skills": [ + { + "flag": false, + "name": "MoveSkill", + "primary": 0, + "secondary": 0 + } + ], + "tolerance": 30 + }, + { + "angel": 0, + "pos-x": 0.49723756906077288, + "pos-y": 1.3387500000000001, + "skills": [ + { + "flag": false, + "name": "MoveSkill", + "primary": 0, + "secondary": 0 + } + ], + "tolerance": 30 + } + ] + }, + { + "ID": 6, + "positions": [ + { + "angel": 0, + "pos-x": -1.182320441988951, + "pos-y": -3.2962499999999997, + "skills": [ + { + "flag": false, + "name": "MoveSkill", + "primary": 0, + "secondary": 0 + } + ], + "tolerance": 30 + }, + { + "angel": 0, + "pos-x": 2.7845303867403306, + "pos-y": -2.9924999999999997, + "skills": [ + { + "flag": false, + "name": "MoveSkill", + "primary": 0, + "secondary": 0 + } + ], + "tolerance": 30 + }, + { + "angel": 0, + "pos-x": 2.4751381215469621, + "pos-y": -1.7999999999999998, + "skills": [ + { + "flag": false, + "name": "MoveSkill", + "primary": 0, + "secondary": 0 + } + ], + "tolerance": 30 + } + ] + }, + { + "ID": 7, + "positions": [ + { + "angel": 0, + "pos-x": -0.95027624309392245, + "pos-y": -2.4524999999999997, + "skills": [ + { + "flag": false, + "name": "MoveSkill", + "primary": 0, + "secondary": 0 + } + ], + "tolerance": 30 + }, + { + "angel": 0, + "pos-x": 0.29834254143646355, + "pos-y": -0.61874999999999947, + "skills": [ + { + "flag": false, + "name": "MoveSkill", + "primary": 0, + "secondary": 0 + } + ], + "tolerance": 30 + }, + { + "angel": 0, + "pos-x": 0.54143646408839885, + "pos-y": -2.2837500000000004, + "skills": [ + { + "flag": false, + "name": "MoveSkill", + "primary": 0, + "secondary": 0 + } + ], + "tolerance": 30 + } + ] + } + ], + "ballInitPos": { + "x": 4.6850828729281773, + "y": -3.7125000000000004 + }, + "chance": 1, + "lastDist": 1.5, + "maxEffective": 2, + "minNeeded": 2, + "planMode": "INDIRECT", + "tags": [ + "" + ] + } + ] +} diff --git a/parsian_ai/plans/jolo2.json b/parsian_ai/plans/jolo2.json new file mode 100644 index 00000000..bbdb997a --- /dev/null +++ b/parsian_ai/plans/jolo2.json @@ -0,0 +1,448 @@ +{ + "apiVersion": 1.2, + "id": 1, + "plans": [ + { + "agentInitPos": [ + { + "x": -100, + "y": -100 + }, + { + "x": 1.0386740331491708, + "y": 3.4874999999999998 + }, + { + "x": 2.2099447513812152, + "y": 3.915 + }, + { + "x": 1.8232044198895032, + "y": 3.8812500000000001 + }, + { + "x": 1.8232044198895032, + "y": 2.0812500000000003 + }, + { + "x": -0.97237569060773499, + "y": 3.0149999999999997 + }, + { + "x": -1.182320441988951, + "y": -3.2962499999999997 + }, + { + "x": -0.95027624309392245, + "y": -2.4524999999999997 + } + ], + "agents": [ + { + "ID": 0, + "positions": [ + { + "angel": 0, + "pos-x": 0.95027624309392245, + "pos-y": -3.6787499999999991, + "skills": [ + { + "flag": false, + "name": "MoveSkill", + "primary": 0, + "secondary": 0 + } + ], + "tolerance": 30 + }, + { + "angel": 0, + "pos-x": 5.1491712707182327, + "pos-y": -3.3637500000000005, + "skills": [ + { + "flag": false, + "name": "MoveSkill", + "primary": 0, + "secondary": 0 + }, + { + "flag": false, + "name": "PassSkill", + "primary": 500, + "secondary": 500, + "target": { + "agent": 1, + "index": 2 + } + } + ], + "tolerance": 30 + } + ] + }, + { + "ID": 1, + "positions": [ + { + "angel": 0, + "pos-x": 1.0386740331491708, + "pos-y": 3.4874999999999998, + "skills": [ + { + "flag": false, + "name": "MoveSkill", + "primary": 0, + "secondary": 0 + } + ], + "tolerance": 30 + }, + { + "angel": 0, + "pos-x": 1.0828729281767959, + "pos-y": -1.7549999999999999, + "skills": [ + { + "flag": false, + "name": "MoveSkill", + "primary": 0, + "secondary": 0 + } + ], + "tolerance": 30 + }, + { + "angel": 0, + "pos-x": 2.9723756906077341, + "pos-y": -1.3387500000000001, + "skills": [ + { + "flag": false, + "name": "MoveSkill", + "primary": 0, + "secondary": 0 + }, + { + "flag": false, + "name": "ShotToGoalSkill", + "primary": 500, + "secondary": 500 + } + ], + "tolerance": 30 + }, + { + "angel": 0, + "pos-x": 1.9889502762430942, + "pos-y": 0.64124999999999988, + "skills": [ + { + "flag": false, + "name": "MoveSkill", + "primary": 0, + "secondary": 0 + } + ], + "tolerance": 30 + } + ] + }, + { + "ID": 2, + "positions": [ + { + "angel": 0, + "pos-x": 2.2099447513812152, + "pos-y": 3.915, + "skills": [ + { + "flag": false, + "name": "MoveSkill", + "primary": 0, + "secondary": 0 + } + ], + "tolerance": 30 + }, + { + "angel": 0, + "pos-x": 5.3591160220994478, + "pos-y": 3.915, + "skills": [ + { + "flag": false, + "name": "MoveSkill", + "primary": 0, + "secondary": 0 + } + ], + "tolerance": 30 + }, + { + "angel": 0, + "pos-x": 4.2541436464088402, + "pos-y": 3.1837499999999999, + "skills": [ + { + "flag": false, + "name": "MoveSkill", + "primary": 0, + "secondary": 0 + } + ], + "tolerance": 30 + } + ] + }, + { + "ID": 3, + "positions": [ + { + "angel": 0, + "pos-x": 1.8232044198895032, + "pos-y": 3.8812500000000001, + "skills": [ + { + "flag": false, + "name": "MoveSkill", + "primary": 0, + "secondary": 0 + } + ], + "tolerance": 30 + }, + { + "angel": 0, + "pos-x": 1.790055248618784, + "pos-y": 2.5987499999999999, + "skills": [ + { + "flag": false, + "name": "MoveSkill", + "primary": 0, + "secondary": 0 + } + ], + "tolerance": 30 + }, + { + "angel": 0, + "pos-x": 3.9668508287292816, + "pos-y": 2.55375, + "skills": [ + { + "flag": false, + "name": "MoveSkill", + "primary": 0, + "secondary": 0 + } + ], + "tolerance": 30 + } + ] + }, + { + "ID": 4, + "positions": [ + { + "angel": 0, + "pos-x": 1.8232044198895032, + "pos-y": 2.0812500000000003, + "skills": [ + { + "flag": false, + "name": "MoveSkill", + "primary": 0, + "secondary": 0 + } + ], + "tolerance": 30 + }, + { + "angel": 0, + "pos-x": 3.2707182320441994, + "pos-y": 1.8225000000000002, + "skills": [ + { + "flag": false, + "name": "MoveSkill", + "primary": 0, + "secondary": 0 + } + ], + "tolerance": 30 + }, + { + "angel": 0, + "pos-x": 4.751381215469614, + "pos-y": 1.99125, + "skills": [ + { + "flag": false, + "name": "MoveSkill", + "primary": 0, + "secondary": 0 + } + ], + "tolerance": 30 + } + ] + }, + { + "ID": 5, + "positions": [ + { + "angel": 0, + "pos-x": -0.97237569060773499, + "pos-y": 3.0149999999999997, + "skills": [ + { + "flag": false, + "name": "MoveSkill", + "primary": 0, + "secondary": 0 + } + ], + "tolerance": 30 + }, + { + "angel": 0, + "pos-x": -0.77348066298342566, + "pos-y": 1.2262499999999998, + "skills": [ + { + "flag": false, + "name": "MoveSkill", + "primary": 0, + "secondary": 0 + } + ], + "tolerance": 30 + }, + { + "angel": 0, + "pos-x": 0.49723756906077288, + "pos-y": 1.3387500000000001, + "skills": [ + { + "flag": false, + "name": "MoveSkill", + "primary": 0, + "secondary": 0 + } + ], + "tolerance": 30 + } + ] + }, + { + "ID": 6, + "positions": [ + { + "angel": 0, + "pos-x": -1.182320441988951, + "pos-y": -3.2962499999999997, + "skills": [ + { + "flag": false, + "name": "MoveSkill", + "primary": 0, + "secondary": 0 + } + ], + "tolerance": 30 + }, + { + "angel": 0, + "pos-x": 0.89502762430939242, + "pos-y": -3.1950000000000003, + "skills": [ + { + "flag": false, + "name": "MoveSkill", + "primary": 0, + "secondary": 0 + } + ], + "tolerance": 30 + }, + { + "angel": 0, + "pos-x": 2.6629834254143656, + "pos-y": -3.0375000000000005, + "skills": [ + { + "flag": false, + "name": "MoveSkill", + "primary": 0, + "secondary": 0 + } + ], + "tolerance": 30 + } + ] + }, + { + "ID": 7, + "positions": [ + { + "angel": 0, + "pos-x": -0.95027624309392245, + "pos-y": -2.4524999999999997, + "skills": [ + { + "flag": false, + "name": "MoveSkill", + "primary": 0, + "secondary": 0 + } + ], + "tolerance": 30 + }, + { + "angel": 0, + "pos-x": 0.055248618784530912, + "pos-y": -1.7099999999999991, + "skills": [ + { + "flag": false, + "name": "MoveSkill", + "primary": 0, + "secondary": 0 + } + ], + "tolerance": 30 + }, + { + "angel": 0, + "pos-x": 0.54143646408839885, + "pos-y": -2.2837500000000004, + "skills": [ + { + "flag": false, + "name": "MoveSkill", + "primary": 0, + "secondary": 0 + } + ], + "tolerance": 30 + } + ] + } + ], + "ballInitPos": { + "x": 5.1491712707182327, + "y": -3.3637500000000005 + }, + "chance": 1, + "lastDist": 1.5, + "maxEffective": 2, + "minNeeded": 2, + "planMode": "INDIRECT", + "tags": [ + "" + ] + } + ] +} diff --git a/parsian_ai/plans/kianPlan.json b/parsian_ai/plans/kianPlan.json new file mode 100644 index 00000000..836776d5 --- /dev/null +++ b/parsian_ai/plans/kianPlan.json @@ -0,0 +1,380 @@ +{ + "apiVersion": 1.2, + "id": 1, + "plans": [ + { + "agentInitPos": [ + { + "x": -100, + "y": -100 + }, + { + "x": 1.2265193370165743, + "y": -2.1375000000000002 + }, + { + "x": 1.9889502762430942, + "y": -2.1037499999999998 + }, + { + "x": 1.5138121546961321, + "y": -2.5650000000000004 + }, + { + "x": 1.3812154696132595, + "y": -3.7575000000000003 + }, + { + "x": 1.2707182320441994, + "y": -2.9024999999999999 + } + ], + "agents": [ + { + "ID": 0, + "positions": [ + { + "angel": -48.652222780306332, + "pos-x": 4.9944751381215475, + "pos-y": -3.4199999999999999, + "skills": [ + { + "flag": false, + "name": "MoveSkill", + "primary": 0, + "secondary": 0 + }, + { + "flag": false, + "name": "PassSkill", + "primary": 700, + "secondary": 200, + "target": { + "agent": 1, + "index": 2 + } + } + ], + "tolerance": 30 + }, + { + "angel": 0, + "pos-x": 5.6464088397790047, + "pos-y": -3.0037500000000001, + "skills": [ + { + "flag": false, + "name": "MoveSkill", + "primary": 0, + "secondary": 0 + } + ], + "tolerance": 30 + }, + { + "angel": 0, + "pos-x": 5.4033149171270729, + "pos-y": -1.6087499999999997, + "skills": [ + { + "flag": false, + "name": "MoveSkill", + "primary": 0, + "secondary": 0 + } + ], + "tolerance": 30 + } + ] + }, + { + "ID": 1, + "positions": [ + { + "angel": 115.01689347810004, + "pos-x": 1.2265193370165743, + "pos-y": -2.1375000000000002, + "skills": [ + { + "flag": false, + "name": "MoveSkill", + "primary": 0, + "secondary": 0 + } + ], + "tolerance": 30 + }, + { + "angel": -27.255328374943069, + "pos-x": 2.0220994475138117, + "pos-y": -0.17999999999999972, + "skills": [ + { + "flag": false, + "name": "MoveSkill", + "primary": 0, + "secondary": 0 + } + ], + "tolerance": 30 + }, + { + "angel": 53.880659150520245, + "pos-x": 4.2541436464088402, + "pos-y": 1.6537500000000001, + "skills": [ + { + "flag": false, + "name": "MoveSkill", + "primary": 0, + "secondary": 0 + }, + { + "flag": false, + "name": "OneTouchSkill", + "primary": 500, + "secondary": 500 + } + ], + "tolerance": 70 + } + ] + }, + { + "ID": 2, + "positions": [ + { + "angel": 0, + "pos-x": 1.9889502762430942, + "pos-y": -2.1037499999999998, + "skills": [ + { + "flag": false, + "name": "MoveSkill", + "primary": 0, + "secondary": 0 + } + ], + "tolerance": 30 + }, + { + "angel": 0, + "pos-x": 0.68508287292817638, + "pos-y": 2.4862500000000001, + "skills": [ + { + "flag": false, + "name": "MoveSkill", + "primary": 0, + "secondary": 0 + } + ], + "tolerance": 30 + }, + { + "angel": 0, + "pos-x": 1.569060773480663, + "pos-y": 2.1374999999999997, + "skills": [ + { + "flag": false, + "name": "MoveSkill", + "primary": 0, + "secondary": 0 + }, + { + "flag": false, + "name": "OneTouchSkill", + "primary": 500, + "secondary": 500 + } + ], + "tolerance": 80 + }, + { + "angel": 0, + "pos-x": 2.9613259668508292, + "pos-y": 2.3174999999999999, + "skills": [ + { + "flag": false, + "name": "MoveSkill", + "primary": 0, + "secondary": 0 + } + ], + "tolerance": 30 + } + ] + }, + { + "ID": 3, + "positions": [ + { + "angel": 0, + "pos-x": 1.5138121546961321, + "pos-y": -2.5650000000000004, + "skills": [ + { + "flag": false, + "name": "MoveSkill", + "primary": 0, + "secondary": 0 + } + ], + "tolerance": 30 + }, + { + "angel": 0, + "pos-x": 4.4640883977900554, + "pos-y": -2.4750000000000005, + "skills": [ + { + "flag": false, + "name": "MoveSkill", + "primary": 0, + "secondary": 0 + } + ], + "tolerance": 30 + }, + { + "angel": 0, + "pos-x": 4.375690607734807, + "pos-y": -0.70875000000000021, + "skills": [ + { + "flag": false, + "name": "MoveSkill", + "primary": 0, + "secondary": 0 + } + ], + "tolerance": 70 + } + ] + }, + { + "ID": 4, + "positions": [ + { + "angel": 0, + "pos-x": 1.3812154696132595, + "pos-y": -3.7575000000000003, + "skills": [ + { + "flag": false, + "name": "MoveSkill", + "primary": 0, + "secondary": 0 + } + ], + "tolerance": 30 + }, + { + "angel": 0, + "pos-x": 4.0994475138121551, + "pos-y": -1.9462500000000009, + "skills": [ + { + "flag": false, + "name": "MoveSkill", + "primary": 0, + "secondary": 0 + } + ], + "tolerance": 30 + }, + { + "angel": 0, + "pos-x": 3.4364640883977895, + "pos-y": -0.83250000000000046, + "skills": [ + { + "flag": false, + "name": "MoveSkill", + "primary": 0, + "secondary": 0 + } + ], + "tolerance": 30 + } + ] + }, + { + "ID": 5, + "positions": [ + { + "angel": 0, + "pos-x": 1.2707182320441994, + "pos-y": -2.9024999999999999, + "skills": [ + { + "flag": false, + "name": "MoveSkill", + "primary": 0, + "secondary": 0 + } + ], + "tolerance": 30 + }, + { + "angel": 0, + "pos-x": 0.50828729281767959, + "pos-y": -0.61874999999999947, + "skills": [ + { + "flag": false, + "name": "MoveSkill", + "primary": 0, + "secondary": 0 + } + ], + "tolerance": 30 + }, + { + "angel": 0, + "pos-x": 0.4640883977900554, + "pos-y": -3.2962499999999997, + "skills": [ + { + "flag": false, + "name": "MoveSkill", + "primary": 0, + "secondary": 0 + } + ], + "tolerance": 30 + }, + { + "angel": 0, + "pos-x": 5.1823204419889493, + "pos-y": -3.0375000000000005, + "skills": [ + { + "flag": false, + "name": "MoveSkill", + "primary": 0, + "secondary": 0 + } + ], + "tolerance": 30 + } + ] + } + ], + "ballInitPos": { + "x": 4.9944751381215475, + "y": -3.4199999999999999 + }, + "chance": 0, + "lastDist": 1.5, + "maxEffective": 4, + "minNeeded": 3, + "planMode": "INDIRECT", + "tags": [ + "" + ] + } + ] +} diff --git a/parsian_ai/plans/kickoff.json b/parsian_ai/plans/kickoff.json new file mode 100644 index 00000000..504f851b --- /dev/null +++ b/parsian_ai/plans/kickoff.json @@ -0,0 +1,448 @@ +{ + "apiVersion": 1.2, + "id": 1, + "plans": [ + { + "agentInitPos": [ + { + "x": -100, + "y": -100 + }, + { + "x": -1.5248618784530388, + "y": 3.3300000000000001 + }, + { + "x": 0.13259668508287259, + "y": -1.3950000000000005 + }, + { + "x": 0.45303867403314868, + "y": -3.6899999999999995 + }, + { + "x": 1.7127071823204423, + "y": -0.73125000000000018 + }, + { + "x": -1.6574585635359114, + "y": 1.1812499999999999 + }, + { + "x": -2.243093922651934, + "y": 2.1712500000000001 + }, + { + "x": -0.66298342541436472, + "y": -1.6200000000000001 + } + ], + "agents": [ + { + "ID": 0, + "positions": [ + { + "angel": 0, + "pos-x": -0.74033149171270729, + "pos-y": -0.34874999999999989, + "skills": [ + { + "flag": false, + "name": "MoveSkill", + "primary": 0, + "secondary": 0 + } + ], + "tolerance": 30 + }, + { + "angel": 0, + "pos-x": -0.022099447513811654, + "pos-y": 0.022499999999999964, + "skills": [ + { + "flag": false, + "name": "MoveSkill", + "primary": 0, + "secondary": 0 + }, + { + "flag": false, + "name": "PassSkill", + "primary": 500, + "secondary": 500, + "target": { + "agent": 1, + "index": 1 + } + } + ], + "tolerance": 30 + }, + { + "angel": 0, + "pos-x": 0.76243093922651894, + "pos-y": -0.24749999999999961, + "skills": [ + { + "flag": false, + "name": "MoveSkill", + "primary": 0, + "secondary": 0 + } + ], + "tolerance": 30 + } + ] + }, + { + "ID": 1, + "positions": [ + { + "angel": 0, + "pos-x": -1.5248618784530388, + "pos-y": 3.3300000000000001, + "skills": [ + { + "flag": false, + "name": "MoveSkill", + "primary": 0, + "secondary": 0 + } + ], + "tolerance": 30 + }, + { + "angel": 0, + "pos-x": 2.5856353591160222, + "pos-y": 3.6675, + "skills": [ + { + "flag": false, + "name": "MoveSkill", + "primary": 0, + "secondary": 0 + }, + { + "flag": false, + "name": "ShotToGoalSkill", + "primary": 500, + "secondary": 500 + } + ], + "tolerance": 30 + }, + { + "angel": 0, + "pos-x": 4.5193370165745854, + "pos-y": 3.6225000000000001, + "skills": [ + { + "flag": false, + "name": "MoveSkill", + "primary": 0, + "secondary": 0 + } + ], + "tolerance": 30 + } + ] + }, + { + "ID": 2, + "positions": [ + { + "angel": 0, + "pos-x": 0.13259668508287259, + "pos-y": -1.3950000000000005, + "skills": [ + { + "flag": false, + "name": "MoveSkill", + "primary": 0, + "secondary": 0 + } + ], + "tolerance": 30 + }, + { + "angel": 0, + "pos-x": 0.375690607734807, + "pos-y": -3.1162499999999991, + "skills": [ + { + "flag": false, + "name": "MoveSkill", + "primary": 0, + "secondary": 0 + } + ], + "tolerance": 30 + }, + { + "angel": 0, + "pos-x": 1.2154696132596676, + "pos-y": -1.4287499999999991, + "skills": [ + { + "flag": false, + "name": "MoveSkill", + "primary": 0, + "secondary": 0 + } + ], + "tolerance": 30 + } + ] + }, + { + "ID": 3, + "positions": [ + { + "angel": 0, + "pos-x": 0.45303867403314868, + "pos-y": -3.6899999999999995, + "skills": [ + { + "flag": false, + "name": "MoveSkill", + "primary": 0, + "secondary": 0 + } + ], + "tolerance": 30 + }, + { + "angel": 0, + "pos-x": 4.0773480662983417, + "pos-y": -3.5662500000000001, + "skills": [ + { + "flag": false, + "name": "MoveSkill", + "primary": 0, + "secondary": 0 + } + ], + "tolerance": 30 + }, + { + "angel": 0, + "pos-x": 5.3591160220994478, + "pos-y": -2.7900000000000009, + "skills": [ + { + "flag": false, + "name": "MoveSkill", + "primary": 0, + "secondary": 0 + } + ], + "tolerance": 30 + } + ] + }, + { + "ID": 4, + "positions": [ + { + "angel": 0, + "pos-x": 1.7127071823204423, + "pos-y": -0.73125000000000018, + "skills": [ + { + "flag": false, + "name": "MoveSkill", + "primary": 0, + "secondary": 0 + } + ], + "tolerance": 30 + }, + { + "angel": 0, + "pos-x": 2.7182320441988956, + "pos-y": -2.8574999999999999, + "skills": [ + { + "flag": false, + "name": "MoveSkill", + "primary": 0, + "secondary": 0 + } + ], + "tolerance": 30 + }, + { + "angel": 0, + "pos-x": 3.4254143646408846, + "pos-y": -1.5975000000000001, + "skills": [ + { + "flag": false, + "name": "MoveSkill", + "primary": 0, + "secondary": 0 + } + ], + "tolerance": 30 + } + ] + }, + { + "ID": 5, + "positions": [ + { + "angel": 0, + "pos-x": -1.6574585635359114, + "pos-y": 1.1812499999999999, + "skills": [ + { + "flag": false, + "name": "MoveSkill", + "primary": 0, + "secondary": 0 + } + ], + "tolerance": 30 + }, + { + "angel": 0, + "pos-x": -1.5580110497237571, + "pos-y": -2.0249999999999995, + "skills": [ + { + "flag": false, + "name": "MoveSkill", + "primary": 0, + "secondary": 0 + } + ], + "tolerance": 30 + }, + { + "angel": 0, + "pos-x": -0.36464088397790029, + "pos-y": 2.0362499999999999, + "skills": [ + { + "flag": false, + "name": "MoveSkill", + "primary": 0, + "secondary": 0 + } + ], + "tolerance": 30 + } + ] + }, + { + "ID": 6, + "positions": [ + { + "angel": 0, + "pos-x": -2.243093922651934, + "pos-y": 2.1712500000000001, + "skills": [ + { + "flag": false, + "name": "MoveSkill", + "primary": 0, + "secondary": 0 + } + ], + "tolerance": 30 + }, + { + "angel": 0, + "pos-x": 0.8397790055248624, + "pos-y": 2.82375, + "skills": [ + { + "flag": false, + "name": "MoveSkill", + "primary": 0, + "secondary": 0 + } + ], + "tolerance": 30 + }, + { + "angel": 0, + "pos-x": 0.19889502762430844, + "pos-y": 2.1937500000000001, + "skills": [ + { + "flag": false, + "name": "MoveSkill", + "primary": 0, + "secondary": 0 + } + ], + "tolerance": 30 + } + ] + }, + { + "ID": 7, + "positions": [ + { + "angel": 0, + "pos-x": -0.66298342541436472, + "pos-y": -1.6200000000000001, + "skills": [ + { + "flag": false, + "name": "MoveSkill", + "primary": 0, + "secondary": 0 + } + ], + "tolerance": 30 + }, + { + "angel": 0, + "pos-x": 1.5911602209944746, + "pos-y": -0.13499999999999979, + "skills": [ + { + "flag": false, + "name": "MoveSkill", + "primary": 0, + "secondary": 0 + } + ], + "tolerance": 30 + }, + { + "angel": 0, + "pos-x": 3.1160220994475143, + "pos-y": -0.48374999999999968, + "skills": [ + { + "flag": false, + "name": "MoveSkill", + "primary": 0, + "secondary": 0 + } + ], + "tolerance": 30 + } + ] + } + ], + "ballInitPos": { + "x": -0.022099447513811654, + "y": 0.022499999999999964 + }, + "chance": 1, + "lastDist": 1.5, + "maxEffective": 2, + "minNeeded": 2, + "planMode": "KICKOFF", + "tags": [ + "" + ] + } + ] +} diff --git a/parsian_ai/plans/kickoff2.json b/parsian_ai/plans/kickoff2.json new file mode 100644 index 00000000..d3aa933c --- /dev/null +++ b/parsian_ai/plans/kickoff2.json @@ -0,0 +1,448 @@ +{ + "apiVersion": 1.2, + "id": 1, + "plans": [ + { + "agentInitPos": [ + { + "x": -100, + "y": -100 + }, + { + "x": -1.027624309392265, + "y": 1.3387500000000001 + }, + { + "x": -1.4254143646408837, + "y": 3.9037500000000001 + }, + { + "x": 3.3038674033149178, + "y": 4.0837500000000002 + }, + { + "x": 1.193370165745856, + "y": -3.2512499999999998 + }, + { + "x": -0.82872928176795568, + "y": -3.1162499999999991 + }, + { + "x": -0.60773480662983381, + "y": -1.3612500000000001 + }, + { + "x": -0.59668508287292799, + "y": -0.74249999999999972 + } + ], + "agents": [ + { + "ID": 0, + "positions": [ + { + "angel": 0, + "pos-x": -0.44198895027624374, + "pos-y": -0.033750000000000391, + "skills": [ + { + "flag": false, + "name": "MoveSkill", + "primary": 0, + "secondary": 0 + } + ], + "tolerance": 30 + }, + { + "angel": 0, + "pos-x": -0.033149171270718369, + "pos-y": 0, + "skills": [ + { + "flag": false, + "name": "MoveSkill", + "primary": 0, + "secondary": 0 + }, + { + "flag": false, + "name": "PassSkill", + "primary": 500, + "secondary": 500, + "target": { + "agent": 1, + "index": 1 + } + } + ], + "tolerance": 30 + }, + { + "angel": 0, + "pos-x": -0.20994475138121516, + "pos-y": 0.61874999999999991, + "skills": [ + { + "flag": false, + "name": "MoveSkill", + "primary": 0, + "secondary": 0 + } + ], + "tolerance": 30 + } + ] + }, + { + "ID": 1, + "positions": [ + { + "angel": 0, + "pos-x": -1.027624309392265, + "pos-y": 1.3387500000000001, + "skills": [ + { + "flag": false, + "name": "MoveSkill", + "primary": 0, + "secondary": 0 + } + ], + "tolerance": 30 + }, + { + "angel": 0, + "pos-x": 4.2983425414364653, + "pos-y": 2.0137499999999999, + "skills": [ + { + "flag": false, + "name": "MoveSkill", + "primary": 0, + "secondary": 0 + }, + { + "flag": false, + "name": "ShotToGoalSkill", + "primary": 500, + "secondary": 500 + } + ], + "tolerance": 30 + }, + { + "angel": 0, + "pos-x": 4.1436464088397784, + "pos-y": 2.52, + "skills": [ + { + "flag": false, + "name": "MoveSkill", + "primary": 0, + "secondary": 0 + } + ], + "tolerance": 30 + } + ] + }, + { + "ID": 2, + "positions": [ + { + "angel": 0, + "pos-x": -1.4254143646408837, + "pos-y": 3.9037500000000001, + "skills": [ + { + "flag": false, + "name": "MoveSkill", + "primary": 0, + "secondary": 0 + } + ], + "tolerance": 30 + }, + { + "angel": 0, + "pos-x": 2.4419889502762437, + "pos-y": 4.0612500000000002, + "skills": [ + { + "flag": false, + "name": "MoveSkill", + "primary": 0, + "secondary": 0 + } + ], + "tolerance": 30 + }, + { + "angel": 0, + "pos-x": 0.54143646408839885, + "pos-y": 2.9587499999999998, + "skills": [ + { + "flag": false, + "name": "MoveSkill", + "primary": 0, + "secondary": 0 + } + ], + "tolerance": 30 + } + ] + }, + { + "ID": 3, + "positions": [ + { + "angel": 0, + "pos-x": 3.3038674033149178, + "pos-y": 4.0837500000000002, + "skills": [ + { + "flag": false, + "name": "MoveSkill", + "primary": 0, + "secondary": 0 + } + ], + "tolerance": 30 + }, + { + "angel": 0, + "pos-x": 5.4364640883977895, + "pos-y": 4.0949999999999998, + "skills": [ + { + "flag": false, + "name": "MoveSkill", + "primary": 0, + "secondary": 0 + } + ], + "tolerance": 30 + }, + { + "angel": 0, + "pos-x": 3.3149171270718227, + "pos-y": 3.0149999999999997, + "skills": [ + { + "flag": false, + "name": "MoveSkill", + "primary": 0, + "secondary": 0 + } + ], + "tolerance": 30 + } + ] + }, + { + "ID": 4, + "positions": [ + { + "angel": 0, + "pos-x": 1.193370165745856, + "pos-y": -3.2512499999999998, + "skills": [ + { + "flag": false, + "name": "MoveSkill", + "primary": 0, + "secondary": 0 + } + ], + "tolerance": 30 + }, + { + "angel": 0, + "pos-x": 4.4972375690607738, + "pos-y": -3.3412499999999996, + "skills": [ + { + "flag": false, + "name": "MoveSkill", + "primary": 0, + "secondary": 0 + } + ], + "tolerance": 30 + }, + { + "angel": 0, + "pos-x": 1.6685082872928181, + "pos-y": -3.723749999999999, + "skills": [ + { + "flag": false, + "name": "MoveSkill", + "primary": 0, + "secondary": 0 + } + ], + "tolerance": 30 + } + ] + }, + { + "ID": 5, + "positions": [ + { + "angel": 0, + "pos-x": -0.82872928176795568, + "pos-y": -3.1162499999999991, + "skills": [ + { + "flag": false, + "name": "MoveSkill", + "primary": 0, + "secondary": 0 + } + ], + "tolerance": 30 + }, + { + "angel": 0, + "pos-x": 2.8066298342541423, + "pos-y": -1.8674999999999997, + "skills": [ + { + "flag": false, + "name": "MoveSkill", + "primary": 0, + "secondary": 0 + } + ], + "tolerance": 30 + }, + { + "angel": 0, + "pos-x": 3.0165745856353592, + "pos-y": -2.6100000000000003, + "skills": [ + { + "flag": false, + "name": "MoveSkill", + "primary": 0, + "secondary": 0 + } + ], + "tolerance": 30 + } + ] + }, + { + "ID": 6, + "positions": [ + { + "angel": 0, + "pos-x": -0.60773480662983381, + "pos-y": -1.3612500000000001, + "skills": [ + { + "flag": false, + "name": "MoveSkill", + "primary": 0, + "secondary": 0 + } + ], + "tolerance": 30 + }, + { + "angel": 0, + "pos-x": 3.0607734806629825, + "pos-y": -0.74249999999999972, + "skills": [ + { + "flag": false, + "name": "MoveSkill", + "primary": 0, + "secondary": 0 + } + ], + "tolerance": 30 + }, + { + "angel": 0, + "pos-x": 1.2486187845303869, + "pos-y": -1.4287499999999991, + "skills": [ + { + "flag": false, + "name": "MoveSkill", + "primary": 0, + "secondary": 0 + } + ], + "tolerance": 30 + } + ] + }, + { + "ID": 7, + "positions": [ + { + "angel": 0, + "pos-x": -0.59668508287292799, + "pos-y": -0.74249999999999972, + "skills": [ + { + "flag": false, + "name": "MoveSkill", + "primary": 0, + "secondary": 0 + } + ], + "tolerance": 30 + }, + { + "angel": 0, + "pos-x": 2.430939226519337, + "pos-y": -0.24749999999999961, + "skills": [ + { + "flag": false, + "name": "MoveSkill", + "primary": 0, + "secondary": 0 + } + ], + "tolerance": 30 + }, + { + "angel": 0, + "pos-x": 1.8342541436464099, + "pos-y": 0.11249999999999982, + "skills": [ + { + "flag": false, + "name": "MoveSkill", + "primary": 0, + "secondary": 0 + } + ], + "tolerance": 30 + } + ] + } + ], + "ballInitPos": { + "x": -0.033149171270718369, + "y": 0 + }, + "chance": 1, + "lastDist": 1.5, + "maxEffective": 2, + "minNeeded": 2, + "planMode": "KICKOFF", + "tags": [ + "" + ] + } + ] +} diff --git a/parsian_ai/plans/ZJu/zjuBug.json b/parsian_ai/plans/oldPlans/ZJu/zjuBug.json similarity index 72% rename from parsian_ai/plans/ZJu/zjuBug.json rename to parsian_ai/plans/oldPlans/ZJu/zjuBug.json index a13991bc..6f7056c6 100644 --- a/parsian_ai/plans/ZJu/zjuBug.json +++ b/parsian_ai/plans/oldPlans/ZJu/zjuBug.json @@ -9,8 +9,8 @@ "y": -100 }, { - "x": 0.27624309392265195, - "y": -3.9937499999999999 + "x": 0.47513812154696033, + "y": 0.033749999999999503 }, { "x": -100, @@ -43,8 +43,22 @@ "positions": [ { "angel": 0, - "pos-x": 5.7127071823204423, - "pos-y": -4.0724999999999998, + "pos-x": 3.3149171270718227, + "pos-y": -3.7349999999999994, + "skills": [ + { + "flag": false, + "name": "MoveSkill", + "primary": 0, + "secondary": 0 + } + ], + "tolerance": 30 + }, + { + "angel": 0, + "pos-x": 5.3591160220994478, + "pos-y": -3.9037500000000005, "skills": [ { "flag": false, @@ -67,8 +81,8 @@ }, { "angel": 0, - "pos-x": 1.160220994475138, - "pos-y": -3.1499999999999999, + "pos-x": 1.348066298342542, + "pos-y": -3.5662500000000001, "skills": [ { "flag": false, @@ -86,8 +100,8 @@ "positions": [ { "angel": 0, - "pos-x": 0.27624309392265195, - "pos-y": -3.9937499999999999, + "pos-x": 0.47513812154696033, + "pos-y": 0.033749999999999503, "skills": [ { "flag": false, @@ -100,8 +114,8 @@ }, { "angel": 0, - "pos-x": 4.1878453038674035, - "pos-y": -3.1499999999999999, + "pos-x": 2.4751381215469621, + "pos-y": -1.3837500000000009, "skills": [ { "flag": false, @@ -113,9 +127,9 @@ "tolerance": 30 }, { - "angel": 0, - "pos-x": 0.36464088397790057, - "pos-y": -2.1825000000000001, + "angel": 37.694240466689173, + "pos-x": 3.3259668508287294, + "pos-y": -0.21375000000000011, "skills": [ { "flag": false, @@ -127,9 +141,9 @@ "tolerance": 30 }, { - "angel": 0, - "pos-x": 2.5524861878453038, - "pos-y": 0.40500000000000003, + "angel": 66.161259816828277, + "pos-x": 4.6519337016574589, + "pos-y": 1.3275000000000001, "skills": [ { "flag": false, @@ -141,7 +155,21 @@ "flag": false, "name": "OneTouchSkill", "primary": 500, - "secondary": 100 + "secondary": 500 + } + ], + "tolerance": 30 + }, + { + "angel": 0, + "pos-x": 4.7624309392265189, + "pos-y": 1.9350000000000001, + "skills": [ + { + "flag": false, + "name": "MoveSkill", + "primary": 0, + "secondary": 0 } ], "tolerance": 30 @@ -180,11 +208,11 @@ } ], "ballInitPos": { - "x": 5.7127071823204423, - "y": -4.0724999999999998 + "x": 5.3591160220994478, + "y": -3.9037500000000005 }, "chance": 1, - "lastDist": 3, + "lastDist": 1, "maxEffective": 2, "minNeeded": 2, "planMode": "INDIRECT", diff --git a/parsian_ai/plans/ZJu/zjuBug2Rush.json b/parsian_ai/plans/oldPlans/ZJu/zjuBug2Rush.json similarity index 100% rename from parsian_ai/plans/ZJu/zjuBug2Rush.json rename to parsian_ai/plans/oldPlans/ZJu/zjuBug2Rush.json diff --git a/parsian_ai/plans/global/middle2P.json b/parsian_ai/plans/oldPlans/global/middle2P.json similarity index 100% rename from parsian_ai/plans/global/middle2P.json rename to parsian_ai/plans/oldPlans/global/middle2P.json diff --git a/parsian_ai/plans/global/middle_imm.json b/parsian_ai/plans/oldPlans/global/middle_imm.json similarity index 100% rename from parsian_ai/plans/global/middle_imm.json rename to parsian_ai/plans/oldPlans/global/middle_imm.json diff --git a/parsian_ai/plans/kickoff/kickoff.json b/parsian_ai/plans/oldPlans/kickoff/kickoff.json similarity index 100% rename from parsian_ai/plans/kickoff/kickoff.json rename to parsian_ai/plans/oldPlans/kickoff/kickoff.json diff --git a/parsian_ai/plans/kickoff/kickoff2.json b/parsian_ai/plans/oldPlans/kickoff/kickoff2.json similarity index 100% rename from parsian_ai/plans/kickoff/kickoff2.json rename to parsian_ai/plans/oldPlans/kickoff/kickoff2.json diff --git a/parsian_ai/plans/kickoff/prs_kickoff_chip.json b/parsian_ai/plans/oldPlans/kickoff/prs_kickoff_chip.json similarity index 100% rename from parsian_ai/plans/kickoff/prs_kickoff_chip.json rename to parsian_ai/plans/oldPlans/kickoff/prs_kickoff_chip.json diff --git a/parsian_ai/plans/kickoff/prs_kickoff_pass.json b/parsian_ai/plans/oldPlans/kickoff/prs_kickoff_pass.json similarity index 100% rename from parsian_ai/plans/kickoff/prs_kickoff_pass.json rename to parsian_ai/plans/oldPlans/kickoff/prs_kickoff_pass.json diff --git a/parsian_ai/plans/mrl/robocupGoal.json b/parsian_ai/plans/oldPlans/mrl/robocupGoal.json similarity index 100% rename from parsian_ai/plans/mrl/robocupGoal.json rename to parsian_ai/plans/oldPlans/mrl/robocupGoal.json diff --git a/parsian_ai/plans/mrl/robocupGoalMain.json b/parsian_ai/plans/oldPlans/mrl/robocupGoalMain.json similarity index 100% rename from parsian_ai/plans/mrl/robocupGoalMain.json rename to parsian_ai/plans/oldPlans/mrl/robocupGoalMain.json diff --git a/parsian_ai/plans/old_plans/8.json b/parsian_ai/plans/oldPlans/old_plans/8.json similarity index 100% rename from parsian_ai/plans/old_plans/8.json rename to parsian_ai/plans/oldPlans/old_plans/8.json diff --git a/parsian_ai/plans/old_plans/8_2.json b/parsian_ai/plans/oldPlans/old_plans/8_2.json similarity index 100% rename from parsian_ai/plans/old_plans/8_2.json rename to parsian_ai/plans/oldPlans/old_plans/8_2.json diff --git a/parsian_ai/plans/old_plans/8_2P.json b/parsian_ai/plans/oldPlans/old_plans/8_2P.json similarity index 100% rename from parsian_ai/plans/old_plans/8_2P.json rename to parsian_ai/plans/oldPlans/old_plans/8_2P.json diff --git a/parsian_ai/plans/old_plans/hshm1.json b/parsian_ai/plans/oldPlans/old_plans/hshm1.json similarity index 100% rename from parsian_ai/plans/old_plans/hshm1.json rename to parsian_ai/plans/oldPlans/old_plans/hshm1.json diff --git a/parsian_ai/plans/old_plans/hshm2.json b/parsian_ai/plans/oldPlans/old_plans/hshm2.json similarity index 100% rename from parsian_ai/plans/old_plans/hshm2.json rename to parsian_ai/plans/oldPlans/old_plans/hshm2.json diff --git a/parsian_ai/plans/old_plans/kianPlan.json b/parsian_ai/plans/oldPlans/old_plans/kianPlan.json similarity index 100% rename from parsian_ai/plans/old_plans/kianPlan.json rename to parsian_ai/plans/oldPlans/old_plans/kianPlan.json diff --git a/parsian_ai/plans/old_plans/nadia1.json b/parsian_ai/plans/oldPlans/old_plans/nadia1.json similarity index 100% rename from parsian_ai/plans/old_plans/nadia1.json rename to parsian_ai/plans/oldPlans/old_plans/nadia1.json diff --git a/parsian_ai/plans/old_plans/nadia2.json b/parsian_ai/plans/oldPlans/old_plans/nadia2.json similarity index 100% rename from parsian_ai/plans/old_plans/nadia2.json rename to parsian_ai/plans/oldPlans/old_plans/nadia2.json diff --git a/parsian_ai/plans/old_plans/semi_nadia1.json b/parsian_ai/plans/oldPlans/old_plans/semi_nadia1.json similarity index 100% rename from parsian_ai/plans/old_plans/semi_nadia1.json rename to parsian_ai/plans/oldPlans/old_plans/semi_nadia1.json diff --git a/parsian_ai/plans/old_plans/semi_nadia1_2.json b/parsian_ai/plans/oldPlans/old_plans/semi_nadia1_2.json similarity index 100% rename from parsian_ai/plans/old_plans/semi_nadia1_2.json rename to parsian_ai/plans/oldPlans/old_plans/semi_nadia1_2.json diff --git a/parsian_ai/plans/plans.ignore b/parsian_ai/plans/plans.ignore index 0fc853ec..a7bd8850 100644 --- a/parsian_ai/plans/plans.ignore +++ b/parsian_ai/plans/plans.ignore @@ -1,21 +1,5 @@ # use python regex; write your comments and data in seperate lines. -/NewPlan.json -/hello2.json # ignore all files and directories in /plans/sth: -/sth/ -/old_plans/ -/another/sth -/\w*/\w*MRL.*\.json -/\w*MRL.*\.json -/blabla/ -/mahi.json -/2pass.json -/hshm2.json -/2p_1.json -/2p_2.json -/chip_2P.json -/kickoff/prs_kickoff_chip.json -/kickoff/prs_kickoff_pass.json -/afterlife/ -/kickoff/kickoff2.json -/ERforce/template.json +oldPlans/#2018 plans +jolo2.json +kickoff2.json diff --git a/parsian_ai/plans/vasat.json b/parsian_ai/plans/vasat.json new file mode 100644 index 00000000..4325287d --- /dev/null +++ b/parsian_ai/plans/vasat.json @@ -0,0 +1,462 @@ +{ + "apiVersion": 1.2, + "id": 1, + "plans": [ + { + "agentInitPos": [ + { + "x": -100, + "y": -100 + }, + { + "x": 0.34254143646408863, + "y": 3.5887500000000001 + }, + { + "x": 0.54143646408839885, + "y": 2.9249999999999998 + }, + { + "x": 0.57458563535911544, + "y": 1.2712499999999998 + }, + { + "x": 0.74033149171270729, + "y": -0.50625000000000053 + }, + { + "x": 3.4254143646408846, + "y": 2.9699999999999998 + }, + { + "x": 1.9005524861878458, + "y": -3.4987500000000002 + }, + { + "x": 3.0497237569060776, + "y": -2.3287500000000003 + } + ], + "agents": [ + { + "ID": 0, + "positions": [ + { + "angel": 0, + "pos-x": 0.50828729281767959, + "pos-y": -2.1712499999999997, + "skills": [ + { + "flag": false, + "name": "MoveSkill", + "primary": 0, + "secondary": 0 + } + ], + "tolerance": 30 + }, + { + "angel": 0, + "pos-x": 1.9116022099447516, + "pos-y": -3.9712499999999995, + "skills": [ + { + "flag": false, + "name": "MoveSkill", + "primary": 0, + "secondary": 0 + }, + { + "flag": false, + "name": "PassSkill", + "primary": 600, + "secondary": 700, + "target": { + "agent": 1, + "index": 1 + } + } + ], + "tolerance": 30 + }, + { + "angel": 0, + "pos-x": 1.3812154696132595, + "pos-y": -3.7575000000000003, + "skills": [ + { + "flag": false, + "name": "MoveSkill", + "primary": 0, + "secondary": 0 + } + ], + "tolerance": 30 + }, + { + "angel": 0, + "pos-x": -2.8176795580110499, + "pos-y": 0.13499999999999979, + "skills": [ + { + "flag": false, + "name": "MoveSkill", + "primary": 0, + "secondary": 0 + } + ], + "tolerance": 30 + } + ] + }, + { + "ID": 1, + "positions": [ + { + "angel": 0, + "pos-x": 0.34254143646408863, + "pos-y": 3.5887500000000001, + "skills": [ + { + "flag": false, + "name": "MoveSkill", + "primary": 0, + "secondary": 0 + } + ], + "tolerance": 30 + }, + { + "angel": 0, + "pos-x": 0.76243093922651894, + "pos-y": -0.99000000000000021, + "skills": [ + { + "flag": false, + "name": "MoveSkill", + "primary": 0, + "secondary": 0 + }, + { + "flag": false, + "name": "ShotToGoalSkill", + "primary": 500, + "secondary": 500 + } + ], + "tolerance": 30 + }, + { + "angel": 0, + "pos-x": 5.1491712707182327, + "pos-y": 4.1399999999999997, + "skills": [ + { + "flag": false, + "name": "MoveSkill", + "primary": 0, + "secondary": 0 + } + ], + "tolerance": 30 + } + ] + }, + { + "ID": 2, + "positions": [ + { + "angel": 0, + "pos-x": 0.54143646408839885, + "pos-y": 2.9249999999999998, + "skills": [ + { + "flag": false, + "name": "MoveSkill", + "primary": 0, + "secondary": 0 + } + ], + "tolerance": 30 + }, + { + "angel": 0, + "pos-x": 2.7071823204419889, + "pos-y": 2.3850000000000002, + "skills": [ + { + "flag": false, + "name": "MoveSkill", + "primary": 0, + "secondary": 0 + } + ], + "tolerance": 30 + }, + { + "angel": 0, + "pos-x": 0.39779005524861866, + "pos-y": 1.8562499999999997, + "skills": [ + { + "flag": false, + "name": "MoveSkill", + "primary": 0, + "secondary": 0 + } + ], + "tolerance": 30 + } + ] + }, + { + "ID": 3, + "positions": [ + { + "angel": 0, + "pos-x": 0.57458563535911544, + "pos-y": 1.2712499999999998, + "skills": [ + { + "flag": false, + "name": "MoveSkill", + "primary": 0, + "secondary": 0 + } + ], + "tolerance": 30 + }, + { + "angel": 0, + "pos-x": 2.5414364640883988, + "pos-y": 0.83250000000000046, + "skills": [ + { + "flag": false, + "name": "MoveSkill", + "primary": 0, + "secondary": 0 + } + ], + "tolerance": 30 + }, + { + "angel": 0, + "pos-x": 0.76243093922651894, + "pos-y": 0.23625000000000007, + "skills": [ + { + "flag": false, + "name": "MoveSkill", + "primary": 0, + "secondary": 0 + } + ], + "tolerance": 30 + } + ] + }, + { + "ID": 4, + "positions": [ + { + "angel": 0, + "pos-x": 0.74033149171270729, + "pos-y": -0.50625000000000053, + "skills": [ + { + "flag": false, + "name": "MoveSkill", + "primary": 0, + "secondary": 0 + } + ], + "tolerance": 30 + }, + { + "angel": 0, + "pos-x": 0.55248618784530379, + "pos-y": -1.2824999999999998, + "skills": [ + { + "flag": false, + "name": "MoveSkill", + "primary": 0, + "secondary": 0 + } + ], + "tolerance": 30 + }, + { + "angel": 0, + "pos-x": 1.790055248618784, + "pos-y": -0.31500000000000039, + "skills": [ + { + "flag": false, + "name": "MoveSkill", + "primary": 0, + "secondary": 0 + } + ], + "tolerance": 30 + } + ] + }, + { + "ID": 5, + "positions": [ + { + "angel": 0, + "pos-x": 3.4254143646408846, + "pos-y": 2.9699999999999998, + "skills": [ + { + "flag": false, + "name": "MoveSkill", + "primary": 0, + "secondary": 0 + } + ], + "tolerance": 30 + }, + { + "angel": 0, + "pos-x": 2.1325966850828735, + "pos-y": 3.1612499999999999, + "skills": [ + { + "flag": false, + "name": "MoveSkill", + "primary": 0, + "secondary": 0 + } + ], + "tolerance": 30 + }, + { + "angel": 0, + "pos-x": 2.2209944751381219, + "pos-y": 4.2862499999999999, + "skills": [ + { + "flag": false, + "name": "MoveSkill", + "primary": 0, + "secondary": 0 + } + ], + "tolerance": 30 + } + ] + }, + { + "ID": 6, + "positions": [ + { + "angel": 0, + "pos-x": 1.9005524861878458, + "pos-y": -3.4987500000000002, + "skills": [ + { + "flag": false, + "name": "MoveSkill", + "primary": 0, + "secondary": 0 + } + ], + "tolerance": 30 + }, + { + "angel": 0, + "pos-x": 5.1049723756906076, + "pos-y": -3.4424999999999999, + "skills": [ + { + "flag": false, + "name": "MoveSkill", + "primary": 0, + "secondary": 0 + } + ], + "tolerance": 30 + }, + { + "angel": 0, + "pos-x": 4.0441988950276251, + "pos-y": -2.5987499999999999, + "skills": [ + { + "flag": false, + "name": "MoveSkill", + "primary": 0, + "secondary": 0 + } + ], + "tolerance": 30 + } + ] + }, + { + "ID": 7, + "positions": [ + { + "angel": 0, + "pos-x": 3.0497237569060776, + "pos-y": -2.3287500000000003, + "skills": [ + { + "flag": false, + "name": "MoveSkill", + "primary": 0, + "secondary": 0 + } + ], + "tolerance": 30 + }, + { + "angel": 0, + "pos-x": 3.8121546961325965, + "pos-y": -0.99000000000000021, + "skills": [ + { + "flag": false, + "name": "MoveSkill", + "primary": 0, + "secondary": 0 + } + ], + "tolerance": 30 + }, + { + "angel": 0, + "pos-x": 4.9834254143646408, + "pos-y": -1.7887500000000003, + "skills": [ + { + "flag": false, + "name": "MoveSkill", + "primary": 0, + "secondary": 0 + } + ], + "tolerance": 30 + } + ] + } + ], + "ballInitPos": { + "x": 1.9116022099447516, + "y": -3.9712499999999995 + }, + "chance": 1, + "lastDist": 1.5, + "maxEffective": 2, + "minNeeded": 2, + "planMode": "INDIRECT", + "tags": [ + "" + ] + } + ] +} diff --git a/parsian_ai/scripts/plan_server/docWatch.py b/parsian_ai/scripts/plan_server/docWatch.py index db135a15..5f2bcb5a 100644 --- a/parsian_ai/scripts/plan_server/docWatch.py +++ b/parsian_ai/scripts/plan_server/docWatch.py @@ -1,481 +1,364 @@ -import time -import os -import signal -import sys +#!/usr/bin/env python -import re +import rospy +import os import json -import random import math - -from parsian_msgs.msg import parsian_plan -from parsian_msgs.msg import parsian_plan_GUI - -from watchdog.observers import Observer +from random import randint from watchdog.events import FileSystemEventHandler -import rospkg - - -class Watcher: - DIRECTORY_TO_WATCH = "" - - def __init__(self): - signal.signal(signal.SIGINT, self.signal_handler) - self.path = rospkg.RosPack().get_path("parsian_ai") - print ("pack path: " + self.path) - self.path += "/plans/" - self.__observer = Observer() - # DIRECTORY_TO_WATCH = DIRECTORY_TO_WATCH + self.__p - self.__event_handler = Handler(self.path) - # l=[] - # l.append("/home/fateme/Workspace/parsian_ws/src/parsian_ssl/parsian_ai/plans/2Pass/pass.json") - # self.__event_handler.update_master_active(l, True, True) - - def run(self): - self.__observer.schedule(self.__event_handler, self.path, recursive=True) - self.__observer.start() - try: - while True: - time.sleep(5) - except: - self.__observer.stop() - # print("Error") - self.__observer.join() - sys.exit(0) - - def get_all_plans(self): - return self.__event_handler.get_all_plans_gui_msgs() - - def update_master_active(self, name_list, plan_index, is_master, is_active): - if len(name_list) != len(plan_index): - print("not all plans are indexed") - return None - return self.__event_handler.update_master_active(name_list, plan_index, is_master, is_active) +from parsian_msgs.msg import parsian_plan +from parsian_msgs.msg import vector2D +from parsian_msgs.msg import parsian_plan_agent +from parsian_msgs.msg import parsian_plan_position +from parsian_msgs.msg import parsian_plan_skill +from parsian_msgs.srv import plan_service +from parsian_msgs.srv import plan_serviceResponse +from parsian_msgs.srv import plan_serviceRequest +from parsian_msgs.srv import parsian_update_plansRequest +from parsian_msgs.msg import parsian_playoff_client - def choose_plan(self, player_num, game_mode, ball_x, ball_y): - return self.__event_handler.choose_plan(player_num, game_mode, ball_x, ball_y) - def signal_handler(self, signal, frame): - print("\nctrl+C pressed!") - sys.exit(0) -class Handler(FileSystemEventHandler): - def __init__(self, p): - self.__final_list = [] - self.__ignore = [] - self.__shuffleCount = 0 - self.__final_dict = [] - self.__all_master_plans = {} - self.__all_active_plans = {} - global final_list, shuffleCount, path - path = p - # read_plan(f[5]) - self.__final_list = self.list_valid_plans(p) - self.plans_to_dict() +class Watcher(FileSystemEventHandler): + patterns = ['*.json'] + def __init__(self, path): + #initiate + self.path = path + rospy.Timer(rospy.Duration(0.5), self.my_callback) + self.is_newEvent_happend = True + self.client_pub = None - self.shuffle_indexing(self.__final_list) # call once + #update_plans + self.all_jsons_root = [] #all files with json extension + self.all_ignoredjsons_root = [] #all ignored files with json extension + self.all_desiredjsons_root = [] #all json files that are not in ignore file + self.all_badjsons_root = [] #all json files that cant be opend + self.desired_plans = {} #all desired plans -> filepath: [parsian_plan] + self.last_ai_respond = None - # self.choose_plan(4, 3) - # print (self.message_generator(self.__final_dict[0])) - # @staticmethod def on_any_event(self, event): - if event.is_directory: - return None - - elif event.event_type == 'modified': - # print("Received modified event - %s" % event.src_path) - # self.add_plan(event.src_path) - self.refresh() - - elif event.event_type == 'deleted': - # print("Received deleted event - %s" % event.src_path) - # self.remove_plan(event.src_path) - self.refresh() - - elif event.event_type == 'created': - self.refresh() - # print("Received created event - %s." % event.src_path) - - def list_valid_plans(self, path): - file_list = [] - for path, dirs, files in os.walk(path): - for filename in files: - fname = os.path.join(path, filename) - # printfname.rsplit('/', 1)[1] # just file name - file_list.append(fname) - - ignore_lst = [] - for f in file_list: # find ignore list - if f != None: - if f.endswith('.ignore'): - ignore_lst = open(str(f)).read().split("\n") - file_list.remove(f) - break - - if '' in ignore_lst: - ignore_lst.remove('') - - no_cmnt_ignr_lst = [li for li in ignore_lst if not li.startswith('#')] - print(no_cmnt_ignr_lst) - self.__ignore = no_cmnt_ignr_lst - - bad_files = [] - for f in file_list: - if f != None: - if f.endswith('.json'): - # if os.stat(str(f)).st_size != 0: - # print("adding " + str(f).rsplit('/', 1)[1] - # with open(str(f)) as json_data: - # plan_data.append(json.load(json_data)) - if os.stat(str(f)).st_size == 0: - bad_files.append(f) - print("empty: " + str(f) + " --> removed!") - else: - bad_files.append(f) - # print("not json: " + str(f) + " --> removed!") - - file_list2 = [f for f in file_list if f not in bad_files] - # printfile_list2 - - self.__final_list = self.ignore_plans(file_list2, ignore_lst) - return self.__final_list - - def add_plan(self, path_to_plan): - flag = 0 - print ("adding plan...") - if str(path_to_plan).endswith(".json"): - if not (str(path_to_plan).split("/plans")[1]) in self.__ignore: - for pattern in self.__ignore: - if pattern != '': - try: - if (re.search(pattern, str(path_to_plan).split("/plans")[1]) and - re.search(pattern, str(path_to_plan).split("/plans")[1]).start() == 0): - flag = 1 - except: - print("Invalid Expression: " + pattern) - else: - flag = 1 - - if flag == 0: - self.__final_list.append(path_to_plan) - print(str(path_to_plan).split("/plans")[1] + " added.") - - return self.__final_list - - def remove_plan(self, path_to_plan): - if path_to_plan in self.__final_list: - print(str(path_to_plan).split("/plans")[1] + " removed.") - self.__final_list.remove(path_to_plan) - - def refresh(self): - global path - self.__final_list = self.list_valid_plans(path) - self.shuffle_indexing(self.__final_list) - self.plans_to_dict() - - def ignore_plans(self, file_list, ignore_list): - # files: - new_file_list = [fi for fi in file_list if (str(fi).split("/plans")[1] not in ignore_list)] - new_file_list2 = [fi for fi in file_list if fi not in new_file_list] - for pattern in ignore_list: - if pattern != '': + self.is_newEvent_happend = True + + ##update all plans every 0.5 sec if a new event happend in directory + def my_callback(self, event): + if not self.is_newEvent_happend: + return + + self.is_newEvent_happend = False + + self.get_all_jsons_root() + + self.get_all_ignoredjsons_root() + + self.get_all_desiredjsons_root() + + self.get_desired_plans() + + self.publish_information() + + def get_all_jsons_root(self): + self.all_jsons_root = [] + for root, dirs, files in os.walk(self.path, topdown=False): + for name in files: + if name.lower().endswith(".json"): + self.all_jsons_root.append(os.path.join(root, name)) + + def get_all_ignoredjsons_root(self): + os.chdir(self.path) + self.all_ignoredjsons_root = [] + if not os.path.exists("plans.ignore") and not os.path.isfile("plans.ignore"): + rospy.loginfo("plans.ignore not found") + return + clear_comment_lines = []#ignore all commented parts + with open("plans.ignore") as file: + for line in file: + if line.find('#') == 0: + continue + elif line.find('#') > 0: + clear_comment_lines.append(line[0: line.find('#')].rstrip()) + elif not len(line.strip()) == 0:#not a blank line + clear_comment_lines.append(line.rstrip()) + + for i in range(len(clear_comment_lines)): + if clear_comment_lines[i].startswith('/'): + clear_comment_lines[i] = clear_comment_lines[i][1:] + + + for line in clear_comment_lines: + os.chdir(self.path) + if os.path.isfile(line) and line.lower().endswith(".json"): + self.all_ignoredjsons_root.append(os.path.join(self.path, line)) + elif os.path.isdir(os.path.join(self.path, line)): + for root, dirs, files in os.walk(os.path.join(self.path, line), topdown=False): + for name in files: + if name.lower().endswith(".json"): + self.all_ignoredjsons_root.append(os.path.join(root, name)) + + def get_all_desiredjsons_root(self): + self.all_desiredjsons_root = [] + self.all_badjsons_root = [] + not_commented = [] + for line in self.all_jsons_root: + if not line in self.all_ignoredjsons_root: + not_commented.append(line) + + for line in not_commented: + is_correct = True + with open(line) as json_file: try: - for f in new_file_list: - if re.search(pattern, str(f).split("/plans")[1]) and re.search(pattern, str(f).split("/plans")[1]).start() == 0: - new_file_list2.append(f) + json.load(json_file) except: - print("Invalid Expression: " + pattern) - - print("ignored plans:") - for fil in new_file_list2: - str_list = str(fil).split("/plans") - if len(str_list) > 0: - print("\t" + str(fil).split("/plans")[1]) - - last = [term for term in new_file_list if not term in new_file_list2] - - print("FINAL LIST:") - for i in last: - print("\t" + str(i).split("/plans")[1]) - - return last - - def shuffle_indexing(self, alist): - random.shuffle(alist) - - def choose_plan(self, player_num, game_mode, ball_x, ball_y): - # DIRECT = 1 INDIRECT = 2 KICKOFF = 3 - plan_mode = "" - if game_mode == 1: - plan_mode = "DIRECT" - elif game_mode == 2: - plan_mode = "INDIRECT" - elif game_mode == 3: - plan_mode = "KICKOFF" - - sublist = [] - rad = 1 - - active_list = self.get_master_active_plans(self.__final_dict) - - for plan in active_list: - if self.check_plan(plan, ball_x, ball_y, rad, player_num, plan_mode): - sublist.append(plan) - - if len(sublist) > 0: - print("# active and valid plans: "+str(len(sublist))+"\n") - i = self.__shuffleCount % len(sublist) - self.__shuffleCount += 1 - print ("\n" + sublist[i]["filename"].split("plans/")[1] + - ": "+str(sublist[i]["index"]) + " " + str(sublist[i]["planMode"])) - return self.ai_message_generator(sublist[i]) - else: - print ("of invalid plans ...") - return self.nearest_plan(player_num, ball_x, ball_y, plan_mode) - - def get_master_active_plans(self, plan_list): - master_list = [] - active_list = [] + is_correct = False + if is_correct: + self.all_desiredjsons_root.append(line) + else: + self.all_badjsons_root.append(line) + + def get_desired_plans(self): + last_desired_plans = self.desired_plans.copy() + self.desired_plans = {} + for root in self.all_desiredjsons_root: + res = self.generate_parsianplan_from_json(root) + if res != None: + self.desired_plans[root] = res + + #check for plans that if they were active or master the last time + self.check_desired_plans_history(last_desired_plans) + + def generate_parsianplan_from_json(self, planpath): + plan_json = None + with open(planpath) as json_file: + try: + plan_json = json.load(json_file) + except: + return False + try: + plan_message = parsian_plan() + plan_message.planFile = planpath + plan_message.isActive = True #need change in client request + plan_message.isMaster = False #need change in client request + plan_message.symmetry = False #need change in ai requests + plan_message.chance = plan_json["plans"][0]["chance"] #could be toggled in client or ai in future + plan_message.lastDist = plan_json["plans"][0]["lastDist"] + + ##start agentsize + #all agents that thier initPos isnt -100, except for the first kicker + agentSize = 0 + agentSize += 1 #first kicker + for agentsPos in plan_json["plans"][0]["agentInitPos"]: + if agentsPos["x"] != -100: + agentSize += 1 + plan_message.agentSize = agentSize + ##finish agentsize + + plan_message.tags = [str(tag) for tag in plan_json["plans"][0]["tags"]] + plan_message.planMode = str(plan_json["plans"][0]["planMode"]) + + ##start ballInitPos + ballpos = vector2D() + ballpos.x = plan_json["plans"][0]["ballInitPos"]["x"] + ballpos.y = plan_json["plans"][0]["ballInitPos"]["y"] + plan_message.ballInitPos = ballpos + ##finish ballInitPos + + plan_message.successRate = 0 #could be toggled in client or ai in future + plan_message.planRepeat = 0 #could be toggled here in future + + ##start agentInitPos + allinitpos = [] + for initpos in plan_json["plans"][0]["agentInitPos"]: + initpos_tmp = vector2D() + initpos_tmp.x = initpos["x"] + initpos_tmp.y = initpos["y"] + allinitpos.append(initpos_tmp) + plan_message.agentInitPos[0:len(allinitpos)] = allinitpos + ##finish agentInitPos + + ##start agents + agents = [] + for agent in plan_json["plans"][0]["agents"]: + agent_tmp = parsian_plan_agent() + agent_tmp.id = agent["ID"] + positions = [] + for position in agent["positions"]: + position_tmp = parsian_plan_position() + position_tmp.angel = position["angel"] + position_tmp.pos.x = position["pos-x"] + position_tmp.pos.y = position["pos-y"] + position_tmp.tolerance = position["tolerance"] + skills = [] + for skill in position["skills"]: + skill_tmp = parsian_plan_skill() + skill_tmp.flag = skill["flag"] + skill_tmp.name = str(skill["name"]) + skill_tmp.primary = skill["primary"] + skill_tmp.secondry = skill["secondary"] + if "target" in skill: + skill_tmp.agent = skill["target"]["agent"] + skill_tmp.index = skill["target"]["index"] + else: + skill_tmp.agent = -1 + skill_tmp.index = -1 + skills.append(skill_tmp) + + position_tmp.skills[0:len(skills)] = skills + position_tmp.skillSize = len(skills) + positions.append(position_tmp) + agent_tmp.positions[0:len(positions)] = positions + agent_tmp.posSize = len(positions) + agents.append(agent_tmp) + plan_message.agents[0:len(agents)] = agents + ##finish agents + + return plan_message + except: + return None - for plan in plan_list: - if plan["isMaster"]: - master_list.append(plan) + def choose_plan(self, req): + #check for master plan + for plan in self.desired_plans: + if self.desired_plans[plan].isMaster: + response = plan_serviceResponse() + response.the_plan = self.desired_plans[plan] + isMatched, isSymmetry = self.check_ballPos(plan, req.plan_req.ballPos.x, req.plan_req.ballPos.y) + response.the_plan.symmetry = isSymmetry + response.time_us = 0 + self.last_ai_respond = plan + self.publish_information() + return response + + #no master plans + all_matched_plans = self.get_all_matched_plans(req.plan_req.gameMode, req.plan_req.playersNum, req.plan_req.ballPos.x, req.plan_req.ballPos.y)#{planpath: isSymmetric} + + if len(all_matched_plans.keys()) == 0: + self.last_ai_respond = None + return + shuffle = randint(0, len(all_matched_plans.keys()) - 1) + + response = plan_serviceResponse() + response.the_plan = self.desired_plans[all_matched_plans.keys()[shuffle]] + response.the_plan.symmetry = all_matched_plans[all_matched_plans.keys()[shuffle]] + response.time_us = 0 + self.last_ai_respond = all_matched_plans.keys()[shuffle] + self.publish_information() + return response + + def get_all_matched_plans(self, gameMode, playersNum, ballPosX, ballPosY): + matched = {} + if gameMode == 3:#KICKOFF + for plan in self.desired_plans: + if self.desired_plans[plan].planMode == "KICKOFF": + if self.desired_plans[plan].agentSize >= playersNum and self.desired_plans[plan].chance > 0 and self.desired_plans[plan].lastDist >= 0 and self.desired_plans[plan].isActive: + matched[plan] = False#not symmetric - if len(master_list) > 0: - active_list = master_list - else: - for plan in plan_list: - if plan["isActive"]: - active_list.append(plan) - - return active_list - - def check_plan(self, plan, ball_x, ball_y, rad, player_num, plan_mode): - DIRECT = 1 - INDIRECT = 2 - KICKOFF = 3 - - planSize = self.plan_size(plan) - - if self.circle_contains(ball_x, ball_y, rad, plan["ballInitPos"]["x"], plan["ballInitPos"]["y"]): - # print("Ball Pos Matched") - if planSize >= player_num and plan["chance"] > 0 and plan["lastDist"] >= 0: - if plan_mode == KICKOFF: - if plan["planMode"] == KICKOFF: - plan["symmetry"] = False - return True - elif plan["planMode"] != KICKOFF: - plan["symmetry"] = False - return True - - if self.circle_contains(ball_x, -ball_y, rad, plan["ballInitPos"]["x"], plan["ballInitPos"]["y"]): - # print("Ball Symm Pos Matched") - if planSize >= player_num and plan["chance"] > 0 and plan["lastDist"] >= 0: - if plan_mode == KICKOFF: - if plan["planMode"] == KICKOFF: - plan["symmetry"] = True - return True - elif plan["planMode"] != KICKOFF: - plan["symmetry"] = True - return True - return False - - def plan_size(self, plan): - i = 0 - for pos in plan["agentInitPos"]: - if pos["x"] != -100: - i += 1 - return i+1 - - def nearest_plan(self, player_num, ball_x, ball_y, plan_mode): - DIRECT = 1 - INDIRECT = 2 - KICKOFF = 3 - - player_num_filter = [] - - for plan in self.__final_dict: - planAgentSize = self.plan_size(plan) - if planAgentSize >= player_num: - player_num_filter.append(plan) - - active_list = self.get_master_active_plans(player_num_filter) - - sublist = sorted(active_list, key=lambda x: self.ball_dist( - x, x["ballInitPos"]["x"], x["ballInitPos"]["y"], ball_x, ball_y)) - - subsublist = [] - if len(sublist) > 0: - for plan in sublist: - if plan_mode == KICKOFF: - if plan["planMode"] == KICKOFF: - subsublist.append(plan) - elif plan["planMode"] != KICKOFF: - subsublist.append(plan) - - print("# active and valid plans after mode check: " + str(len(subsublist)) + "\n") - - if len(subsublist) > 0: - print("# active and valid plans: " + str(len(subsublist)) + "\n") - - print ("\n" + subsublist[0]["filename"].split("plans/")[1] + - ": " + str(subsublist[0]["index"]) + " " + str(subsublist[0]["planMode"])) - return self.ai_message_generator(subsublist[0]) else: - print ("There is No master or active plan with proper number of players :/") - return None - - @staticmethod - def circle_contains(x, y, r, point_x, point_y): - if (x - point_x) * (x - point_x) + (y - point_y) * (y - point_y) > r * r: - return False + for plan in self.desired_plans: + if self.desired_plans[plan].agentSize >= playersNum and self.desired_plans[plan].chance > 0 and self.desired_plans[plan].lastDist >= 0 and self.desired_plans[plan].isActive: + isMatched, isSymmetry = self.check_ballPos(plan, ballPosX, ballPosY) + if isMatched: + matched[plan] = isSymmetry + return matched + + def check_ballPos(self, plan, ballPosX, ballPosY): + actual_distX = self.desired_plans[plan].ballInitPos.x - ballPosX + actual_distY = self.desired_plans[plan].ballInitPos.y - ballPosY + actual_dist = math.sqrt(math.pow(actual_distX, 2) + math.pow(actual_distY, 2)) + + symm_distX = self.desired_plans[plan].ballInitPos.x - ballPosX + symm_distY = -self.desired_plans[plan].ballInitPos.y - ballPosY + symm_dist = math.sqrt(math.pow(symm_distX, 2) + math.pow(symm_distY, 2)) + + if actual_dist <= symm_dist and actual_dist < 2: + return (True, False) #isMatched - isSymmetry + if actual_dist > symm_dist and symm_dist < 2: + return (True, True) #isMatched - isSymmetry else: - return True - - @staticmethod - def ball_dist(plan, x1, y1, x2, y2): - a = (x1 - x2) * (x1 - x2) + (y1 - y2) * (y1 - y2) - b = (x1 - x2) * (x1 - x2) + (y1 + y2) * (y1 + y2) - if a < b: - plan["symmetry"] = False - return math.sqrt(a) + return (False, False) + + def gui_request(self, req): + os.chdir(self.path) + if req.Mode == 1:#ACTIVATE + for path in req.Plans: + path = os.path.join(self.path, path) + if os.path.isfile(path) and path in self.desired_plans.keys(): + self.desired_plans[path].isActive = True + elif os.path.isdir(path): + for root, dirs, files in os.walk(path, topdown=False): + for name in files: + name = os.path.join(root, name) + if name in self.desired_plans.keys(): + self.desired_plans[name].isActive = True + + elif req.Mode == 2:#DEACTIVATE + for path in req.Plans: + path = os.path.join(self.path, path) + if os.path.isfile(path) and path in self.desired_plans.keys(): + self.desired_plans[path].isActive = False + elif os.path.isdir(path): + for root, dirs, files in os.walk(path, topdown=False): + for name in files: + name = os.path.join(root, name) + if name in self.desired_plans.keys(): + self.desired_plans[name].isActive = False + + elif req.Mode == 3:#MASTER + if req.Plans[0] != '' or req.Plans[0] != None: + path = os.path.join(self.path, req.Plans[0]) + if os.path.isfile(path) and path in self.desired_plans.keys(): + for plan in self.desired_plans: + self.desired_plans[plan].isMaster = False + self.desired_plans[path].isMaster = True + + elif req.Mode == 4:#DEMASTER + for plan in self.desired_plans: + self.desired_plans[plan].isMaster = False + + elif req.Mode == 5:#ACTIVATE_ALL + for plan in self.desired_plans: + self.desired_plans[plan].isActive = True + + elif req.Mode == 6:#DEACTIVATE_ALL + for plan in self.desired_plans: + self.desired_plans[plan].isActive = False + + self.publish_information() + + # print("active") + # for plan in self.desired_plans: + # if self.desired_plans[plan].isActive: + # print(plan) + # print("master") + # for plan in self.desired_plans: + # if self.desired_plans[plan].isMaster: + # print(plan) + # print("--------------------------------------") + + def check_desired_plans_history(self, last_desired_plans): + for plan in self.desired_plans: + if plan in last_desired_plans.keys(): + self.desired_plans[plan].isActive = last_desired_plans[plan].isActive + self.desired_plans[plan].isMaster = last_desired_plans[plan].isMaster + + def publish_information(self): + message = parsian_playoff_client() + if self.last_ai_respond == None: + message.last_ai_response = "" else: - plan["symmetry"] = True - return math.sqrt(b) - - def get_all_plans_gui_msgs(self): - self.refresh(); - plans_msg = [] - for plan in self.__final_dict: - plans_msg.append(self.gui_message_generator(plan)) - return plans_msg - - def update_master_active(self, name_list, plan_index_list, is_master, is_active): - for plan in self.__final_dict: - for i in range(0, len(name_list)): - if plan["filename"] == name_list[i] and plan["index"] == plan_index_list[i]: - plan["isMaster"] = is_master - plan["isActive"] = is_active - self.__all_active_plans[(plan["filename"], plan_index_list[i])] = is_active - self.__all_master_plans[(plan["filename"], plan_index_list[i])] = is_master - - for plan in self.__final_dict: - print(plan["filename"] + ": " + "master: " + str(plan["isMaster"]) + " , " + "active: " + str(plan["isActive"])) - return self.get_all_plans_gui_msgs() - - def plans_to_dict(self): - self.__final_dict = [] - # print ("plans_to_dict") - # print (len(self.__final_list)) - for plan in self.__final_list: - with open(str(plan)) as json_data: - tmp = json.load(json_data) - for i in range(0, len(tmp["plans"])): - dict1 = tmp["plans"][i] - dict1.update({"index": i}) - dict1.update({"filename": str(plan)}) - dict1.update({"successRate": 0}) - dict1.update({"planRepeat": 0}) - dict1.update({"symmetry": False}) - - if (dict1["filename"], dict1["index"]) in self.__all_active_plans: - dict1.update({"isActive": self.__all_active_plans[dict1["filename"], dict1["index"]]}) - else: - dict1.update({"isActive": True}) - - if (dict1["filename"], dict1["index"]) in self.__all_master_plans: - dict1.update({"isMaster": self.__all_master_plans[dict1["filename"], dict1["index"]]}) - else: - dict1.update({"isMaster": False}) - - self.__final_dict.append(dict1) - - return self.__final_dict - - def gui_message_generator(self, plan_dict): - plan_gui_msg = parsian_plan_GUI() - plan_gui_msg.isActive = plan_dict["isActive"] - plan_gui_msg.isMaster = plan_dict["isMaster"] - plan_gui_msg.planFile = plan_dict["filename"] - plan_gui_msg.agentSize = self.plan_size(plan_dict) - plan_gui_msg.chance = plan_dict["chance"] - plan_gui_msg.lastDist = plan_dict["lastDist"] - plan_gui_msg.tags = plan_dict["tags"] - plan_gui_msg.planMode = plan_dict["planMode"] - plan_gui_msg.ballInitPos.x = plan_dict["ballInitPos"]["x"] - plan_gui_msg.ballInitPos.y = plan_dict["ballInitPos"]["y"] - plan_gui_msg.planRepeat = plan_dict["planRepeat"] - plan_gui_msg.successRate = plan_dict["successRate"] - - return plan_gui_msg - - def ai_message_generator(self, plan_dict): - plan_msg = parsian_plan() - plan_msg.isActive = plan_dict["isActive"] - plan_msg.isMaster = plan_dict["isMaster"] - plan_msg.planFile = plan_dict["filename"] - plan_msg.agentSize = self.plan_size(plan_dict) - plan_msg.chance = plan_dict["chance"] - plan_msg.lastDist = plan_dict["lastDist"] - plan_msg.tags = plan_dict["tags"] - plan_msg.planMode = plan_dict["planMode"] - plan_msg.ballInitPos.x = plan_dict["ballInitPos"]["x"] - plan_msg.ballInitPos.y = plan_dict["ballInitPos"]["y"] - plan_msg.planRepeat = plan_dict["planRepeat"] - plan_msg.successRate = plan_dict["successRate"] - plan_msg.symmetry = plan_dict["symmetry"] - - i = 0 - j = 0 - k = 0 - for agent in plan_dict["agents"]: - plan_msg.agents[i].id = agent["ID"] - plan_msg.agents[i].posSize = len(agent["positions"]) - j = 0 - for pos in agent["positions"]: - # print ("---------------------------------------------pos "+str(i)) - plan_msg.agents[i].positions[j].angel = pos["angel"] - plan_msg.agents[i].positions[j].pos.x = pos["pos-x"] - plan_msg.agents[i].positions[j].pos.y = pos["pos-y"] - plan_msg.agents[i].positions[j].tolerance = pos["tolerance"] - plan_msg.agents[i].positions[j].skillSize = len(pos["skills"]) - k = 0 - for skill in pos["skills"]: - plan_msg.agents[i].positions[j].skills[k].flag = skill["flag"] - plan_msg.agents[i].positions[j].skills[k].name = skill["name"] - plan_msg.agents[i].positions[j].skills[k].primary = skill["primary"] - plan_msg.agents[i].positions[j].skills[k].secondry = skill["secondary"] - if "target" in skill: - plan_msg.agents[i].positions[j].skills[k].agent = skill["target"]["agent"] - plan_msg.agents[i].positions[j].skills[k].index = skill["target"]["index"] - else: - plan_msg.agents[i].positions[j].skills[k].agent = -1 - plan_msg.agents[i].positions[j].skills[k].index = -1 - k += 1 - j += 1 - i += 1 - - i = 0 - for pos in plan_dict["agentInitPos"]: - plan_msg.agentInitPos[i].x = pos["x"] - plan_msg.agentInitPos[i].y = pos["y"] - i += 1 - - return plan_msg - - -if __name__ == '__main__': - # w = Watcher() - # w.run() - path = rospkg.RosPack().get_path("parsian_ai") - print ("pack path: " + path) - path += "/plans/" - - event_handler = Handler(path) - event_handler.nearest_plan(2, 2, 2, 1) - event_handler.get_all_plans_gui_msgs() + message.last_ai_response = self.last_ai_respond[len(self.path) + 1:len(self.last_ai_respond)] + for plan in self.desired_plans: + message.desired_plans.append(plan[len(self.path) + 1:len(plan)]) + if self.desired_plans[plan].isActive: + message.active_plans.append(plan[len(self.path) + 1:len(plan)]) + if self.desired_plans[plan].isMaster: + message.master_plan = plan[len(self.path) + 1:len(plan)] + for plan in self.all_ignoredjsons_root: + message.ignored_plans.append(plan[len(self.path) + 1:len(plan)]) + + if self.client_pub != None: + self.client_pub.publish(message) + diff --git a/parsian_ai/scripts/plan_server/planserver_node.py b/parsian_ai/scripts/plan_server/planserver_node.py index 235a72f5..c4f10bc0 100755 --- a/parsian_ai/scripts/plan_server/planserver_node.py +++ b/parsian_ai/scripts/plan_server/planserver_node.py @@ -1,55 +1,46 @@ #!/usr/bin/env python -# license removed for brevity import rospy -from parsian_msgs.srv import * -import docWatch -import time +import rospkg +import os +from docWatch import Watcher +from watchdog.observers import Observer +from parsian_msgs.srv import plan_service +from parsian_msgs.srv import parsian_update_plans +from parsian_msgs.msg import parsian_playoff_client -class getPlan: + +class PlanServer: def __init__(self): + self.path = os.path.join(rospkg.RosPack().get_path("parsian_ai"), "plans")#/home/kian/parsian_ws/src/parsian_ssl/parsian_ai/plans + self.watcher = Watcher(self.path) + + self.ai_service = rospy.Service('get_plans', plan_service, self.handle_ai_request) + self.gui_service = rospy.Service('update_plans', parsian_update_plans, self.handle_gui_request) + self.client_pub = rospy.Publisher('playoff_client', parsian_playoff_client, queue_size=1, latch=True) + self.watcher.client_pub = self.client_pub + + self.observer = Observer() + self.observer.schedule(self.watcher, self.path, recursive=True) + self.observer.start() + + def handle_ai_request(self, req): + res = self.watcher.choose_plan(req) + # print("chooso: ", res.the_plan.planFile) + return res + + def handle_gui_request(self, req): + self.watcher.gui_request(req) + - rospy.init_node('plan_server') - self.s1 = rospy.Service('update_plans', parsian_update_plans, self.handle_gui_plan_request) - self.s2 = rospy.Service('get_plans', plan_service, self.handle_plan_request) - self.__w = docWatch.Watcher() - self.response = parsian_update_plansResponse() - self.__w.run() - - def handle_gui_plan_request(self, req): - # type:(parsian_update_plansRequest) -> req - - print("---> request:") - print(req) - received = req.newPlans - if '' in received: - received.remove('') - if len(received) > 0: - print ("response to gui...... update plans") - self.response.allPlans = self.__w.update_master_active(received, req.index, req.isMaster, req.isActive) - else: - print ("response to gui...... return all plans") - self.response.allPlans = self.__w.get_all_plans() - self.response.allPlans = sorted(self.response.allPlans, key=lambda x: x.planFile) - return self.response - - def handle_plan_request(self, req): - # type: (plan_serviceRequest) -> req - t = int(round(time.time() * 1000000)) - response = plan_serviceResponse() - out = self.__w.choose_plan(req.plan_req.playersNum, req.plan_req.gameMode, req.plan_req.ballPos.x, req.plan_req.ballPos.y) - if out is not None: - t2 = int(round(time.time() * 1000000)) - t - response.the_plan = out - response.time_us = t2 - print("REQUEST:: # plan players: " + str(response.the_plan.agentSize) + " # req players: " + str(req.plan_req.playersNum) + " game mode: " + str(req.plan_req.gameMode) + "\n") - print ("time: " + str(t2) + " ms") - return response - else: - print ("No plan sent :(") if __name__ == '__main__': - g = getPlan() - rospy.spin() + try: + rospy.init_node('plan_server', anonymous=True) + rospy.loginfo("plan_server is running") + planServer = PlanServer() + rospy.spin() + except rospy.ROSInterruptException: + pass diff --git a/parsian_ai/src/parsian_ai/ai.cpp b/parsian_ai/src/parsian_ai/ai.cpp index ba5bf872..04978e0a 100644 --- a/parsian_ai/src/parsian_ai/ai.cpp +++ b/parsian_ai/src/parsian_ai/ai.cpp @@ -63,35 +63,48 @@ parsian_msgs::parsian_robot_task AI::getTask(int robotID) { } } - + if(conf.parsianWorkshop) + validateRobotTask(&robotsTask[robotID]); return robotsTask[robotID]; } +void AI::validateRobotTask(parsian_msgs::parsian_robot_task* task) +{ + int flag = -1; + if(teamConfig.side == teamConfig.LEFT) + flag = 1; + switch (task->select) { + case task->GOTOPOINTAVOID: + if (task->gotoPointAvoidTask.base.targetPos.x * flag > 0) task->gotoPointAvoidTask.base.targetPos.x = -0.7 * flag; + break; + case task->GOTOPOINT: + if (task->gotoPointTask.targetPos.x * flag > 0) task->gotoPointTask.targetPos.x = -0.7 * flag; + break; + case task->RECIVEPASS: + if (task->receivePassTask.target.x * flag > 0) task->receivePassTask.target.x = -0.7 * flag; + break; + case task->ONETOUCH: + if (task->oneTouchTask.waitPos.x * flag > 0) task->oneTouchTask.waitPos.x = -0.7 * flag; + break; + default: + break; + } +} + void AI::updateRobotStatus(const parsian_msgs::parsian_robotConstPtr & _rs) { } -void AI::updateRobotFaults(const parsian_msgs::parsian_robot_fault & _rs) +void AI::updateRobotSubstitutes(const parsian_msgs::parsian_robot_substitution &_rs) { - if(_rs.select == 0) - { - soccer->agents[_rs.robot_id]->fault = false; - soccer->agents[_rs.robot_id]->faultstate = Agent::FaultState::HEALTHY; - } - if(_rs.select == 1) - { - ROS_INFO_STREAM("kian: " << soccer->agents[_rs.robot_id]->id() << " disrepaired"); - } - if(_rs.select == 2) + for(int i{}; i < _rs.substitutional_IDs.size(); i++) { - soccer->agents[_rs.robot_id]->fault = true; - soccer->agents[_rs.robot_id]->faultstate = Agent::FaultState::DAMEGED; - } - if(_rs.select == 3) - { - soccer->agents[_rs.robot_id]->fault = true; - soccer->agents[_rs.robot_id]->faultstate = Agent::FaultState::DESTROYED; + if(_rs.substitutional_IDs[i]) + soccer->agents[i]->substitutePermission = true; + else + soccer->agents[i]->substitutePermission = false; } + } void AI::updateWM(const parsian_msgs::parsian_world_modelConstPtr & _wm) { diff --git a/parsian_ai/src/parsian_ai/ai_nodelet.cpp b/parsian_ai/src/parsian_ai/ai_nodelet.cpp index c8de39ad..006ac51c 100644 --- a/parsian_ai/src/parsian_ai/ai_nodelet.cpp +++ b/parsian_ai/src/parsian_ai/ai_nodelet.cpp @@ -23,7 +23,7 @@ void AINodelet::onInit() { teamConfSub = nh.subscribe("/team_config", 100, &AINodelet::teamConfCb, this); mousePosSub = nh.subscribe("/mousePos", 100, &AINodelet::mousePosCb, this); forceRefereeSub = nh.subscribe("/force_referee", 100, &AINodelet::forceRefereeCallBack, this); - robotfaultSub = nh.subscribe("/autofault", 100, &AINodelet::faultdetectionCallBack, this); + robotSubstituteSub = nh.subscribe("/substitute", 100, &AINodelet::substitutedetectionCallBack, this); drawPub = nh.advertise("/draws", 1000); @@ -64,8 +64,8 @@ void AINodelet::refereeCallBack(const parsian_msgs::ssl_refree_wrapperConstPtr & ai->updateReferee(_ref); } -void AINodelet::faultdetectionCallBack(const parsian_msgs::parsian_robot_fault & _rs) { - ai->updateRobotFaults(_rs); +void AINodelet::substitutedetectionCallBack(const parsian_msgs::parsian_robot_substitution & _rs) { + ai->updateRobotSubstitutes(_rs); } void AINodelet::forceRefereeCallBack(const parsian_msgs::ssl_force_refereeConstPtr & _command){ diff --git a/parsian_ai/src/parsian_ai/coach.cpp b/parsian_ai/src/parsian_ai/coach.cpp index f0e82dd9..74b30654 100644 --- a/parsian_ai/src/parsian_ai/coach.cpp +++ b/parsian_ai/src/parsian_ai/coach.cpp @@ -29,6 +29,7 @@ CCoach::CCoach(Agent**_agents) ourBallPlacement = new COurBallPlacement; halftimeLineup = new CHalftimeLineup; theirBallPlacement = new CTheirBallPlacement; + substitution = new CSubstitution; // New Plays @@ -62,6 +63,7 @@ CCoach::CCoach(Agent**_agents) i = 0; } firstTime = true; + firsttime_forsubstitution = true; haltAction = new NoAction; @@ -80,36 +82,37 @@ CCoach::~CCoach() { delete stopPlay ; delete ourPlayOff ; delete dynamicAttack ; + delete substitution ; } void CCoach::decidePreferredDefenseAgentsCount() { missMatchIds.clear(); if (gameState->getState() == States::Stop || gameState->getState() == States::Halt || first) { - if (workingIDs.size() != 0u) { + if (wm->our.data->activeAgents.size() != 0u) { robotsIdHist.clear(); - for (int workingID : workingIDs) { - robotsIdHist.append(workingID); + for (int id : wm->our.data->activeAgents) { + robotsIdHist.append(id); } } first = false; } - if (workingIDs.size() > _NUM_PLAYERS) { + if (wm->our.data->activeAgents.size() > _NUM_PLAYERS) { missMatchIds.clear(); - for (int workingID : workingIDs) { + for (int id : wm->our.data->activeAgents) { for (int k = 0 ; k < robotsIdHist.count() ; k++) { - if (robotsIdHist.at(k) == workingID) { + if (robotsIdHist.at(k) == id) { break; } if (k == robotsIdHist.count() - 1) { - missMatchIds.append(workingID); + missMatchIds.append(id); } } } } - int agentsCount = workingIDs.size() - missMatchIds.count(); + int agentsCount = wm->our.data->activeAgents.size() - missMatchIds.count(); if (goalieAgent != nullptr) { if (goalieAgent->isVisible()) { agentsCount--; @@ -124,9 +127,9 @@ void CCoach::decidePreferredDefenseAgentsCount() { preferredDefenseCounts = conf.Defense; } } else if (gameState->isStart()) { - //todo remove this - preferredDefenseCounts = 1; - return; +// //todo remove this +// preferredDefenseCounts = 1; +// return; if (know->variables["transientFlag"].toBool()) { //// Add Playmake after time @@ -149,7 +152,7 @@ void CCoach::decidePreferredDefenseAgentsCount() { // preferredDefenseCounts += agentsCount - 1 - preferredDefenseCounts - selectedPlay->markPlan.findNeededMark(); } } - } else if (gameState->ourPlayOffKick()) { + } else if (gameState->ourIndirectKick() || gameState->ourDirectKick() || gameState->ourFreeKick() || gameState->ourKickoff()) { if (wm->ball->pos.x < 1) { preferredDefenseCounts = (selectedPlay->defensePlan.findNeededDefense() == 1) ? 1 : 2; @@ -186,8 +189,10 @@ void CCoach::decidePreferredDefenseAgentsCount() { void CCoach::assignGoalieAgent(int goalieID) { goalieAgent = nullptr; - if (workingIDs.contains(goalieID)) { - goalieAgent = agents[goalieID]; + if (goalieID != -1){ + if (wm->our.data->activeAgents.contains(goalieID)) { + goalieAgent = agents[goalieID]; + } } } @@ -219,38 +224,6 @@ BallPossesion CCoach::isBallOurs() { return decidePState; } -double CCoach::timeNeeded(Agent *_agentT,const Vector2D& posT, double vMax) { - - double acc; - double dec = 3.5; - Vector2D tAgentVel = _agentT->vel(); - Vector2D tAgentDir = _agentT->dir(); - double dist = 0; - QList _result; - Vector2D _target; - double tAgentVelTanjent = tAgentVel.length() * cos(Vector2D::angleBetween(posT - _agentT->pos() , _agentT->vel().norm()).radian()); - - double vXvirtual = (posT - _agentT->pos()).x; - double vYvirtual = (posT - _agentT->pos()).y; - double veltanV = (vXvirtual) * cos(tAgentDir.th().radian()) + (vYvirtual) * sin(tAgentDir.th().radian()); - double velnormV = -1 * (vXvirtual) * sin(tAgentDir.th().radian()) + (vYvirtual) * cos(tAgentDir.th().radian()); - double accCoef; - - accCoef = atan(std::fabs(veltanV) / std::fabs(velnormV)) / _PI * 2; - acc = accCoef * 4.5 + (1 - accCoef) * 3.5; - double tDec = vMax / dec; - double tAcc = (vMax - tAgentVelTanjent) / acc; - dist = posT.dist(_agentT->pos()); - double dB = tDec * vMax / 2 + tAcc * (vMax + tAgentVelTanjent) / 2; - - if (dist > dB) { - return tAcc + tDec + (dist - dB) / vMax; - } else { - return ((1 / dec) + (1 / acc)) * sqrt(dist * (2 * dec * acc / (acc + dec)) + (tAgentVelTanjent * tAgentVelTanjent / (2 * acc))) - (tAgentVelTanjent) / acc; - } - -} - double CCoach::kickTimeEstimation(Agent * _agent, const Vector2D& target) { Vector2D agentPos = _agent->pos(); Vector2D agentDir = _agent->dir(); @@ -262,16 +235,16 @@ double CCoach::kickTimeEstimation(Agent * _agent, const Vector2D& target) { if(wm->ball->vel.length() < 0.1) { - return timeNeeded(_agent, ballPos + (ballPos - target).norm()*0.1, 4.5); + return know->timeNeeded(_agent, ballPos + (ballPos - target).norm()*0.1, 4.5); } if (Circle2D(agentPos, 0.1).intersection(Segment2D(ballPos, wm->ball->getPosInFuture(0.5)), &s1, &s2)) { finalPos = ballPath.nearestPoint(agentPos); - return timeNeeded(_agent, finalPos, 4.5); + return know->timeNeeded(_agent, finalPos, 4.5); } else { for (double i = 0.5; i < 5; i += 0.1) { finalPos = wm->ball->getPosInFuture(i); - agentTime = timeNeeded(_agent, finalPos - (finalPos-target).norm()*0.1, 4.5); + agentTime = know->timeNeeded(_agent, finalPos - (finalPos-target).norm()*0.1, 4.5); if (agentTime < (i - (0.5))) { return i; } @@ -292,13 +265,18 @@ void CCoach::assignDefenseAgents(int defenseCount) { return; } - QList ids = workingIDs; + QList ids = wm->our.data->activeAgents; if (goalieAgent != nullptr) { ids.removeOne(goalieAgent->id()); } if (playmakeId != -1) { ids.removeOne(playmakeId); } + //remove damaged robots in stop for substitution[substitution] + if(gameState->isStop()) + for(auto id: ids) + if(damagedIDs.contains(id)) + ids.removeOne(id); selectedPlay->defensePlan.fillDefencePositionsTo(defenseTargets); double nearestDist; @@ -438,8 +416,8 @@ double CCoach::findMostPossible(Vector2D agentPos) { obstacles.append(Circle2D(wm->opp.active(i)->pos, 0.1)); } - for (int i = 0 ; i < workingIDs.size() ; i++) { - if (workingIDs[i] != playmakeId) { + for (int i = 0 ; i < wm->our.data->activeAgents.size() ; i++) { + if (wm->our[i]->id != playmakeId) { obstacles.append(Circle2D(wm->our.active(i)->pos, 0.1)); } } @@ -480,7 +458,7 @@ int CCoach::choosePlayMake(const QList &_agentsID){ void CCoach::decideAttack() { // find unused agents! - QList ourPlayersID = workingIDs; + QList ourPlayersID = wm->our.data->activeAgents; if (goalieAgent != nullptr) { ourPlayersID.removeOne(goalieAgent->id()); } @@ -579,7 +557,7 @@ void CCoach::decidePlayOn(QList& ourPlayers, QList& lastPlayers) { if (pushingPenalty.contains(wm->ball->pos)) { dynamicAttack->setDirectShot(true); - } else if (mostPossible > shotToGoalthr) { + } else if (mostPossible > shotToGoalthr || true) { dynamicAttack->setDirectShot(true); shotToGoalthr = conf.DirectTrsh * .6; } else { @@ -673,88 +651,53 @@ void CCoach::checkTransitionToForceStart() { } } -void CCoach::generateWorkingRobotIds() +void CCoach::seperateHealthyAndDamagedRobots() { - workingIDs.clear(); - workingIDs = wm->our.data->activeAgents; + healthyIDs.clear(); + damagedIDs.clear(); for(int i{}; i < _MAX_NUM_PLAYERS; i++) { - if(agents[i] != nullptr) - { - if(agents[i]->fault && agents[i]->faultstate == Agent::FaultState::DESTROYED) - { - if(workingIDs.contains(agents[i]->id())) - { - workingIDs.removeOne(agents[i]->id()); - } - } - if(gameState->isStop() && agents[i]->fault && agents[i]->faultstate == Agent::FaultState::DAMEGED) - { - if(workingIDs.contains(agents[i]->id())) - { - workingIDs.removeOne(agents[i]->id()); - } - } - } + if(!agents[i]->substitutePermission) + healthyIDs.push_back(agents[i]->id()); + else if(agents[i]->substitutePermission) + damagedIDs.push_back(agents[i]->id()); } } void CCoach::replaceFaultedRobots() { - //faulted robots replacement - QList ourPlayers = wm->our.data->activeAgents; - QList faultPlayers; + + QList ouragents; for(int i{}; i < _MAX_NUM_PLAYERS; i++) - { - if(agents[i] != nullptr) - { - if(agents[i]->fault && agents[i]->faultstate == Agent::FaultState::DAMEGED) - { - if(ourPlayers.contains(agents[i]->id())) - { - faultPlayers.push_back(agents[i]->id()); - //ROS_INFO_STREAM("kian:assign to faultPlayers " << agents[i]->id()); - } - } - } - } + if(damagedIDs.contains(agents[i]->id())) + ouragents.push_back(agents[i]); - for (int i = 0; i < faultPlayers.size(); i++) { - faultRoles[i]->assign(agents[faultPlayers.at(i)]); - } - for (auto &faultRole : faultRoles) { - if (faultRole->agent != nullptr) { - faultRole->execute(); - } - } + if(firsttime_forsubstitution) + substitution->init(ouragents); + substitution->execute(ouragents); + +// for(int i{}; i < damagedIDs.size(); i++) +// faultRoles[i]->assign(agents[damagedIDs[i]]); + +// for (auto &faultRole : faultRoles) +// if (faultRole->agent != nullptr) +// faultRole->execute(); } void CCoach::resetNonVisibleAgents() { - for(int i{}; i < _MAX_NUM_PLAYERS; i++) { - if(agents[i] != nullptr) { - bool isvisible{false}; - for(int j{}; j < wm->our.activeAgentsCount(); j++) { - if (agents[i]->id() == wm->our.activeAgentID(j)) isvisible = true; - } - - if(!isvisible) { - ROS_INFO_STREAM("kian: reset: " << agents[i]->id()); - agents[i]->fault = false; - agents[i]->faultstate = Agent::FaultState::HEALTHY; - } - } - } + for(int i{}; i < _MAX_NUM_PLAYERS; i++) + if(!wm->our.data->activeAgents.contains(i)) + agents[i]->substitutePermission = false; } void CCoach::execute() { - resetNonVisibleAgents(); - generateWorkingRobotIds(); - if(gameState->isStop()) replaceFaultedRobots(); + resetNonVisibleAgents();//[substitution] + seperateHealthyAndDamagedRobots();//[substitution] int goalie = findGoalie(); - assignGoalieAgent(goalie); + if(goalie != -1) + assignGoalieAgent(goalie); decidePreferredDefenseAgentsCount(); - // choose playmake agent bool defenseFirst = wm->ball->vel.length() > 1 && wm->field->ourGoalLine().intersection(wm->ball->seg()).isValid(); @@ -779,12 +722,17 @@ void CCoach::execute() for (auto &stopRole : stopRoles) { stopRole->assign(nullptr); } + for (auto &faultRole : faultRoles) { + faultRole->assign(nullptr); + } + decideAttack(); for (auto &stopRole : stopRoles) { if (stopRole->agent != nullptr) { stopRole->execute(); } } + if(gameState->isStop() && damagedIDs.size() > 0) replaceFaultedRobots();//[substitution] } @@ -821,7 +769,7 @@ void CCoach::decideStop(const QList & _ourPlayers) { } for (int i = 0; i < _ourPlayers.size(); i++) { - if(!agents[_ourPlayers.at(i)]->fault) + if(!damagedIDs.contains(_ourPlayers[i]))//[substitution] stopRoles[i]->assign(agents[_ourPlayers.at(i)]); } // _ourPlayers.clear(); TODO: CHECK THIS @@ -845,21 +793,17 @@ void CCoach::decideTheirIndirect(const QList &_ourPlayers) { } void CCoach::decideOurPenalty(QList &_ourPlayers) { - ROS_INFO_STREAM("penalty: decideourpenalty"); - selectedPlay = ourPenalty; - if (0 <= playmakeId && playmakeId <= 11) { + if(conf.PenaltyKickerFromGUI) + ourPenalty->setPlaymake(agents[conf.PenaltyKicker]); + else if (0 <= playmakeId && playmakeId <= 11) { ourPenalty->setPlaymake(agents[playmakeId]); _ourPlayers.removeOne(playmakeId); } if(!gameState->ready()) ourPenalty->setState(PenaltyState::Positioning); - else if(gameState->ready()) - { - ROS_INFO_STREAM("kian: normal start -> penalty"); ourPenalty->setState(PenaltyState::Kicking); - } - DBUG("penalty", D_MHMMD); + selectedPlay = ourPenalty; } void CCoach::decideTheirPenalty(const QList &_ourPlayers) { @@ -950,8 +894,10 @@ bool CCoach::useGoalieInPlayOff() { QList CCoach::remainingAgent() { QList ourPlayers = wm->our.data->activeAgents; - if(ourPlayers.contains(goalieAgent->id())) { - ourPlayers.removeOne(goalieAgent->id()); + if(goalieAgent != nullptr) { + if (ourPlayers.contains(goalieAgent->id())) { + ourPlayers.removeOne(goalieAgent->id()); + } } for (auto& d : defenseAgents) { if (ourPlayers.contains(d->id())) ourPlayers.removeOne(d->id()); @@ -960,21 +906,21 @@ QList CCoach::remainingAgent() { } void CCoach::handlePlayMake(const QList &_agentsID) { - if (!gameState->isStart() || _agentsID.empty()) { - + bool ispenalty = gameState->ourPenaltyKick() || gameState->ourPenaltyShootout(); + bool noPlaymakeNeeded = !gameState->isStart() && !ispenalty; + if (noPlaymakeNeeded || _agentsID.empty()) { playmakeId = -1; lastPlayMake = -1; playMakeIntention.restart(); - } else if (playMakeIntention.elapsed() < conf.playMakeIntention) { + } else if (playMakeIntention.elapsed() < conf.playMakeIntention && lastPlayMake != -1) { playmakeId = lastPlayMake; } else { - - if (lastPlayMake != playmakeId || isBallcollide(3, 30)) playMakeIntention.restart(); playmakeId = choosePlayMake(_agentsID); - + if (lastPlayMake != playmakeId || isBallcollide(3, 30)) playMakeIntention.restart(); } + lastPlayMake = playmakeId; } diff --git a/parsian_ai/src/parsian_ai/gamestate.cpp b/parsian_ai/src/parsian_ai/gamestate.cpp index 5033a690..a993a84c 100644 --- a/parsian_ai/src/parsian_ai/gamestate.cpp +++ b/parsian_ai/src/parsian_ai/gamestate.cpp @@ -23,13 +23,14 @@ void GameState::setRefree(ssl_refree_wrapperConstPtr ref_wrapper) { return; } command_ctr = ref_wrapper->command_counter; - ///////////////////// when we are ready any command means force start - if (isReady && (state != States::Start) && (ref_wrapper->command.command != ssl_refree_command::HALT) - && (ref_wrapper->command.command != ssl_refree_command::STOP)) { - state = States::Stop; - isReady = false; - return; - } + // TODO : Remove it +// ///////////////////// when we are ready any command means force start +// if (isReady && (state != States::Start) && (ref_wrapper->command.command != ssl_refree_command::HALT) +// && (ref_wrapper->command.command != ssl_refree_command::STOP)) { +// state = States::Stop; +// isReady = false; +// return; +// } stage = ref_wrapper->stage; switch (stage.stage) { @@ -63,7 +64,7 @@ bool GameState::allowedNearBall() { return isStart() || ourPlayOffKick(); } bool GameState::canKickBall() { - return isStart() || (ourPlayOffKick() && isReady); + return isReady && (!theirPenaltyKick() || !theirBallPlacement() || !theirPlayOffKick() || !theirPenaltyKick() || !theirPenaltyShootout()); } bool GameState::playOffKick() { diff --git a/parsian_ai/src/parsian_ai/plans/defenseplan.cpp b/parsian_ai/src/parsian_ai/plans/defenseplan.cpp index 2a2fd99d..392e522f 100644 --- a/parsian_ai/src/parsian_ai/plans/defenseplan.cpp +++ b/parsian_ai/src/parsian_ai/plans/defenseplan.cpp @@ -1,6 +1,5 @@ #include "parsian_ai/plans/defenseplan.h" -#include "parsian_util/tools/blackboard.h" -#include "parsian_util/geom/polygon_2d.h" + using namespace std; @@ -21,7 +20,7 @@ QList DefensePlan::getPositionJustForZJU(int numberOfOverDefenders){ } } for(size_t i = 0 ; i < defendersForZJU.size() ; i++){ - drawer->draw(Circle2D(defendersForZJU.at(i) , 0.4) , 0 , 360 , "blue"); + drawer->draw(Circle2D(defendersForZJU.at(i) , 0.4) , 0 , 360 , "red"); } return defendersForZJU; } @@ -51,7 +50,7 @@ Vector2D DefensePlan::getGKPositionInOneDefense(Vector2D firstPoint , Vector2D o return goalkeeperPosition; } -Vector2D DefensePlan::getGKPositionInTwoDefense(Vector2D firstPoint , Vector2D originPoint , Vector2D secondPoint , double downLimit , double upLimit){ +Vector2D DefensePlan::getGKPositionInMoreThanTwoDefense(Vector2D firstPoint , Vector2D originPoint , Vector2D secondPoint , double downLimit , double upLimit){ Vector2D goalkeeperPosition; Vector2D sol[2]; int numberOfAgents = 1; @@ -62,16 +61,6 @@ Vector2D DefensePlan::getGKPositionInTwoDefense(Vector2D firstPoint , Vector2D o return goalkeeperPosition; } -Vector2D DefensePlan::getGKPositionInThreeDefense(Vector2D firstPoint , Vector2D originPoint , Vector2D secondPoint , double downLimit , double upLimit){ - Vector2D goalkeeperPosition; - Vector2D sol[2]; - int numberOfAgents = 1; - Circle2D defenseArea(wm->field->ourGoal(),findBestRadiusForGK(getBestLineWithTallesForGK(numberOfAgents , firstPoint , originPoint , secondPoint) , firstPoint , originPoint , secondPoint , downLimit , upLimit)); - defenseArea.intersection(getBisectorLine(firstPoint , originPoint , secondPoint) , &sol[0] , &sol[1]); - goalkeeperPosition = sol[0].isValid() && sol[0].dist(originPoint) < sol[1].dist(originPoint) ? sol[0] : sol[1]; - return goalkeeperPosition; -} - Vector2D DefensePlan::getGKPositionWithoutDefense(double downLimit , double upLimit){ Vector2D goalkeeperPosition; Vector2D sol[2]; @@ -87,34 +76,19 @@ Vector2D DefensePlan::getGKPositionWithoutDefense(double downLimit , double upLi Vector2D DefensePlan::getGKPositionAccordingToTheDefense(int numberOfDefenders , Vector2D firstPoint , Vector2D originPoint , Vector2D secondPoint){ Vector2D goalKeeperPosition; - double downLimit , upLimit; + double downLimit = wm->field->ourGoalL().dist(wm->field->ourGoalR()) / 2 , upLimit = 1.2;//RADIUS_FOR_CRITICAL_DEFENSE_AREA - Robot::robot_radius_new; switch (numberOfDefenders){ - case 0:{ - downLimit = wm->field->ourGoalL().dist(wm->field->ourGoalR()) / 2; - upLimit = 1.2;//RADIUS_FOR_CRITICAL_DEFENSE_AREA - Robot::robot_radius_new; + case 0: goalKeeperPosition = getGKPositionWithoutDefense(downLimit , upLimit); break; - } - case 1:{ - downLimit = wm->field->ourGoalL().dist(wm->field->ourGoalR()) / 2; - upLimit = 1.2;//RADIUS_FOR_CRITICAL_DEFENSE_AREA - Robot::robot_radius_new; + case 1: goalKeeperPosition = getGKPositionInOneDefense(firstPoint , originPoint , secondPoint , downLimit , upLimit); break; - } - case 2:{ - PDEBUG("AYA" ,2, D_AHZ); - downLimit = wm->field->ourGoalL().dist(wm->field->ourGoalR()) / 2; - upLimit = 1.2;//RADIUS_FOR_CRITICAL_DEFENSE_AREA - Robot::robot_radius_new; - goalKeeperPosition = getGKPositionInTwoDefense(firstPoint , originPoint , secondPoint , downLimit , upLimit); - break; - } - case 3:{ - downLimit = wm->field->ourGoalL().dist(wm->field->ourGoalR()) / 2; - upLimit = 1.2;//RADIUS_FOR_CRITICAL_DEFENSE_AREA - Robot::robot_radius_new; - goalKeeperPosition = getGKPositionInThreeDefense(firstPoint , originPoint , secondPoint , downLimit , upLimit); + case 2: + case 3: + goalKeeperPosition = getGKPositionInMoreThanTwoDefense(firstPoint , originPoint , secondPoint , downLimit , upLimit); break; - } default: break; } @@ -144,38 +118,6 @@ double DefensePlan::findBestRadiusForGK(Line2D bestLineWithTalles ,Vector2D firs return bestRadiusForDefenseArea; } -Segment2D DefensePlan::getBestSegmentWithTallesForGK(int defenseCount , Vector2D firstPoint , Vector2D originPoint , Vector2D secondPoint) { - double robotDiameter = 2 * Robot::robot_radius_new; - Vector2D sol[2]; - Vector2D suitablePoint; - Segment2D ourGoalLine(firstPoint, secondPoint); - Segment2D biggerFrontageOfTriangle; - Segment2D smallerFrontageOfTriangle; - if (getLinesOfBallTriangle().at(0).length() > getLinesOfBallTriangle().at(1).length()) { - biggerFrontageOfTriangle = getLinesOfBallTriangle().at(0); - smallerFrontageOfTriangle = getLinesOfBallTriangle().at(1); - } else{ - biggerFrontageOfTriangle = getLinesOfBallTriangle().at(1); - smallerFrontageOfTriangle = getLinesOfBallTriangle().at(0); - } - Segment2D tempAimLessLine(ourGoalLine.intersection(smallerFrontageOfTriangle) , biggerFrontageOfTriangle.nearestPoint(ourGoalLine.intersection(smallerFrontageOfTriangle))); - if(tempAimLessLine.length() >= robotDiameter){ - if(biggerFrontageOfTriangle.intersection(Line2D(Vector2D(originPoint.x - (defenseCount * robotDiameter * (6 - fabs(originPoint.x)) / tempAimLessLine.length()), -4.5), - Vector2D(originPoint.x - (defenseCount * robotDiameter * (6 - fabs(originPoint.x)) / tempAimLessLine.length()), 4.5))).dist(wm->field->ourGoal()) <= 0.4 - || (originPoint.x - (defenseCount * robotDiameter * (6 - fabs(originPoint.x)) / tempAimLessLine.length())) < -6){ - Circle2D(wm->field->ourGoal() , 0.4).intersection(biggerFrontageOfTriangle , &sol[0] , &sol[1]); - drawer->draw("is" , Vector2D(-1,0) , 40); - suitablePoint = sol[0].isValid() && sol[0].dist(wm->ball->pos) < sol[1].dist(wm->ball->pos) ? sol[0] : sol[1]; - tempAimLessLine = Segment2D(Vector2D(suitablePoint.x, -4.5), Vector2D(suitablePoint.x, 4.5)); - } - else{ - tempAimLessLine = Segment2D(Vector2D(originPoint.x - (defenseCount * robotDiameter * (6 - fabs(originPoint.x)) / tempAimLessLine.length()), -4.5), - Vector2D(originPoint.x - (defenseCount * robotDiameter * (6 - fabs(originPoint.x)) / tempAimLessLine.length()), 4.5)); - } - } - return tempAimLessLine; -} - Line2D DefensePlan::getBestLineWithTallesForGK(int defenseCount , Vector2D firstPoint , Vector2D originPoint , Vector2D secondPoint) { double robotDiameter = 2 * Robot::robot_radius_new; Vector2D sol[2]; @@ -212,47 +154,6 @@ Line2D DefensePlan::getBestLineWithTallesForGK(int defenseCount , Vector2D first return aimLessLine; } -double DefensePlan::timeNeeded(Agent *_agentT, Vector2D posT, double vMax, QList _ourRelax, QList _oppRelax , bool avoidPenalty, double ballObstacleReduce, bool _noAvoid){ - - double _x3; - double acc = 4.5; - double dec = 3.5; - double xSat; - Vector2D tAgentVel = _agentT->vel(); - Vector2D tAgentDir = _agentT->dir(); - double veltan = (tAgentVel.x) * cos(tAgentDir.th().radian()) + (tAgentVel.y) * sin(tAgentDir.th().radian()); - double offset = 0; - double velnorm = -1 * (tAgentVel.x) * sin(tAgentDir.th().radian()) + (tAgentVel.y) * cos(tAgentDir.th().radian()); - double distCoef = 1, distEffect = 1, angCoef = 0.003; - double dist = 0; - double rrtAngSum = 0; - QList _result; - Vector2D _target; - - double tAgentVelTanjent = tAgentVel.length() * cos(Vector2D::angleBetween(posT - _agentT->pos() , _agentT->vel().norm()).radian()); - - double vXvirtual = (posT - _agentT->pos()).x; - double vYvirtual = (posT - _agentT->pos()).y; - double veltanV = (vXvirtual) * cos(tAgentDir.th().radian()) + (vYvirtual) * sin(tAgentDir.th().radian()); - double velnormV = -1 * (vXvirtual) * sin(tAgentDir.th().radian()) + (vYvirtual) * cos(tAgentDir.th().radian()); - double accCoef = 1, realAcc = 4; - - accCoef = atan(fabs(veltanV) / fabs(velnormV)) / _PI * 2; - acc = accCoef * 4.5 + (1 - accCoef) * 3.5; - - double tDec = vMax / dec; - double tAcc = (vMax - tAgentVelTanjent) / acc; - dist = posT.dist(_agentT->pos()); - double dB = tDec * vMax / 2 + tAcc * (vMax + tAgentVelTanjent) / 2; - - if (dist > dB) { - return tAcc + tDec + (dist - dB) / vMax; - } else { - return ((1 / dec) + (1 / acc)) * sqrt(dist * (2 * dec * acc / (acc + dec)) + (tAgentVelTanjent * tAgentVelTanjent / (2 * acc))) - (tAgentVelTanjent) / acc; - } - -} - QList DefensePlan::defenseFormation(QList circularPositions, QList rectangularPositions){ suitableRadius = RADIUS_FOR_CRITICAL_DEFENSE_AREA; Circle2D defenseArea(wm->field->ourGoal() , suitableRadius); @@ -285,7 +186,7 @@ QList DefensePlan::defenseFormationForCircularPositioning(int neededDe defensePosiotion = threeDefenseFormationForCircularPositioning(downLimit , upLimit); } if(wm->ball->pos.y > 0){ - for(int i = 0 ; i < (allOfDefenseAgents - neededDefenseAgents) ; i++){ + for(int i = 0 ; i < (allOfDefenseAgents - neededDefenseAgents) ; i++) { defensePosiotion.append(Vector2D(-4.7 , -(i+1)/2)); } } @@ -1089,7 +990,6 @@ bool DefensePlan::isInIndirectArea(Vector2D aPoint) { Line2D DefensePlan::getBisectorLine(Vector2D firstPoint , Vector2D originPoint , Vector2D thirdPoint) { //// gets the bisector line of an angle //// that is made up by this 3 points. - Line2D bisectorLine(originPoint , AngleDeg::bisect((firstPoint - originPoint).th() , (thirdPoint - originPoint).th())); return bisectorLine; } @@ -1125,7 +1025,6 @@ void DefensePlan::manToManMarkBlockPassInPlayOff(QList opponentAgentsT QList > sortDangerAgentsToBeMarkBlockPassPlayOff; QList > tempSortDangerAgentsToBeBlockPassPlayOff; //////////////////// Clear QLists for update the states //////////////////// - stopMode = gameState->isStop(); ourMarkAgentsPossition.clear(); tempOpponentAgentsToBeMarkedPosition.clear(); markPoses.clear(); @@ -1501,7 +1400,11 @@ void DefensePlan::manToManMarkBlockShotInPlayOff(int _markAgentSize) { } } -void DefensePlan::setGoalKeeperState(){ +bool DefensePlan::dangerForGK(){ + return 0; +} + +GKState DefensePlan::setGoalKeeperState() { //// In this function,we determine the specific states that goalkeeper must //// have a logical behavior by good conditions.In other word we have some //// exceptions mode for goalkeeper.These modes are : @@ -1511,531 +1414,157 @@ void DefensePlan::setGoalKeeperState(){ //// 4- Ball is out of the field. ////////////////// Variables of this function ////////////////////////////// - dangerForGoalKeeperClear = false; - dangerForInsideOfThePenaltyArea = false; - dangerForGoalKeeperClearByOurAgents = false; - dangerForGoalKeeperClearByOppAgents = false; - isCrowdedInFrontOfPenaltyAreaByOppAgents = false; - Rect2D ourLeftPole(wm->field->ourGoalL() + Vector2D(0.2 , 0.3) , wm->field->ourGoalL() - Vector2D(0 , 0.3)); - Rect2D ourRightPole(wm->field->ourGoalR() + Vector2D(0.2 , 0.3) , wm->field->ourGoalR() - Vector2D(0 , 0.3)); - isCrowdedInFrontOfPenaltyAreaByOurAgents = false; - playOnMode = gameState->isStart(); - //////////////////////////////////////////////////////////////////////////// - Circle2D dangerCircle; - Circle2D dangerCircle1; - Vector2D sol[2]; - if (goalKeeperAgent != nullptr && goalKeeperAgent->id() != -1) { - if (wm->field->isInField(wm->ball->pos)){ - ballIsOutOfField = false; - QList solutions; - Segment2D ballLine(wm->ball->pos, wm->ball->pos + wm->ball->vel.norm() * 100); - Segment2D goalLine(wm->field->ourGoal() + Vector2D(0 , 1) , wm->field->ourGoal() - Vector2D(0 , 1)); - QList defs; - double AZBisecOpenAngle = 0, AZBigestOpenAngle = 0, AZDangerPercent = 0; - for (int g = 0; g < defenseAgents.count(); g++) { - defs.append(Circle2D(defenseAgents[g]->pos(), Robot::robot_radius_new)); - } - know->getEmptyAngle(wm->ball->pos, wm->field->ourGoalL(), wm->field->ourGoalR(), defs, AZDangerPercent, AZBisecOpenAngle, AZBigestOpenAngle, false); - /////////////////////// Added danger mode for not switching between "ball behindGoalie && danger mode ///////////// - wm->field->ourBigPenaltyArea(1,0,0).intersection(Segment2D(wm->ball->pos , wm->field->ourGoal()) , &sol[0] , &sol[1]); - if(sol[0].isValid()){ - solutions.append(sol[0]); - } - if(sol[1].isValid()){ - solutions.append(sol[1]); - } - if (solutions.size()){ - if (solutions.size() == 2) { - dangerCircle = Circle2D(solutions.at(0).dist(wm->ball->pos) < solutions.at(1).dist(wm->ball->pos) ? solutions.at(0) : solutions.at(1), 0.40); - dangerCircle1 = Circle2D(solutions.at(0).dist(wm->ball->pos) < solutions.at(1).dist(wm->ball->pos) ? solutions.at(0) : solutions.at(1), 0.40); - } - if(wm->our.activeAgentsCount() > 0 || wm->opp.activeAgentsCount() > 0){ - for (int i = 0; i < wm->our.activeAgentsCount() ; i++) { - if (wm->our.active(i)->id != goalKeeperAgent->id()) { - if (dangerCircle.contains(wm->our.active(i)->pos)) { - isCrowdedInFrontOfPenaltyAreaByOurAgents = true; - } - } - } - for (int i = 0 ; i < wm->opp.activeAgentsCount() ; i++) { - if (dangerCircle.contains(wm->opp.active(i)->pos)) { - isCrowdedInFrontOfPenaltyAreaByOppAgents = true; - } - } - } - if (isCrowdedInFrontOfPenaltyAreaByOurAgents) { - DBUG("Crowded" , D_AHZ); - if (dangerCircle.contains(wm->ball->pos)) { - dangerForGoalKeeperClearByOurAgents = true; - } - } - if (isCrowdedInFrontOfPenaltyAreaByOppAgents) { - if (dangerCircle1.contains(wm->ball->pos)) { - dangerForGoalKeeperClearByOppAgents = true; - } - } - if (dangerForGoalKeeperClearByOurAgents || dangerForGoalKeeperClearByOppAgents) { - dangerForGoalKeeperClear = true; - } - } - /////////////////////////////////////////////////////////////////////// - if (playOnMode && !dangerForGoalKeeperClear) { - if ((wm->ball->vel.length() > 1.3) && (goalLine.intersection(ballLine).valid() || oneTouchCnt < 5)) { - ballIsBesidePoles = false; - goalKeeperOneTouch = true; - goalKeeperClearMode = false; - ballIsOutOfField = false; - if (!goalLine.intersection(ballLine).valid()) { - oneTouchCnt++; - return; - } - oneTouchCnt = 0; - return; - } - else if (wm->field->isInOurPenaltyArea(wm->ball->pos)){ - if(wm->ball->vel.length() < 0.1 && (ourLeftPole.contains(wm->ball->pos) || ourRightPole.contains(wm->ball->pos) || - (wm->field->ourGoalL().y >= wm->ball->pos.y - && wm->field->ourGoalR().y < wm->ball->pos.y - && wm->field->ourGoal().x < wm->ball->pos.x - && wm->field->ourGoal().x + 0.05 > wm->ball->pos.x))){ - ballIsBesidePoles = true; - goalKeeperOneTouch = false; - goalKeeperClearMode = false; - ballIsOutOfField = false; - return; - } - else {//Lhum thinks it is wrong - ballIsBesidePoles = false; - goalKeeperOneTouch = false; - goalKeeperClearMode = true; - ballIsOutOfField = false; - return; - } - } - else { - goalKeeperClearMode = false; - goalKeeperOneTouch = false; - ballIsBesidePoles = false; - ballIsOutOfField = false; - return; - } - } - } - else { - ballIsBesidePoles = false; - goalKeeperOneTouch = false; - goalKeeperClearMode = false; - ballIsOutOfField = true; - return; + QList empty; + empty.clear(); + Rect2D ourLeftPole(wm->field->ourGoalL() + Vector2D(0.2, 0.3), wm->field->ourGoalL() - Vector2D(0, 0.3)); + Rect2D ourRightPole(wm->field->ourGoalR() + Vector2D(0.2, 0.3), wm->field->ourGoalR() - Vector2D(0, 0.3)); + Segment2D goalLine(wm->field->ourGoal() + Vector2D(0, 1), wm->field->ourGoal() - Vector2D(0, 1)); + Segment2D ballLine(wm->ball->pos, wm->ball->pos + wm->ball->vel.norm() * 100); + if (goalKeeperTarget.dist(wm->ball->pos) < 2 * Robot::robot_radius_new) { + differentialTime = 0; + } else { + differentialTime = 0.2; + } + /////////////////////////////Gamestate/////////////////////////////////////// + + if(gameState->isStop()) + return GKState :: Stop; + + if(wm->field->isInOurPenaltyArea(wm->ball->pos)) { + if (wm->ball->vel.length() < 0.1 && + (ourLeftPole.contains(wm->ball->pos) || ourRightPole.contains(wm->ball->pos) || + (wm->field->ourGoalL().y >= wm->ball->pos.y + && wm->field->ourGoalR().y < wm->ball->pos.y + && wm->field->ourGoal().x < wm->ball->pos.x + && wm->field->ourGoal().x + 0.05 > wm->ball->pos.x))) { + return GKState::ballIsBesidePoles; } } - else { - drawer->draw("GoalKeeper is gone !!!!" , Vector2D(0, 0) , "red"); + if (gameState->directKick() || gameState->indirectKick() || gameState->kickoff()){ + return GKState::playoff; } -} + if(know->variables["transientFlag"].toBool()) { + stateBallBesidepoles = -1; + if (wm->field->ourPenaltyRect().contains(wm->ball->getPosInFuture(know->timeNeeded(goalKeeperAgent, know->getPointInDirection(wm->field->ourGoal(), ballPrediction(true), 1), 4) + differentialTime))) + return GKState :: GKReciveBallInTS; + else + return GKState :: GKPredictInTs; + } + if(!wm->field->isInField(wm->ball->pos)) + return GKState :: ballIsOutOfField; -void DefensePlan::setGoalKeeperTargetPoint() { - //// This function determine the target point that goalkeeper must go to it. - //// For producing the target point, we certainly consider the states that - //// is result from the "setGoalKeeperState" function. :) + if(dangerForGK())//TODO: write dangerForGK function or add another if + return GKState :: dangerForClear; - ////////////////////////// Variables of this function ////////////////////// - Vector2D ballPos; - Vector2D ballVel; - Vector2D predictedBall; - Vector2D oppPasser; - Vector2D upBallRectanglePoint; - Vector2D downBallRectanglePoint; - Circle2D dangerCircle; - Circle2D dangerCircle1; - Vector2D goalKeeperTargetOffSet = Vector2D(0.2 , 0.0); + if (wm->ball->vel.length() > 1.3 && (goalLine.intersection(ballLine).valid())){ + oneTouchCnt = 0; + return GKState :: oneTouch; + } + + if(oneTouchCnt < 5){ + oneTouchCnt++; + return GKState :: oneTouch; + } + + if(wm->field->isInOurPenaltyArea(wm->ball->pos)) + return GKState :: clearMode; + + return GKState :: strictFollow; +} + +Vector2D DefensePlan::movePointToPenaltyArea(const Vector2D& point){ QList tempSol; - QList ballRectanglePoints; - QList solutions; Vector2D sol[2]; - dangerForGoalKeeperClear = false; - dangerForInsideOfThePenaltyArea = false; - dangerForGoalKeeperClearByOurAgents = false; - dangerForGoalKeeperClearByOppAgents = false; - isCrowdedInFrontOfPenaltyAreaByOppAgents = false; - isCrowdedInFrontOfPenaltyAreaByOurAgents = false; - playOffMode = gameState->theirDirectKick() || gameState->theirIndirectKick() || gameState->kickoff() || gameState->ourDirectKick() || gameState->ourIndirectKick(); - playOnMode = gameState->isStart(); - stopMode = gameState->isStop(); - tempSol.clear(); - ballRectanglePoints.clear(); - /////////////////////////////////////////////////////////////////////////// - if (goalKeeperAgent != nullptr && goalKeeperAgent->id() != -1) { - //ROS_INFO_STREAM("E: ____________"); - ballPos = wm->ball->pos; - ballVel = wm->ball->vel; - predictedBall = ballPos + ballVel; - wm->field->ourBigPenaltyArea(1,0,0).intersection(Segment2D(wm->ball->pos , wm->field->ourGoal()) , &sol[0] , &sol[1]); + if (!wm->field->isInOurPenaltyArea(point)) { + wm->field->ourBigPenaltyArea(1,0,0).intersection(Segment2D(point , wm->field->ourGoal()) , &sol[0] , &sol[1]);//nimsaz pas chi if(sol[0].isValid()){ - solutions.append(sol[0]); + tempSol.append(sol[0]); } if(sol[1].isValid()){ - solutions.append(sol[1]); + tempSol.append(sol[1]); } - if (solutions.size()) { - if (solutions.size() == 2) { - dangerCircle = Circle2D(solutions.at(0).dist(wm->ball->pos) < solutions.at(1).dist(wm->ball->pos) ? solutions.at(0) : solutions.at(1), 0.40); - dangerCircle1 = Circle2D(solutions.at(0).dist(wm->ball->pos) < solutions.at(1).dist(wm->ball->pos) ? solutions.at(0) : solutions.at(1), 0.40); - } + if (tempSol.size() == 1){ + return tempSol.at(0); } - drawer->draw(dangerCircle , "yellow"); - drawer->draw(dangerCircle1 , "yellow"); - if (ballIsOutOfField || stopMode){ - lastStateForGoalKeeper = QString("noBesidePoleMode"); - dangerForGoalKeeperClear = false; - drawer->draw(QString("Ball Is Out Of Field"), Vector2D(0, 1), "red"); - goalKeeperTarget = wm->field->ourGoal()+ goalKeeperTargetOffSet; - return; + else if (tempSol.size() == 2){ + return tempSol.at(0).dist(wm->ball->pos) < tempSol.at(1).dist(wm->ball->pos) ? tempSol.at(0) : tempSol.at(1); } - else if(playOffMode){ - lastStateForGoalKeeper = QString("noBesidePoleMode"); - dangerForGoalKeeperClear = false; - goalKeeperPredictionModeInPlayOff = true; - DBUG(QString("Their Indirect") , D_AHZ); - if(gameState->theirIndirectKick() || gameState->ourDirectKick() || gameState->ourIndirectKick() ){ - goalKeeperTarget = wm->field->ourGoal() + goalKeeperTargetOffSet; - } - else { - goalKeeperTarget = strictFollowBall(ballPrediction(true)); - } - return; - } - else if (know->variables["transientFlag"].toBool()) {//Lhum// - goalKeeperTarget = know->getPointInDirection(wm->field->ourGoal() , wm->ball->pos , 0.3); - /*if (!wm->field->isInOurPenaltyArea(wm->ball->pos)) { - wm->field->ourBigPenaltyArea(1, -0.2, 0).intersection( - Segment2D(wm->ball->pos, wm->field->ourGoal()), &sol[0], &sol[1]);//nimsaz pas chi - if (sol[0].isValid()) { - tempSol.append(sol[0]); - } - if (sol[1].isValid()) { - tempSol.append(sol[1]); - } - if (tempSol.size() == 1) { - goalKeeperTarget = tempSol.at(0); - drawer->draw(Circle2D(goalKeeperTarget, 0.8), "yellow"); - } else if (tempSol.size() == 2) { - goalKeeperTarget = tempSol.at(0).dist(wm->ball->pos) < tempSol.at(1).dist(wm->ball->pos) - ? tempSol.at(0) : tempSol.at(1); - drawer->draw(Circle2D(goalKeeperTarget, 0.7), "cyan"); + } + return point; +} - } - }*/ - lastStateForGoalKeeper = QString("noBesidePoleMode"); - dangerForGoalKeeperClear = false; - PDEBUG("TS Mode :", 7 , D_AHZ); - goalKeeperPredictionModeInPlayOff = false; - goalKeeperTarget = know->getPointInDirection(wm->field->ourGoal() , ballPrediction(true) , 1); - drawer->draw(Circle2D(goalKeeperTarget , 0.5) , "red"); - if(wm->field->ourBigPenaltyArea(1,0,0).contains(goalKeeperTarget)) { - QList empty; - empty.clear(); - if (goalKeeperTarget.dist(wm->ball->pos) < 2 * Robot::robot_radius_new) { - differentialTime = 0; - } else { - differentialTime = 0.2; - } - if (ballIntersectOurPenaltyArea && wm->field->ourPenaltyRect().contains(wm->ball->getPosInFuture(timeNeeded(goalKeeperAgent, goalKeeperTarget, 4, empty, empty, false, 0, false) + differentialTime))){//!Lhum 4 0.2 - GKReciveBallInTS = true; - return; - } - else { - predictedBall = ballPrediction(true); - goalKeeperTarget = strictFollowBall(predictedBall); - GKReciveBallInTS = false; - return; - } - } - drawer->draw(Segment2D(goalKeeperTarget , wm->field->ourGoal()) , "blue"); - PDEBUGV2D("GK target" , goalKeeperTarget , D_AHZ); - if (!wm->field->isInOurPenaltyArea(goalKeeperTarget)) { - wm->field->ourBigPenaltyArea(1,0,0).intersection(Segment2D(goalKeeperTarget , wm->field->ourGoal()) , &sol[0] , &sol[1]);//nimsaz pas chi - if(sol[0].isValid()){ - tempSol.append(sol[0]); - } - if(sol[1].isValid()){ - tempSol.append(sol[1]); - } - if (tempSol.size() == 1){ - goalKeeperTarget = tempSol.at(0); - } - else if (tempSol.size() == 2){ - goalKeeperTarget = tempSol.at(0).dist(wm->ball->pos) < tempSol.at(1).dist(wm->ball->pos) ? tempSol.at(0) : tempSol.at(1); - } - } - return; - } - else if (goalKeeperOneTouch){ - lastStateForGoalKeeper = QString("noBesidePoleMode"); - Segment2D ballLine(ballPos, ballPos + ballVel.norm() * 100); - goalKeeperTarget = ballLine.nearestPoint(goalKeeperAgent->pos()); - return; - } - else if (goalKeeperClearMode){ - lastStateForGoalKeeper = QString("noBesidePoleMode"); - ////////////// Danger Mode for inside of the penalty area/////////// - if (wm->our.activeAgentsCount() > 0 || wm->opp.activeAgentsCount() > 0) { - for (int i = 0; i < wm->our.activeAgentsCount() ; i++) { - if (wm->our.active(i)->id != goalKeeperAgent->id()) { - if (dangerCircle.contains(wm->our.active(i)->pos)) { - isCrowdedInFrontOfPenaltyAreaByOurAgents = true; - } - } - } - for (int i = 0 ; i < wm->opp.activeAgentsCount() ; i++) { - if (dangerCircle.contains(wm->opp.active(i)->pos)) { - isCrowdedInFrontOfPenaltyAreaByOppAgents = true; - } - } - } - if (isCrowdedInFrontOfPenaltyAreaByOurAgents) { - DBUG("Crowded" , D_AHZ); - if (dangerCircle1.contains(wm->ball->pos)) { - dangerForGoalKeeperClearByOurAgents = true; - } - } - if (isCrowdedInFrontOfPenaltyAreaByOppAgents) { - if (dangerCircle1.contains(wm->ball->pos)) { - dangerForGoalKeeperClearByOppAgents = true; - } - } - if (dangerForGoalKeeperClearByOurAgents || dangerForGoalKeeperClearByOppAgents) { - dangerForGoalKeeperClear = true; - } - if (dangerForGoalKeeperClear) { - dangerForInsideOfThePenaltyArea = true; - DBUG(QString("inside : %1").arg(dangerForInsideOfThePenaltyArea) , D_AHZ); - if (dangerForGoalKeeperClearByOppAgents) { - // goalKeeperTarget = know->getPointInDirection(wm->ball->pos , wm->field->ourGoal() ,0.2); - } - else if (dangerForGoalKeeperClearByOurAgents){ - DBUG("danger" , D_AHZ); - // penaltyArea.intersection(Line2D(wm->ball->pos , wm->field->ourGoal()),&Solutions[0] , &Solutions[1]); - // goalieTarget = Solutions[0].dist(wm->ball->pos) < Solutions[1].dist(wm->ball->pos) ? Solutions[0] : Solutions[1]; - // dangerIntersection.append(wm->field->ourPAreaIntersect(Line2D(Vector2D(wm->ball->pos.x , -3) , Vector2D(wm->ball->pos.x , 3)))); - // if(!dangerIntersection.isEmpty()){ - // if(dangerIntersection.size() == 2){ - // goalieTarget = dangerIntersection.at(0).isValid() - // && (dangerIntersection.at(0).dist(wm->ball->pos) < dangerIntersection.at(1).dist(wm->ball->pos)) ? dangerIntersection.at(0) : dangerIntersection.at(1); - // goalieDirection = wm->ball->pos - goalieTarget; - // goalieTarget = know->getPointInDirection(wm->ball->pos , wm->field->ourGoal() ,0.2); - - // } - // else if(dangerIntersection.size() == 1){ - // goalieTarget = dangerIntersection.at(0); - // } - // } - } - } - /////////////// End of Danger Mode /////////////////////////////// - else { - drawer->draw(QString("Clear"), Vector2D(0, 1), "red"); - } - return; - } - else if (ballIsBesidePoles) { - Rect2D ballRectangle(wm->ball->pos + Vector2D(0.25 , 0.25) , wm->ball->pos + Vector2D(-0.25 , -0.25)); - if (wm->field->isInField(ballRectangle.topLeft())) { - ballRectanglePoints.append(ballRectangle.topLeft()); - } - if (wm->field->isInField(ballRectangle.topRight())) { - ballRectanglePoints.append(ballRectangle.topRight()); - } - if (wm->field->isInField(ballRectangle.bottomLeft())) { - ballRectanglePoints.append(ballRectangle.bottomLeft()); - } - if (wm->field->isInField(ballRectangle.bottomRight())) { - ballRectanglePoints.append(ballRectangle.bottomRight()); - } - if (lastStateForGoalKeeper == QString("noBesidePoleMode")) { - isPermissionToKick = false; - goalKeeperTarget = ballRectanglePoints.at(0).dist(goalKeeperAgent->pos()) < ballRectanglePoints.at(1).dist(goalKeeperAgent->pos()) ? ballRectanglePoints.at(0) : ballRectanglePoints.at(1); - } - if(wm->field->ourGoalL().y >= wm->ball->pos.y && - wm->field->ourGoalR().y < wm->ball->pos.y && - wm->field->ourGoal().x < wm->ball->pos.x && - (wm->field->ourGoal().x + 0.05) > wm->ball->pos.x){ - goalKeeperTarget = Vector2D(wm->field->ourGoal().x + 0.6 , 0); - return; - } - if((fabs(wm->ball->pos.x - wm->field->ourGoalR().x) < 0.1 && fabs(wm->ball->pos.y - wm->field->ourGoalR().y) < 0.1) || (fabs(wm->ball->pos.x - wm->field->ourGoalL().x) < 0.1 && fabs(wm->ball->pos.y - wm->field->ourGoalL().y) < 0.1)){ - return; - } - if (goalKeeperAgent->pos().dist(goalKeeperTarget) < 0.05 && f == 0) { - f = 1; - if (wm->ball->pos.y > 0) { - if (ballRectanglePoints.at(0).y < ballRectanglePoints.at(1).y) { - downBallRectanglePoint = ballRectanglePoints.at(0); - upBallRectanglePoint = ballRectanglePoints.at(1); - } - else { - downBallRectanglePoint = ballRectanglePoints.at(1); - upBallRectanglePoint = ballRectanglePoints.at(0); - } - if(fabs(wm->ball->pos.y - wm->field->ourGoalL().y) < 0.1) { - goalKeeperTarget = downBallRectanglePoint - Vector2D(0.25, 0); - } - else if(wm->ball->pos.y < wm->field->ourGoalL().y) - goalKeeperTarget = downBallRectanglePoint; - else - goalKeeperTarget = upBallRectanglePoint; - } - else { - if (ballRectanglePoints.at(0).y < ballRectanglePoints.at(1).y) { - upBallRectanglePoint = ballRectanglePoints.at(1); - downBallRectanglePoint = ballRectanglePoints.at(0); - } else { - upBallRectanglePoint = ballRectanglePoints.at(0); - downBallRectanglePoint = ballRectanglePoints.at(1); - } - if (fabs(wm->ball->pos.y - wm->field->ourGoalR().y) < 0.1){ - goalKeeperTarget = upBallRectanglePoint; - } - else if (wm->ball->pos.y < wm->field->ourGoalR().y) - goalKeeperTarget = downBallRectanglePoint; - else - goalKeeperTarget = upBallRectanglePoint; - } - } - else if (goalKeeperAgent->pos().dist(goalKeeperTarget) < 0.05 && f == 1) { - f = 2; - if (wm->ball->pos.y > 0) { - if (ballRectanglePoints.at(0).y < ballRectanglePoints.at(1).y) { - downBallRectanglePoint = ballRectanglePoints.at(0); - upBallRectanglePoint = ballRectanglePoints.at(1); - } - else { - downBallRectanglePoint = ballRectanglePoints.at(1); - upBallRectanglePoint = ballRectanglePoints.at(0); - } - if(fabs(wm->ball->pos.y - wm->field->ourGoalL().y) < 0.1) - goalKeeperTarget = downBallRectanglePoint - Vector2D(0.25, 0); - else if(wm->ball->pos.y < wm->field->ourGoalL().y ) - goalKeeperTarget = downBallRectanglePoint - Vector2D(0.4 , 0); - else - goalKeeperTarget = upBallRectanglePoint - Vector2D(0.4 , 0); - } - else { - if (ballRectanglePoints.at(0).y < ballRectanglePoints.at(1).y) { - upBallRectanglePoint = ballRectanglePoints.at(1); - downBallRectanglePoint = ballRectanglePoints.at(0); - } - else { - upBallRectanglePoint = ballRectanglePoints.at(0); - downBallRectanglePoint = ballRectanglePoints.at(1); - } - if(fabs(wm->ball->pos.y - wm->field->ourGoalR().y) < 0.1) - goalKeeperTarget = upBallRectanglePoint - Vector2D(0.25 ,0); - else if(wm->ball->pos.y < wm->field->ourGoalR().y) - goalKeeperTarget = downBallRectanglePoint - Vector2D(0.4 , 0); - else - goalKeeperTarget = upBallRectanglePoint - Vector2D(0.4 , 0); - } - } - else if (goalKeeperAgent->pos().dist(goalKeeperTarget) < 0.05 && f == 2) { - f = 3; - if (wm->ball->pos.y > 0) { - if (ballRectanglePoints.at(0).y < ballRectanglePoints.at(1).y) { - downBallRectanglePoint = ballRectanglePoints.at(0); - upBallRectanglePoint = ballRectanglePoints.at(1); - } - else { - downBallRectanglePoint = ballRectanglePoints.at(1); - upBallRectanglePoint = ballRectanglePoints.at(0); - } - if(fabs(wm->ball->pos.y - wm->field->ourGoalL().y) < 0.1) - goalKeeperTarget = downBallRectanglePoint - Vector2D(0.25, 0); - else if(wm->ball->pos.y < wm->field->ourGoalL().y) - goalKeeperTarget = Vector2D(downBallRectanglePoint.x , 0) - Vector2D(0.4 , 0) + Vector2D(0 , wm->ball->pos.y); - else - goalKeeperTarget = Vector2D(upBallRectanglePoint.x , 0) - Vector2D(0.4 , 0) + Vector2D(0 , wm->ball->pos.y); - } - else { - if (ballRectanglePoints.at(0).y < ballRectanglePoints.at(1).y) { - upBallRectanglePoint = ballRectanglePoints.at(1); - downBallRectanglePoint = ballRectanglePoints.at(0); - } - else { - upBallRectanglePoint = ballRectanglePoints.at(0); - downBallRectanglePoint = ballRectanglePoints.at(1); - } - if(fabs(wm->ball->pos.y - wm->field->ourGoalR().y) < 0.1) - goalKeeperTarget = upBallRectanglePoint - Vector2D(0.25 ,0); - else if(wm->ball->pos.y < wm->field->ourGoalR().y) - goalKeeperTarget = Vector2D(downBallRectanglePoint.x , 0) - Vector2D(0.4 , 0) + Vector2D(0 , wm->ball->pos.y); - else - goalKeeperTarget = Vector2D(upBallRectanglePoint.x , 0) - Vector2D(0.4 , 0) + Vector2D(0 , wm->ball->pos.y); - } - } - else if (goalKeeperAgent->pos().dist(goalKeeperTarget) < 0.05 && f >= 3) { - f = 4; - } - lastStateForGoalKeeper = QString("ballIsBesidePoles"); - return; - } - else { - lastStateForGoalKeeper = QString("noBesidePoleMode"); - ////////////// Danger Mode for out of the penalty area ///////////// - if (wm->our.activeAgentsCount() > 0 || wm->opp.activeAgentsCount() > 0) { - for (int i = 0; i < wm->our.activeAgentsCount() ; i++) { - if(goalKeeperAgent->id() != -1) { - if (wm->our.active(i)->id != goalKeeperAgent->id()) { - if (dangerCircle.contains(wm->our.active(i)->pos)) { - isCrowdedInFrontOfPenaltyAreaByOurAgents = true; - } - } - } - } - } - if (isCrowdedInFrontOfPenaltyAreaByOurAgents) { - if (dangerCircle.contains(wm->ball->pos)) { - dangerForGoalKeeperClear = true; - } - } - if (isCrowdedInFrontOfPenaltyAreaByOppAgents) { - if (dangerCircle.contains(wm->ball->pos)) { - dangerForGoalKeeperClearByOppAgents = true; - } - } - if (dangerForGoalKeeperClearByOurAgents || dangerForGoalKeeperClearByOppAgents) { - dangerForGoalKeeperClear = true; - } - if (dangerForGoalKeeperClear) { - DBUG("danger" , D_AHZ); - goalKeeperTarget = know->getPointInDirection(wm->ball->pos , wm->field->ourGoal() , 0.15); - if (!wm->field->isInOurPenaltyArea(goalKeeperTarget)) { - solutions = wm->field->ourPAreaIntersect(Line2D(wm->ball->pos , wm->field->ourGoal())); - if (solutions.size()) { - if (solutions.size() == 1) { - goalKeeperTarget = solutions.at(0); - } else if (solutions.size() == 2) { - goalKeeperTarget = solutions.at(0).dist(wm->ball->pos) < solutions.at(1).dist(wm->ball->pos) ? solutions.at(0) : solutions.at(1); - } - } - } - } - //////////////// End of Danger Mode //////////////////////////////// - else { - lastStateForGoalKeeper = QString("noBesidePoleMode"); - DBUG(QString("strict follow"), D_AHZ); - predictedBall = ballPrediction(true); - /*if(predictedBall.x - 0.02 < goalKeeperAgent->pos().x) {//Lhum??? - ROS_INFO_STREAM("E: !!!!!!!!!!!!!!!!!"); - Segment2D ball2PredictedBall(ballPos, predictedBall); - Line2D robotPrGoalLine(goalKeeperAgent->pos(), Vector2D(goalKeeperAgent->pos().x, (goalKeeperAgent->pos().y + 0.01))); - if (ball2PredictedBall.intersection(robotPrGoalLine).valid()) { - predictedBall = ball2PredictedBall.intersection(robotPrGoalLine); - } - }*/ - goalKeeperTarget = strictFollowBall(predictedBall); - return; - } - } +Vector2D DefensePlan::ballIsBesidePoles(){//TODO:slow mode and add other state + + Vector2D upBallRectanglePoint; + Vector2D downBallRectanglePoint; + QList ballRectanglePoints; + bool isBallLeftOfPole = ((wm->ball->pos.y < 0 && wm->ball->pos.y >= wm->field->ourGoalR().y) || (wm->ball->pos.y >= 0 && wm->ball->pos.y >= wm->field->ourGoalL().y)) ? true : false; + Rect2D ballRectangle(wm->ball->pos + Vector2D(0.25 , 0.25) , wm->ball->pos + Vector2D(-0.25 , -0.25)); + Vector2D Target[4][2] = {{ballRectangle.bottomRight() , ballRectangle.topRight()} , + {ballRectangle.bottomLeft() , ballRectangle.topLeft()} , + {Vector2D(ballRectangle.bottomLeft().x , wm->ball->pos.y) , Vector2D(ballRectangle.topLeft().x , wm->ball->pos.y)} , + {wm->ball->pos - Vector2D(0 , 0.02) , wm->ball->pos - Vector2D(0 , 0.02)}}; + for(int i = 0 ; i < 4 ; i++) { + Target[i][0] = Vector2D(max(Target[i][0].x , -6.15) , Target[i][0].y); + Target[i][1] = Vector2D(max(Target[i][1].x , -6.15) , Target[i][1].y); + } + if(wm->field->ourGoalL().y >= wm->ball->pos.y && + wm->field->ourGoalR().y < wm->ball->pos.y && + wm->field->ourGoal().x < wm->ball->pos.x && + wm->field->ourGoal().x + 0.05 > wm->ball->pos.x){ + return Vector2D(wm->field->ourGoal().x + 0.6 , 0); + } + if(stateBallBesidepoles == -1 && (ballRectangle.bottomRight().dist(goalKeeperAgent->pos()) < 0.05 || ballRectangle.topRight().dist(goalKeeperAgent->pos()) < 0.05)) + stateBallBesidepoles++; + if (stateBallBesidepoles == -1) { + drawer->draw(Circle2D(ballRectangle.topRight().dist(goalKeeperAgent->pos()) < ballRectangle.bottomRight().dist(goalKeeperAgent->pos()) ? ballRectangle.topRight() : ballRectangle.bottomRight() , 0.3) , "blue"); + return ballRectangle.topRight().dist(goalKeeperAgent->pos()) < ballRectangle.bottomRight().dist(goalKeeperAgent->pos()) ? ballRectangle.topRight() : ballRectangle.bottomRight(); + } + if(goalKeeperAgent->pos().dist(Target[stateBallBesidepoles][isBallLeftOfPole]) < 0.09 && stateBallBesidepoles < 3) + stateBallBesidepoles++; + drawer->draw(Circle2D(Target[stateBallBesidepoles][isBallLeftOfPole], 0.3) , "red"); + return Target[stateBallBesidepoles][isBallLeftOfPole]; +} + +Vector2D DefensePlan::setGoalKeeperTargetPoint(GKState state) { + //// This function determine the target point that goalkeeper must go to it. + //// For producing the target point, we certainly consider the states that + //// is result from the "setGoalKeeperState" function. :) + + ////////////////////////// Variables of this function ////////////////////// + QList solutions; + /////////////////////////////////////////////////////////////////////////// + switch (state){ + case GKState :: GKReciveBallInTS: + return movePointToPenaltyArea(ballPrediction(true)); + case GKState :: GKPredictInTs: + return movePointToPenaltyArea(strictFollowBall(ballPrediction(true))); + case GKState ::playoff: + return wm->field->ourGoal() + Vector2D(0.2 , 0.0); + case GKState :: Stop: + case GKState :: ballIsOutOfField: + return wm->field->ourGoal()+ Vector2D(0.2 , 0.0); + case GKState :: ballIsBesidePoles: + return ballIsBesidePoles(); + case GKState :: clearMode: + return Vector2D(5000 , 5000); + case GKState :: oneTouch: + return Segment2D(wm->ball->pos, wm->ball->pos + wm->ball->vel.norm() * 100).nearestPoint(goalKeeperAgent->pos()); + case GKState :: dangerForClear: + if (!wm->field->isInOurPenaltyArea(know->getPointInDirection(wm->ball->pos , wm->field->ourGoal() , 0.15))) { + solutions = wm->field->ourPAreaIntersect(Line2D(wm->ball->pos , wm->field->ourGoal())); + if (solutions.size() == 1) { + return solutions.at(0); + } + else if (solutions.size() == 2) { + return solutions.at(0).dist(wm->ball->pos) < solutions.at(1).dist(wm->ball->pos) ? solutions.at(0) : solutions.at(1); + } + } + return know->getPointInDirection(wm->ball->pos , wm->field->ourGoal() , 0.15); + case GKState :: strictFollow: + return strictFollowBall(ballPrediction(true)); + default: + break; } } @@ -2070,57 +1599,23 @@ DefensePlan::DefensePlan(){ //// Constructor function of DefensePlan class thr = 0; - isOnetouch = false; - inPenaltyAreaFlag = false; - noDefThr = 0; - differentialTime = 0; - clearCnt = 0; - defenseCount = defenseAgents.size(); defExceptions.active = false; defExceptions.exeptionMode = NoneExep; defExceptions.exepAgentId = -1; - defClearThr = 0; - defClearFlag = false; - - overDefThr = 0; - - goalKeeperOneTouch = false; - goalKeeperClearMode = false; - ballIsOutOfField = false; - - ballIsBesidePoles = false; - oneTouchCnt = 5; - markRadius = 1.6; segmentpershoot = conf.ShootRatioBlock / 100.0; segmentperpass = conf.PassRatioBlock / 100.0; dir = Vector2D(1, 0); - MantoManAllTransientFlag = conf.ManToManAllTransiant; - predictThresh = 0; isInOneTouch = false; oneTouchCycleTest = 0; cycleCounter = 0; - timeToReach = 0; - doBlockPass = false; - goalieAreaHis = 0; - isBallGoingToOppAreaCnt = -1; - pushBallHist = 0; - failureAtempCnt = 0; - goaliePassBlockCnt = -1; - GOTThresh = 0.0; - GOTCounter = 0; - lastClearID = -1; - lastTouchTheGoalie = -1; - lastStateOffPlay = -1; oneToucher = 0; /////////// AHZ ////////////// lastMarkRoles.append(markRoles); goalKeeperTarget = Vector2D(0, 0); - dangerModeThresholdForClear = false; - dangerModeThresholdForDanger = false; /////////////// For Adding TS Mode in Mark /////////////////////////////// xLimitForblockingPass = 0; manToManMarkBlockPassFlag = conf.PlayOffManToMan; @@ -2133,8 +1628,6 @@ DefensePlan::DefensePlan(){ } //////////////////////////////// - striker_Robot = new GotopointavoidAction; - for (int i = 0; i < _MAX_NUM_PLAYERS; i++) { lastMarker[i] = -1; @@ -2147,8 +1640,6 @@ DefensePlan::DefensePlan(){ } kickSkill = new KickAction; AHZSkills = nullptr; - defenderForMark = false; - doubleMarking = false; } @@ -2166,7 +1657,6 @@ void DefensePlan::matchingDefPos(int _defenseNum){ QList matchResult; Vector2D tempPoint; Vector2D sol[2]; - stopMode = gameState->isStop(); ourAgents.clear(); matchPoints.clear(); ahzMatchDirections.clear(); @@ -2182,7 +1672,7 @@ void DefensePlan::matchingDefPos(int _defenseNum){ // } // } ///////////////// Added By AHZ for segment (before MRL game) /////////////// - if(stopMode){ + if(gameState->isStop()){ ourAgents.clear(); ourAgents.append(defenseAgents); } @@ -2263,7 +1753,7 @@ void DefensePlan::matchingDefPos(int _defenseNum){ gpa[ourAgents[i]->id()]->setAvoidpenaltyarea(false); gpa[ourAgents[i]->id()]->setBallobstacleradius(0.5); } - else if(stopMode){ + else if(gameState->isStop()){ gpa[ourAgents[i]->id()]->setNoavoid(true); gpa[ourAgents[i]->id()]->setSlowmode(true); gpa[ourAgents[i]->id()]->setDivemode(false); @@ -2293,19 +1783,7 @@ void DefensePlan::matchingDefPos(int _defenseNum){ } } -void DefensePlan::execute(){ - ///// All of the goalKeeper && defense functions are linked in this function. - ///// First of all, we determine the behavior of goalKeeper. - ///// (first in penalty mode then other mode) - ///// Now,we identify the number of defense agents && then this number is - ///// sent to "matchingDefPos()" function to match between the produced - ///// points && our agents in defense plan. - int realDefSize = 0; - stopMode = gameState->isStop(); - suitableRadius = RADIUS_FOR_CRITICAL_DEFENSE_AREA; - drawer->draw(Circle2D(wm->field->ourGoal() , suitableRadius) , 0 , 180 , "blue" , false); - drawer->draw(getLinesOfBallTriangle().at(0)); - drawer->draw(getLinesOfBallTriangle().at(1)); +void DefensePlan::drawGameState(){ if(gameState->isStart()){ drawer->draw("START" , Vector2D(-6.2 , 3) , 30); } @@ -2330,300 +1808,249 @@ void DefensePlan::execute(){ else if(gameState->ourDirectKick()){ drawer->draw("OUR DIRECT" , Vector2D(-6.2 , 3) , 30); } - ballPosHistory.prepend(Vector2D(wm->ball->pos.x, wm->ball->pos.y)); - if (ballPosHistory.count() > 7){ - ballPosHistory.removeLast(); + else if(gameState->theirPenaltyKick() && !gameState->penaltyShootout()) { + if (goalKeeperAgent != nullptr) + drawer->draw(QString("Penalty"), Vector2D(1, 2), "white"); + else + drawer->draw(QString("No Goalie!"), Vector2D(1, 2), "white"); } - ////////////////////////////////////// - playOnMode = gameState->isStart(); - if(gameState->theirPenaltyKick() && !gameState->penaltyShootout()){ - if (goalKeeperAgent != nullptr) { - drawer->draw(QString("Penalty") , Vector2D(1, 2) , "white"); + +} + +void DefensePlan::execute(){ + ///// All of the goalKeeper && defense functions are linked in this function. + ///// First of all, we determine the behavior of goalKeeper. + ///// (first in penalty mode then other mode) + ///// Now,we identify the number of defense agents && then this number is + ///// sent to "matchingDefPos()" function to match between the produced + ///// points && our agents in defense plan. + int realDefSize = 0; + suitableRadius = RADIUS_FOR_CRITICAL_DEFENSE_AREA; + drawer->draw(Circle2D(wm->field->ourGoal() , suitableRadius) , 0 , 180 , "blue" , false); + drawer->draw(getLinesOfBallTriangle().at(0)); + drawer->draw(getLinesOfBallTriangle().at(1)); + drawGameState(); + + if (goalKeeperAgent != nullptr && goalKeeperAgent->id() != -1){ + if (gameState->theirPenaltyKick()) { penaltyMode(); + return; } - else { - drawer->draw(QString("No Goalie!") , Vector2D(1, 2) , "white"); + if(gameState->penaltyShootout()){ + penaltyShootOutMode();// hamid penalty + return; } - return; - } - if(gameState->theirPenaltyKick()){ - //TO DO: add penalty goalie for penalty shootout - penaltyShootOutMode();// hamid penalty - lastBallPosition = wm->ball->pos; - return; + GKState state = setGoalKeeperState(); + Vector2D GKTarget = setGoalKeeperTargetPoint(state); + executeGoalKeeper(GKTarget , state); + assignSkill(goalKeeperAgent , AHZSkills); } - if(gameState->isStart() && gameState->penaltyShootout()) { - penaltyShootOutMode();// hamid penalty - } - else{ - if(goalKeeperAgent != nullptr){ - setGoalKeeperState(); - setGoalKeeperTargetPoint(); - executeGoalKeeper(); - assignSkill(goalKeeperAgent , AHZSkills); - } - if(!defenseAgents.empty()){ - if(wm->our.activeAgentsCount() <= _NUM_PLAYERS){ - if(playOnMode || stopMode){ + if(!defenseAgents.empty()){ + if(wm->our.activeAgentsCount() <= _NUM_PLAYERS){ + if(gameState->isStart() || gameState->isStop()){ // checkDefenseExeptions(); // if (defExceptions.active && !know->variables["transientFlag"].toBool()) { // runDefenseExeptions(); // defenseCount = defenseAgents.size() - 1; // } // else { - defExceptions.exepAgentId = -1; - defExceptions.exeptionMode = NoneExep; - defenseCount = defenseAgents.size(); - know->variables["defenseOneTouchMode"] = false; -// } - } - else{ - know->variables["defenseOneTouchMode"] = false; + defExceptions.exepAgentId = -1; + defExceptions.exeptionMode = NoneExep; defenseCount = defenseAgents.size(); - } - if(defenseCount > 0){ - if(conf.ThreeDefenseMode){ - realDefSize = min(3 , defenseCount); - if(realDefSize == 3){ - if(findNeededDefense() == 3){ - AHZDefPoints = defenseFormation(defenseFormationForCircularPositioning(defenseNumber() , realDefSize , conf.DownLimit , conf.UpLimit), - defenseFormationForRectangularPositioning(defenseNumber() , realDefSize , 1.4 , 2.5)); - } - else{ - AHZDefPoints = defenseFormation(defenseFormationForCircularPositioning(findNeededDefense() , realDefSize , conf.DownLimit , conf.UpLimit), - defenseFormationForRectangularPositioning(findNeededDefense() , realDefSize , 1.4 , 2.5)); - for(size_t i = 0 ; i < getPositionJustForZJU(realDefSize - findNeededDefense()).size() ; i++){ - AHZDefPoints.append(getPositionJustForZJU(realDefSize - findNeededDefense()).at(i)); - } - } - } - else if(realDefSize < 3){ + know->variables["defenseOneTouchMode"] = false; +// } + } + else{ + know->variables["defenseOneTouchMode"] = false; + defenseCount = defenseAgents.size(); + } + if(defenseCount > 0){ + if(conf.ThreeDefenseMode){ + realDefSize = min(3 , defenseCount); + if(realDefSize == 3){ + if(findNeededDefense() == 3){ AHZDefPoints = defenseFormation(defenseFormationForCircularPositioning(defenseNumber() , realDefSize , conf.DownLimit , conf.UpLimit), defenseFormationForRectangularPositioning(defenseNumber() , realDefSize , 1.4 , 2.5)); } + else{ + AHZDefPoints = defenseFormation(defenseFormationForCircularPositioning(findNeededDefense() , realDefSize , conf.DownLimit , conf.UpLimit), + defenseFormationForRectangularPositioning(findNeededDefense() , realDefSize , 1.4 , 2.5)); + for(size_t i = 0 ; i < getPositionJustForZJU(realDefSize - findNeededDefense()).size() ; i++){ + AHZDefPoints.append(getPositionJustForZJU(realDefSize - findNeededDefense()).at(i)); + } + } } - else{ - realDefSize = defenseCount - decideNumOfMarks(); + else if(realDefSize < 3){ AHZDefPoints = defenseFormation(defenseFormationForCircularPositioning(defenseNumber() , realDefSize , conf.DownLimit , conf.UpLimit), defenseFormationForRectangularPositioning(defenseNumber() , realDefSize , 1.4 , 2.5)); } - matchingDefPos(realDefSize); } + else{ + realDefSize = defenseCount - decideNumOfMarks(); + AHZDefPoints = defenseFormation(defenseFormationForCircularPositioning(defenseNumber() , realDefSize , conf.DownLimit , conf.UpLimit), + defenseFormationForRectangularPositioning(defenseNumber() , realDefSize , 1.4 , 2.5)); + } + matchingDefPos(realDefSize); } - else{ - drawer->draw("Vision Problem", Vector2D(0, 0), "red" , 20); - } + } + else{ + drawer->draw("Vision Problem", Vector2D(0, 0), "red" , 20); } } } -bool DefensePlan::agentEffectOnBallProbability(Vector2D ballPos, Vector2D ballVel, Vector2D agentPos, Vector2D agentVel, bool isTowardOurgoal) { - Vector2D goal; - bool isInPenaltyRect; - - if (isTowardOurgoal) { - goal = wm->field->ourGoal(); - isInPenaltyRect = wm->field->isInOurPenaltyArea(ballPos); - } else { - goal = wm->field->oppGoal(); - isInPenaltyRect = wm->field->isInOppPenaltyArea(ballPos); - } - - - if (ballVel.length() > 4 || ballPos.dist(goal) < 3) { +bool DefensePlan::agentEffectOnBallProbability(const Vector2D& agentPos) { + if (wm->ball->vel.length() > 4 || wm->ball->pos.dist(wm->field->ourGoal()) < 3) { agentEffectOnBallProbabilityRes = true; // skyDive - } else if (ballVel.length() < 2 || ballPos.dist(agentPos) > 1) { + } else if (wm->ball->vel.length() < 2 || wm->ball->pos.dist(agentPos) > 1) { agentEffectOnBallProbabilityRes = false; // ballBisector } - return agentEffectOnBallProbabilityRes; } Vector2D DefensePlan::getGoalieShootOutTarget(bool isSkyDive) { Vector2D degree; Vector2D finalTarget; - Line2D ballPath(wm->ball->pos , wm->ball->pos + (wm->ball->vel.norm() * 10)); // Line2D ballLine(lastBallPos.first(), lastBallPos.last()); // Line2D ballRay(wm->ball->pos, wm->ball->pos + wm->opp[knowledge->nearestOppToBall]->dir); Line2D oppAgentLine(wm->opp[know->nearestOppToBall()]->pos, wm->opp[know->nearestOppToBall()]->pos + wm->opp[know->nearestOppToBall()]->dir * 10); - Vector2D a, b; - Circle2D c1 = Circle2D(wm->field->ourGoal(), 1); - if(goalKeeperAgent != nullptr && goalKeeperAgent->id() != -1) { - if (!isSkyDive) { - degree = (wm->field->ourGoalL() - (wm->ball->pos + 0.2 * wm->ball->vel)).norm() - + (wm->field->ourGoalR() - (wm->ball->pos + 0.2 * wm->ball->vel)).norm(); - - Line2D bisectorLine(wm->ball->pos + 0.2 * wm->ball->vel, wm->ball->pos + degree * 10); - if (know->chipGoalPropability(false, goalKeeperAgent->pos()) > 0.1 || - know->chipGoalPropability(false, goalKeeperAgent->pos()) < 0.05) { - shootOutDiam = min(2 * wm->ball->pos.dist(wm->field->ourGoal()) / 5.0, 2); - } + if (!isSkyDive) { + degree = (wm->field->ourGoalL() - (wm->ball->pos + 0.2 * wm->ball->vel)).norm() + + (wm->field->ourGoalR() - (wm->ball->pos + 0.2 * wm->ball->vel)).norm(); - DBUG(QString("ballBisector, diam:%1").arg(shootOutDiam), D_FATEME); - Circle2D c = Circle2D(wm->field->ourGoal(), shootOutDiam); + Line2D bisectorLine(wm->ball->pos + 0.2 * wm->ball->vel, wm->ball->pos + degree * 10); - if (c.intersection(bisectorLine, &a, &b)) { - finalTarget = (a.x > b.x) ? a : b; - } + if (know->chipGoalPropability(false) > 0.1 || + know->chipGoalPropability(false) < 0.05) { + shootOutDiam = min(2 * wm->ball->pos.dist(wm->field->ourGoal()) / 5.0, 2); + } - drawer->draw(finalTarget, QColor(Qt::cyan)); - } else { - if (wm->ball->vel.length() > 4 - || (wm->ball->pos.dist(wm->field->ourGoal()) < 2.7 - && wm->ball->pos.dist(wm->opp[know->nearestOppToBall()]->pos) > 0.15) - || wm->ball->pos.dist(wm->field->ourGoal()) < 1.4) { - DBUG("skydive, ballpath", D_FATEME); - finalTarget = ballPath.perpendicular(wm->our[goalKeeperAgent->id()]->pos).intersection(ballPath); - } else { - finalTarget = oppAgentLine.perpendicular(wm->our[goalKeeperAgent->id()]->pos).intersection( - oppAgentLine); - DBUG("skydive, oppAgentLine", D_FATEME); - } + DBUG(QString("ballBisector, diam:%1").arg(shootOutDiam), D_FATEME); + Circle2D c = Circle2D(wm->field->ourGoal(), shootOutDiam); - if (!wm->field->isInOurPenaltyArea(finalTarget)) { - c1.intersection(oppAgentLine, &a, &b); - finalTarget = (a.x > b.x) ? a : b; - } + if (c.intersection(bisectorLine, &a, &b)) { + finalTarget = (a.x > b.x) ? a : b; + } - drawer->draw(finalTarget, "blue"); + drawer->draw(finalTarget, QColor(Qt::cyan)); + } + else { + if (wm->ball->vel.length() > 4 + || (wm->ball->pos.dist(wm->field->ourGoal()) < 2.7 + && wm->ball->pos.dist(wm->opp[know->nearestOppToBall()]->pos) > 0.15) + || wm->ball->pos.dist(wm->field->ourGoal()) < 1.4) { + DBUG("skydive, ballpath", D_FATEME); + finalTarget = ballPath.perpendicular(wm->our[goalKeeperAgent->id()]->pos).intersection(ballPath); + } else { + finalTarget = oppAgentLine.perpendicular(wm->our[goalKeeperAgent->id()]->pos).intersection( + oppAgentLine); + DBUG("skydive, oppAgentLine", D_FATEME); } - return finalTarget; + if (!wm->field->isInOurPenaltyArea(finalTarget)) { + drawer->draw(Circle2D(Vector2D(0 , 0) , 0.1) , "red"); + Circle2D(wm->field->ourGoal(), 1).intersection(oppAgentLine, &a, &b); + finalTarget = (a.x > b.x) ? a : b; + } + drawer->draw(finalTarget, "blue"); } + return finalTarget; } -bool DefensePlan::canReachToBall(int ourAgentId, int theirAgentId) { +bool DefensePlan::canReachToBall(const int& ourAgentId, const int& theirAgentId) { if (wm->our[ourAgentId] == nullptr || wm->opp[theirAgentId] == nullptr) { return false; } - Vector2D ballPosAndVel; - ballPosAndVel = wm->ball->pos + wm->ball->vel; - + Vector2D ballPosAndVel= wm->ball->pos + wm->ball->vel; return wm->our[ourAgentId]->pos.dist(ballPosAndVel) < wm->opp[theirAgentId]->pos.dist(ballPosAndVel) - 0.3 - && wm->ball->pos.dist(wm->field->ourGoal()) > 2.5 && wm->ball->pos.dist(wm->opp[theirAgentId]->pos) > 1; + && wm->ball->pos.dist(wm->field->ourGoal()) > 2.5 && wm->ball->pos.dist(wm->opp[theirAgentId]->pos) > 1; } -int DefensePlan::decideShootOutMode() { - if (goalKeeperAgent == nullptr || goalKeeperAgent->id() == -1) { - return 0; - } - int result; - - - if (lastBallPosition.dist(wm->ball->pos) < 0.04) { +shootOutMode DefensePlan::decideShootOutMode() { + if (wm->ball->pos.dist(Vector2D(0 , 0)) < 0.2 || wm->ball->pos.x > 0) { DBUG("beforeTouch", D_FATEME); shootOutClearModeSelected = false; - result = beforeTouch; - } else if (canReachToBall(goalKeeperAgent->id(), know->nearestOppToBall()) - || (!Circle2D(wm->ball->pos, 0.10).contains(wm->opp[know->nearestOppToBall()]->pos) - && wm->ball->pos.dist(wm->field->ourGoal()) < 1.7) + return shootOutMode :: beforeTouch; + } + if (canReachToBall(goalKeeperAgent->id(), know->nearestOppToBall()) + || (!Circle2D(wm->ball->pos, 0.10).contains(wm->opp[know->nearestOppToBall()]->pos) && wm->ball->pos.dist(wm->field->ourGoal()) < 1.7) || shootOutClearModeSelected - ) { + ){ DBUG("shootOutClear", D_FATEME); shootOutClearModeSelected = true; - result = shootOutClear; - } else if (!agentEffectOnBallProbability(wm->ball->pos, wm->ball->vel, wm->opp[know->nearestOppToBall()]->pos, wm->opp[know->nearestOppToBall()]->vel, true)) { + return shootOutMode :: shootOutClear; + } + if (!agentEffectOnBallProbability(wm->opp[know->nearestOppToBall()]->pos)) { DBUG("ballBisector", D_FATEME); - result = ballBisector; - } else { - DBUG("skydive", D_FATEME); - result = skyDive; + return shootOutMode :: ballBisector; } - - return result; + DBUG("skydive", D_FATEME); + return shootOutMode :: skyDive; } void DefensePlan::penaltyShootOutMode() { - if (goalKeeperAgent == nullptr || goalKeeperAgent->id() == -1) { - return; - } - penaltyShootoutMode = decideShootOutMode(); - - Vector2D targetDir(10, 5), agentTarget; - targetDir = wm->opp[know->nearestOppToBall()]->pos - wm->field->ourGoal(); - - if (lastBallPos.count() < 15) { - lastBallPos.append(wm->ball->pos); - } else { - lastBallPos.removeFirst(); - } - - - switch (penaltyShootoutMode) { - case beforeTouch: - assignSkill(goalKeeperAgent , gpa[goalKeeperAgent->id()]); - gpa[goalKeeperAgent->id()]->setSlowmode(false); - gpa[goalKeeperAgent->id()]->setDivemode(true); - gpa[goalKeeperAgent->id()]->setTargetpos(wm->field->ourGoal() + Vector2D(0.1, 0)); - gpa[goalKeeperAgent->id()]->setTargetdir(targetDir); - - break; - - case shootOutClear: - assignSkill(goalKeeperAgent, kickSkill); - kickSkill->setTolerance(50); - kickSkill->setDontkick(false); - kickSkill->setSlow(false); - kickSkill->setSpin(0); - kickSkill->setAvoidpenaltyarea(false); - kickSkill->setGoaliemode(false); - kickSkill->setChip(true); - kickSkill->setChipdist(4.5); - kickSkill->setTarget(wm->field->oppGoal()); - kickSkill->setSagmode(true); - - break; - - case ballBisector: - assignSkill(goalKeeperAgent , gpa[goalKeeperAgent->id()]); - gpa[goalKeeperAgent->id()]->setSlowmode(false); - gpa[goalKeeperAgent->id()]->setDivemode(false); - gpa[goalKeeperAgent->id()]->setLookat(wm->ball->pos); - agentTarget = getGoalieShootOutTarget(false); + Vector2D agentTarget; + Vector2D targetDir = wm->opp[know->nearestOppToBall()]->pos - wm->field->ourGoal(); - gpa[goalKeeperAgent->id()]->setTargetpos(agentTarget); - gpa[goalKeeperAgent->id()]->setTargetdir(targetDir); - - drawer->draw(agentTarget, QColor(Qt::darkRed)); - break; - - case skyDive: - assignSkill(goalKeeperAgent , gpa[goalKeeperAgent->id()]); - gpa[goalKeeperAgent->id()]->setSlowmode(false); - gpa[goalKeeperAgent->id()]->setDivemode(true); - gpa[goalKeeperAgent->id()]->setLookat(wm->ball->pos); - agentTarget = getGoalieShootOutTarget(true) ; + switch (decideShootOutMode()) { + case shootOutMode :: beforeTouch: + assignSkill(goalKeeperAgent , gpa[goalKeeperAgent->id()]); + gpa[goalKeeperAgent->id()]->setSlowmode(false); + gpa[goalKeeperAgent->id()]->setDivemode(true); + gpa[goalKeeperAgent->id()]->setNoavoid(true); + gpa[goalKeeperAgent->id()]->setTargetpos(wm->field->ourGoal() + Vector2D(0.1, 0)); + gpa[goalKeeperAgent->id()]->setTargetdir(targetDir); + break; + case shootOutMode :: shootOutClear: + assignSkill(goalKeeperAgent, kickSkill); + kickSkill->setTolerance(50); + kickSkill->setDontkick(false); + kickSkill->setSlow(false); + kickSkill->setSpin(0); + kickSkill->setAvoidpenaltyarea(false); + kickSkill->setGoaliemode(false); + kickSkill->setChip(true); + kickSkill->setChipdist(4.5); + kickSkill->setTarget(wm->field->oppGoal()); + kickSkill->setSagmode(true); + break; - gpa[goalKeeperAgent->id()]->setTargetpos(agentTarget); - gpa[goalKeeperAgent->id()]->setTargetdir(targetDir); + case shootOutMode :: ballBisector: + assignSkill(goalKeeperAgent , gpa[goalKeeperAgent->id()]); + gpa[goalKeeperAgent->id()]->setSlowmode(false); + gpa[goalKeeperAgent->id()]->setDivemode(false); + gpa[goalKeeperAgent->id()]->setLookat(wm->ball->pos); + gpa[goalKeeperAgent->id()]->setTargetpos(getGoalieShootOutTarget(false)); + gpa[goalKeeperAgent->id()]->setTargetdir(targetDir); + drawer->draw(agentTarget, QColor(Qt::darkRed)); + break; - drawer->draw(agentTarget, QColor(Qt::darkBlue)); - break; - default: - break; + case shootOutMode :: skyDive: + assignSkill(goalKeeperAgent , gpa[goalKeeperAgent->id()]); + gpa[goalKeeperAgent->id()]->setSlowmode(false); + gpa[goalKeeperAgent->id()]->setDivemode(true); + gpa[goalKeeperAgent->id()]->setLookat(wm->ball->pos); + gpa[goalKeeperAgent->id()]->setTargetpos(getGoalieShootOutTarget(true)); + gpa[goalKeeperAgent->id()]->setTargetdir(targetDir); + drawer->draw(agentTarget, QColor(Qt::darkBlue)); + break; + default: + break; } - - } void DefensePlan::penaltyMode() { //// By this function goalKeeper is able to move according to the direction //// of the opponent agents that will shot to our goal in pentalty mode. - - Vector2D ballPos = wm->ball->pos; const float goalLineExtra = 0.03; const double xDiff = 0.10; Line2D goalLine(wm->field->ourGoalL() + Vector2D(+xDiff, +goalLineExtra), wm->field->ourGoalR() + Vector2D(+xDiff, -goalLineExtra)); - const double epsilon = 0.12; - Vector2D target(-2.93, 0.0); - - Line2D ballRay(ballPos, ballPos + wm->opp[know->nearestOppToBall()]->dir); - - + Vector2D target(-5.92, 0.0); + Line2D ballRay(wm->ball->pos, wm->ball->pos + wm->opp[know->nearestOppToBall()]->dir); Vector2D intersectionPoint = goalLine.intersection(ballRay); double ang = ballRay.a() * goalLine.b() - ballRay.b() * goalLine.a(); @@ -2634,11 +2061,9 @@ void DefensePlan::penaltyMode() { intersectionPoint.y = wm->field->oppGoalL().y; } } - if (ang <= 0.95) { intersectionPoint.y *= -1; } - intersectionPoint.y *= (7.0 / 10.0); // if(fabs(knowledge->getAgent(goalKeeperAgent->id())->pos().y) > fabs(wm->field->ourGoalR().y)) @@ -2646,32 +2071,21 @@ void DefensePlan::penaltyMode() { // *(fabs((intersectionPoint-knowledge->getAgent(goalKeeperAgent->id())->pos()).y)/(intersectionPoint-knowledge->getAgent(goalKeeperAgent->id())->pos()).y); // sign if (intersectionPoint.valid()) { - target = intersectionPoint; - drawer->draw(target, "red"); + target.y = intersectionPoint.y; } else { - target.y = 0.0; + target.y = 0.0;//TODO:change it } // target.y = min(max(target.y, wm->field->ourGoalR().y + epsilon), wm->field->ourGoalL().y - epsilon + 0.03); - Vector2D targetDir(10, 5); - targetDir.setDir(AngleDeg(0)); - targetDir.setLength(1); - //check - target.x = -5.92; - - drawer->draw(target, "blue"); - assignSkill(goalKeeperAgent , gpa[goalKeeperAgent->id()]); gpa[goalKeeperAgent->id()]->setSlowmode(false); gpa[goalKeeperAgent->id()]->setDivemode(true); - + gpa[goalKeeperAgent->id()]->setNoavoid(true); gpa[goalKeeperAgent->id()]->setTargetpos(target); //HINT : gpa->init - gpa[goalKeeperAgent->id()]->setTargetdir(targetDir); - + gpa[goalKeeperAgent->id()]->setTargetdir(Vector2D (1 , 0)); //gpa[goalKeeperAgent->id()]->init(target , targetDir); - } Vector2D* DefensePlan::getIntersectWithDefenseArea(const Line2D& line, const Vector2D& blockPoint) { @@ -2779,155 +2193,64 @@ Vector2D* DefensePlan::getIntersectWithDefenseArea(const Segment2D& segment, con return retPoint; } -void DefensePlan::executeGoalKeeper() { +void DefensePlan::executeGoalKeeper(const Vector2D &GKTarget , const GKState & state) { //// This Function execute the goalkeeper skills according to the //// target point that have been produced in the "setGoalKeeperTargetPoint" //// function. In this function also like the other functions for goalkeeper, //// we have some mode for handling the goalkeeper behavior. - - playOffMode = gameState->theirDirectKick() || gameState->theirIndirectKick() || gameState->kickoff() || gameState->ourDirectKick() || gameState->ourIndirectKick(); - playOnMode = gameState->isStart(); - stopMode = gameState->isStop(); QList tempSol; - tempSol.clear(); - if (!goalKeeperOneTouch) { - firstTimeGoalKeeperOneTouch = false; - } - if(!ballIsBesidePoles){ - FlagBesidePoles = false; - f = 0; - Rect2D ballRectangle(wm->ball->pos + Vector2D(0.25 , 0.25) , wm->ball->pos + Vector2D(-0.25 , -0.25)); - drawer->draw(ballRectangle); - QList ballRectanglePoints; - if (wm->field->isInField(ballRectangle.topLeft())) { - ballRectanglePoints.append(ballRectangle.topLeft()); - } - if (wm->field->isInField(ballRectangle.topRight())) { - ballRectanglePoints.append(ballRectangle.topRight()); - } - if (wm->field->isInField(ballRectangle.bottomLeft())) { - ballRectanglePoints.append(ballRectangle.bottomLeft()); - } - if (wm->field->isInField(ballRectangle.bottomRight())) { - ballRectanglePoints.append(ballRectangle.bottomRight()); - } - //goalKeeperTarget = ballRectanglePoints.at(0).dist(goalKeeperAgent->pos()) < ballRectanglePoints.at(1).dist(goalKeeperAgent->pos()) ? ballRectanglePoints.at(0) : ballRectanglePoints.at(1); - } - if (goalKeeperAgent != nullptr && goalKeeperAgent->id() != -1) { - DBUG(QString("goalKeeper clear mode : %1").arg(know->variables["goalKeeperClearMode"].toBool()) , D_AHZ); - DBUG(QString("goalKeeper oneTouch mode : %1").arg(know->variables["goalKeeperOneTouchMode"].toBool()) , D_AHZ); - if (playOffMode) { - DBUG("Their Indirect" , D_AHZ); - know->variables["goalKeeperClearMode"] = false; - know->variables["goalKeeperOneTouchMode"] = false; - AHZSkills = gpa[goalKeeperAgent->id()]; - gpa[goalKeeperAgent->id()]->setDivemode(false); - gpa[goalKeeperAgent->id()]->setOnetouchmode(false); - gpa[goalKeeperAgent->id()]->setOnetouchflag(false); - gpa[goalKeeperAgent->id()]->setChip(false); - gpa[goalKeeperAgent->id()]->setSlowmode(false); - gpa[goalKeeperAgent->id()]->setAvoidpenaltyarea(false); - gpa[goalKeeperAgent->id()]->setTargetpos(goalKeeperTarget); - gpa[goalKeeperAgent->id()]->setTargetdir(wm->ball->pos - wm->field->ourGoal()); + tempSol.clear(); - } - else if(know->variables["transientFlag"].toBool()){ - drawer->draw(Circle2D(wm->field->ourGoal() , 0.2) , "red"); - DBUG("TS Mode" , D_AHZ); - know->variables["goalKeeperClearMode"] = false; - know->variables["goalKeeperOneTouchMode"] = false; - AHZSkills = gpa[goalKeeperAgent->id()]; - gpa[goalKeeperAgent->id()]->setDivemode(false); - gpa[goalKeeperAgent->id()]->setSlowmode(false); - gpa[goalKeeperAgent->id()]->setOnetouchmode(false); - gpa[goalKeeperAgent->id()]->setOnetouchflag(false); - gpa[goalKeeperAgent->id()]->setChip(false); - gpa[goalKeeperAgent->id()]->setNoavoid(true); - gpa[goalKeeperAgent->id()]->setAvoidpenaltyarea(false); - gpa[goalKeeperAgent->id()]->setTargetpos(goalKeeperTarget); - drawer->draw(Circle2D(goalKeeperTarget , 0.2) , "cyan"); - if(GKReciveBallInTS) - gpa[goalKeeperAgent->id()]->setTargetdir(wm->ball->pos - goalKeeperTarget); - else - gpa[goalKeeperAgent->id()]->setTargetdir(goalKeeperTarget - wm->field->ourGoal()); - } - else if(stopMode){ - DBUG("Stop Mode" , D_AHZ); - know->variables["goalKeeperClearMode"] = false; - know->variables["goalKeeperOneTouchMode"] = false; - AHZSkills = gpa[goalKeeperAgent->id()]; - goalKeeperAgent->action = gpa[goalKeeperAgent->id()]; - gpa[goalKeeperAgent->id()]->setDivemode(false); - gpa[goalKeeperAgent->id()]->setOnetouchmode(false); - gpa[goalKeeperAgent->id()]->setOnetouchflag(false); - gpa[goalKeeperAgent->id()]->setChip(false); + //init skill + AHZSkills = gpa[goalKeeperAgent->id()]; + know->variables["goalKeeperClearMode"] = false; + know->variables["goalKeeperOneTouchMode"] = false; + gpa[goalKeeperAgent->id()]->setDivemode(false); + gpa[goalKeeperAgent->id()]->setOnetouchmode(false); + gpa[goalKeeperAgent->id()]->setOnetouchflag(false); + gpa[goalKeeperAgent->id()]->setChip(false); + gpa[goalKeeperAgent->id()]->setSlowmode(false); + gpa[goalKeeperAgent->id()]->setAvoidpenaltyarea(false); + gpa[goalKeeperAgent->id()]->setSlowmode(false); + gpa[goalKeeperAgent->id()]->setNoavoid(true); + gpa[goalKeeperAgent->id()]->setTargetpos(GKTarget); + /////gpa[goalKeeperAgent->id()]->setTargetdir Don't forget add it to your execute state. + + ////////////////////////////////////////////////////////////////////////// + switch (state){ + case GKState :: GKReciveBallInTS: + gpa[goalKeeperAgent->id()]->setTargetdir(wm->ball->pos - GKTarget); + break; + case GKState :: GKPredictInTs: + gpa[goalKeeperAgent->id()]->setTargetdir(GKTarget - wm->field->ourGoal()); + break; + case GKState :: playoff: + gpa[goalKeeperAgent->id()]->setTargetdir(wm->ball->pos - wm->field->ourGoal()); + break; + case GKState :: Stop: gpa[goalKeeperAgent->id()]->setSlowmode(true); - gpa[goalKeeperAgent->id()]->setAvoidpenaltyarea(false); - gpa[goalKeeperAgent->id()]->setTargetpos(goalKeeperTarget); //HINT : gpa->init gpa[goalKeeperAgent->id()]->setTargetdir(wm->ball->pos - wm->field->ourGoal()); - } - else if (ballIsOutOfField) { - DBUG("Ball is out of field" , D_AHZ); - know->variables["goalKeeperClearMode"] = false; - know->variables["goalKeeperOneTouchMode"] = false; - AHZSkills = gpa[goalKeeperAgent->id()]; - gpa[goalKeeperAgent->id()]->setDivemode(false); - gpa[goalKeeperAgent->id()]->setOnetouchmode(false); - gpa[goalKeeperAgent->id()]->setOnetouchflag(false); - gpa[goalKeeperAgent->id()]->setChip(false); + break; + case GKState :: ballIsOutOfField: gpa[goalKeeperAgent->id()]->setSlowmode(true); - gpa[goalKeeperAgent->id()]->setAvoidpenaltyarea(false); - gpa[goalKeeperAgent->id()]->setTargetpos(goalKeeperTarget); //HINT : gpa->init gpa[goalKeeperAgent->id()]->setTargetdir(wm->ball->pos - wm->field->ourGoal()); - } - else if (ballIsBesidePoles){ - DBUG("Ball is beside the poles" , D_AHZ); - counterBallWasBesidePoles = 0; - know->variables["goalKeeperClearMode"] = false; - know->variables["goalKeeperOneTouchMode"] = false; - if(f > 3){ - AHZSkills = gpa[goalKeeperAgent->id()]; - gpa[goalKeeperAgent->id()]->setDivemode(false); - gpa[goalKeeperAgent->id()]->setOnetouchmode(false); - gpa[goalKeeperAgent->id()]->setOnetouchflag(false); - gpa[goalKeeperAgent->id()]->setChip(false); + break; + case GKState :: ballIsBesidePoles: + if(stateBallBesidepoles >= 3){ gpa[goalKeeperAgent->id()]->setSlowmode(true); - gpa[goalKeeperAgent->id()]->setAvoidpenaltyarea(false); - gpa[goalKeeperAgent->id()]->setTargetpos(wm->ball->pos); - gpa[goalKeeperAgent->id()]->setTargetdir(wm->ball->pos - goalKeeperTarget); - goalKeeperAgent->action = gpa[goalKeeperAgent->id()]; + gpa[goalKeeperAgent->id()]->setTargetdir(wm->field->oppGoal()); } else{ - AHZSkills = gpa[goalKeeperAgent->id()]; - gpa[goalKeeperAgent->id()]->setDivemode(false); - gpa[goalKeeperAgent->id()]->setOnetouchmode(false); - gpa[goalKeeperAgent->id()]->setOnetouchflag(false); - gpa[goalKeeperAgent->id()]->setChip(false); gpa[goalKeeperAgent->id()]->setSlowmode(true); - gpa[goalKeeperAgent->id()]->setAvoidpenaltyarea(false); - gpa[goalKeeperAgent->id()]->setTargetpos(goalKeeperTarget); //HINT : gpa->init gpa[goalKeeperAgent->id()]->setBallobstacleradius(0.2); - gpa[goalKeeperAgent->id()]->setTargetdir(wm->ball->pos - goalKeeperTarget); - } - } - else if (goalKeeperClearMode && !dangerForGoalKeeperClear) { - know->variables["goalKeeperClearMode"] = true; - know->variables["goalKeeperOneTouchMode"] = false; - drawer->draw(Circle2D(wm->ball->pos , 0.1) , "red"); + gpa[goalKeeperAgent->id()]->setTargetdir(wm->ball->pos - GKTarget); + } + break; + case GKState :: clearMode: if (wm->ball->vel.length() > 0.4 && wm->ball->vel.length() < 1.3) { - AHZSkills = gpa[goalKeeperAgent->id()]; - DBUG("Clear slow ball" , D_AHZ); - gpa[goalKeeperAgent->id()]->setDivemode(false); - gpa[goalKeeperAgent->id()]->setOnetouchmode(false); - gpa[goalKeeperAgent->id()]->setOnetouchflag(false); - gpa[goalKeeperAgent->id()]->setChip(false); - gpa[goalKeeperAgent->id()]->setSlowmode(false); - gpa[goalKeeperAgent->id()]->setAvoidpenaltyarea(false); - gpa[goalKeeperAgent->id()]->setTargetpos(goalKeeperTarget); gpa[goalKeeperAgent->id()]->setTargetdir(wm->ball->pos - wm->field->ourGoal()); } else{ - drawer->draw(Circle2D(Vector2D(0 , 0) , 0.1) , "black"); AHZSkills = kickSkill; if(lastStateForGoalKeeper == QString("BesidePoleMode") || counterBallWasBesidePoles < 100){ counterBallWasBesidePoles++; @@ -2936,106 +2259,41 @@ void DefensePlan::executeGoalKeeper() { } else{ kickSkill->setSlow(false); - if(wm->ball->pos.y >= 0){ - kickSkill->setTarget(Vector2D(-10 , -4) - wm->field->ourGoal()); - } else { - kickSkill->setTarget(Vector2D(-10 , 4) - wm->field->ourGoal()); - } + kickSkill->setTarget(Vector2D(0 , wm->ball->pos.y)); } - DBUG("Clear Mode" , D_AHZ); kickSkill->setTolerance(4); - kickSkill->setDontkick(false); + kickSkill->setDontkick(false); kickSkill->setSpin(0); kickSkill->setAvoidpenaltyarea(false); kickSkill->setGoaliemode(true); kickSkill->setChip(true); kickSkill->setIskickchargetime(true); kickSkill->setKickchargetime(1023); - ////lhum thinking kick avoid penalty area -// -// AHZSkills = gpa[goalKeeperAgent->id()]; -// DBUG("Clear slow ball" , D_AHZ); -// gpa[goalKeeperAgent->id()]->setTargetpos(goalKeeperAgent->pos()); - } - } - else { - if (goalKeeperOneTouch) { - DBUG("One touch Mode" , D_AHZ); - know->variables["goalKeeperClearMode"] = false; - know->variables["goalKeeperOneTouchMode"] = true; - AHZSkills = gpa[goalKeeperAgent->id()]; - gpa[goalKeeperAgent->id()]->setSlowmode(false); - gpa[goalKeeperAgent->id()]->setDivemode(true); - gpa[goalKeeperAgent->id()]->setOnetouchmode(true); - gpa[goalKeeperAgent->id()]->setTargetdir(-goalKeeperTarget + wm->ball->pos); - gpa[goalKeeperAgent->id()]->setAvoidpenaltyarea(false); - gpa[goalKeeperAgent->id()]->setNoavoid(true); - gpa[goalKeeperAgent->id()]->setTargetpos(goalKeeperTarget); //HINT : gpa->init - gpa[goalKeeperAgent->id()]->setOnetouchflag(true); - gpa[goalKeeperAgent->id()]->setChip(true); - gpa[goalKeeperAgent->id()]->setChipdist(1023); - gpa[goalKeeperAgent->id()]->setKickspeed(0); - } - else if (dangerForGoalKeeperClear) { - drawer->draw(Circle2D(goalKeeperTarget , 0.5), QColor(Qt::red)); -// if (dangerForInsideOfThePenaltyArea) { -// know->variables["goalKeeperClearMode"] = true; -// know->variables["goalKeeperOneTouchMode"] = false; -// DBUG("Danger Mode" , D_AHZ); -// AHZSkills = kickSkill; -// kickSkill->setTolerance(10); -// kickSkill->setDontkick(false); -// kickSkill->setSlow(true); -// kickSkill->setSpin(0); -// kickSkill->setChip(true); -// kickSkill->setAvoidpenaltyarea(false); -// kickSkill->setGoaliemode(true); -// kickSkill->setChipdist(4.5); -// if (wm->ball->pos.y >= 0){ -// kickSkill->setTarget(Vector2D(-10 , -4) - wm->field->ourGoal()); -// } -// else{ -// kickSkill->setTarget(Vector2D(-10 , 4) - wm->field->ourGoal()); -// } -// } -// else{ - goalKeeperTarget = strictFollowBall(ballPrediction(true)); - know->variables["goalKeeperClearMode"] = false; - know->variables["goalKeeperOneTouchMode"] = false; - AHZSkills = gpa[goalKeeperAgent->id()]; - gpa[goalKeeperAgent->id()]->setSlowmode(false); - gpa[goalKeeperAgent->id()]->setOnetouchmode(false); - gpa[goalKeeperAgent->id()]->setDivemode(false); - gpa[goalKeeperAgent->id()]->setTargetpos(goalKeeperTarget); - gpa[goalKeeperAgent->id()]->setTargetdir(goalKeeperAgent->pos() - wm->field->ourGoal()); - gpa[goalKeeperAgent->id()]->setAvoidpenaltyarea(false); - gpa[goalKeeperAgent->id()]->setNoavoid(true); -// } - } - else { - ROS_INFO_STREAM("TS: sf"); - //// strict follow - know->variables["goalKeeperClearMode"] = false; - know->variables["goalKeeperOneTouchMode"] = false; - AHZSkills = gpa[goalKeeperAgent->id()]; - gpa[goalKeeperAgent->id()]->setSlowmode(false); - gpa[goalKeeperAgent->id()]->setDivemode(false); - gpa[goalKeeperAgent->id()]->setOnetouchmode(false); - gpa[goalKeeperAgent->id()]->setOnetouchflag(false); - gpa[goalKeeperAgent->id()]->setChip(false); - gpa[goalKeeperAgent->id()]->setTargetpos(goalKeeperTarget); - gpa[goalKeeperAgent->id()]->setTargetdir(ballPrediction(true) - wm->field->ourGoal()); - gpa[goalKeeperAgent->id()]->setAvoidpenaltyarea(false); - goalKeeperAgent->action = gpa[goalKeeperAgent->id()]; - } - drawer->draw(Circle2D(goalKeeperTarget , 0.05) , 0 , 360 , "black" , true); - } - DBUG(QString("beside flag : %1").arg(ballIsBesidePoles) , D_AHZ); + break; + case GKState :: oneTouch: + gpa[goalKeeperAgent->id()]->setDivemode(true); + gpa[goalKeeperAgent->id()]->setOnetouchmode(true); + gpa[goalKeeperAgent->id()]->setOnetouchflag(true); + gpa[goalKeeperAgent->id()]->setChip(true); + gpa[goalKeeperAgent->id()]->setTargetdir(-GKTarget + wm->ball->pos); + gpa[goalKeeperAgent->id()]->setChipdist(1023); + gpa[goalKeeperAgent->id()]->setKickspeed(0); + break; + case GKState :: dangerForClear: + gpa[goalKeeperAgent->id()]->setTargetpos(strictFollowBall(ballPrediction(true))); + gpa[goalKeeperAgent->id()]->setTargetdir(goalKeeperAgent->pos() - wm->field->ourGoal()); + break; + case GKState :: strictFollow: + gpa[goalKeeperAgent->id()]->setTargetdir(ballPrediction(true) - wm->field->ourGoal()); + break; + default: + break; } + drawer->draw(Circle2D(GKTarget , 0.05) , 0 , 360 , "black" , true); } -bool DefensePlan::defenseOneTouchOrNot() { +bool DefensePlan:: defenseOneTouchOrNot() { //// First of All this function checks if other defense agents (that isn't //// one toucher) can be one toucher , those role changes to it. Then we //// check that if ball is shot to the our goal, we active the one touch flag. @@ -3052,7 +2310,7 @@ bool DefensePlan::defenseOneTouchOrNot() { otherAgents.append(i); } } - for (int i = 0 ; i < otherAgents.size() ; i++) { + for(int i = 0 ; i < otherAgents.size() ; i++) { Segment2D otherAgentIntersect(wm->ball->pos , pointForKick); Vector2D sol1 , sol2; Circle2D circ(defenseAgents.at(otherAgents.at(i))->pos() , Robot::robot_radius_new); @@ -3061,8 +2319,8 @@ bool DefensePlan::defenseOneTouchOrNot() { Line2D ballWay(wm->ball->pos , wm->ball->vel + wm->ball->pos); Segment2D ballWay1(wm->ball->pos , wm->ball->vel + wm->ball->pos); //////// danger for defense one touch /////// - for (int i = 0 ; i < defenseAgents.size() ; i++) { - if(Circle2D(defenseAgents.at(i)->pos() , Robot::robot_radius_new).intersection(ballWay1 , &sol[0] , &sol[1])) { + for(int j = 0 ; j < defenseAgents.size() ; j++) { + if(Circle2D(defenseAgents.at(j)->pos() , Robot::robot_radius_new).intersection(ballWay1 , &sol[0] , &sol[1])) { isIntersect = true; break; } @@ -3346,8 +2604,6 @@ int DefensePlan::decideNumOfMarks(){ //// This function returns the "defenseCount" in all states, except when ball //// is near the corners , returns the 1. - playOnMode = gameState->isStart(); - playOffMode = gameState->theirDirectKick() || gameState->theirIndirectKick(); if (defenseCount > 0){ if(conf.ThreeDefenseMode){ return defenseCount - defenseNumber(); @@ -3356,13 +2612,13 @@ int DefensePlan::decideNumOfMarks(){ if (gameState->isStop()){ return defenseCount - defenseNumber(); } - if (playOffMode) { + if (gameState->theirDirectKick() || gameState->theirIndirectKick()) { return defenseCount; } else if (know->variables["transientFlag"].toBool()) { return defenseCount; } - else if (playOnMode) { + else if (gameState->isStart()) { return 0; } } @@ -3370,7 +2626,7 @@ int DefensePlan::decideNumOfMarks(){ return 0; } -Vector2D DefensePlan::ballPrediction(bool _isGoalie) { +Vector2D DefensePlan::ballPrediction(const bool _isGoalie) { //// When ballLine is in field we predict the ball line : If ball moves toward the //// our field, we consider the ballLine (ballPos + ballVel) && If moves toward //// the opponent field we consider the ballPos + ballVel.y for the location @@ -3378,17 +2634,8 @@ Vector2D DefensePlan::ballPrediction(bool _isGoalie) { //// of field with ballLine.(algorithm is just like when ballLine isn't in the //// field).Also , If the ballLine has intersection with opponent agent, we //// consider the intersection point for the locaiton of the ball. - Vector2D BallPos = wm->ball->pos; - Vector2D BallVel; - if(wm->ball->vel.length() < 1){ - BallVel = wm->ball->vel * 0.7; - } else{ - BallVel = wm->ball->vel.norm(); - } - Segment2D ballPosVel(BallPos, BallPos + (BallVel)); - Vector2D predictedBall = BallPos; + Vector2D predictedBall = wm->ball->pos; Vector2D solu[6]; - Rect2D fieldRect(Vector2D(- wm->field->_FIELD_WIDTH / 2.0 , - wm->field->_FIELD_HEIGHT / 2.0) + Vector2D(-0.005, -0.005), Vector2D(wm->field->_FIELD_WIDTH / 2.0 , wm->field->_FIELD_HEIGHT / 2.0) + Vector2D(+0.005, +0.005)); double dist2Ball = 1000; // predictedBall = wm->ball->pos; // return predictedBall; @@ -3397,25 +2644,21 @@ Vector2D DefensePlan::ballPrediction(bool _isGoalie) { //} // wm->opp.update(); //LhumTS - ballIntersectOurPenaltyArea = false; if (_isGoalie && know->variables["transientFlag"].toBool()){ wm->field->ourBigPenaltyArea(1, -0.1 , 0).intersection(Segment2D(wm->ball->pos , wm->ball->pos + wm->ball->vel.norm() * 100 ), &solu[0], &solu[1]);/////////////////Lhum if((solu[0].isValid() && solu[1].isValid()) && ((solu[0].y > 0.6 && solu[1].y < -0.6) || (solu[1].y > 0.6 && solu[0].y < -0.6))){ - predictedBall = (BallPos.dist(solu[0]) < BallPos.dist(solu[1])) ? (solu[1]) : (solu[0]); + predictedBall = (wm->ball->pos.dist(solu[0]) < wm->ball->pos.dist(solu[1])) ? (solu[1]) : (solu[0]); drawer->draw(Circle2D(predictedBall , 0.2) , "blue"); - ballIntersectOurPenaltyArea = true; return predictedBall; } else if(solu[0].isValid() && wm->field->isInOurPenaltyArea(wm->ball->pos)){ predictedBall = solu[0]; drawer->draw(Circle2D(predictedBall , 0.2) , "blue"); - ballIntersectOurPenaltyArea = true; return predictedBall; } else if(solu[1].isValid() && wm->field->isInOurPenaltyArea(wm->ball->pos)){ predictedBall = solu[1]; drawer->draw(Circle2D(predictedBall , 0.2) , "blue"); - ballIntersectOurPenaltyArea = true; return predictedBall; } } @@ -3565,12 +2808,10 @@ void DefensePlan::findPos(int _markAgentSize){ bool playOff = (gameState->theirDirectKick()|| (gameState->theirIndirectKick())); bool MantoManAllTransientFlag = conf.ManToManAllTransiant; xLimitForblockingPass = 0; - manToManMarkBlockPassFlag = conf.PlayOffManToMan; - stopMode = gameState->isStop(); markPoses.clear(); markAngs.clear(); ///////////////// Man To Man AllTransiant Mode for Mark //////////////////// - if (MantoManAllTransientFlag){ + if (conf.PlayOffManToMan){ if (know->variables["transientFlag"].toBool()){ segmentperpass = 0.1; segmentpershoot = 0.97; @@ -3586,7 +2827,7 @@ void DefensePlan::findPos(int _markAgentSize){ } //////////////// Determine the plan of mark from GUI //////////////////// if(manToManMarkBlockPassFlag || wm->ball->pos.x > xLimitForblockingPass){ - if(playOff || stopMode){ + if(playOff || gameState->isStop()){ know->variables["stateForMark"] = QString("BlockPass"); manToManMarkBlockPassInPlayOff(oppAgentsToMarkPos, _markAgentSize , segmentperpass); } @@ -3602,7 +2843,7 @@ void DefensePlan::findPos(int _markAgentSize){ } } else { - if (playOff || stopMode) { + if (playOff || gameState->isStop()) { know->variables["stateForMark"] = QString("BlockShot");; manToManMarkBlockShotInPlayOff(_markAgentSize); } @@ -3838,73 +3079,54 @@ Vector2D DefensePlan::strictFollowBall(Vector2D _ballPos) {//!!!!!!!!!!!:)))) Vector2D target; Vector2D goalKeeperTargetOffSet = Vector2D(0.11 , -0.06); QList defs; - double AZBisecOpenAngle = 0, AZBigestOpenAngle = 0, AZDangerPercent = 0, aimLessChord = 0, GKcoveredAngle = 0; - double topFaceLength; - double bottomFaceLength; - double ballheight; + double AZBisecOpenAngle = 0, AZBigestOpenAngle = 0, AZDangerPercent = 0, GKcoveredAngle = 0; double nearestDist2Ball; - int nearestDef2BallId = -1 ; int g; ////////////////////////////////////////////////////////////////////////// tempSol.clear(); - if (goalKeeperAgent != nullptr) { - Segment2D goalLine(wm->field->ourGoal() + Vector2D(0, -0.8) , wm->field->ourGoal() + Vector2D(0, 0.8)); - Segment2D downFieldLine(Vector2D(-wm->field->_FIELD_WIDTH / 2, -wm->field->_FIELD_HEIGHT / 2), Vector2D(-wm->field->_FIELD_WIDTH / 2, wm->field->_FIELD_HEIGHT / 2)); - //////////////////////////////// Appending circles on defense agents ///////////////////////////////////////// - for (g = 0; g < defenseAgents.count() ; g++) { - defs.append(Circle2D(defenseAgents[g]->pos(), Robot::robot_radius_new)); - if (defenseAgents[g]->pos().dist(wm->ball->pos) < nearestDist2Ball) { - nearestDef2BallId = defenseAgents[g]->id(); - nearestDist2Ball = defenseAgents[g]->pos().dist(wm->ball->pos); - ROS_INFO_STREAM("E: @@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@"); - } - } - ///////////////////////////// Empty region between defense agents ////////////////////////// - CKnowledge::getEmptyAngle(*wm->field , _ballPos, wm->field->ourGoalL(), wm->field->ourGoalR(), defs, AZDangerPercent, AZBisecOpenAngle, AZBigestOpenAngle, false); - ////////// Bisector of triangle that is made up of with this points : [ballPossition , topGoal , bottom Goal] ////////////////////////// - Segment2D AZBisecOpenSeg(_ballPos ,_ballPos + (Vector2D(cos(_PI * AZBisecOpenAngle / 180) , sin(_PI * AZBisecOpenAngle / 180)).norm() * 12) ); - ////////// Top and bottom line of triangle that is made up of with this points : [ballPossition , topGoal , bottom Goal] ////////////////////////// - Segment2D AZTopOfOpenSeg(_ballPos , _ballPos + Vector2D(cos(_PI * (AZBisecOpenAngle + (AZBigestOpenAngle / 2)) / 180), sin(_PI * (AZBisecOpenAngle + (AZBigestOpenAngle / 2)) / 180)).norm() * 12); - Segment2D AZBottomOfOpenSeg(_ballPos , _ballPos + Vector2D(cos(_PI * (AZBisecOpenAngle - (AZBigestOpenAngle / 2)) / 180), sin(_PI * (AZBisecOpenAngle - (AZBigestOpenAngle / 2)) / 180)).norm() * 12); - ///////////// Intersection of the top and bottom line of the triangle with goalLine //////////////////////////////////////////// - Vector2D openAngGoalIntersectionTop(AZTopOfOpenSeg.intersection(goalLine)); - Vector2D openAngGoalIntersectionBottom(AZBottomOfOpenSeg.intersection(goalLine)); - /////////////////// Real length of top and bottom line of the triangle for talles ////////////////////////////////////////////////////////////// - drawer->draw(AZTopOfOpenSeg , "black"); - drawer->draw(AZBisecOpenSeg , "red"); - drawer->draw(AZBottomOfOpenSeg , "cyan"); - PDEBUG("AZOPEN" , AZBisecOpenAngle , D_AHZ); - PDEBUG("AZbig" , AZBigestOpenAngle , D_AHZ); - topFaceLength = ballPos.dist(openAngGoalIntersectionTop); - bottomFaceLength = ballPos.dist(openAngGoalIntersectionBottom); - //////////////////////////Height of the triangle /////////////////////////////////////////////////////// - ballheight = ballPos.dist(downFieldLine.nearestPoint(ballPos)); - goal2Ball.assign(wm->field->ourGoal(), _ballPos); - thr = 0.1; - target = getGKPositionAccordingToTheDefense(findNeededDefense(), openAngGoalIntersectionTop , _ballPos , openAngGoalIntersectionBottom); - if(AZBigestOpenAngle > GKcoveredAngle + AHZDegThreshOld || AZBisecOpenSeg.dist(goalKeeperAgent->pos()) < 0.1){ - if (goalKeeperAgent->pos().dist(AZBisecOpenSeg.nearestPoint(goalKeeperAgent->pos())) > 0.2 + thr) { - target = AZBisecOpenSeg.nearestPoint(goalKeeperAgent->pos()); - thr = 0; - AHZDegThreshOld = 0; - lastTargetForStrictFollow = target; - } - else { - AHZDegThreshOld = 1; - lastTargetForStrictFollow = target; - } - GKcoveredAngle = AZBigestOpenAngle; + Segment2D goalLine(wm->field->ourGoal() + Vector2D(0, -0.8) , wm->field->ourGoal() + Vector2D(0, 0.8)); + Segment2D downFieldLine(Vector2D(-wm->field->_FIELD_WIDTH / 2, -wm->field->_FIELD_HEIGHT / 2), Vector2D(-wm->field->_FIELD_WIDTH / 2, wm->field->_FIELD_HEIGHT / 2)); + ///////////////////////////// Empty region between defense agents ////////////////////////// + CKnowledge::getEmptyAngle(*wm->field , _ballPos, wm->field->ourGoalL(), wm->field->ourGoalR(), defs, AZDangerPercent, AZBisecOpenAngle, AZBigestOpenAngle, false); + ////////// Bisector of triangle that is made up of with this points : [ballPossition , topGoal , bottom Goal] ////////////////////////// + Segment2D AZBisecOpenSeg(_ballPos ,_ballPos + (Vector2D(cos(_PI * AZBisecOpenAngle / 180) , sin(_PI * AZBisecOpenAngle / 180)).norm() * 12) ); + ////////// Top and bottom line of triangle that is made up of with this points : [ballPossition , topGoal , bottom Goal] ////////////////////////// + Segment2D AZTopOfOpenSeg(_ballPos , _ballPos + Vector2D(cos(_PI * (AZBisecOpenAngle + (AZBigestOpenAngle / 2)) / 180), sin(_PI * (AZBisecOpenAngle + (AZBigestOpenAngle / 2)) / 180)).norm() * 12); + Segment2D AZBottomOfOpenSeg(_ballPos , _ballPos + Vector2D(cos(_PI * (AZBisecOpenAngle - (AZBigestOpenAngle / 2)) / 180), sin(_PI * (AZBisecOpenAngle - (AZBigestOpenAngle / 2)) / 180)).norm() * 12); + ///////////// Intersection of the top and bottom line of the triangle with goalLine //////////////////////////////////////////// + Vector2D openAngGoalIntersectionTop(AZTopOfOpenSeg.intersection(goalLine)); + Vector2D openAngGoalIntersectionBottom(AZBottomOfOpenSeg.intersection(goalLine)); + /////////////////// Real length of top and bottom line of the triangle for talles ////////////////////////////////////////////////////////////// + drawer->draw(AZTopOfOpenSeg , "black"); + drawer->draw(AZBisecOpenSeg , "red"); + drawer->draw(AZBottomOfOpenSeg , "cyan"); + PDEBUG("AZOPEN" , AZBisecOpenAngle , D_AHZ); + PDEBUG("AZbig" , AZBigestOpenAngle , D_AHZ); + //////////////////////////Height of the triangle /////////////////////////////////////////////////////// + goal2Ball.assign(wm->field->ourGoal(), _ballPos); + thr = 0.1; + target = getGKPositionAccordingToTheDefense(findNeededDefense(), openAngGoalIntersectionTop , _ballPos , openAngGoalIntersectionBottom); + if(AZBigestOpenAngle > GKcoveredAngle + AHZDegThreshOld || AZBisecOpenSeg.dist(goalKeeperAgent->pos()) < 0.1){ + if (goalKeeperAgent->pos().dist(AZBisecOpenSeg.nearestPoint(goalKeeperAgent->pos())) > 0.2 + thr) { + target = AZBisecOpenSeg.nearestPoint(goalKeeperAgent->pos()); + thr = 0; + AHZDegThreshOld = 0; + lastTargetForStrictFollow = target; } - else{ + else { AHZDegThreshOld = 1; - target = lastTargetForStrictFollow; - } - if(!wm->field->isInField(target)){ - //target = know->getPointInDirection(wm->field->ourGoal() , wm->ball->pos , 0.35);//LhumChangeSomething - target = Line2D(wm->field->ourGoalL() + Vector2D(0.2 , 0) , wm->field->ourGoalR() + Vector2D(0.2 , 0)).intersection(Line2D(wm->field->ourGoal() , _ballPos)); lastTargetForStrictFollow = target; } } + else{ + AHZDegThreshOld = 1; + target = lastTargetForStrictFollow; + } + if(!wm->field->isInField(target)){ + //target = know->getPointInDirection(wm->field->ourGoal() , wm->ball->pos , 0.35);//LhumChangeSomething + target = Line2D(wm->field->ourGoalL() + Vector2D(0.2 , 0) , wm->field->ourGoalR() + Vector2D(0.2 , 0)).intersection(Line2D(wm->field->ourGoal() , _ballPos)); + lastTargetForStrictFollow = target; + } return target; } diff --git a/parsian_ai/src/parsian_ai/plays/dynamicattack.cpp b/parsian_ai/src/parsian_ai/plays/dynamicattack.cpp index 367b37de..ceb662b4 100644 --- a/parsian_ai/src/parsian_ai/plays/dynamicattack.cpp +++ b/parsian_ai/src/parsian_ai/plays/dynamicattack.cpp @@ -1,5 +1,6 @@ #include #include +#include const int CDynamicAttack::REGION_NUM = 7; @@ -50,7 +51,7 @@ CDynamicAttack::~CDynamicAttack() { } -void CDynamicAttack::init(QList& _agents) { +void CDynamicAttack::init(QList &_agents) { agents.clear(); agents.append(_agents); initMaster(); @@ -63,9 +64,9 @@ void CDynamicAttack::reset() { void CDynamicAttack::execute_x() { ROS_INFO_STREAM("Dynamic Attack : " << agents.size()); - ROS_INFO_STREAM("ali:state "<< static_cast(attackState)); + ROS_INFO_STREAM("ali:state " << static_cast(attackState)); globalExecute(agents.size()); - for (auto &p : semiDynamicPosition) { + for (auto &p : amirSemiDynamicPosition) { drawer->draw(Circle2D(p, .1), QColor(Qt::red), true); } } @@ -275,7 +276,8 @@ void CDynamicAttack::assignTasks() { } if (currentPlan.agentSize > 0) { ROS_INFO_STREAM("kian: too if positioning : currentPlan.agentSize:" << currentPlan.agentSize); - positioning(semiDynamicPosition); + positioning(passPoints);//semiDynamicPosition); + //positioning(amirSemiDynamicPosition); } } @@ -284,17 +286,20 @@ void CDynamicAttack::assignTasks() { * @param agentSize number of positioning Agents */ void CDynamicAttack::dynamicPlanner(int agentSize) { - for (size_t i = 0; i < 8; i++) { + for (int i{}; i < matchingIDs.size(); i++) { matchingIDs[i] = -1; } for (int i = 0; i < REGION_NUM; i++) drawer->draw(regions[i].rectangle); + updateAttackState(); makePlan(agentSize); + // if (agentSize > 0 && (lastAgentCount != agentSize || isPlayMakeChanged())) { chooseBestPositons(); + assignId(); chooseReceiverAndBestPosForPass(); @@ -411,7 +416,7 @@ void CDynamicAttack::playMake() { } } -void CDynamicAttack::positioning(QList _points) { +void CDynamicAttack::positioning(QList& passPoints){//(QList _points) { // hamid pos ROS_INFO_STREAM("hamid inside positioning2"); bool check = false; @@ -419,18 +424,28 @@ void CDynamicAttack::positioning(QList _points) { if (matchingIDs[i] >= 0) { roleAgents[i]->setAgent(agents.at(i)); roleAgents[i]->setAvoidPenaltyArea(true); - if (i < _points.size()) { + if (i < passPoints.size()) { switch (currentPlan.positionAgents[i].skill) { case PositionSkill::Ready: // Ready For Pass ROS_INFO_STREAM("kian: too switch set : skill: ready"); - roleAgents[i]->setTarget(_points.at(i)); - roleAgents[i]->setReceiveRadius(.5); - roleAgents[i]->setSelectedPositionSkill(PositionSkill::Ready);// Receive Skill - + if(recievePoint.ID != -1 && roleAgents[i]->getAgent()->id() == recievePoint.ID) { + ROS_INFO_STREAM("amirty id : " << recievePoint.ID); + roleAgents[i]->setTarget(recievePoint.point); + roleAgents[i]->setReceiveRadius(.5); + roleAgents[i]->setSelectedPositionSkill(PositionSkill::Ready);// Receive Skill + break; + } + else + { + roleAgents[i]->setTarget(passPoints[i].point); + roleAgents[i]->setReceiveRadius(.5); + roleAgents[i]->setSelectedPositionSkill(PositionSkill::Ready);// Receive Skill + break; + } break; case PositionSkill::OneTouch: // OneTouch Reflects ROS_INFO_STREAM("kian: too switch set : skill: onetouch"); - roleAgents[i]->setWaitPos(_points.at(i)); + roleAgents[i]->setWaitPos(passPoints[i].point); roleAgents[i]->setReceiveRadius( std::max(0.5, 2 - roleAgents[i]->getAgent()->pos() .dist(roleAgents[i]->getTarget()))); @@ -445,7 +460,7 @@ void CDynamicAttack::positioning(QList _points) { roleAgents[i]->setReceiveRadius( std::max(0.5, 2 - roleAgents[i]->getAgent()->pos() .dist(roleAgents[i]->getTarget()))); - roleAgents[i]->setTarget(_points.at(i)); + roleAgents[i]->setTarget(passPoints[i].point); roleAgents[i]->setTargetDir(wm->ball->pos - roleAgents[i]->getAgent()->pos()); roleAgents[i]->setSelectedPositionSkill(PositionSkill::Move); @@ -486,6 +501,76 @@ bool CDynamicAttack::keepOrNot() { return true; } + + +bool CDynamicAttack::isClear(Vector2D _pos1, Vector2D _pos2, + double _radius, double treshold, QString str, Vector2D point) +{ + Vector2D sol1, sol2, sol3; + Line2D _path(_pos1, _pos2); + Polygon2D _poly; + Circle2D(_pos2, _radius + treshold). + intersection(_path.perpendicular(_pos2), &sol1, &sol2); + + _poly.addVertex(sol1); + sol3 = sol1; + _poly.addVertex(sol2); + Circle2D(_pos1, Robot::robot_radius_new + treshold). + intersection(_path.perpendicular(_pos1), &sol1, &sol2); + + _poly.addVertex(sol2); + _poly.addVertex(sol1); + _poly.addVertex(sol3); + + if(str == "positionInOurWay") + { + if (_poly.contains(point)) { + return false; + } + } + else if(str == "isPassPathOpen") + { + if (treshold < 0.15) { + ROS_INFO_STREAM("amirf draw1"); + drawer->draw(_poly, QColor(180, 180, 180)); + } + + + for (int i{}; i < wm->opp.activeAgentsCount(); i++) { + if (_poly.contains(wm->opp.active(i)->pos)) { + return false; + } + } + } + else if (str == "positionClear") + { + if (treshold < 0.15) { + ROS_INFO_STREAM("amirf draw2"); + drawer->draw(_poly, QColor(200, 200, 200)); + } + + + for (int i{}; i < wm->opp.activeAgentsCount(); i++) { + + if (wm->opp.active(i)->id != wm->opp.data->goalieID) { + if (_poly.contains(wm->opp.active(i)->pos)) { + //ROS_INFO_STREAM("amirq im here!"); + return false; + } + } + } + for (int i{}; i < passPoints.size(); i++) { + if (_poly.contains(passPoints[i].point)) //it shouldnt be ouractive. it should be position. + { + //ROS_INFO_STREAM("amirq im here!"); + return false; //TODO: make it right + } + } + } + return true; +} + + bool CDynamicAttack::isPathClear(Vector2D _pos1, Vector2D _pos2, double _radius, double treshold) { Vector2D sol1, sol2, sol3; @@ -510,8 +595,8 @@ bool CDynamicAttack::isPathClear(Vector2D _pos1, Vector2D _pos2, } } - for (int i = 0; i < wm->opp.activeAgentsCount(); i++) { - if (_poly.contains(wm->opp.active(i)->pos)) { + for (int i = 0; i < wm->our.activeAgentsCount(); i++) { + if (_poly.contains(wm->our.active(i)->pos)) { return false; } } @@ -569,6 +654,114 @@ int CDynamicAttack::appropriatePassSpeed() { } +/*bool CDynamicAttack::isPositionInOurWay(Vector2D _pos1, Vector2D _pos2, + double _radius, double treshold, Vector2D point) { + Vector2D sol1, sol2, sol3; + Line2D _path(_pos1, _pos2); + Polygon2D _poly; + Circle2D(_pos2, _radius + treshold). + intersection(_path.perpendicular(_pos2), &sol1, &sol2); + + _poly.addVertex(sol1); + sol3 = sol1; + _poly.addVertex(sol2); + Circle2D(_pos1, Robot::robot_radius_new + treshold). + intersection(_path.perpendicular(_pos1), &sol1, &sol2); + + _poly.addVertex(sol2); + _poly.addVertex(sol1); + _poly.addVertex(sol3); + drawer->draw(_poly); + + + //for (int i = 0; i < wm->our.activeAgentsCount(); i++) { + if (_poly.contains(point)) { + return false; + } + //} + + + return true; +}*/ + +/*bool CDynamicAttack::isPassPathOpen(Vector2D _pos1, Vector2D _pos2, + double _radius, double treshold) { + + //ROS_INFO_STREAM("amirk im here"); + Vector2D sol1, sol2, sol3; + Line2D _path(_pos1, _pos2); + Polygon2D _poly; + Circle2D(_pos2, _radius + treshold). + intersection(_path.perpendicular(_pos2), &sol1, &sol2); + + _poly.addVertex(sol1); + sol3 = sol1; + _poly.addVertex(sol2); + Circle2D(_pos1, Robot::robot_radius_new + treshold). + intersection(_path.perpendicular(_pos1), &sol1, &sol2); + + _poly.addVertex(sol2); + _poly.addVertex(sol1); + _poly.addVertex(sol3); + + + if (treshold < 0.15) { + ROS_INFO_STREAM("amirf draw1"); + drawer->draw(_poly, QColor(180, 180, 180)); + } + + + for (int i{}; i < wm->opp.activeAgentsCount(); i++) { + if (_poly.contains(wm->opp.active(i)->pos)) { + return false; + } + } + return true; +}*/ + + +/*bool CDynamicAttack::isPositionClear(Vector2D _pos1, Vector2D _pos2, double _radius, double treshold) { + Vector2D sol1, sol2, sol3; + Line2D _path(_pos1, _pos2); + Polygon2D _poly; + Circle2D(_pos2, _radius + treshold). + intersection(_path.perpendicular(_pos2), &sol1, &sol2); + + _poly.addVertex(sol1); + sol3 = sol1; + _poly.addVertex(sol2); + Circle2D(_pos1, Robot::robot_radius_new + treshold). + intersection(_path.perpendicular(_pos1), &sol1, &sol2); + + _poly.addVertex(sol2); + _poly.addVertex(sol1); + _poly.addVertex(sol3); + if (treshold < 0.15) { + ROS_INFO_STREAM("amirf draw2"); + drawer->draw(_poly, QColor(200, 200, 200)); + } + + + for (int i{}; i < wm->opp.activeAgentsCount(); i++) { + + if (wm->opp.active(i)->id != wm->opp.data->goalieID) { + if (_poly.contains(wm->opp.active(i)->pos)) { + //ROS_INFO_STREAM("amirq im here!"); + return false; + } + } + } + for (int i{}; i < passPoints.size(); i++) { + if (_poly.contains(passPoints[i].point)) //it shouldnt be ouractive. it should be position. + { + //ROS_INFO_STREAM("amirq im here!"); + return false; //TODO: make it right + } + } + return true; +}*/ + + int CDynamicAttack::appropriateChipSpeed() { double tempDistance = 0; @@ -958,6 +1151,160 @@ void CDynamicAttack::createRegions() { } } + + +void CDynamicAttack::regionByBall(int ballR) +{ + regionPriority.clear(); + switch (ballR) { + case 0: + //regionPriority << 4 << 2 << 1 << 5 << 3 << 6; + { + regions[4].pointPriority = 6; + regions[2].pointPriority = 5; + regions[1].pointPriority = 4; + regions[5].pointPriority = 6; // for onetouch chance. otherwise this is 3 + regions[3].pointPriority = 2; + regions[6].pointPriority = 1; + regions[0].pointPriority = 1; + break; + } + case 1: + //regionPriority << 4 << 3 << 0 << 5 << 2 << 6; + { + regions[4].pointPriority = 6; + regions[3].pointPriority = 5; + regions[0].pointPriority = 4; + regions[5].pointPriority = 6; // for onetouch chance. otherwise this is 3 + regions[2].pointPriority = 2; + regions[6].pointPriority = 1; + regions[1].pointPriority = 1; + break; + } + case 2: + //regionPriority << 4 << 0 << 5 << 3 << 1 << 6; + { + regions[4].pointPriority = 6; + regions[0].pointPriority = 4; + regions[5].pointPriority = 5; + regions[3].pointPriority = 3; + regions[1].pointPriority = 2; + regions[6].pointPriority = 1; + regions[2].pointPriority = 1; + break; + } + case 3: + //regionPriority << 4 << 1 << 5 << 2 << 0 << 6; + { + regions[4].pointPriority = 6; + regions[1].pointPriority = 4; + regions[5].pointPriority = 5; + regions[2].pointPriority = 3; + regions[0].pointPriority = 2; + regions[6].pointPriority = 1; + regions[3].pointPriority = 1; + break; + } + case 4: + //regionPriority << 0 << 1 << 2 << 3 << 5 << 6; + { + regions[0].pointPriority = 6; + regions[1].pointPriority = 5; + regions[2].pointPriority = 4; + regions[3].pointPriority = 3; + regions[5].pointPriority = 2; + regions[6].pointPriority = 1; + regions[4].pointPriority = 1; + break; + } + case 5: + //regionPriority << 0 << 1 << 2 << 3 << 6 << 4; + { + regions[0].pointPriority = 6; + regions[1].pointPriority = 5; + regions[2].pointPriority = 4; + regions[3].pointPriority = 3; + regions[6].pointPriority = 2; + regions[4].pointPriority = 1; + regions[5].pointPriority = 1; + break; + } + case 6: + //regionPriority << 0 << 1 << 2 << 3 << 4 << 5; + { + regions[0].pointPriority = 6; + regions[1].pointPriority = 5; + regions[2].pointPriority = 4; + regions[3].pointPriority = 3; + regions[4].pointPriority = 2; + regions[5].pointPriority = 6; //if it is in corner it is good. + regions[6].pointPriority = 1; + break; + } + default: + //regionPriority << 0 << 1 << 2 << 3 << 4 << 5; + { + regions[0].pointPriority = 6; + regions[1].pointPriority = 5; + regions[2].pointPriority = 4; + regions[3].pointPriority = 3; + regions[4].pointPriority = 2; + regions[5].pointPriority = 1; + regions[6].pointPriority = 0; + break; + } + } +} + +void CDynamicAttack::oppInregion() { + for (size_t i{}; i < REGION_NUM; i++) { + regions[i].oppInside = 0; + regions[i].oppInNeighbor = 0; + } + for (size_t i{}; i < wm->opp.activeAgentsCount(); i++) { + Vector2D position{wm->opp.active(i)->pos}; + if (regions[0].rectangle.contains(position)) { + regions[0].oppInside++; + regions[2].oppInNeighbor++; + regions[4].oppInNeighbor++; + regions[5].oppInNeighbor++; + } else if (regions[1].rectangle.contains(position)) { + regions[1].oppInside++; + regions[3].oppInNeighbor++; + regions[4].oppInNeighbor++; + regions[5].oppInNeighbor++; + } else if (regions[2].rectangle.contains(position)) { + regions[2].oppInside++; + regions[0].oppInNeighbor++; + regions[4].oppInNeighbor++; + //regions[5].oppInNeighbor++; + } else if (regions[3].rectangle.contains(position)) { + regions[3].oppInside++; + regions[1].oppInNeighbor++; + regions[4].oppInNeighbor++; + //regions[5].oppInNeighbor++; + } else if (regions[4].rectangle.contains(position)) { + regions[4].oppInside++; + regions[0].oppInNeighbor++; + regions[1].oppInNeighbor++; + regions[5].oppInNeighbor++; + } else if (regions[5].rectangle.contains(position)) { + regions[5].oppInside++; + regions[0].oppInNeighbor++; + regions[1].oppInNeighbor++; + regions[2].oppInNeighbor++; + regions[3].oppInNeighbor++; + regions[4].oppInNeighbor++; + } else if (regions[6].rectangle.contains(position)) { + regions[6].oppInside++; + regions[2].oppInNeighbor++; + regions[3].oppInNeighbor++; + //regions[5].oppInNeighbor++; + } + + } +} + void CDynamicAttack::chooseBestPositons() { clearRobotsRegionsWeights(); @@ -969,37 +1316,32 @@ void CDynamicAttack::chooseBestPositons() { QList avoidRects; avoidRects.append(wm->field->oppPenaltyRect()); + QList sortRegions; + ROS_INFO_STREAM("amirn regions done once"); if (attackState == DynamicAttackState::PlaymakeControl) { int ballR = -1; for (int i{0}; i < REGION_NUM; i++) if (regions[i].rectangle.contains(wm->ball->pos + wm->ball->vel))ballR = regions[i].id; - regionPriority.clear(); - switch (ballR) { - case 0: - regionPriority << 4 << 2 << 1 << 5 << 3 << 6; - break; - case 1: - regionPriority << 4 << 3 << 0 << 5 << 2 << 6; - break; - case 2: - regionPriority << 4 << 0 << 5 << 3 << 1 << 6; - break; - case 3: - regionPriority << 4 << 1 << 5 << 2 << 0 << 6; - break; - case 4: - regionPriority << 0 << 1 << 2 << 3 << 5 << 6; - break; - case 5: - regionPriority << 0 << 1 << 2 << 3 << 6 << 4; - break; - case 6: - regionPriority << 0 << 1 << 2 << 3 << 4 << 5; - break; - default: - regionPriority << 0 << 1 << 2 << 3 << 4 << 5; - break; + + regionByBall(ballR); + oppInregion(); + + for (size_t i{}; i < REGION_NUM; i++) { + if(regions[i].rectangle.contains(recievePoint.point)) + regions[i].pointPriority = -1000; + else + regions[i].pointPriority -= (regions[i].oppInside + regions[i].oppInNeighbor); + sortRegions.append(regions[i]); + } + + + std::sort(sortRegions.begin(), sortRegions.end()); // baraks sort shode!!! + + for (int i{REGION_NUM - 1}; i >= 0; i--) { + regionPriority.append(sortRegions[i].id); + + //ROS_INFO_STREAM("amiry ids : " << sortRegions[i].id << " and point is : " << sortRegions[i].pointPriority << " and i is : " << i); } } } @@ -1016,7 +1358,7 @@ void CDynamicAttack::assignId() { if (regionPriority.isEmpty() || playmake == nullptr) return; QList robotIDs; MWBM matcher; - for (int i = 0; i < 8; i++) { matchingIDs[i] = -1; } + for (int i = 0; i < matchingIDs.size(); i++) { matchingIDs[i] = -1; } for (const auto &a : agents) { if (a->id() != playmake->id()) robotIDs.append(a->id()); } @@ -1025,21 +1367,326 @@ void CDynamicAttack::assignId() { for (int i{0}; i < robotIDs.count(); i++) { for (int j{0}; j < robotIDs.count(); j++) { auto agentPos = agents.at(i)->pos(); - matcher.setWeight(i, j, agentPos.dist(regions[regionPriority[i]].rectangle.center())); + matcher.setWeight(i, j, agentPos.dist(regions[regionPriority[i]].rectangle.center())); //TODO : not center } } + + // matcher.findMaxMinMatching(); + matcher.findMatching(); + semiDynamicPosition.clear(); - for (int v = 0; v < robotIDs.count(); v++) { - // todo : find best pos in region from searchRegions.points - semiDynamicPosition.append(regions[regionPriority[matcher.getMatch(v)]].rectangle.center()); - matchingIDs[v] = matcher.getMatch(v); + + amirSemiDynamicPosition.clear(); + //for (int v = 0; v < robotIDs.count(); v++) { + // todo : find best pos in region from searchRegions.points + //semiDynamicPosition.append(regions[regionPriority[matcher.getMatch(v)]].rectangle.center()); + // matchingIDs[robot_id] = matcher.getMatch(v); + //} + + + matchingIDs.clear(); + for (int v{}; v < robotIDs.count(); v++) + matchingIDs.append(matcher.getMatch(v)); + + passPositions(robotIDs, matcher); + + + + //finalPassReciever(); + + +} + + +void CDynamicAttack::passPositions(const QList& robotIDs, MWBM& matcher) +{ + + bestPos(robotIDs, matcher); + isChipOrPass(passPoints); + findOneTouch(passPoints); + isInPosition(passPoints); + toPassOrNotToPass(passPoints); + passPriority(passPoints); + stayPassReciever(passPoints); + + showPasser(passPoints, matcher); + + + passDecision(); +} + + +void CDynamicAttack::passDecision() { + //choose pass reciever + if(recievePoint.ID == -1) { + ROS_INFO_STREAM("amirty : point is about to change"); + for (int i{}; i < passPoints.size(); i++) { + if (passPoints[i].stay == 1) { + recievePoint.ID = passPoints[i].ID; + recievePoint.point.x = passPoints[i].point.x; + recievePoint.point.y = passPoints[i].point.y; + break; + } + } + } + + Segment2D ballseg{wm->ball->seg()}; + Line2D pointGoal{recievePoint.point, wm->field->oppGoal()}; + /*if (ballseg.intersection(pointGoal).dist(recievePoint.point) > 0.5){ + recievePoint.ID = -1; + ROS_INFO_STREAM("amirty : pass calnceled!"); + }*/ +} + + + +RecievePoint::RecievePoint() +{ + ID = -1; + point.x = 0; + point.y = 0; +} + + +void CDynamicAttack::stayPassReciever(QList &passPoints) { + + /*for (int i{}; i < passPoints.size(); i++) { + passPoints[i].chance = passPoints[i].stay; } + std::sort(passPoints.begin(), passPoints.end()); + + for (int i{passPoints.size() - 1}; i >= 0; i--) { + amirSemiDynamicPosition.append(passPoints[i].point); + }*/ + + } -Vector2D CDynamicAttack::getBestPosToShootToGoal(Vector2D from, double ®ionWidth, bool oppGaol) { +void CDynamicAttack::showPasser(QList& passPoints, MWBM& matcher) { + ROS_INFO_STREAM("amirf 4"); + for (int i{}; i < passPoints.size(); i++) { + /*if(passPoints[i].amIReciever) + { + ROS_INFO_STREAM("amirf 5"); + if(passPoints[i].chipOrPass && passPoints[i].oneTouch) + drawer->draw(passPoints[i].point,QColor(170,100,160),0.3); + else if(passPoints[i].chipOrPass && !passPoints[i].oneTouch) + drawer->draw(passPoints[i].point,QColor(40,140,220),0.3); + else if(!passPoints[i].chipOrPass && passPoints[i].oneTouch) + drawer->draw(passPoints[i].point,QColor(230,30,30),0.3); + else if(!passPoints[i].chipOrPass && !passPoints[i].oneTouch) + drawer->draw(passPoints[i].point,QColor(240,100,10),0.3); + + }*/ + if (passPoints[i].stay) { + drawer->draw(passPoints[i].point, QColor(255, 255, 0), 0.4); + //ROS_INFO_STREAM("amirtr id : " << agents[matcher.getMatch(i)]->id()); + passPoints[i].ID = agents[matcher.getMatch(i)]->id(); + } + } +} + + +void CDynamicAttack::passPriority(QList& passPoints) { + ROS_INFO_STREAM("amirf 3"); + for (int i{}; i < passPoints.size(); i++) { + if (passPoints[i].amIReciever && passPoints[i].oneTouch) { + passPoints[i].stay = 1; + //playmake.setchip + //playmake.onetouch + } else if (passPoints[i].amIReciever) { + //passPoints[i].stay = 1; + //playmake.setchip + } + } +} + + +passPoint::passPoint() { + point.x = 0; + point.y = 0; + amIReciever = false; + stay = false; + inPostion = false; + oneTouch = false; + chipOrPass = false; + finalPassReciever = false; + int ID = -1; + +} + +passPoint::passPoint(vector2D p) { + point.x = p.x; + point.y = p.y; + amIReciever = false; + stay = false; + inPostion = false; + oneTouch = false; + chipOrPass = false; + finalPassReciever = false; + ID = -1; +} + + +void CDynamicAttack::isChipOrPass(QList& passPoints) { + double dist_treshold{4.5}; + for (int i{}; i < passPoints.size(); i++) { + passPoint p{passPoints[i]}; + if (playmake->pos().dist(p.point) < dist_treshold) { + if (isPathClear(playmake->pos(), p.point, Robot::robot_radius_new, 0.2)) + p.chipOrPass = false; + else { + p.chipOrPass = true; + } + } else { + if (isPathClear(playmake->pos(), p.point, Robot::robot_radius_new, 0.2)) + p.chipOrPass = false; + } + } + +} + + + + +void CDynamicAttack::isInPosition(QList& passPoints) { + for (int i{}; i < passPoints.size(); i++) { + for (int j{}; j < wm->our.activeAgentsCount(); j++) { + //if(passPoints[i].inPostion)//edit + //continue; + if (wm->our.active(j)->pos.dist(passPoints[i].point) < 0.1) { + passPoints[i].inPostion = true; + break; + } else + passPoints[i].inPostion = false; + } + } +} + +void CDynamicAttack::toPassOrNotToPass(QList& passPoints) { + //ROS_INFO_STREAM("amirf 2"); + double chip_dist_treshold{4.5}; + for (int i{}; i < passPoints.size(); i++) { + if (passPoints[i].inPostion) { + //ROS_INFO_STREAM("amirf they are in position"); + if (playmake->pos().dist(wm->ball->pos) < 0.5) { + //ROS_INFO_STREAM("amirf in if"); + if (isClear(wm->ball->pos, passPoints[i].point, + Robot::robot_radius_new + (wm->ball->pos.dist(passPoints[i].point) / 15), 0.1,"passPathOpen") && + isClear(passPoints[i].point, wm->field->oppGoal(), + wm->field->oppGoalL().y - wm->field->oppGoal().y, 0.1,"positionClear")) { + passPoints[i].amIReciever = true; + + } + else if(playmake->pos().dist(passPoints[i].point) < chip_dist_treshold && isClear(passPoints[i].point, wm->field->oppGoal(), + wm->field->oppGoalL().y - wm->field->oppGoal().y, 0.1,"positionClear") + && passPoints[i].chipOrPass) //this is for chip then onetouch + { + passPoints[i].amIReciever = true; + } + else { + passPoints[i].amIReciever = false; + } + } + } + } +} + +void CDynamicAttack::findOneTouch(QList& passPoints) { + for (int i{}; i < passPoints.size(); i++) { + + Segment2D posGoal(passPoints[i].point, wm->field->oppGoal()); + Segment2D playMakePos(wm->ball->pos, passPoints[i].point); + //double angle{angleOfTwoSegment(posGoal, playMakePos)}; + double angle{std::fabs(Vector2D::angleBetween(wm->field->oppGoal() - passPoints[i].point, + wm->ball->pos - passPoints[i].point).degree())}; + //ROS_INFO_STREAM("amirp angle : " << angle); + //ROS_INFO_STREAM("amirp point.x : " << passPoints[i].point.x); + //ROS_INFO_STREAM("amirp point.y : " << passPoints[i].point.y); + if (angle < conf.MaxOnetouchAngle && + (passPoints[i].region == 4 || passPoints[i].region == 5 || passPoints[i].region == 2 || + passPoints[i].region == 3 || passPoints[i].region == 1 + || passPoints[i].region == 0)) { + //ROS_INFO_STREAM("amirw here 1"); + if (isClear(passPoints[i].point, wm->field->oppGoal(), + wm->field->oppGoalL().y - wm->field->oppGoal().y, 0.05,"positionClear")) { + passPoints[i].oneTouch = true; + //ROS_INFO_STREAM("amirw point for onetouch.x = " << passPoints[i].point.x); + //ROS_INFO_STREAM("amirw point for onetouch.y = " << passPoints[i].point.y); + //drawer->draw(passPoints[i].point, QColor(255,255,255), 0.3); + } + } + } + +} + + +void CDynamicAttack::bestPos(const QList &robotIDs, MWBM &matcher) { + + //stayPassReciever(); + + for (int v{}; v < robotIDs.count(); v++) { + double chance{0}; + Vector2D tmp_point; + + matchingIDs[v] = matcher.getMatch(v); //commented this. if it was wrong make it right? it made it bad. so i umcommented this. + + //if(agents[matchingIDs[v]]->id() == recievePoint.ID) + //continue; + + for (size_t i{}; i < regions[0].points.size(); i++) { + auto tmp_chance = calcRegionProperties(v, i); + if (tmp_chance >= chance) { + chance = tmp_chance; + tmp_point = regions[regionPriority[matchingIDs[v]]].points[i]; + } + } + regions[regionPriority[matchingIDs[v]]].chance = chance; +// regions[regionPriority[matchingIDs[v]]].theirNearestRobot = max_dist; + semiDynamicPosition.append(tmp_point); + } + + if (robotIDs.size() > semiDynamicPosition.size()) { + for (int i{semiDynamicPosition.size()}; i < robotIDs.size(); i++) { + semiDynamicPosition.append(regions[regionPriority[matchingIDs[i]]].rectangle.center()); + } + } + + passPoints.clear(); + + for (int i{}; i < semiDynamicPosition.size(); i++) { + passPoint tmp; + tmp.point.x = semiDynamicPosition[i].x; + tmp.point.y = semiDynamicPosition[i].y; + for (int j{}; j < REGION_NUM; j++) { + if (regions[j].rectangle.contains(tmp.point)) { + tmp.region = j; + } + } + passPoints.append(tmp); + } +} + +void CDynamicAttack::finalPassReciever() { + for (int i{}; i < passPoints.size(); i++) { + if (passPoints[i].amIReciever && passPoints[i].oneTouch) { + passPoints[i].stay = 1; + break; + } + } +} + + +CRobot *CDynamicAttack::findOppGoalKeaper() { + return (wm->opp.active(wm->opp.data->goalieID)); +} + + +Vector2D CDynamicAttack::getBestPosToShootToGoal(Vector2D from, + double ®ionWidth, bool oppGaol) { Rect2D playingField(wm->field->ourCornerL(), wm->field->oppCornerR()); if (!playingField.contains(from)) { regionWidth = 0.0; @@ -1075,9 +1722,10 @@ Vector2D CDynamicAttack::getBestPosToShootToGoal(Vector2D from, double ®ionWi EndPos = y; if (WasLastPosClear) { RegionCenterTemp = Segment2D(goalL, goalR).intersection(Line2D(from, (from + Vector2D( - Vector2D(goal.x, BeginPos) - from).rotate(Vector2D::angleBetween(Vector2D(goal.x, BeginPos) - from, - Vector2D(goal.x, EndPos) - - from).degree() / 2)))); + Vector2D(goal.x, BeginPos) - from).rotate( + Vector2D::angleBetween(Vector2D(goal.x, BeginPos) - from, + Vector2D(goal.x, EndPos) - + from).degree() / 2)))); if (RegionCenterTemp.x == Vector2D::ERROR_VALUE || RegionCenterTemp.y == Vector2D::ERROR_VALUE) RegionCenterTemp = Vector2D(goal.x, (EndPos - BeginPos) / 2); MaxRegionTemp = (EndPos - BeginPos + 0.001) * from.dist(Line2D(goalL, goalR).projection(from)) / @@ -1126,7 +1774,8 @@ Vector2D CDynamicAttack::getBestPosToShootToGoal(Vector2D from, double ®ionWi return shootPos; } -bool CDynamicAttack::isPathClear(Vector2D point, Vector2D from, double rad, bool considerRelaxedIDs) { +bool CDynamicAttack::isPathClear(Vector2D point, Vector2D from, double rad, + bool considerRelaxedIDs) { Vector2D posIntersect1(Vector2D::ERROR_VALUE, Vector2D::ERROR_VALUE); Vector2D posIntersect2(Vector2D::ERROR_VALUE, Vector2D::ERROR_VALUE); Segment2D l(from, point); @@ -1236,7 +1885,8 @@ double CDynamicAttack::calcOneTouchAngleFactor(Vector2D robotPos) { // return 0.0; // - auto effectiveHigh = ((highIntersect - (Vector2D(wm->field->oppGoal()))).length() > fieldWidth / 2) ? fieldWidth / 2 + auto effectiveHigh = ((highIntersect - (Vector2D(wm->field->oppGoal()))).length() > fieldWidth / 2) ? + fieldWidth / 2 : highIntersect.dist( wm->field->oppGoal()); auto effectiveLow = ((highIntersect - (Vector2D(wm->field->oppGoal()))).length() > (fieldWidth / 2)) ? -( @@ -1246,7 +1896,8 @@ double CDynamicAttack::calcOneTouchAngleFactor(Vector2D robotPos) { auto extendedWidth = penaltyWidth + 2 * penaltyOffset; auto resultRatio = ((effectiveHigh > extendedWidth / 2) ? extendedWidth / 2 : effectiveHigh - - (effectiveLow < -extendedWidth / 2) + - (effectiveLow < + -extendedWidth / 2) ? -extendedWidth : effectiveLow) / extendedWidth; return resultRatio; @@ -1369,7 +2020,8 @@ void CDynamicAttack::validateSegment(Segment2D &seg) { sol1.invalidate(); sol2.invalidate(); Vector2D t; - if (t = Segment2D(Vector2D(0, wm->field->_FIELD_WIDTH / 2), Vector2D(0, -wm->field->_FIELD_WIDTH / 2)).intersection( + if (t = Segment2D(Vector2D(0, wm->field->_FIELD_WIDTH / 2), + Vector2D(0, -wm->field->_FIELD_WIDTH / 2)).intersection( Segment2D(seg.a(), mid)), t.isValid()) { seg.assign(t, seg.b()); mid = (seg.a() + seg.b()) / 2; @@ -1419,7 +2071,8 @@ double CDynamicAttack::calcNotInWayFactor(Vector2D passSenderPos, Vector2D point } -bool CDynamicAttack::isPathClearFromOpp(Vector2D _pos1, Vector2D _pos2, double _radius, double treshold) { +bool CDynamicAttack::isPathClearFromOpp(Vector2D _pos1, Vector2D _pos2, + double _radius, double treshold) { Vector2D sol1, sol2, sol3; Line2D _path(_pos1, _pos2); Polygon2D _poly; @@ -1449,31 +2102,30 @@ void CDynamicAttack::updateAttackState() { switch (attackState) { case DynamicAttackState::PlaymakeControl: if (!directShot) - attackState = DynamicAttackState ::PlaymakePass; + attackState = DynamicAttackState::PlaymakePass; break; case DynamicAttackState::PlaymakePass: if (passDone()) { attackState = DynamicAttackState::PositioningControl; if (isGoodForOneTouch()) { - positionSkill = PositionSkill::OneTouch; - oneTouchFailState = 0; - oneTouchDoneState = 0; - } - else + positionSkill = PositionSkill::OneTouch; + oneTouchFailState = 0; + oneTouchDoneState = 0; + } else positionSkill = PositionSkill::Ready; - } - else if (directShot || passFailed()) - attackState = DynamicAttackState ::PlaymakeControl; + } else if (directShot || passFailed()) + attackState = DynamicAttackState::PlaymakeControl; break; case DynamicAttackState::PositioningControl: if (positionTaskDone()) - attackState = DynamicAttackState ::PlaymakeControl; + attackState = DynamicAttackState::PlaymakeControl; break; default: break; } } + bool CDynamicAttack::passDone() { double ballDistanceToTarget = currentPlan.passPos.dist(wm->ball->pos); double ballDistanceToPlaymake = playmake->pos().dist(wm->ball->pos); @@ -1494,14 +2146,15 @@ bool CDynamicAttack::isGoodForOneTouch() { bool CDynamicAttack::positionTaskDone() { if (positionSkill == PositionSkill::Ready) - if ((wm->ball->vel.length() < .02) || (wm->ball->vel.length() < .1 && wm->ball->pos.dist(currentPlan.passPos) > 2)) + if ((wm->ball->vel.length() < .02) || + (wm->ball->vel.length() < .1 && wm->ball->pos.dist(currentPlan.passPos) > 2)) return true; if (positionSkill == PositionSkill::OneTouch) { double dist = wm->ball->pos.dist(currentPlan.passPos); if (dist > 2) - oneTouchFailState ++; + oneTouchFailState++; if (dist < 1.5) - oneTouchDoneState ++; + oneTouchDoneState++; if (oneTouchDoneState > 30 && dist > 2) return true; @@ -1513,6 +2166,7 @@ bool CDynamicAttack::positionTaskDone() { } + bool CDynamicAttack::passFailed() { double ballDistanceToTarget = currentPlan.passPos.dist(wm->ball->pos); double ballDistanceToPlaymake = playmake->pos().dist(wm->ball->pos); @@ -1520,3 +2174,85 @@ bool CDynamicAttack::passFailed() { return true; return false; } + +double CDynamicAttack::calcRegionProperties(int robot_id, int region_index) { + // finding nearest opp + double max_dist{-1}; + + double tmp_angle{}; + double tmp_chance{}; + + double nearest_opp_robot_dist{100000}; + double angle_weight{15}/*conf.PositionOpenAngle}*/, dist_weight{ + 0.1/*conf.PositionOppNearest*/}; // TODO: show in controling in game + + double dist_weight1{0.05}; + double angle_weight1{45}; + double treshold1{0.3}; + double treshold2{0.5}; + + double PassMarkChance{-5};//conf.OppPassMarkChance}; + //double chance1{-10}; + for (size_t j{}; j < wm->opp.activeAgentsCount(); j++) { // cal nearest_opp_robot + //auto tmp_d = regions[regionPriority[matchingIDs[v]]].points[i].dist(wm->opp.active(j)->pos); + auto tmp = regions[regionPriority[matchingIDs[robot_id]]].points[region_index].dist(wm->opp.active(j)->pos); + if (tmp < nearest_opp_robot_dist) { + nearest_opp_robot_dist = tmp; + } + } + + //if (nearest_opp_robot_dist > max_dist) { + //max_dist = nearest_opp_robot_dist; + //tmp_point = regions[regionPriority[matchingIDs[v]]].points[region_index]; + //} + + + CRobot *oppGoalKeaper = findOppGoalKeaper(); + double angle_max{}; + if (oppGoalKeaper != nullptr) { + Segment2D posRobot_oppGoalK{regions[regionPriority[matchingIDs[robot_id]]].points[region_index], oppGoalKeaper->pos}; + Segment2D posRobot_oppGoalR{regions[regionPriority[matchingIDs[robot_id]]].points[region_index], + wm->field->oppGoalR()}; + Segment2D posRobot_oppGoalL{regions[regionPriority[matchingIDs[robot_id]]].points[region_index], + wm->field->oppGoalL()}; + const double &&angle_R{angleOfTwoSegment(posRobot_oppGoalK, posRobot_oppGoalR)}; + const double &&angle_L{angleOfTwoSegment(posRobot_oppGoalK, posRobot_oppGoalL)}; + if (angle_R > angle_L) + angle_max = angle_R; + else + angle_max = angle_L; + } + + + auto tmp_a = angle_max; + double tresholdDist{3}; + if (nearest_opp_robot_dist < tresholdDist) + tmp_chance = nearest_opp_robot_dist * dist_weight + tmp_a * angle_weight; + else { + tmp_chance = nearest_opp_robot_dist * dist_weight1 + tmp_a * angle_weight1; + + } + + if (!isClear(/*playmake->pos()*/wm->ball->pos, wm->field->oppGoal(), + wm->field->oppGoalL().y - wm->field->oppGoal().y, treshold2,"positionInOurWay", + regions[regionPriority[matchingIDs[robot_id]]].points[region_index])) { + tmp_chance = 0; + //continue; + } + if (!isClear(regions[regionPriority[matchingIDs[robot_id]]].points[region_index], wm->field->oppGoal(), + wm->field->oppGoalL().y - wm->field->oppGoal().y, + 0.2, "positionClear")) { + tmp_chance = 0; + //continue; + } + double passPathWeight{15}; + if (!isClear(/*playmake->pos()*/ wm->ball->pos, regions[regionPriority[matchingIDs[robot_id]]].points[region_index], + (Robot::robot_radius_new + playmake->pos().dist( + regions[regionPriority[matchingIDs[robot_id]]].points[region_index]) / + passPathWeight), + treshold1,"passPathOpen")) { + tmp_chance = 0; + } + return tmp_chance; +} + diff --git a/parsian_ai/src/parsian_ai/plays/ourpenalty.cpp b/parsian_ai/src/parsian_ai/plays/ourpenalty.cpp index 2d6440ed..1ad8aa58 100644 --- a/parsian_ai/src/parsian_ai/plays/ourpenalty.cpp +++ b/parsian_ai/src/parsian_ai/plays/ourpenalty.cpp @@ -3,7 +3,6 @@ COurPenalty::COurPenalty() : CMasterPlay() { initMaster(); - playmakeRole = new CRolePlayMake(nullptr); PMgotopoint = new GotopointavoidAction(); PMkick = new KickAction(); } @@ -19,64 +18,39 @@ void COurPenalty::init(QList& _agents) { agents = _agents; } +void COurPenalty::execute_x() { + ROS_INFO_STREAM("penalty: execute_x"); + if (playMakeAgent == nullptr || (playMakeAgent->id() == -1)) { + ROS_INFO_STREAM("penalty: playmakeagent is null"); + return; + } + if(penaltyState == PenaltyState::Positioning) + { + executeNormalPositioning(); + playmakePositioning(); + } + if(penaltyState == PenaltyState::Kicking) + playmakeKick(); +} + void COurPenalty::setPlaymake(Agent* _playmakeAgent) { - ROS_INFO_STREAM("penalty: before set playmake to: "); if(_playmakeAgent != nullptr) { playMakeAgent = _playmakeAgent; - playmakeRole->assign(playMakeAgent); } - ROS_INFO_STREAM("penalty: set playmake to: " << playMakeAgent->id()); -} - -void COurPenalty::executeShootoutPositioning() -{ - } void COurPenalty::executeNormalPositioning() { - ROS_INFO_STREAM("penalty: normal positioning"); if(agents.isEmpty()) return; generatePositions(); assignSkills(); } -void COurPenalty::execute_x() { - ROS_INFO_STREAM("penalty: execute_x"); - if (playMakeAgent == nullptr || (playMakeAgent->id() == -1)) { - ROS_INFO_STREAM("penalty: playmakeagent is null"); - return; - } - isPenaltyShootOut = gameState->ourPenaltyShootout(); - //playmakeRole->execute(); - if(penaltyState == PenaltyState::Positioning) - { - if(isPenaltyShootOut) - { - ROS_INFO_STREAM("penalty: shootout"); - executeShootoutPositioning(); - } - else - { - ROS_INFO_STREAM("penalty: norma"); - executeNormalPositioning(); - } - - playmakePositioning(); - } - if(penaltyState == PenaltyState::Kicking) - { - ROS_INFO_STREAM("penalty: in kicking state"); - playmakeKick(); - } -} - void COurPenalty::generatePositions() { - ROS_INFO_STREAM("penalty: generate positions"); positions.clear(); double penaltyPositioningOffset = 0.4; double penaltyRuleOffset = 0.4; @@ -127,7 +101,6 @@ Vector2D COurPenalty::getEmptyTarget(Vector2D _position, double _radius) void COurPenalty::assignSkills() { - ROS_INFO_STREAM("penalty: assign skills"); moveSkills.clear(); for(int i{0}; i CDynamicPlayOff::CDynamicPlayOff() { - dummyPositions[1] = Vector2D{3.2, 0.7}; - dummyPositions[2] = Vector2D{3.2, -0.7}; - dummyPositions[3] = Vector2D{3, 1.5}; - dummyPositions[4] = Vector2D{3, -2.5}; - dummyPositions[5] = Vector2D{3, 2.5}; - dummyPositions[6] = Vector2D{3, 3.5}; - dummyPositions[7] = Vector2D{3, -3.5}; - - reset(); + + +dynamicSelect = DynamicSelect::Chip; +lastselect =DynamicSelect :: Chip; + reset(); } CDynamicPlayOff::~CDynamicPlayOff(){ } void CDynamicPlayOff::reset() { + dynamicSelect=DynamicSelect::Chip; state = DynamicState::None; dynamicStartTime = 0; } +void CDynamicPlayOff ::Setposition(){ + + + + theirdist=100; + for(int i=0;i< wm->opp.activeAgentsCount();i++) + { + if(wm->opp.active(i)->pos.dist(wm->ball->pos)opp.active(i)->pos.dist(wm->ball->pos); + theirpos=wm->opp.active(i)->pos; + + } + } + ROS_INFO_STREAM("maral: blocker"<opp.active(n)->id); + + + Line2D line1= Line2D(wm->ball->pos,wm->field->oppGoal()); + Segment2D segment1=Segment2D(wm->ball->pos,wm->field->oppGoal()); + drawer->draw(segment1,QColor("blue")); + + Line2D line2= line1.perpendicular(theirpos); + + Vector2D vect=line1.intersection(line2); + Segment2D segment2=Segment2D(vect,theirpos); + drawer->draw(vect,QColor("red"), 20); + drawer->draw(segment2,QColor("black")); + Vector2D VECTOR=(wm->field->oppGoal()-vect).norm()*conf.khafandist; + dummyPositions[0]=VECTOR+vect; + drawer->draw(dummyPositions[0],QColor("red"), 20); + + + if(wm->field->isInOppPenaltyArea(dummyPositions[0])) + dynamicSelect = DynamicSelect::Chip; + + + if(agents.size()>4){ + dummyPositions[1] = Vector2D{3.2, 0.7}; + dummyPositions[2] = Vector2D{3, -0.7}; + dummyPositions[3] = Vector2D{3, 1.5}; + dummyPositions[4] = Vector2D{-3, -2.5}; + dummyPositions[5] = Vector2D{-3, 2.5}; + dummyPositions[6] = Vector2D{-3, 3.5}; + dummyPositions[7] = Vector2D{-3, -3.5}; + + } + else + { + dummyPositions[1] = Vector2D{3, 0.7}; + dummyPositions[2] = Vector2D{3, -0.7}; + dummyPositions[3] = Vector2D{2.5, 1.5}; + dummyPositions[4] = Vector2D{2.5, -2.5}; + dummyPositions[5] = Vector2D{2.5, 2.5}; + dummyPositions[6] = Vector2D{2.5, 3.5}; + dummyPositions[7] = Vector2D{2.5, -3.5}; + + } +} void CDynamicPlayOff::execute() { + EvalPlayKhafan(); + ROS_INFO_STREAM("EVAL "<60 && conf.UseKhafan){ + if(lastselect!=DynamicSelect ::Khafan) + state = DynamicState::Ready; + + dynamicSelect = DynamicSelect::Khafan; + lastselect=DynamicSelect ::Khafan; + } + else if(eval<30){ + if(lastselect!= DynamicSelect::Chip) + state=DynamicState::Ready; + dynamicSelect = DynamicSelect::Chip; + lastselect=DynamicSelect::Chip; + } switch (dynamicSelect) { case DynamicSelect::NoSelect: @@ -37,12 +108,14 @@ void CDynamicPlayOff::execute() { checkEndChipToGoal(); break; case DynamicSelect::Khafan: + Setposition(); dynamicPlayKhafan(); - if (state == DynamicState::Ready) matchAgent(); + if (state == DynamicState::Ready) matchAgent(); checkEndKhafan(); break; } + for (int i = 0; i < agents.size(); i++) { roleAgents[i]->execute(); } @@ -68,13 +141,13 @@ void CDynamicPlayOff::dynamicPlayChipToGoal(bool isChip) { roleAgents[i] -> setTimeBased(false); roleAgents[i] -> setTarget(dummyPositions[i + 1]); roleAgents[i] -> setLookAt(-wm->field->oppGoal()); - roleAgents[i] -> setEventDist(0.3); + roleAgents[i] -> setEventDist(0.05); roleAgents[i] -> setSlow(false); roleAgents[i] -> setSelectedSkill(RoleSkill::GotopointAvoid); } } break; - case DynamicState::None:break; + case DynamicState::None: state=DynamicState ::Ready; case DynamicState::Pass:break; case DynamicState::Shot: roleAgents[0]->setDoPass(true); @@ -84,6 +157,7 @@ void CDynamicPlayOff::dynamicPlayChipToGoal(bool isChip) { } void CDynamicPlayOff::dynamicPlayKhafan() { + switch (state) { case DynamicState::Ready: roleAgents[0] -> setAvoidCenterCircle(false); @@ -114,7 +188,7 @@ void CDynamicPlayOff::dynamicPlayKhafan() { roleAgents[1] -> setAvoidCenterCircle(false); roleAgents[1] -> setAvoidPenaltyArea(true); roleAgents[1] -> setChip(false); - roleAgents[1] -> setKickSpeed(1023); // Vartypes This + roleAgents[1] -> setKickSpeed(conf.MediumSpeedPass); // Vartypes This roleAgents[1] -> setTarget(wm->field->oppGoal()); roleAgents[1] -> setDoPass(true); roleAgents[1] -> setIntercept(false); @@ -129,10 +203,58 @@ void CDynamicPlayOff::dynamicPlayKhafan() { roleAgents[0] -> setSlow(false); roleAgents[0] -> setSelectedSkill(RoleSkill::GotopointAvoid); break; - case DynamicState::None:break; + case DynamicState::None: state=DynamicState ::Ready; } } +int CDynamicPlayOff:: EvalPlayKhafan(){ + Setposition(); + for(int i=0;iopp.activeAgentsCount();i++) + { + ROS_INFO_STREAM("EVAL:DIST"<opp.active(i)->pos))); + if(dummyPositions[0].dist(wm->opp.active(i)->pos)<0.3 && theirpos!=wm->opp.active(i)->pos){ + dynamicSelect=DynamicSelect ::Chip; + eval=0; + return eval; + } + } + if(agents.size()<2) + eval=0; + if(wm->opp.activeAgentsCount()==0) + eval=100; + +theirdist=100; + +sum=0; +for(int i=0;i< wm->opp.activeAgentsCount();i++) +{ + if(wm->opp.active(i)->pos.dist(wm->ball->pos)opp.active(i)->pos.dist(wm->ball->pos); + theirpos= wm->opp.active(i)->pos; + + } +} +if(theirpos.dist(wm->ball->pos)>0.9) + eval=0; + + POLYGON.addVertex(theirpos); + POLYGON.addVertex(wm->field->oppGoalL()); + POLYGON.addVertex(wm->field->oppGoalR()); + ROS_INFO_STREAM("DRAW"); + drawer->draw(POLYGON,QColor("red")); + +for (int i=0;i< wm->opp.activeAgentsCount();i++){ + + if(POLYGON.contains(wm->opp.active(i)->pos)) + + sum++; +} + +eval=100-sum*10; + + +return eval; +} void CDynamicPlayOff::checkEndKhafan() { @@ -140,16 +262,16 @@ void CDynamicPlayOff::checkEndKhafan() { switch (state) { case DynamicState ::Ready: if (roleAgents[1] -> getAgent() -> pos().dist(roleAgents[1] -> getTarget()) - < roleAgents[1] -> getEventDist()) { + < roleAgents[1] -> getEventDist() && roleAgents[1]->getAgent()->vel().length()<0.2) { state = DynamicState::Pass; } break; case DynamicState::None: - break; + state=DynamicState ::Ready; case DynamicState::Pass: DBUG(QString("ENDKHAFAN : %1").arg(ros::Time::now().sec - dynamicStartTime), D_MAHI); - if (wm->ball->pos.dist(wm->field->oppGoal()) - 0.5 < roleAgents[1]->getAgent()->pos().dist(wm->field->oppGoal())) { + if (wm->ball->pos.dist(wm->field->oppGoal())-0.4 < roleAgents[1]->getAgent()->pos().dist(wm->field->oppGoal())) { state = DynamicState::Shot; } if (!Circle2D(roleAgents[0]->getAgent()->pos(), 0.5).contains(wm->ball->pos) && dynamicStartTime == 0) { @@ -196,7 +318,7 @@ void CDynamicPlayOff::checkEndChipToGoal() { } break; - case DynamicState::None:break; + case DynamicState::None: state=DynamicState ::Ready; case DynamicState::Pass:break; } @@ -207,13 +329,5 @@ void CDynamicPlayOff::init(const QList &_agents) { agents.clear(); agents.append(_agents); - dummyPositions[0] = wm->ball->pos + (wm->field->oppGoal() - wm->ball->pos).norm() * 3; - dummyPositions[0].y += 0.3; - if (_agents.size() < 2) { - dynamicSelect = DynamicSelect::Chip; - } else { - dynamicSelect = DynamicSelect::Khafan; - } - state = DynamicState::Ready; } diff --git a/parsian_ai/src/parsian_ai/plays/playoff/staticplayoff.cpp b/parsian_ai/src/parsian_ai/plays/playoff/staticplayoff.cpp index 6402610f..7dcb7fdf 100644 --- a/parsian_ai/src/parsian_ai/plays/playoff/staticplayoff.cpp +++ b/parsian_ai/src/parsian_ai/plays/playoff/staticplayoff.cpp @@ -84,8 +84,9 @@ bool CStaticPlayOff::isTimeOver() { if (!Circle2D(lastBallPos, 0.06).contains(wm->ball->pos)) { setTimer = false; ROS_INFO_STREAM("MAHI: Time That Left: " << ros::Time::now().sec - startTime); - if(ros::Time::now().sec - startTime >= 3*masterPlan->execution.passCount) { // 3 Second TODO: ADD TO CFG + if(ros::Time::now().sec - startTime >= conf.StaticTimeOver*masterPlan->execution.passCount) { // 3 Second TODO: ADD TO CFG setTimer = true; + ROS_INFO_STREAM("kianoff TIME OVER"); return true; } } @@ -102,11 +103,19 @@ bool CStaticPlayOff::isBallDirChanged() { const int& passer = masterPlan->execution.passer.at(0).id; const int& receiver = masterPlan->execution.receiver.at(0).id; const int receiverID = masterPlan->matchedID.value(receiver); + if(roleAgents[receiverID]->getWaitPos().x == 5000) + return false; if (wm->ball->pos.dist(lastBallPos) > 0.5 && !roleAgents[passer]->getChip()) { - Circle2D c(roleAgents[receiverID]->getWaitPos(), 1); // TODO : CHECK radius + Circle2D c(roleAgents[receiverID]->getWaitPos(), conf.StaticBallDirChanged); // TODO : CHECK radius drawer->draw(wm->ball->seg(), QColor(Qt::blue)); drawer->draw(c, QColor(Qt::red)); - return !c.intersection(wm->ball->seg()); + bool res = !c.intersection(wm->ball->seg()); + if(res) + { + ROS_INFO_STREAM("kianoff BALL DIR CHANGED"); + return true; + } + return false; } return false; @@ -289,6 +298,7 @@ void CStaticPlayOff::fillRoleProperties() { roleAgents[i]->setFirstMove(positionAgent[i].stateNumber == 0); roleAgents[i]->setAgent(getAgent(i)); + roleAgents[i]->setAgentID(getAgent(i)->id()); //// Handle OneTouch Faster if (positionAgent[i].stateNumber + 1 < positionAgent[i].positionArg.size()) { @@ -645,7 +655,17 @@ void CStaticPlayOff::init(const QList& _agents) { } bool CStaticPlayOff::firstKickFailed() { - return (lastBallPos.dist(wm->ball->pos) > 0.25 && wm->ball->vel.length() < 0.1); + bool ballNearRobot = false; + for(int i{}; i < wm->our.activeAgentsCount(); i++) + if(wm->our.active(i)->pos.dist(wm->ball->pos) < conf.StaticFirstKickFailed) + ballNearRobot = true; + bool res = (lastBallPos.dist(wm->ball->pos) > conf.StaticFirstKickFailed && wm->ball->vel.length() < 0.1 && !ballNearRobot); + if(res) + { + ROS_INFO_STREAM("kianoff First Kick Failed"); + return true; + } + return false; } /*! diff --git a/parsian_ai/src/parsian_ai/plays/substitution.cpp b/parsian_ai/src/parsian_ai/plays/substitution.cpp new file mode 100644 index 00000000..9eab5330 --- /dev/null +++ b/parsian_ai/src/parsian_ai/plays/substitution.cpp @@ -0,0 +1,110 @@ +#include "parsian_ai/plays/substitution.h" + +CSubstitution::CSubstitution() { + for(int i{}; i < _MAX_NUM_PLAYERS; i++) + gpa.push_back(new GotopointavoidAction); +} + +CSubstitution::~CSubstitution() { + for(int i{}; i < _MAX_NUM_PLAYERS; i++) + delete gpa[i]; +} + +void CSubstitution::reset(){ + positioningPlan.reset(); + executedCycles = 0; + for(int i{}; i < _MAX_NUM_PLAYERS; i++) + delete gpa[i]; + for(int i{}; i < _MAX_NUM_PLAYERS; i++) + gpa.push_back(new GotopointavoidAction); +} + +void CSubstitution::init(QList& _agents) { + agents = _agents; + initMaster(); +} + + +void CSubstitution::execute_x(){ + std::string faultIDs{"The faulted robots are >> "}; + for(auto agent: agents) + faultIDs += std::to_string(agent->id()) + " - "; + ROS_INFO_STREAM("kian : " << faultIDs); + + QList positions = generatepositions(agents.size()); + for(int i{}; i < agents.size(); i++) + { + gpa[agents[i]->id()]->setSlowmode(true); + if(positions.size() >= i+1) + gpa[agents[i]->id()]->setTargetpos(positions[i]); + else + gpa[agents[i]->id()]->setTargetpos(Vector2D{0, wm->field->_FIELD_HEIGHT/2}); + gpa[agents[i]->id()]->setAvoidpenaltyarea(true); + + gpa[agents[i]->id()]->setBallobstacleradius(0.50); + agents[i]->action = gpa[agents[i]->id()]; + + } + +} + +QList CSubstitution::generatepositions(int count) +{ + float radius = 0.5; + float dist = 0.3; + float thresholdFromtop = 0.2; + float angle_step = (dist/radius) * 180/3.14;//degree + QList positions; + + //up side region + Vector2D centerup{0, wm->field->_FIELD_HEIGHT/2}; + Circle2D locationup{centerup, radius}; + drawer->draw(locationup, QColor(Qt::black)); + + for(float ang{180}; ang <= 360; ang += angle_step) + { + Vector2D *sol1, *sol2; + sol1 = new Vector2D{}; + sol2 = new Vector2D{}; + Line2D direction(centerup, ang); + locationup.intersection(direction, sol1, sol2); + if(sol1->y < wm->field->_FIELD_HEIGHT/2 - thresholdFromtop) + positions.push_back(*sol1); + else if(sol2->y < wm->field->_FIELD_HEIGHT/2 - thresholdFromtop) + positions.push_back(*sol2); + if(positions.size() == count) + return positions; + } + + //down side region + Vector2D centerdown{0, -wm->field->_FIELD_HEIGHT/2}; + Circle2D locationdown{centerdown, radius}; + drawer->draw(locationdown, QColor(Qt::black)); + + for(float ang{0}; ang <= 180; ang += angle_step) + { + Vector2D *sol1, *sol2; + sol1 = new Vector2D{}; + sol2 = new Vector2D{}; + Line2D direction(centerdown, ang); + locationdown.intersection(direction, sol1, sol2); + if(sol1->y > -wm->field->_FIELD_HEIGHT/2 + thresholdFromtop) + positions.push_back(*sol1); + else if(sol2->y > -wm->field->_FIELD_HEIGHT/2 + thresholdFromtop) + positions.push_back(*sol2); + if(positions.size() == count) + return positions; + } + + if(positions.size() < count) + for(int i{}; i < count - positions.size(); i++) + positions.push_back(positions[i%positions.size()]); + return positions; + +// for(auto pos: positions) +// drawer->draw(pos, QColor(Qt::white), 0.07); +} + + + + diff --git a/parsian_ai/src/parsian_ai/util/knowledge.cpp b/parsian_ai/src/parsian_ai/util/knowledge.cpp index afe1f8fa..9f35f345 100644 --- a/parsian_ai/src/parsian_ai/util/knowledge.cpp +++ b/parsian_ai/src/parsian_ai/util/knowledge.cpp @@ -361,6 +361,7 @@ double Knowledge::getEmptyAngle(Vector2D p, Vector2D p1, Vector2D p2, double Knowledge::getEmptyAngle(Vector2D p, Vector2D p1, Vector2D p2, QList obs, double& percent, double &mostOpenAngle, double& biggestAngle) { QList r; + emptyAngleStruct q1{}, q2{}; q1.begin = false; q1.angle = (p1 - p).th().degree(); @@ -472,6 +473,7 @@ double Knowledge::getEmptyAngle(Vector2D p, Vector2D p1, Vector2D p2, QList ourRelaxedIDs, QList oppRelaxedIDs, double wOpenness, bool _draw) { QList c; + c.append(Circle2D(Vector2D(0 , 0) , 0.2)); for (int i = 0; i < wm->our.activeAgentsCount(); i++) { if (!ourRelaxedIDs.contains(wm->our.active(i)->id)) { c.append(Circle2D(wm->our.active(i)->pos, wm->our.active(i)->robotRadius())); @@ -883,6 +885,35 @@ FastestToBall Knowledge::findFastestToBall(QList ourList, QList oppLis return f; } + +double Knowledge::timeNeeded(Agent *_agentT,const Vector2D& posT, double vMax) { + double acc; + double dec = 3.5; + Vector2D tAgentVel = _agentT->vel(); + Vector2D tAgentDir = _agentT->dir(); + double dist = 0; + double tAgentVelTanjent = tAgentVel.length() * cos(Vector2D::angleBetween(posT - _agentT->pos() , _agentT->vel().norm()).radian()); + + double vXvirtual = (posT - _agentT->pos()).x; + double vYvirtual = (posT - _agentT->pos()).y; + double veltanV = (vXvirtual) * cos(tAgentDir.th().radian()) + (vYvirtual) * sin(tAgentDir.th().radian()); + double velnormV = -1 * (vXvirtual) * sin(tAgentDir.th().radian()) + (vYvirtual) * cos(tAgentDir.th().radian()); + double accCoef; + + accCoef = atan(std::fabs(veltanV) / std::fabs(velnormV)) / _PI * 2; + acc = accCoef * 4.5 + (1 - accCoef) * 3.5; + double tDec = vMax / dec; + double tAcc = (vMax - tAgentVelTanjent) / acc; + dist = posT.dist(_agentT->pos()); + double dB = tDec * vMax / 2 + tAcc * (vMax + tAgentVelTanjent) / 2; + + if (dist > dB) { + return tAcc + tDec + (dist - dB) / vMax; + } else { + return ((1 / dec) + (1 / acc)) * sqrt(dist * (2 * dec * acc / (acc + dec)) + (tAgentVelTanjent * tAgentVelTanjent / (2 * acc))) - (tAgentVelTanjent) / acc; + } +} + NewFastestToBall Knowledge::newFastestToBall(double timeStep, QList ourList, QList oppList) { //// ////Code By Sepehr @@ -984,7 +1015,7 @@ NewFastestToBall Knowledge::newFastestToBall(double timeStep, QList ourList return result; } -double Knowledge::chipGoalPropability(bool isOurChip, Vector2D _goaliePos) { +double Knowledge::chipGoalPropability(bool isOurChip) { double GoalDistanceToBall; double GoalieDistanseToBall; double GoalDistanceToGoalie; @@ -995,7 +1026,7 @@ double Knowledge::chipGoalPropability(bool isOurChip, Vector2D _goaliePos) { } else { goal = wm->field->ourGoal(); - goaliePos = _goaliePos; + goaliePos = wm->our[wm->our.data->goalieID]->pos; } GoalDistanceToBall = wm->ball->pos.dist(goal) / 1.9; @@ -1004,11 +1035,8 @@ double Knowledge::chipGoalPropability(bool isOurChip, Vector2D _goaliePos) { if (goaliePos.dist(wm->ball->pos) < 0.35 || wm->ball->pos.dist(goal) < 1) { return 0; - } else if (((GoalDistanceToBall - GoalieDistanseToBall) / GoalDistanceToGoalie) * 2 > 0) { - return ((GoalDistanceToBall - GoalieDistanseToBall) / GoalDistanceToGoalie) * 2; - } else { - return 0; } + return max(((GoalDistanceToBall - GoalieDistanseToBall) / GoalDistanceToGoalie) * 2 , 0); } int Knowledge::getNearestOppToPoint(Vector2D point) { @@ -1031,33 +1059,4 @@ int Knowledge::nearestOppToBall() { return getNearestOppToPoint(wm->ball->pos); } -double Knowledge::chipGoalPropability(bool isOurChip) { - double GoalDistanceToBall; - double GoalieDistanseToBall; - double GoalDistanceToGoalie; - Vector2D goal, goaliePos; - if (isOurChip) { - goal = wm->field->oppGoal(); - goaliePos = wm->opp[wm->opp.data->goalieID]->pos; - - } else { - goal = wm->field->ourGoal(); - goaliePos = wm->our[wm->our.data->goalieID]->pos; - } - - GoalDistanceToBall = wm->ball->pos.dist(goal) / 1.9; - GoalieDistanseToBall = wm->ball->pos.dist(goaliePos); - GoalDistanceToGoalie = goaliePos.dist(goal); - if (goaliePos.dist(wm->ball->pos) < 0.35 - || wm->ball->pos.dist(goal) < 1) { - return 0; - } else if (((GoalDistanceToBall - GoalieDistanseToBall) / GoalDistanceToGoalie) * 2 > 0) { - return ((GoalDistanceToBall - GoalieDistanseToBall) / GoalDistanceToGoalie) * 2; - } else { - return 0; - } - - -} - Knowledge * know = new Knowledge(); diff --git a/parsian_ai/test/parsian_ai/utest.cpp b/parsian_ai/test/parsian_ai/utest.cpp index 33db56af..08e43d9d 100644 --- a/parsian_ai/test/parsian_ai/utest.cpp +++ b/parsian_ai/test/parsian_ai/utest.cpp @@ -17,18 +17,21 @@ TEST(GameState, ForceCommand) { /// HALT c.command = parsian_msgs::ssl_refree_command::HALT; r->command = c; + r->command_number++; ai.forceUpdateReferee(r); EXPECT_EQ(gameState->getState(), States::Halt); - // STOP + /// STOP c.command = parsian_msgs::ssl_refree_command::STOP; r->command = c; + r->command_number++; ai.forceUpdateReferee(r); EXPECT_EQ(gameState->getState(), States::Stop); /// Start after Stop c.command = parsian_msgs::ssl_refree_command::FORCE_START; r->command = c; + r->command_number++; ai.forceUpdateReferee(r); EXPECT_EQ(gameState->getState(), States::Start); @@ -37,6 +40,7 @@ TEST(GameState, ForceCommand) { for (int i = parsian_msgs::ssl_refree_command::FORCE_START; i < parsian_msgs::ssl_refree_command::INDIRECT_FREE_THEM; i++) { c.command = static_cast(i); r->command = c; + r->command_number++; ai.forceUpdateReferee(r); ROS_INFO_STREAM(static_cast(gameState->getState())); start &= (gameState->getState() == States::Start); @@ -46,23 +50,27 @@ TEST(GameState, ForceCommand) { /// Stop During Start c.command = parsian_msgs::ssl_refree_command::STOP; r->command = c; + r->command_number++; ai.forceUpdateReferee(r); EXPECT_EQ(gameState->getState(), States::Stop); /// Direct Free-Kick after Stop c.command = parsian_msgs::ssl_refree_command::DIRECT_FREE_US; r->command = c; + r->command_number++; ai.forceUpdateReferee(r); EXPECT_EQ(gameState->getState(), States::OurDirectKick); /// Ball Placement Position and Command c.command = parsian_msgs::ssl_refree_command::STOP; r->command = c; + r->command_number++; ai.forceUpdateReferee(r); c.command = parsian_msgs::ssl_refree_command::BALL_PLACEMENT_US; parsian_msgs::vector2D bp_pos; bp_pos.x = 3000; bp_pos.y = 3000; r->ballPlacementPos = bp_pos; r->command = c; + r->command_number++; ai.forceUpdateReferee(r); EXPECT_EQ(gameState->getState(), States::OurBallPlacement); EXPECT_EQ(wm->ballplacementPoint().x, bp_pos.x); @@ -79,6 +87,26 @@ TEST(GameState, RefereeCommand) { } +TEST(Amir, bayat) { + wm = new WorldModel(); + auto test = new CRoleStopInfo("stop"); + auto res = test->getEmptyTarget(Vector2D(0.0,0.0), 1); + EXPECT_DOUBLE_EQ(0.0, res.x); + EXPECT_DOUBLE_EQ(0.0, res.y); + parsian_msgs::parsian_world_modelPtr _wm; + parsian_msgs::parsian_robotPtr robot; + robot->pos.x = 0; + robot->pos.y = 0; + robot->id = 0; + + _wm->opp.push_back(*robot); + wm->update(_wm); + auto res2 = test->getEmptyTarget(Vector2D(0.0,0.0), 1); + EXPECT_NE(0.0, res2.x); + EXPECT_NE(0.0, res2.y); + +} + // Run all the tests that were declared with TEST() int main(int argc, char **argv){ testing::InitGoogleTest(&argc, argv); diff --git a/parsian_msgs/msg/mouse_event.msg b/parsian_msgs/msg/mouse_event.msg new file mode 100644 index 00000000..41aee83c --- /dev/null +++ b/parsian_msgs/msg/mouse_event.msg @@ -0,0 +1,2 @@ +vector2D pos +bool isLeftClicked diff --git a/parsian_msgs/msg/parsian_playoff_client.msg b/parsian_msgs/msg/parsian_playoff_client.msg new file mode 100644 index 00000000..d392142a --- /dev/null +++ b/parsian_msgs/msg/parsian_playoff_client.msg @@ -0,0 +1,6 @@ +string last_ai_response +string master_plan +string[] desired_plans +string[] active_plans +string[] ignored_plans + diff --git a/parsian_msgs/msg/parsian_robot_substitution.msg b/parsian_msgs/msg/parsian_robot_substitution.msg new file mode 100644 index 00000000..0cc126fa --- /dev/null +++ b/parsian_msgs/msg/parsian_robot_substitution.msg @@ -0,0 +1,2 @@ +Header header +bool[] substitutional_IDs diff --git a/parsian_msgs/msg/parsian_robots_fault.msg b/parsian_msgs/msg/parsian_robots_fault.msg new file mode 100644 index 00000000..73a67b85 --- /dev/null +++ b/parsian_msgs/msg/parsian_robots_fault.msg @@ -0,0 +1 @@ +parsian_robot_fault[] robots diff --git a/parsian_msgs/scripts/meta/templates/action.cpp.mustache b/parsian_msgs/scripts/meta/templates/action.cpp.mustache index 5966ce68..19b29d07 100644 --- a/parsian_msgs/scripts/meta/templates/action.cpp.mustache +++ b/parsian_msgs/scripts/meta/templates/action.cpp.mustache @@ -16,6 +16,10 @@ void {{action_name}}::setMessage(const void* _msg) { {{#parsian_properties}} {{local}} = msg.{{local}}; {{/parsian_properties}} + {{#list_properties}} + {{local}}.clear(); + for (auto _v : msg.{{local}}) { {{local}}.push_back(_v); } + {{/list_properties}} {{#has_base}} {{base_action}}::setMessage(&msg.base); @@ -33,6 +37,10 @@ void* {{action_name}}::getMessage() { {{#parsian_properties}} _msg->{{local}} = {{local}}.toParsianMessage(); {{/parsian_properties}} +{{#list_properties}} + _msg->{{local}}.clear(); + for (auto _v : {{local}}) { _msg->{{local}}.push_back(_v); } +{{/list_properties}} return _msg; } diff --git a/parsian_msgs/srv/parsian_update_plans.srv b/parsian_msgs/srv/parsian_update_plans.srv index 27c34808..42ab2655 100644 --- a/parsian_msgs/srv/parsian_update_plans.srv +++ b/parsian_msgs/srv/parsian_update_plans.srv @@ -1,9 +1,11 @@ -string[] newPlans -uint32[] index -bool isActive -bool isMaster +uint8 ACTIVATE = 1 +uint8 DEACTIVATE = 2 +uint8 MASTER = 3 +uint8 DEMASTER = 4 +uint8 ACTIVATE_ALL = 5 +uint8 DEACTIVATE_ALL = 6 +uint8 Mode +string[] Plans --- -parsian_plan_GUI[] allPlans - diff --git a/parsian_protobuf_wrapper/CMakeLists.txt b/parsian_protobuf_wrapper/CMakeLists.txt index cf81e689..2889a096 100644 --- a/parsian_protobuf_wrapper/CMakeLists.txt +++ b/parsian_protobuf_wrapper/CMakeLists.txt @@ -373,6 +373,9 @@ install(FILES DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} ) +catkin_install_python(PROGRAMS src/ssl-refbox/script/parsian_game_controller_node.py + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) + ############# ## Testing ## ############# diff --git a/parsian_protobuf_wrapper/cfg/referee.cfg b/parsian_protobuf_wrapper/cfg/referee.cfg index 05c0092b..0da0db4f 100755 --- a/parsian_protobuf_wrapper/cfg/referee.cfg +++ b/parsian_protobuf_wrapper/cfg/referee.cfg @@ -8,5 +8,7 @@ gen = ParameterGenerator() refBox = gen.add_group("RefBox", state=False) refBox.add("refree_multicast_port", int_t, 0, "RefBox Multicast Port", 10003, 0, 65535) refBox.add("refree_multicast_ip" , str_t, 0, "RefBox Multicast IP", "224.5.23.1") +refBox.add("refree_listen_port", int_t, 0, "GameController listen Port", 10008, 0, 65535) +refBox.add("refree_listen_ip" , str_t, 0, "GameController listen IP", "224.5.23.3") -exit(gen.generate(PACKAGE, "protobuf_wrapper_config", "referee")) \ No newline at end of file +exit(gen.generate(PACKAGE, "protobuf_wrapper_config", "referee")) diff --git a/parsian_protobuf_wrapper/deps.sh b/parsian_protobuf_wrapper/deps.sh new file mode 100755 index 00000000..bea9d933 --- /dev/null +++ b/parsian_protobuf_wrapper/deps.sh @@ -0,0 +1,4 @@ +#!/bin/bash + +sudo pip2 install protobuf +sudo pip2 install rsa diff --git a/parsian_protobuf_wrapper/proto/ssl_game_controller_auto_ref.proto b/parsian_protobuf_wrapper/proto/ssl_game_controller_auto_ref.proto new file mode 100644 index 00000000..f148326a --- /dev/null +++ b/parsian_protobuf_wrapper/proto/ssl_game_controller_auto_ref.proto @@ -0,0 +1,54 @@ +syntax = "proto2"; + +import "ssl_game_controller_common.proto"; +import "ssl_game_event_2019.proto"; + +// AutoRefRegistration is the first message that a client must send to the controller to identify itself +message AutoRefRegistration { + // identifier is a unique name of the client + required string identifier = 1; + // signature can optionally be specified to enable secure communication + optional Signature signature = 2; +} + +// AutoRefToController is the wrapper message for all subsequent messages from the autoRef to the controller +message AutoRefToController { + // signature can optionally be specified to enable secure communication + optional Signature signature = 1; + // game_event is an optional event that the autoRef detected during the game + optional GameEvent game_event = 2; + // auto_ref_message is an optional message that describes the current state or situation of the game/autoRef + optional AutoRefMessage auto_ref_message = 3; +} + +// ControllerToAutoRef is the wrapper message for all messages from controller to autoRef +message ControllerToAutoRef { + oneof msg { + // a reply from the controller + ControllerReply controller_reply = 1; + } +} + +// a message from autoRef, describing the current state or situation +message AutoRefMessage { + oneof message { + // a custom message + // an empty string indicates that there is no message + string custom = 1; + // one or more bots are at invalid locations and need to move + WaitForBots wait_for_bots = 2; + } + + // the bots that is waited for + message WaitForBots { + // the bots that are waited for + repeated Violator violators = 1; + + message Violator { + // the id of the violator + required BotId bot_id = 1; + // the distance to the next valid position + required float distance = 2; + } + } +} \ No newline at end of file diff --git a/parsian_protobuf_wrapper/proto/ssl_game_controller_common.proto b/parsian_protobuf_wrapper/proto/ssl_game_controller_common.proto new file mode 100644 index 00000000..252e4699 --- /dev/null +++ b/parsian_protobuf_wrapper/proto/ssl_game_controller_common.proto @@ -0,0 +1,72 @@ +syntax = "proto2"; + +// Team is either blue or yellow +enum Team { + // team not set + UNKNOWN = 0; + // yellow team + YELLOW = 1; + // blue team + BLUE = 2; +} + +// BotId is the combination of a team and a robot id +message BotId { + // the robot id - a negative value indicates that the id is not set + optional int32 id = 1; + // the team that the robot belongs to + optional Team team = 2; +} + +// Location is a 2d-coordinate on the field in ssl-vision coordinate system. Units are in meters. +message Location { + // the x-coordinate in [m] in the ssl-vision coordinate system + required float x = 1; + // the y-coordinate in [m] in the ssl-vision coordinate system + required float y = 2; +} + +// a reply that is sent by the controller for each request from teams or autoRefs +message ControllerReply { + // status_code is an optional code that indicates the result of the last request + optional StatusCode status_code = 1; + // reason is an optional explanation of the status code + optional string reason = 2; + // next_token must be send with the next request, if secure communication is used + // the token is used to avoid replay attacks + // the token is always present in the very first message before the registration starts + // the token is not present, if secure communication is not used + optional string next_token = 3; + // verification indicates if the last request could be verified (secure communication) + optional Verification verification = 4; + + enum StatusCode { + UNKNOWN_STATUS_CODE = 0; + OK = 1; + REJECTED = 2; + } + + enum Verification { + UNKNOWN_VERIFICATION = 0; + VERIFIED = 1; + UNVERIFIED = 2; + } +} + +// Signature can be added to a request to let it be verfied by the controller +message Signature { + // the token that was received with the last controller reply + required string token = 1; + // the PKCS1v15 signature of this message + required bytes pkcs1v15 = 2; +} + +// BallSpeedMeasurement is a single measurement sample of the ball speed +message BallSpeedMeasurement { + // The UNIX timestamp [μs] when the measurement was taken. + required uint64 timestamp = 1; + // the ball speed measurement [m/s] + required float ball_speed = 2; + // the estimated initial ball speed (kick speed) [m/s] + optional float initial_ball_speed = 3; +} \ No newline at end of file diff --git a/parsian_protobuf_wrapper/proto/ssl_game_controller_team.proto b/parsian_protobuf_wrapper/proto/ssl_game_controller_team.proto new file mode 100644 index 00000000..8981c90b --- /dev/null +++ b/parsian_protobuf_wrapper/proto/ssl_game_controller_team.proto @@ -0,0 +1,62 @@ +syntax = "proto2"; + +import "ssl_game_controller_common.proto"; + +// a registration that must be send by teams and autoRefs to the controller as the very first message +message TeamRegistration { + // the exact team name as published by the game-controller + required string team_name = 1; + // signature can optionally be specified to enable secure communication + optional Signature signature = 2; +} + +// wrapper for all messages from a team's computer to the controller +message TeamToController { + // signature can optionally be specified to enable secure communication + optional Signature signature = 1; + + oneof msg { + // request a new desired keeper id + // this is only allowed during STOP and will be rejected otherwise + int32 desired_keeper = 2; + // response to an advantage choice request + AdvantageResponse advantage_response = 3; + // request to substitute a robot at the next possibility + bool substitute_bot = 4; + } + + enum AdvantageResponse { + option allow_alias = true; + // no choice -> will default to STOP + UNDECIDED = 0; + // stop the game and handle the foul immediately + STOP = 0; + // continue the game until the next stop of the game, then handle the foul + CONTINUE = 1; + } +} + +// wrapper for all messages from controller to a team's computer +message ControllerToTeam { + oneof msg { + // a reply from the controller + ControllerReply controller_reply = 1; + // the team is offered an advantage choice + AdvantageChoice advantage_choice = 2; + } +} + +// information about the advantage choice that is offered to a team +message AdvantageChoice { + // the type of foul that occurred + required Foul foul = 1; + + enum Foul { + // default value when not set + UNKNOWN = 0; + // an opponent bot has crashed with one of your bots + COLLISION = 1; + // an opponent bot has pushed one of your bots + PUSHING = 2; + } +} \ No newline at end of file diff --git a/parsian_protobuf_wrapper/proto/ssl_game_event.proto b/parsian_protobuf_wrapper/proto/ssl_game_event.proto new file mode 100644 index 00000000..246e4215 --- /dev/null +++ b/parsian_protobuf_wrapper/proto/ssl_game_event.proto @@ -0,0 +1,100 @@ +syntax = "proto2"; + +// a game event that caused a referee command +message Game_Event { + + enum GameEventType { + // not set + UNKNOWN = 0; + + // an event that is not listed in this enum yet. + // Give further details in the message below + CUSTOM = 1; + + // Law 3: Number of players + NUMBER_OF_PLAYERS = 2; + + // Law 9: Ball out of play + BALL_LEFT_FIELD = 3; + + // Law 10: Team scored a goal + GOAL = 4; + + // Law 9.3: lack of progress while bringing the ball into play + KICK_TIMEOUT = 5; + + // Law ?: There is no progress in game for (10|15)? seconds + NO_PROGRESS_IN_GAME = 6; + + // Law 12: Pushing / Substantial Contact + BOT_COLLISION = 7; + + // Law 12.2: Defender is completely inside penalty area + MULTIPLE_DEFENDER = 8; + + // Law 12: Defender is partially inside penalty area + MULTIPLE_DEFENDER_PARTIALLY = 9; + + // Law 12.3: Attacker in defense area + ATTACKER_IN_DEFENSE_AREA = 10; + + // Law 12: Icing (kicking over midline and opponent goal line) + ICING = 11; + + // Law 12: Ball speed + BALL_SPEED = 12; + + // Law 12: Robot speed during STOP + ROBOT_STOP_SPEED = 13; + + // Law 12: Maximum dribbling distance + BALL_DRIBBLING = 14; + + // Law 12: Touching the opponent goalkeeper + ATTACKER_TOUCH_KEEPER = 15; + + // Law 12: Double touch + DOUBLE_TOUCH = 16; + + // Law 13-17: Attacker not too close to the opponent's penalty area when ball enters play + ATTACKER_TO_DEFENCE_AREA = 17; + + // Law 13-17: Keeping the correct distance to the ball during opponents freekicks + DEFENDER_TO_KICK_POINT_DISTANCE = 18; + + // Law 12: A robot holds the ball deliberately + BALL_HOLDING = 19; + + // Law 12: The ball entered the goal directly after an indirect kick was performed + INDIRECT_GOAL = 20; + + // Law 9.2: Ball placement + BALL_PLACEMENT_FAILED = 21; + + // Law 10: A goal is only scored if the ball has not exceeded a robot height (150mm) between the last + // kick of an attacker and the time the ball crossed the goal line. + CHIP_ON_GOAL = 22; + } + + // the game event type that happened + required GameEventType gameEventType = 1; + + // a team + enum Team { + TEAM_UNKNOWN = 0; + TEAM_YELLOW = 1; + TEAM_BLUE = 2; + } + + // information about an originator + message Originator { + required Team team = 1; + optional uint32 botId = 2; + } + + // the team and optionally a designated robot that is the originator of the game event + optional Originator originator = 2; + + // a message describing further details of this game event + optional string message = 3; +} \ No newline at end of file diff --git a/parsian_protobuf_wrapper/proto/ssl_game_event_2019.proto b/parsian_protobuf_wrapper/proto/ssl_game_event_2019.proto new file mode 100644 index 00000000..574cced7 --- /dev/null +++ b/parsian_protobuf_wrapper/proto/ssl_game_event_2019.proto @@ -0,0 +1,453 @@ +syntax = "proto2"; + +import "ssl_game_controller_common.proto"; + +// GameEvent contains exactly one game event +// Each game event has optional and required fields. The required fields are mandatory to process the event. +// Some optional fields are only used for visualization, others are required to determine the ball placement position. +// If fields are missing that are required for the ball placement position, no ball placement command will be issued. +// Fields are marked optional to make testing and extending of the protocol easier. +// An autoRef should ideally set all fields, except if there are good reasons to not do so. +message GameEvent { + + required GameEventType type = 40; + + // the origin of this game event + // empty, if it originates from game controller + // autoRef name(s), if it originates from one or more autoRefs + repeated string origin = 41; + + // the event that occurred + oneof event { + + // Match proceeding events + + Prepared prepared = 1; + NoProgressInGame no_progress_in_game = 2; + PlacementFailed placement_failed = 3; + PlacementSucceeded placement_succeeded = 5; + BotSubstitution bot_substitution = 37; + TooManyRobots too_many_robots = 38; + + // Ball out of field events + + BallLeftField ball_left_field_touch_line = 6; + BallLeftField ball_left_field_goal_line = 7; + Goal possible_goal = 39; + Goal goal = 8; + IndirectGoal indirect_goal = 9; + ChippedGoal chipped_goal = 10; + + // Minor offense events + + AimlessKick aimless_kick = 11; + KickTimeout kick_timeout = 12; + KeeperHeldBall keeper_held_ball = 13; + AttackerDoubleTouchedBall attacker_double_touched_ball = 14; + AttackerInDefenseArea attacker_in_defense_area = 15; + AttackerTouchedKeeper attacker_touched_keeper = 16; + BotDribbledBallTooFar bot_dribbled_ball_too_far = 17; + BotKickedBallTooFast bot_kicked_ball_too_fast = 18; + + // Foul events + + AttackerTooCloseToDefenseArea attacker_too_close_to_defense_area = 19; + BotInterferedPlacement bot_interfered_placement = 20; + BotCrashDrawn bot_crash_drawn = 21; + BotCrashUnique bot_crash_unique = 22; + BotCrashUnique bot_crash_unique_skipped = 23; + BotPushedBot bot_pushed_bot = 24; + BotPushedBot bot_pushed_bot_skipped = 25; + BotHeldBallDeliberately bot_held_ball_deliberately = 26; + BotTippedOver bot_tipped_over = 27; + BotTooFastInStop bot_too_fast_in_stop = 28; + DefenderTooCloseToKickPoint defender_too_close_to_kick_point = 29; + DefenderInDefenseAreaPartially defender_in_defense_area_partially = 30; + DefenderInDefenseArea defender_in_defense_area = 31; + + // Repeated events + + MultipleCards multiple_cards = 32; + MultiplePlacementFailures multiple_placement_failures = 33; + MultipleFouls multiple_fouls = 34; + + // Unsporting behavior events + + UnsportingBehaviorMinor unsporting_behavior_minor = 35; + UnsportingBehaviorMajor unsporting_behavior_major = 36; + } + + // the ball left the field normally + message BallLeftField { + // the team that last touched the ball + required Team by_team = 1; + // the bot that last touched the ball + optional uint32 by_bot = 2; + // the location where the ball left the field + optional Location location = 3; + } + // the ball left the field via goal line and a team committed an aimless kick + message AimlessKick { + // the team that last touched the ball + required Team by_team = 1; + // the bot that last touched the ball + optional uint32 by_bot = 2; + // the location where the ball left the field + optional Location location = 3; + // the location where the ball was last touched + optional Location kick_location = 4; + } + // a team shot a goal + message Goal { + // the team that scored the goal + required Team by_team = 1; + // the team that shot the goal (different from by_team for own goals) + optional Team kicking_team = 6; + // the bot that shot the goal + optional uint32 kicking_bot = 2; + // the location where the ball entered the goal + optional Location location = 3; + // the location where the ball was kicked + optional Location kick_location = 4; + } + // the ball entered the goal directly during an indirect free kick + message IndirectGoal { + // the team that tried to shoot the goal + required Team by_team = 1; + // the bot that kicked the ball - at least the team must be set + optional uint32 by_bot = 2; + // the location where the ball entered the goal + optional Location location = 3; + // the location where the ball was kicked + optional Location kick_location = 4; + } + // the ball entered the goal, but was initially chipped + message ChippedGoal { + // the team that tried to shoot the goal + required Team by_team = 1; + // the bot that kicked the ball + optional uint32 by_bot = 2; + // the location where the ball entered the goal + optional Location location = 3; + // the location where the ball was kicked + optional Location kick_location = 4; + // the maximum height [m] of the ball, before it entered the goal and since the last kick + optional float max_ball_height = 5; + } + // a bot moved too fast while the game was stopped + message BotTooFastInStop { + // the team that found guilty + required Team by_team = 1; + // the bot that was too fast + optional uint32 by_bot = 2; + // the location of the bot + optional Location location = 3; + // the bot speed [m/s] + optional float speed = 4; + } + // a bot of the defending team got too close to the kick point during a free kick + message DefenderTooCloseToKickPoint { + // the team that found guilty + required Team by_team = 1; + // the bot that violates the distance to the kick point + optional uint32 by_bot = 2; + // the location of the bot + optional Location location = 3; + // the distance [m] from bot to the kick point (including the minimum radius) + optional float distance = 4; + } + // two robots crashed into each other with similar speeds + message BotCrashDrawn { + // the bot of the yellow team + optional uint32 bot_yellow = 1; + // the bot of the blue team + optional uint32 bot_blue = 2; + // the location of the crash (center between both bots) + optional Location location = 3; + // the calculated crash speed [m/s] of the two bots + optional float crash_speed = 4; + // the difference [m/s] of the velocity of the two bots + optional float speed_diff = 5; + // the angle [rad] in the range [0, π] of the bot velocity vectors + // an angle of 0 rad ( 0°) means, the bots barely touched each other + // an angle of π rad (180°) means, the bots crashed frontal into each other + optional float crash_angle = 6; + } + // two robots crashed into each other and one team was found guilty to due significant speed difference + message BotCrashUnique { + // the team that caused the crash + required Team by_team = 1; + // the bot that caused the crash + optional uint32 violator = 2; + // the bot of the opposite team that was involved in the crash + optional uint32 victim = 3; + // the location of the crash (center between both bots) + optional Location location = 4; + // the calculated crash speed vector [m/s] of the two bots + optional float crash_speed = 5; + // the difference [m/s] of the velocity of the two bots + optional float speed_diff = 6; + // the angle [rad] in the range [0, π] of the bot velocity vectors + // an angle of 0 rad ( 0°) means, the bots barely touched each other + // an angle of π rad (180°) means, the bots crashed frontal into each other + optional float crash_angle = 7; + } + // a bot pushed another bot over a significant distance + message BotPushedBot { + // the team that pushed the other team + required Team by_team = 1; + // the bot that pushed the other bot + optional uint32 violator = 2; + // the bot of the opposite team that was pushed + optional uint32 victim = 3; + // the location of the push (center between both bots) + optional Location location = 4; + // the pushed distance [m] + optional float pushed_distance = 5; + } + // a bot tipped over + message BotTippedOver { + // the team that found guilty + required Team by_team = 1; + // the bot that tipped over + optional uint32 by_bot = 2; + // the location of the bot + optional Location location = 3; + } + // a defender other than the keeper was fully located inside its own defense and touched the ball + message DefenderInDefenseArea { + // the team that found guilty + required Team by_team = 1; + // the bot that is inside the penalty area + optional uint32 by_bot = 2; + // the location of the bot + optional Location location = 3; + // the distance [m] from bot case to the nearest point outside the defense area + optional float distance = 4; + } + // a defender other than the keeper was partially located inside its own defense area and touched the ball + message DefenderInDefenseAreaPartially { + // the team that found guilty + required Team by_team = 1; + // the bot that is partially inside the penalty area + optional uint32 by_bot = 2; + // the location of the bot + optional Location location = 3; + // the distance [m] that the bot is inside the penalty area + optional float distance = 4; + } + // an attacker touched the ball inside the opponent defense area + message AttackerInDefenseArea { + // the team that found guilty + required Team by_team = 1; + // the bot that is inside the penalty area + optional uint32 by_bot = 2; + // the location of the bot + optional Location location = 3; + // the distance [m] that the bot is inside the penalty area + optional float distance = 4; + } + // a bot kicked the ball too fast + message BotKickedBallTooFast { + // the team that found guilty + required Team by_team = 1; + // the bot that kicked too fast + optional uint32 by_bot = 2; + // the location of the ball at the time of the highest speed + optional Location location = 3; + // the absolute initial ball speed (kick speed) [m/s] + optional float initial_ball_speed = 4; + // the maximum height [m] that the ball has reached during the kick + optional float max_ball_height = 5; + } + // a bot dribbled to ball too far + message BotDribbledBallTooFar { + // the team that found guilty + required Team by_team = 1; + // the bot that dribbled too far + optional uint32 by_bot = 2; + // the location where the dribbling started + optional Location start = 3; + // the location where the maximum dribbling distance was reached + optional Location end = 4; + } + // an attacker touched the opponent keeper + message AttackerTouchedKeeper { + // the team that found guilty + required Team by_team = 1; + // the bot that touched the opponent keeper + optional uint32 by_bot = 2; + // the location of the contact point between bot and keeper + optional Location location = 3; + } + // an attacker touched the ball multiple times when it was not allowed to + message AttackerDoubleTouchedBall { + // the team that found guilty + required Team by_team = 1; + // the bot that touched the ball twice + optional uint32 by_bot = 2; + // the location of the ball when it was first touched + optional Location location = 3; + } + // an attacker was located too near to the opponent defense area when ball entered play + message AttackerTooCloseToDefenseArea { + // the team that found guilty + required Team by_team = 1; + // the bot that is too close to the defense area + optional uint32 by_bot = 2; + // the location of the bot + optional Location location = 3; + // the distance [m] of the bot to the penalty area + optional float distance = 4; + } + // a bot held the ball for too long + message BotHeldBallDeliberately { + // the team that found guilty + required Team by_team = 1; + // the bot that holds the ball + optional uint32 by_bot = 2; + // the location of the ball + optional Location location = 3; + // the duration [s] that the bot hold the ball + optional float duration = 4; + } + // a bot interfered the ball placement of the other team + message BotInterferedPlacement { + // the team that found guilty + required Team by_team = 1; + // the bot that interfered the placement + optional uint32 by_bot = 2; + // the location of the bot + optional Location location = 3; + } + // a team collected multiple cards (yellow and red), which results in a penalty kick + message MultipleCards { + // the team that received multiple yellow cards + required Team by_team = 1; + } + // a team collected multiple fouls, which results in a yellow card + message MultipleFouls { + // the team that collected multiple fouls + required Team by_team = 1; + } + // a team failed to place the ball multiple times in a row + message MultiplePlacementFailures { + // the team that failed multiple times + required Team by_team = 1; + } + // timeout waiting for the attacking team to perform the free kick + message KickTimeout { + // the team that that should have kicked + required Team by_team = 1; + // the location of the ball + optional Location location = 2; + // the time [s] that was waited + optional float time = 3; + } + // game was stuck + message NoProgressInGame { + // the location of the ball + optional Location location = 1; + // the time [s] that was waited + optional float time = 2; + } + // ball placement failed + message PlacementFailed { + // the team that failed + required Team by_team = 1; + // the remaining distance [m] from ball to placement position + optional float remaining_distance = 2; + } + // a team was found guilty for minor unsporting behavior + message UnsportingBehaviorMinor { + // the team that found guilty + required Team by_team = 1; + // an explanation of the situation and decision + required string reason = 2; + } + // a team was found guilty for major unsporting behavior + message UnsportingBehaviorMajor { + // the team that found guilty + required Team by_team = 1; + // an explanation of the situation and decision + required string reason = 2; + } + // a keeper held the ball in its defense area for too long + message KeeperHeldBall { + // the team that found guilty + required Team by_team = 1; + // the location of the ball + optional Location location = 2; + // the duration [s] that the keeper hold the ball + optional float duration = 3; + } + // a team successfully placed the ball + message PlacementSucceeded { + // the team that did the placement + required Team by_team = 1; + // the time [s] taken for placing the ball + optional float time_taken = 2; + // the distance [m] between placement location and actual ball position + optional float precision = 3; + // the distance [m] between the initial ball location and the placement position + optional float distance = 4; + } + // both teams are prepared - all conditions are met to continue (with kickoff or penalty kick) + message Prepared { + // the time [s] taken for preparing + optional float time_taken = 1; + } + // bots are being substituted by at least one team + message BotSubstitution { + // the team that substitutes robots + required Team by_team = 1; + } + // a team has too many robots on the field + message TooManyRobots { + // the team that has too many robots + required Team by_team = 1; + } +} + +enum GameEventType { + UNKNOWN_GAME_EVENT_TYPE = 0; + + PREPARED = 1; + NO_PROGRESS_IN_GAME = 2; + PLACEMENT_FAILED = 3; + PLACEMENT_SUCCEEDED = 5; + BOT_SUBSTITUTION = 37; + TOO_MANY_ROBOTS = 38; + BALL_LEFT_FIELD_TOUCH_LINE = 6; + BALL_LEFT_FIELD_GOAL_LINE = 7; + POSSIBLE_GOAL = 39; + GOAL = 8; + INDIRECT_GOAL = 9; + CHIPPED_GOAL = 10; + AIMLESS_KICK = 11; + KICK_TIMEOUT = 12; + KEEPER_HELD_BALL = 13; + ATTACKER_DOUBLE_TOUCHED_BALL = 14; + ATTACKER_IN_DEFENSE_AREA = 15; + ATTACKER_TOUCHED_KEEPER = 16; + BOT_DRIBBLED_BALL_TOO_FAR = 17; + BOT_KICKED_BALL_TOO_FAST = 18; + ATTACKER_TOO_CLOSE_TO_DEFENSE_AREA = 19; + BOT_INTERFERED_PLACEMENT = 20; + BOT_CRASH_DRAWN = 21; + BOT_CRASH_UNIQUE = 22; + BOT_CRASH_UNIQUE_SKIPPED = 23; + BOT_PUSHED_BOT = 24; + BOT_PUSHED_BOT_SKIPPED = 25; + BOT_HELD_BALL_DELIBERATELY = 26; + BOT_TIPPED_OVER = 27; + BOT_TOO_FAST_IN_STOP = 28; + DEFENDER_TOO_CLOSE_TO_KICK_POINT = 29; + DEFENDER_IN_DEFENSE_AREA_PARTIALLY = 30; + DEFENDER_IN_DEFENSE_AREA = 31; + MULTIPLE_CARDS = 32; + MULTIPLE_PLACEMENT_FAILURES = 33; + MULTIPLE_FOULS = 34; + UNSPORTING_BEHAVIOR_MINOR = 35; + UNSPORTING_BEHAVIOR_MAJOR = 36; +} \ No newline at end of file diff --git a/parsian_protobuf_wrapper/proto/ssl_referee.proto b/parsian_protobuf_wrapper/proto/ssl_referee.proto new file mode 100644 index 00000000..9297f400 --- /dev/null +++ b/parsian_protobuf_wrapper/proto/ssl_referee.proto @@ -0,0 +1,202 @@ +syntax = "proto2"; + +import "ssl_game_event.proto"; +import "ssl_game_event_2019.proto"; + +// Each UDP packet contains one of these messages. +message Referee { + // The UNIX timestamp when the packet was sent, in microseconds. + // Divide by 1,000,000 to get a time_t. + required uint64 packet_timestamp = 1; + + // These are the "coarse" stages of the game. + enum Stage { + // The first half is about to start. + // A kickoff is called within this stage. + // This stage ends with the NORMAL_START. + NORMAL_FIRST_HALF_PRE = 0; + // The first half of the normal game, before half time. + NORMAL_FIRST_HALF = 1; + // Half time between first and second halves. + NORMAL_HALF_TIME = 2; + // The second half is about to start. + // A kickoff is called within this stage. + // This stage ends with the NORMAL_START. + NORMAL_SECOND_HALF_PRE = 3; + // The second half of the normal game, after half time. + NORMAL_SECOND_HALF = 4; + // The break before extra time. + EXTRA_TIME_BREAK = 5; + // The first half of extra time is about to start. + // A kickoff is called within this stage. + // This stage ends with the NORMAL_START. + EXTRA_FIRST_HALF_PRE = 6; + // The first half of extra time. + EXTRA_FIRST_HALF = 7; + // Half time between first and second extra halves. + EXTRA_HALF_TIME = 8; + // The second half of extra time is about to start. + // A kickoff is called within this stage. + // This stage ends with the NORMAL_START. + EXTRA_SECOND_HALF_PRE = 9; + // The second half of extra time. + EXTRA_SECOND_HALF = 10; + // The break before penalty shootout. + PENALTY_SHOOTOUT_BREAK = 11; + // The penalty shootout. + PENALTY_SHOOTOUT = 12; + // The game is over. + POST_GAME = 13; + } + required Stage stage = 2; + + // The number of microseconds left in the stage. + // The following stages have this value; the rest do not: + // NORMAL_FIRST_HALF + // NORMAL_HALF_TIME + // NORMAL_SECOND_HALF + // EXTRA_TIME_BREAK + // EXTRA_FIRST_HALF + // EXTRA_HALF_TIME + // EXTRA_SECOND_HALF + // PENALTY_SHOOTOUT_BREAK + // + // If the stage runs over its specified time, this value + // becomes negative. + optional sint32 stage_time_left = 3; + + // These are the "fine" states of play on the field. + enum Command { + // All robots should completely stop moving. + HALT = 0; + // Robots must keep 50 cm from the ball. + STOP = 1; + // A prepared kickoff or penalty may now be taken. + NORMAL_START = 2; + // The ball is dropped and free for either team. + FORCE_START = 3; + // The yellow team may move into kickoff position. + PREPARE_KICKOFF_YELLOW = 4; + // The blue team may move into kickoff position. + PREPARE_KICKOFF_BLUE = 5; + // The yellow team may move into penalty position. + PREPARE_PENALTY_YELLOW = 6; + // The blue team may move into penalty position. + PREPARE_PENALTY_BLUE = 7; + // The yellow team may take a direct free kick. + DIRECT_FREE_YELLOW = 8; + // The blue team may take a direct free kick. + DIRECT_FREE_BLUE = 9; + // The yellow team may take an indirect free kick. + INDIRECT_FREE_YELLOW = 10; + // The blue team may take an indirect free kick. + INDIRECT_FREE_BLUE = 11; + // The yellow team is currently in a timeout. + TIMEOUT_YELLOW = 12; + // The blue team is currently in a timeout. + TIMEOUT_BLUE = 13; + // The yellow team just scored a goal. + // For information only. + // For rules compliance, teams must treat as STOP. + // Deprecated: Use the score field from the team infos instead. That way, you can also detect revoked goals. + GOAL_YELLOW = 14 [deprecated = true]; + // The blue team just scored a goal. See also GOAL_YELLOW. + GOAL_BLUE = 15 [deprecated = true]; + // Equivalent to STOP, but the yellow team must pick up the ball and + // drop it in the Designated Position. + BALL_PLACEMENT_YELLOW = 16; + // Equivalent to STOP, but the blue team must pick up the ball and drop + // it in the Designated Position. + BALL_PLACEMENT_BLUE = 17; + } + required Command command = 4; + + // The number of commands issued since startup (mod 2^32). + required uint32 command_counter = 5; + + // The UNIX timestamp when the command was issued, in microseconds. + // This value changes only when a new command is issued, not on each packet. + required uint64 command_timestamp = 6; + + // Information about a single team. + message TeamInfo { + // The team's name (empty string if operator has not typed anything). + required string name = 1; + // The number of goals scored by the team during normal play and overtime. + required uint32 score = 2; + // The number of red cards issued to the team since the beginning of the game. + required uint32 red_cards = 3; + // The amount of time (in microseconds) left on each yellow card issued to the team. + // If no yellow cards are issued, this array has no elements. + // Otherwise, times are ordered from smallest to largest. + repeated uint32 yellow_card_times = 4 [packed = true]; + // The total number of yellow cards ever issued to the team. + required uint32 yellow_cards = 5; + // The number of timeouts this team can still call. + // If in a timeout right now, that timeout is excluded. + required uint32 timeouts = 6; + // The number of microseconds of timeout this team can use. + required uint32 timeout_time = 7; + // The pattern number of this team's goalkeeper. + required uint32 goalkeeper = 8; + // The total number of countable fouls that act towards yellow cards + optional uint32 foul_counter = 9; + // The number of consecutive ball placement failures of this team + optional uint32 ball_placement_failures = 10; + // Indicate if the team is able and allowed to place the ball + optional bool can_place_ball = 12; + // The maximum number of bots allowed on the field based on division and cards + optional uint32 max_allowed_bots = 13; + } + + // Information about the two teams. + required TeamInfo yellow = 7; + required TeamInfo blue = 8; + + // The coordinates of the Designated Position. These are measured in + // millimetres and correspond to SSL-Vision coordinates. These fields are + // always either both present (in the case of a ball placement command) or + // both absent (in the case of any other command). + message Point { + required float x = 1; + required float y = 2; + } + optional Point designated_position = 9; + + // Information about the direction of play. + // True, if the blue team will have it's goal on the positive x-axis of the ssl-vision coordinate system. + // Obviously, the yellow team will play on the opposite half. + optional bool blue_team_on_positive_half = 10; + + // The game event that caused the referee command. + // deprecated in favor of game_events. + optional Game_Event game_event = 11 [deprecated = true]; + + // The command that will be issued after the current stoppage and ball placement to continue the game. + optional Command next_command = 12; + + // All game events that were detected since the last RUNNING state. + // Will be cleared as soon as the game is continued. + repeated GameEvent game_events = 13; + + // All non-finished proposed game events that may be processed next. + repeated ProposedGameEvent proposed_game_events = 14; + + // The time in microseconds that is remaining until the current action times out + // The time will not be reset. It can get negative. + // An autoRef would raise an appropriate event, if the time gets negative. + // Possible actions where this time is relevant: + // * free kicks + // * kickoff, penalty kick, force start + // * ball placement + optional int32 current_action_time_remaining = 15; +} + +message ProposedGameEvent { + // The UNIX timestamp when the game event proposal will time out, in microseconds. + required uint64 valid_until = 1; + // The identifier of the proposer. + required string proposer_id = 2; + // The proposed game event. + required GameEvent game_event = 3; +} \ No newline at end of file diff --git a/parsian_protobuf_wrapper/src/ssl-refbox/convert/convert_referee.cpp b/parsian_protobuf_wrapper/src/ssl-refbox/convert/convert_referee.cpp index af040c21..22d848e3 100644 --- a/parsian_protobuf_wrapper/src/ssl-refbox/convert/convert_referee.cpp +++ b/parsian_protobuf_wrapper/src/ssl-refbox/convert/convert_referee.cpp @@ -1,5 +1,5 @@ -#include +#include #include "parsian_protobuf_wrapper/ssl-refbox/convert/convert_referee.h" #include "parsian_protobuf_wrapper/ssl-vision/convert/convert_units.h" diff --git a/parsian_protobuf_wrapper/src/ssl-refbox/script/GameControllerCommon.py b/parsian_protobuf_wrapper/src/ssl-refbox/script/GameControllerCommon.py new file mode 100644 index 00000000..26721ad8 --- /dev/null +++ b/parsian_protobuf_wrapper/src/ssl-refbox/script/GameControllerCommon.py @@ -0,0 +1,117 @@ +import os +import socket +import rsa +import rospy +from time import sleep +from parsian_protobuf_wrapper import ssl_game_controller_team_pb2 +#import ssl_game_controller_team_pb2 +import varint + + +class GameControllerCommon: + def __init__(self): + self.token = '' + self.verification = False + self.status = False + + def readPrivateKey(self, path, filename): + os.chdir(path) + if not os.path.isfile(filename): + return False, 'empty' + else: + privatefile = open(filename) + keydata = privatefile.read() + privatekey = rsa.PrivateKey.load_pkcs1(keydata, 'PEM') + return True, privatekey + + def readControllerToTeam(self, socket): + # type:(socket.socket) ->(object, object) + result = socket.recv(131072) + result_msg = ssl_game_controller_team_pb2.ControllerToTeam() + result_msg.ParseFromString(result[1:]) + ##ControllerReply + isControllerReply = False + if result_msg.HasField("controller_reply"): + isControllerReply = True + verification_tmp = -1 + if result_msg.controller_reply.HasField("verification"): + verification_tmp = result_msg.controller_reply.verification + if verification_tmp == 1: + self.verification = True + else: + self.verification = False + if result_msg.controller_reply.HasField("next_token"): + self.token = result_msg.controller_reply.next_token + else: + rospy.loginfo("NO TOKEN RECEIVED") + status_tmp = -1 + if result_msg.controller_reply.HasField("status_code"): + status_tmp = result_msg.controller_reply.status_code + if status_tmp == 1: + self.status = True + else: + self.status = False + ##AdvantageChoice = Do Nothing just wait for ControllerReply + else: + self.readControllerToTeam(socket) + + #rospy.loginfo(result_msg) + return (result_msg, isControllerReply) + + def teamSerializedRegistration(self, socket, teamname, token, privatekey): + # type:(socket.socket, str, str, rsa.privatekey) ->object + registration = ssl_game_controller_team_pb2.TeamRegistration() + registration.team_name = teamname + registration.signature.token = token + registration.signature.pkcs1v15 = bytes() + ##creating signature + serialized = registration.SerializeToString() + hash = rsa.compute_hash(serialized, 'SHA-256') + signature = rsa.sign_hash(hash, privatekey, 'SHA-256') + registration.signature.pkcs1v15 = signature + ##sending registration + serializedmessage = registration.SerializeToString() + return serializedmessage + + def teamSerializedAssignGoalie(self, socket, goalie_id, token, privatekey): + # type:(socket.socket, int, str, rsa.privatekey) ->object + assigngoalie = ssl_game_controller_team_pb2.TeamToController() + assigngoalie.desired_keeper = goalie_id + assigngoalie.signature.token = token + assigngoalie.signature.pkcs1v15 = bytes() + ##creating signature + serialized = assigngoalie.SerializeToString() + hash = rsa.compute_hash(serialized, 'SHA-256') + signature = rsa.sign_hash(hash, privatekey, 'SHA-256') + assigngoalie.signature.pkcs1v15 = signature + ##sending registration + serializedmessage = assigngoalie.SerializeToString() + return serializedmessage + + def teamSerializedsubstitute(self, socket, token, privatekey): + # type:(socket.socket, str, rsa.privatekey) ->object + substitute = ssl_game_controller_team_pb2.TeamToController() + substitute.substitute_bot = True + substitute.signature.token = token + substitute.signature.pkcs1v15 = bytes() + ##creating signature + serialized = substitute.SerializeToString() + hash = rsa.compute_hash(serialized, 'SHA-256') + signature = rsa.sign_hash(hash, privatekey, 'SHA-256') + substitute.signature.pkcs1v15 = signature + ##sending registration + serializedmessage = substitute.SerializeToString() + return serializedmessage + + def sendSerializedMessage(self, socket, serializedmsg, repeat): + # type:(socket.socket, str, bool) ->object + size = len(serializedmsg) + data = varint.encode(size) + serializedmsg + try: + socket.send(data) + return True + except: + print("cannot send data to the gamecontroller") + if repeat: + sleep(0.5) + self.sendSerializedMessage(socket, serializedmsg, repeat) \ No newline at end of file diff --git a/parsian_protobuf_wrapper/src/ssl-refbox/script/Test Team.key.pem b/parsian_protobuf_wrapper/src/ssl-refbox/script/Test Team.key.pem new file mode 100644 index 00000000..478a2fed --- /dev/null +++ b/parsian_protobuf_wrapper/src/ssl-refbox/script/Test Team.key.pem @@ -0,0 +1,27 @@ +-----BEGIN RSA PRIVATE KEY----- +MIIEowIBAAKCAQEApogmLRlwxd8hrU8h3zSLRzvMZVKKPi1EaVDTU5n6utSus1H6 +FqyBEykQbJm3dQ2X6nrnH2z3bimIBEeKAfeFNMBTq1CpNzCDmQFE1snBj3fKdRXz +mKCvwZPGO+qKCNzHi+vhCtXTTZX/wbMRkRjrOq24gAiHxmFczLa94MpzeD3xLpq+ +7P01DrsyVvHktiIcTHs+1OT4VLMJvtr9slbCeWcXgMFvfRME201FcaOjmCg9jrKH +HjQF0MHnd3hhsSOARoffQ/X/Lr1xOA/ixPEo5hxEGiRuUAPrn7iZWSpH6iRWbJWe +2ZH+oj+WgEc7BR2F7odQ91irRrSxNsVs+I55WwIDAQABAoIBAGggZU55w8vVkucc +vZ8k6ZlmyIzqKUprX4VCZoC1nNLJPVsefPNEdYiXeo+NJerozv5sTquVpLia+1NB +sAc+z2mGgEp0Kvo5OW+oHXT3vjGIw2ymhyP+BSdS0PaR1jFoatUQbiwqOu8eRUbG +Qsuo+xw7l0tnCg5+vlm6QcuWitC5V9H/t5AViXOfFaPBj9acHuWpszSiH5RBmZXi +MBfwQiXtVDGentHca2B4tcNjSc89Wuv1DK3Y1v1/s7HukDFTgE8UdMFgaLRzt1N4 +OQUBhi15ZBFb2xKIZzRFoA9BWlksQ6BBPkVLt5Ka3Rfb34MDmAuroVdc1LLmCKeC +6yQsUuECgYEA2WDk/T0K+GVBOfIciytkYx5/qNsvzC2bQ1E0X1x9VV2B0e1luKPt +WOwet5UjVXnkjFXmLORQR0MUxLn97e/3+pWBjFUlhYQl52z6GRmTumscR3VbkJnv +mrqAP8mzbtv7b4a5R8BD1OtgzeFRVyIb8jrLFBO8mXyWzyUxEAb7CbkCgYEAxB6T +zJvV8JaU1o2yB8Oqlbi30pvz/lhwVlF9WWozX6a2fdhPdFMPktb6vOpARpsQlrZ7 +DydNAV83p6WDIpQUSOO8Me+YH+AdpKldgNGYQsS4YjtbP8DT8ihl3YJpQZ5a5mUW +PItuJLoduGxtOqsGGxjhYLExd0i7REdCNSROlbMCgYEAvy2uduHG0isLMJE0dVlW +Uq4yDCmpYeMCWDQE4ZGQURGJ6TzmZ3sUdU5EvaSWjMhFLv8lDnpF+EaQ72u8XhTc +fTAb3XXNKB3O0DhRxN1vxVYKavZV71jTF7vKq08TVf52peFQ9j+r6IiSfL8bMIy5 +E1KN5DxvdHXUlJ3bBoN9KVECgYADGCBo2ASWGSocAHxQlwu39QQhdIhy+N483mhF +4uEQn0a90Y3fXfge7vlhxahh9MxcNGDYqlwSq3frUzcwcnmndMBhYVBbIGQXVvy8 +rZHja8sk8Z7M8LPnXC/PQOF8QY1ZmTqyldiVB8K0SDGo/U3JW6kip2kKYsFhoGYx +BHOg9QKBgAgClFnzxWoCA6EzwJM/zi5ulHqbkA5quPTfoFlsat30oCq/zCZFGBCt +exYQ5nA1qO6FxHsWEnGcZySmpoHI8E+s/nFLGtb4M4KE+Kc96pWPxKyqGDepvyWK +GNegmvzaTa5JqA2zmDQw0j4xhdgdhjdvyBt6tEVdWZKjB13Cax5B +-----END RSA PRIVATE KEY----- diff --git a/parsian_protobuf_wrapper/src/ssl-refbox/script/parsian_game_controller_node.py b/parsian_protobuf_wrapper/src/ssl-refbox/script/parsian_game_controller_node.py new file mode 100755 index 00000000..978c798d --- /dev/null +++ b/parsian_protobuf_wrapper/src/ssl-refbox/script/parsian_game_controller_node.py @@ -0,0 +1,213 @@ +#!/usr/bin/env python +# license removed for brevity + +import rospy +import socket +import rospkg +from time import sleep +from GameControllerCommon import GameControllerCommon +import dynamic_reconfigure.client +from parsian_protobuf_wrapper.cfg import refereeConfig +from parsian_ai.cfg import aiConfig +from parsian_msgs.msg import parsian_draws +from parsian_msgs.msg import parsian_draw +from parsian_msgs.msg import parsian_draw_text +from parsian_msgs.msg import parsian_world_model +from parsian_msgs.msg import ssl_refree_wrapper +from parsian_msgs.msg import parsian_robots_fault +from parsian_msgs.msg import parsian_robot_substitution + +TEAM_NAME = 'Test Team' + + +class GameController(): + def __init__(self): + + ##variables + self.registered = False + self.goalie_id = -1 + self.isFirstGoalieAssignment = True + self.isgoalieassigned = False + self.gc = GameControllerCommon() + ##load rsa key + is_privatekey_exist, self.privatekey = self.gc.readPrivateKey(rospkg.RosPack().get_path("parsian_protobuf_wrapper")+"/src/ssl-refbox/script/", TEAM_NAME + '.key.pem') + if not is_privatekey_exist: + rospy.loginfo('COULDNT FIND ANY PRIVATE KEY') + exit(0) + + ##socket + self.socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM) + #dynamic reconfigure + self.IP = '127.0.0.1' + self.PORT = 10008 + self.client_net = dynamic_reconfigure.client.Client("/refbox", timeout=None, config_callback=self.cfg_callback_net) + self.client_ai = dynamic_reconfigure.client.Client("/ai_node", timeout=None, config_callback=self.cfg_callback_ai) + #self.srv_net = Server(refereeConfig, self.cfg_callback_net, "/refbox") + #self.srv_ai = Server(aiConfig, self.cfg_callback_ai, "/ai_node") + + #draw + self.draw_pub = rospy.Publisher('/draws', parsian_draws, queue_size=1, latch=True) + self.wm_sub = rospy.Subscriber('world_model', parsian_world_model, self.wmCallback, queue_size=1, + buff_size=2 ** 24) + + ##substitude + self.ref_sub = rospy.Subscriber('/referee', ssl_refree_wrapper, self.refCallback, queue_size=1, + buff_size=2 ** 24) + self.autofault_sub = rospy.Subscriber('/autofault', parsian_robots_fault, self.faultCallback, queue_size=1, + buff_size=2 ** 24) + self.substitute_pub = rospy.Publisher('/substitute', parsian_robot_substitution, queue_size=1, latch=True) + self.isStop = False + self.robots_status = [False for i in range(12)] + + + def cfg_callback_net(self, config): + self.IP = config.refree_listen_ip + self.PORT = config.refree_listen_port + #self.IP = '127.0.0.1' #TODO delete this + #register + self.register() + + + def cfg_callback_ai(self, config): + if self.goalie_id != config.Goalie: + self.goalie_id = config.Goalie + if self.registered: + self.assigngoalie() + + + + def register(self): + + self.registered = False + self.isgoalieassigned = False + self.socket.close() + self.socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM) + try: + self.socket.connect((self.IP, self.PORT)) + except: + print('cannot bind to the gamecontroller', self.IP, self.PORT) + return + #sleep(0.5) + #self.register() + ##getting initiate response + initiate_result_msg, iscontrollerreply = self.gc.readControllerToTeam(self.socket) + if not self.gc.verification: + rospy.loginfo('problem in getting the initiate response from controller') + return + #sleep(0.5) + #self.register() + ##send registration data + registration_serialized = self.gc.teamSerializedRegistration(self.socket, TEAM_NAME, self.gc.token, self.privatekey) + self.gc.sendSerializedMessage(self.socket, registration_serialized, False) + ##getting registration result message + registration_result_msg, iscontrollerreply = self.gc.readControllerToTeam(self.socket) + if self.gc.verification != 1:##NOT VERIFIED + rospy.loginfo("error in registration message: " + registration_result_msg.controller_reply.reason) + return + #sleep(0.5) + #self.register() + + rospy.loginfo("TEAM REGISTERED") + self.registered = True + if not self.isgoalieassigned: + self.assigngoalie() + + + def assigngoalie(self): + self.isgoalieassigned = False + if not self.registered: + return + ##send assigngoalie data + assigngoalie_serialized = self.gc.teamSerializedAssignGoalie(self.socket, self.goalie_id, self.gc.token, self.privatekey) + self.gc.sendSerializedMessage(self.socket, assigngoalie_serialized, False) + ##getting assigngoalie result message + assigngoalie_result_msg, iscontrollerreply = self.gc.readControllerToTeam(self.socket) + if not self.gc.status: ##NOT VERIFIED + rospy.loginfo("error in goalie assignment message: " + assigngoalie_result_msg.controller_reply.reason) + return + + rospy.loginfo("GOALIE ASSIGNED") + self.isgoalieassigned = True + + + def substitute(self): + if not self.registered: + return False + ##send substitute data + substitute_serialized = self.gc.teamSerializedsubstitute(self.socket, self.gc.token, self.privatekey) + self.gc.sendSerializedMessage(self.socket, substitute_serialized, False) + ##getting substitute result message + substitute_result_msg, iscontrollerreply = self.gc.readControllerToTeam(self.socket) + if not self.gc.status: ##NOT VERIFIED + rospy.loginfo("error in goalie assignment message: " + substitute_result_msg.controller_reply.reason) + return False + + rospy.loginfo("SUBSTITUDE VERIFIED") + return True + + def faultCallback(self, data): + #type:(parsian_robots_fault)->None + if self.isStop: + for i in range(12): + if data.robots[i].select == 2: #DAMAGED + if not self.robots_status[data.robots[i].robot_id]: + result = self.substitute() + self.robots_status[data.robots[i].robot_id] = result + else: + self.robots_status[data.robots[i].robot_id] = False + substitude_msg = parsian_robot_substitution() + substitude_msg.substitutional_IDs = self.robots_status + self.substitute_pub.publish(substitude_msg) + + + def refCallback(self, data): + #type:(ssl_refree_wrapper)->None + if data.command.command == data.command.STOP: + self.isStop = True + else: + if self.isStop: + self.robots_status = [False for i in range(12)] + self.isStop = False + + + def wmCallback(self, data): + #type:(parsian_world_model)->None + if self.registered and self.isgoalieassigned: + return + draws = parsian_draws() + drawText1 = parsian_draw() + drawText2 = parsian_draw() + if not self.registered: + drawText1.text = "TEAM NOT REGISTERED TO REFEREE" + drawText1.primary.x = 0 + drawText1.primary.y = 4.3 + drawText1.size = 30 + drawText1.color.r = 1 + drawText1.color.g = 0 + drawText1.color.b = 0 + drawText1.type = drawText1.TEXT + draws.draws.append(drawText1) + if not self.isgoalieassigned: + drawText2.text = "GOALIE NOT ASSIGNED TO REFEREE" + drawText2.primary.x = 0 + drawText2.primary.y = 3.8 + drawText2.size = 30 + drawText2.color.r = 1 + drawText2.color.g = 0 + drawText2.color.b = 0 + drawText2.type = drawText2.TEXT + draws.draws.append(drawText2) + self.draw_pub.publish(draws) + + + + + +if __name__ == '__main__': + try: + rospy.init_node('game_controller', anonymous=True) + rospy.loginfo("game_controller is running") + control = GameController() + rospy.spin() + except rospy.ROSInterruptException: + pass diff --git a/parsian_protobuf_wrapper/src/ssl-refbox/script/varint.py b/parsian_protobuf_wrapper/src/ssl-refbox/script/varint.py new file mode 100644 index 00000000..1701f1ae --- /dev/null +++ b/parsian_protobuf_wrapper/src/ssl-refbox/script/varint.py @@ -0,0 +1,62 @@ +"""Varint encoder/decoder +varints are a common encoding for variable length integer data, used in +libraries such as sqlite, protobuf, v8, and more. +Here's a quick and dirty module to help avoid reimplementing the same thing +over and over again. +""" + +# byte-oriented StringIO was moved to io.BytesIO in py3k +try: + from io import BytesIO +except ImportError: + from StringIO import StringIO as BytesIO + +import sys + +if sys.version > '3': + def _byte(b): + return bytes((b, )) +else: + def _byte(b): + return chr(b) + + +def encode(number): + """Pack `number` into varint bytes""" + buf = b'' + while True: + towrite = number & 0x7f + number >>= 7 + if number: + buf += _byte(towrite | 0x80) + else: + buf += _byte(towrite) + break + return buf + +def decode_stream(stream): + """Read a varint from `stream`""" + shift = 0 + result = 0 + while True: + i = _read_one(stream) + result |= (i & 0x7f) << shift + shift += 7 + if not (i & 0x80): + break + + return result + +def decode_bytes(buf): + """Read a varint from from `buf` bytes""" + return decode_stream(BytesIO(buf)) + + +def _read_one(stream): + """Read a byte from the file (as an integer) + raises EOFError if the stream ends while reading bytes. + """ + c = stream.read(1) + if c == b'': + raise EOFError("Unexpected EOF while reading bytes") + return ord(c) \ No newline at end of file diff --git a/parsian_tools/GraphicalClient.perspective b/parsian_tools/GraphicalClient.perspective new file mode 100644 index 00000000..d6a90528 --- /dev/null +++ b/parsian_tools/GraphicalClient.perspective @@ -0,0 +1,62 @@ +{ + "keys": {}, + "groups": { + "pluginmanager": { + "keys": { + "running-plugins": { + "type": "repr", + "repr": "{u'rqt_parsian_gui/GraphicalClient': [1]}" + } + }, + "groups": { + "plugin__rqt_parsian_gui__GraphicalClient__1": { + "keys": {}, + "groups": { + "dock_widget__": { + "keys": { + "dockable": { + "type": "repr", + "repr": "True" + }, + "parent": { + "type": "repr", + "repr": "None" + }, + "dock_widget_title": { + "type": "repr", + "repr": "u''" + } + }, + "groups": {} + } + } + } + } + }, + "mainwindow": { + "keys": { + "geometry": { + "type": "repr(QByteArray.hex)", + "repr(QByteArray.hex)": "QtCore.QByteArray('01d9d0cb000200000000004100000018000003e0000004370000004100000034000003e00000043700000000000000000780')", + "pretty-print": " A 7 A 4 7 " + }, + "state": { + "type": "repr(QByteArray.hex)", + "repr(QByteArray.hex)": "QtCore.QByteArray('000000ff00000000fd0000000100000003000003a0000003eefc0100000001fb0000004a007200710074005f007000610072007300690061006e005f006700750069005f005f00470072006100700068006900630061006c0043006c00690065006e0074005f005f0031005f005f0100000000000003a00000018a00ffffff000003a00000000000000004000000040000000800000008fc00000001000000030000000100000036004d0069006e0069006d0069007a006500640044006f0063006b00570069006400670065007400730054006f006f006c0062006100720000000000ffffffff0000000000000000')", + "pretty-print": " Jrqt_parsian_gui__GraphicalClient__1__ 6MinimizedDockWidgetsToolbar " + } + }, + "groups": { + "toolbar_areas": { + "keys": { + "MinimizedDockWidgetsToolbar": { + "type": "repr", + "repr": "8" + } + }, + "groups": {} + } + } + } + } +} \ No newline at end of file diff --git a/parsian_tools/Main.perspective b/parsian_tools/Main.perspective new file mode 100644 index 00000000..3c0752b6 --- /dev/null +++ b/parsian_tools/Main.perspective @@ -0,0 +1,135 @@ +{ + "keys": {}, + "groups": { + "pluginmanager": { + "keys": { + "running-plugins": { + "type": "repr", + "repr": "{u'rqt_reconfigure/Param': [1], u'rqt_parsian_gui/Referee': [1], u'rqt_parsian_gui/ModeChooser': [1]}" + } + }, + "groups": { + "plugin__rqt_publisher__Publisher__1": { + "keys": {}, + "groups": { + "plugin": { + "keys": { + "publishers": { + "type": "repr", + "repr": "u'[]'" + } + }, + "groups": {} + } + } + }, + "plugin__rqt_parsian_gui__Referee__1": { + "keys": {}, + "groups": { + "dock_widget__": { + "keys": { + "dockable": { + "type": "repr", + "repr": "True" + }, + "parent": { + "type": "repr", + "repr": "None" + }, + "dock_widget_title": { + "type": "repr", + "repr": "u''" + } + }, + "groups": {} + } + } + }, + "plugin__rqt_reconfigure__Param__1": { + "keys": {}, + "groups": { + "dock_widget___plugincontainer_top_widget": { + "keys": { + "dockable": { + "type": "repr", + "repr": "True" + }, + "parent": { + "type": "repr", + "repr": "None" + }, + "dock_widget_title": { + "type": "repr", + "repr": "u'Dynamic Reconfigure'" + } + }, + "groups": {} + }, + "plugin": { + "keys": { + "splitter": { + "type": "repr(QByteArray.hex)", + "repr(QByteArray.hex)": "QtCore.QByteArray('000000ff0000000100000002000000ae0000006401ffffffff010000000100')", + "pretty-print": " d " + }, + "_splitter": { + "type": "repr(QByteArray.hex)", + "repr(QByteArray.hex)": "QtCore.QByteArray('000000ff00000001000000020000012c000000640100000009010000000200')", + "pretty-print": " , d " + } + }, + "groups": {} + } + } + }, + "plugin__rqt_parsian_gui__ModeChooser__1": { + "keys": {}, + "groups": { + "dock_widget__": { + "keys": { + "dockable": { + "type": "repr", + "repr": "True" + }, + "parent": { + "type": "repr", + "repr": "None" + }, + "dock_widget_title": { + "type": "repr", + "repr": "u''" + } + }, + "groups": {} + } + } + } + } + }, + "mainwindow": { + "keys": { + "geometry": { + "type": "repr(QByteArray.hex)", + "repr(QByteArray.hex)": "QtCore.QByteArray('01d9d0cb00020000000003e0000000180000077f00000437000003e0000000340000077f0000043700000000000000000780')", + "pretty-print": " 7 4 7 " + }, + "state": { + "type": "repr(QByteArray.hex)", + "repr(QByteArray.hex)": "QtCore.QByteArray('000000ff00000000fd0000000100000003000003a0000003eefc0100000005fc0000000000000322000002af00fffffffc0200000002fb0000006c007200710074005f007200650063006f006e006600690067007500720065005f005f0050006100720061006d005f005f0031005f005f005f0070006c007500670069006e0063006f006e007400610069006e00650072005f0074006f0070005f0077006900640067006500740100000000000003a20000010f00fffffffb0000003a007200710074005f007000610072007300690061006e005f006700750069005f005f0052006500660065007200650065005f005f0031005f005f01000003a8000000460000004200fffffffc0000010f000002af0000000000fffffffc0200000001fb0000004a007200710074005f007000610072007300690061006e005f006700750069005f005f00470072006100700068006900630061006c0043006c00690065006e0074005f005f0031005f005f01000000000000040a0000000000000000fb0000004c007200710074005f0074006f007000690063005f005f0054006f0070006900630050006c007500670069006e005f005f0031005f005f0054006f007000690063005700690064006700650074010000034d000003f20000000000000000fb00000058007200710074005f007000750062006c00690073006800650072005f005f005000750062006c00690073006800650072005f005f0031005f005f005000750062006c0069007300680065007200570069006400670065007401000004da000002650000000000000000fb00000042007200710074005f007000610072007300690061006e005f006700750069005f005f004d006f0064006500430068006f006f007300650072005f005f0031005f005f0100000328000000780000007800ffffff000003a00000000000000004000000040000000800000008fc00000001000000030000000100000036004d0069006e0069006d0069007a006500640044006f0063006b00570069006400670065007400730054006f006f006c0062006100720000000000ffffffff0000000000000000')", + "pretty-print": " \" F B Jrqt_parsian_gui__GraphicalClient__1__ Lrqt_topic__TopicPlugin__1__TopicWidget Xrqt_publisher__Publisher__1__PublisherWidget Brqt_parsian_gui__ModeChooser__1__ 6MinimizedDockWidgetsToolbar " + } + }, + "groups": { + "toolbar_areas": { + "keys": { + "MinimizedDockWidgetsToolbar": { + "type": "repr", + "repr": "8" + } + }, + "groups": {} + } + } + } + } +} \ No newline at end of file diff --git a/parsian_tools/launch/grsim-with-ai.launch b/parsian_tools/launch/grsim-with-ai.launch index a1706653..b86348e3 100644 --- a/parsian_tools/launch/grsim-with-ai.launch +++ b/parsian_tools/launch/grsim-with-ai.launch @@ -2,6 +2,7 @@ + @@ -39,6 +40,7 @@ + diff --git a/parsian_tools/launch/grsim.launch b/parsian_tools/launch/grsim.launch index bd0d2ee4..f6e8cb9f 100644 --- a/parsian_tools/launch/grsim.launch +++ b/parsian_tools/launch/grsim.launch @@ -2,6 +2,7 @@ + @@ -39,6 +40,7 @@ + diff --git a/parsian_tools/launch/real-with-ai.launch b/parsian_tools/launch/real-with-ai.launch index ee477e65..73590f07 100644 --- a/parsian_tools/launch/real-with-ai.launch +++ b/parsian_tools/launch/real-with-ai.launch @@ -2,6 +2,7 @@ + @@ -39,6 +40,7 @@ + diff --git a/parsian_tools/launch/real.launch b/parsian_tools/launch/real.launch index 8bee1771..3f3ade51 100644 --- a/parsian_tools/launch/real.launch +++ b/parsian_tools/launch/real.launch @@ -2,6 +2,7 @@ + @@ -39,6 +40,7 @@ + diff --git a/parsian_tools/script/env.zsh b/parsian_tools/script/env.zsh index eca33392..1b71f293 100755 --- a/parsian_tools/script/env.zsh +++ b/parsian_tools/script/env.zsh @@ -85,6 +85,14 @@ function parsian() { rebuild) catkin clean -y catkin build "${@:2}" + ;; + window) + rqt --perspective-file "$PARSIAN_ROOT/src/ssl/parsian_tools/Main.perspective"& + while : + do + rqt --perspective-file "$PARSIAN_ROOT/src/ssl/parsian_tools/GraphicalClient.perspective" + done + ;; behavior) case $2 in diff --git a/parsian_util/CMakeLists.txt b/parsian_util/CMakeLists.txt index 095d08b1..fcbd0c50 100644 --- a/parsian_util/CMakeLists.txt +++ b/parsian_util/CMakeLists.txt @@ -140,7 +140,23 @@ include_directories( ) ## Declare a C++ library -add_library(${PROJECT_NAME}_geom +add_library( + ${PROJECT_NAME} + src/geom/angle_deg.cpp + src/geom/circle_2d.cpp + src/geom/line_2d.cpp + src/geom/matrix_2d.cpp + src/geom/polygon_2d.cpp + src/geom/ray_2d.cpp + src/geom/segment_2d.cpp + src/geom/rect_2d.cpp + src/geom/sector_2d.cpp + src/geom/triangle_2d.cpp + src/geom/vector_2d.cpp + test/geom/test_polygon_2d.h test/geom/test_matrix_2d.h test/geom/test_rect_2d.h test/geom/test_segment_2d.h + test/geom/test_triangle_2d.h test/geom/test_vector_2d.h test/geom/test_voronoi_diagram.h test/geom/config.h) +add_library( + ${PROJECT_NAME}_geom src/geom/angle_deg.cpp src/geom/circle_2d.cpp src/geom/line_2d.cpp @@ -152,7 +168,7 @@ add_library(${PROJECT_NAME}_geom src/geom/sector_2d.cpp src/geom/triangle_2d.cpp src/geom/vector_2d.cpp - ) + ) add_library(${PROJECT_NAME}_math src/mathtools.cpp @@ -172,8 +188,7 @@ add_library(${PROJECT_NAME}_actions src/action/autogenerate/receivepassaction.cpp src/action/autogenerate/onetouchaction.cpp src/action/autogenerate/kickaction.cpp - src/action/autogenerate/noaction.cpp - ) + src/action/autogenerate/noaction.cpp) add_library(${PROJECT_NAME}_core src/core/agent.cpp @@ -275,7 +290,8 @@ target_link_libraries(${PROJECT_NAME}_actions # ) ## Mark executables and/or libraries for installation -install(TARGETS +install(TARGETS + ${PROJECT_NAME} ${PROJECT_NAME}_geom ${PROJECT_NAME}_math ${PROJECT_NAME}_actions @@ -305,10 +321,14 @@ install(DIRECTORY include/${PROJECT_NAME}/ ############# ## Add gtest based cpp test target and link libraries -# catkin_add_gtest(${PROJECT_NAME}-test test/test_parsian_util.cpp) -# if(TARGET ${PROJECT_NAME}-test) -# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) -# endif() +catkin_add_gtest(${PROJECT_NAME}-test test/geom/utest.cpp) +if(TARGET ${PROJECT_NAME}-test) + target_link_libraries(${PROJECT_NAME}-test + ${PROJECT_NAME} + ${catkin_LIBRARIES} + ${QT_LIBRARIES} + ) +endif() ## Add folders to be run by python nosetests # catkin_add_nosetests(test) diff --git a/parsian_util/include/parsian_util/geom/delaunay_triangulation.h b/parsian_util/include/parsian_util/geom/delaunay_triangulation.h index ce446d93..033bdd54 100644 --- a/parsian_util/include/parsian_util/geom/delaunay_triangulation.h +++ b/parsian_util/include/parsian_util/geom/delaunay_triangulation.h @@ -32,8 +32,8 @@ #ifndef RCSC_GEOM_DELAUNAY_TRIANGULATION_H #define RCSC_GEOM_DELAUNAY_TRIANGULATION_H -#include -#include +#include +#include #include diff --git a/parsian_util/include/parsian_util/geom/segment_2d.h b/parsian_util/include/parsian_util/geom/segment_2d.h index 595e8bce..1ab7eef7 100644 --- a/parsian_util/include/parsian_util/geom/segment_2d.h +++ b/parsian_util/include/parsian_util/geom/segment_2d.h @@ -70,7 +70,6 @@ class Segment2D { , M_b(b) { } - /*! \brief construct directly using raw coordinate values \param ax 1st point x value of segment edge @@ -85,6 +84,18 @@ class Segment2D { : M_a(ax, ay) , M_b(bx, by) { } + /*! + \brief construct using origin, direction and length + \param origin origin point + \param length length of line segment + \param dir line direction from origin point + */ + Segment2D( const Vector2D & a, + const double & length, + const AngleDeg & dir ) + : M_a( a ), + M_b( a + Vector2D::from_polar( length, dir ) ) + { } /*! \brief construct from 2 points @@ -116,7 +127,22 @@ class Segment2D { M_b.assign(bx, by); return *this; } - + /*! + \brief construct using origin, direction and length + \param origin origin point + \param length length of line segment + \param dir line direction from origin point + \return const reference to this object + */ + const + Segment2D & assign( const Vector2D & a, + const double & length, + const AngleDeg & dir ) + { + M_a = a; + M_b = a + Vector2D::from_polar( length, dir ); + return *this; + } /*! \brief swap segment edge point \return const reference to itself @@ -265,8 +291,6 @@ class Segment2D { */ bool onSegment(const Vector2D & p) const; - bool onSegmentWeakly(const Vector2D & p) const; - Vector2D projection(const Vector2D & p) const; }; } diff --git a/parsian_util/include/parsian_util/geom/voronoi_diagram.h b/parsian_util/include/parsian_util/geom/voronoi_diagram.h index 79dc3247..169618ba 100644 --- a/parsian_util/include/parsian_util/geom/voronoi_diagram.h +++ b/parsian_util/include/parsian_util/geom/voronoi_diagram.h @@ -30,8 +30,8 @@ #ifndef RCSC_GEOM_VORONOI_DIAGRAM_H #define RCSC_GEOM_VORONOI_DIAGRAM_H -#include -#include +#include +#include namespace rcsc { diff --git a/parsian_util/include/parsian_util/geom/voronoi_diagram_original.h b/parsian_util/include/parsian_util/geom/voronoi_diagram_original.h index c2002926..9568f1a0 100644 --- a/parsian_util/include/parsian_util/geom/voronoi_diagram_original.h +++ b/parsian_util/include/parsian_util/geom/voronoi_diagram_original.h @@ -30,11 +30,11 @@ #ifndef RCSC_GEOM_VORONOI_DIAGRAM_ORIGINAL_H #define RCSC_GEOM_VORONOI_DIAGRAM_ORIGINAL_H -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include #include #include diff --git a/parsian_util/include/parsian_util/geom/voronoi_diagram_triangle.h b/parsian_util/include/parsian_util/geom/voronoi_diagram_triangle.h index b11bb143..9d86d264 100644 --- a/parsian_util/include/parsian_util/geom/voronoi_diagram_triangle.h +++ b/parsian_util/include/parsian_util/geom/voronoi_diagram_triangle.h @@ -30,10 +30,10 @@ #ifndef RCSC_GEOM_VORONOI_DIAGRAM_TRIANGLE_H #define RCSC_GEOM_VORONOI_DIAGRAM_TRIANGLE_H -#include -#include -#include -#include +#include +#include +#include +#include #include #include diff --git a/parsian_util/package.xml b/parsian_util/package.xml index 05c57171..6c6b39ad 100644 --- a/parsian_util/package.xml +++ b/parsian_util/package.xml @@ -40,7 +40,7 @@ - + gtest catkin roscpp rospy diff --git a/parsian_util/src/action/autogenerate/gotopointavoidaction.cpp b/parsian_util/src/action/autogenerate/gotopointavoidaction.cpp index 6f2fd5d0..170915d7 100644 --- a/parsian_util/src/action/autogenerate/gotopointavoidaction.cpp +++ b/parsian_util/src/action/autogenerate/gotopointavoidaction.cpp @@ -28,6 +28,10 @@ void GotopointavoidAction::setMessage(const void* _msg) { drawPath = msg.drawPath; diveMode = msg.diveMode; addVel = msg.addVel; + ourrelax.clear(); + for (auto _v : msg.ourrelax) { ourrelax.push_back(_v); } + theirrelax.clear(); + for (auto _v : msg.theirrelax) { theirrelax.push_back(_v); } GotopointAction::setMessage(&msg.base); } @@ -46,6 +50,10 @@ void* GotopointavoidAction::getMessage() { _msg->drawPath = drawPath; _msg->diveMode = diveMode; _msg->addVel = addVel.toParsianMessage(); + _msg->ourrelax.clear(); + for (auto _v : ourrelax) { _msg->ourrelax.push_back(_v); } + _msg->theirrelax.clear(); + for (auto _v : theirrelax) { _msg->theirrelax.push_back(_v); } return _msg; } diff --git a/parsian_util/src/geom/polygon_2d.cpp b/parsian_util/src/geom/polygon_2d.cpp index 3851537f..054faacb 100644 --- a/parsian_util/src/geom/polygon_2d.cpp +++ b/parsian_util/src/geom/polygon_2d.cpp @@ -141,24 +141,21 @@ Polygon2D::getBoundingBox() const { x_max = p.x; } - if ( p.x < x_min ) { x_min = p.x; } - if ( p.y > y_max ) { y_max = p.y; } - if ( p.y < y_min ) { y_min = p.y; } } - return( Rect2D( x_min, y_min, (x_max - x_min), (y_max - y_min) ) ); + return( Rect2D( x_min, y_max, (x_max - x_min), (y_max - y_min) ) ); } /*-------------------------------------------------------------------*/ diff --git a/parsian_util/test/geom/config.h b/parsian_util/test/geom/config.h new file mode 100644 index 00000000..f529f646 --- /dev/null +++ b/parsian_util/test/geom/config.h @@ -0,0 +1,19 @@ +// +// Created by raziyeh on 3/30/19. +// + +#ifndef PARSIAN_UTIL_TEST_CONSTS_H +#define PARSIAN_UTIL_TEST_CONSTS_H + +#include + +using namespace rcsc; + +const double EPS = 1.0e-10; +const Vector2D ERROR_VALUE(5000.0, 5000.0); +const double DISTANCE = 1.0e-6; +const double DISTANCE2 = 1.0e-3; +const double DISTANCE3 = 1.0e-4; +const Vector2D ZERO( 0.0, 0.0 ); + +#endif //PARSIAN_UTIL_TEST_CONSTS_H diff --git a/parsian_util/test/geom/test_matrix_2d.h b/parsian_util/test/geom/test_matrix_2d.h new file mode 100644 index 00000000..155bf167 --- /dev/null +++ b/parsian_util/test/geom/test_matrix_2d.h @@ -0,0 +1,127 @@ +// +// Created by raziyeh on 3/29/19. +// + +#ifndef PARSIAN_UTIL_TEST_MATRIX_2D_CPP_H +#define PARSIAN_UTIL_TEST_MATRIX_2D_CPP_H + +#include +#include "config.h" + +#include +#include + +using namespace rcsc; + +void testTranslate() +{ + { + rcsc::Matrix2D m; + m.translate( 2.0, 4.0 ); + + rcsc::Vector2D v( 0.0, 0.0 ); + m.transform( &v ); + + EXPECT_NEAR( rcsc::Vector2D( 2.0, 4.0 ).dist( v ), 0.0, EPS ); + } + + { + rcsc::Matrix2D m = rcsc::Matrix2D::make_translation( 3.0, 4.0 ); + + rcsc::Vector2D v( 0.0, 0.0 ); + m.transform( &v ); + + EXPECT_NEAR( rcsc::Vector2D( 3.0, 4.0 ).dist( v ), 0.0, EPS ); + } +} + +void testScale() +{ + { + rcsc::Matrix2D m; + m.scale( 2.0, 4.0 ); + + rcsc::Vector2D v1( 0.0, 0.0 ); + rcsc::Vector2D v2( 1.0, 1.0 ); + rcsc::Vector2D v3( 2.0, 3.0 ); + + m.transform( &v1 ); + m.transform( &v2 ); + m.transform( &v3 ); + + EXPECT_NEAR( rcsc::Vector2D( 0.0, 0.0 ).dist( v1 ), 0.0, EPS ); + EXPECT_NEAR( rcsc::Vector2D( 2.0, 4.0 ).dist( v2 ), 0.0, EPS ); + EXPECT_NEAR( rcsc::Vector2D( 4.0, 12.0 ).dist( v3 ), 0.0, EPS ); + } + + { + rcsc::Matrix2D m = rcsc::Matrix2D::make_scaling( -3.0, 1.0 ); + + rcsc::Vector2D v1( 0.0, 0.0 ); + rcsc::Vector2D v2( -1.0, 1.0 ); + rcsc::Vector2D v3( 2.0, -3.0 ); + + m.transform( &v1 ); + m.transform( &v2 ); + m.transform( &v3 ); + + EXPECT_NEAR( rcsc::Vector2D( 0.0, 0.0 ).dist( v1 ), 0.0, EPS ); + EXPECT_NEAR( rcsc::Vector2D( 3.0, 1.0 ).dist( v2 ), 0.0, EPS ); + EXPECT_NEAR( rcsc::Vector2D( -6.0, -3.0 ).dist( v3 ), 0.0, EPS ); + } +} + +void testRotate() +{ + rcsc::AngleDeg angle = 90.0; + rcsc::Matrix2D m = rcsc::Matrix2D::make_rotation( angle ); + //rcsc::Matrix2D m; + //m.rotate( 90.0 ); + + rcsc::Vector2D v1( 0.0, 0.0 ); + rcsc::Vector2D v2( 1.0, 0.0 ); + rcsc::Vector2D v3( 0.0, 1.0 ); + rcsc::Vector2D v4( 2.0, 3.0 ); + + rcsc::Vector2D rv1 = v1.rotatedVector( angle ); + rcsc::Vector2D rv2 = v2.rotatedVector( angle ); + rcsc::Vector2D rv3 = v3.rotatedVector( angle ); + rcsc::Vector2D rv4 = v4.rotatedVector( angle ); + + m.transform( &v1 ); + m.transform( &v2 ); + m.transform( &v3 ); + m.transform( &v4 ); + + EXPECT_NEAR( rcsc::Vector2D( 0.0, 0.0 ).dist( v1 ), 0.0, EPS ); + EXPECT_NEAR( rcsc::Vector2D( 0.0, 1.0 ).dist( v2 ), 0.0, EPS ); + EXPECT_NEAR( rcsc::Vector2D( -1.0, 0.0 ).dist( v3 ), 0.0, EPS ); + EXPECT_NEAR( rcsc::Vector2D( -3.0, 2.0 ).dist( v4 ), 0.0, EPS ); + + EXPECT_NEAR( rv1.dist( v1 ), 0.0, EPS ); + EXPECT_NEAR( rv2.dist( v2 ), 0.0, EPS ); + EXPECT_NEAR( rv3.dist( v3 ), 0.0, EPS ); + EXPECT_NEAR( rv4.dist( v4 ), 0.0, EPS ); +} + +void testMultiplication() +{ + rcsc::Vector2D v( 3.5821, -292.23 ); + rcsc::Vector2D scale( 5.62, 92.092 ); + rcsc::Vector2D translate( 3.2, 5.4 ); + rcsc::AngleDeg rotate = 14.0; + + rcsc::Matrix2D m1; + m1.rotate( rotate ); + m1.translate( translate.x, translate.y ); + m1.scale( scale.x, scale.y ); + + rcsc::Matrix2D m2 + = rcsc::Matrix2D::make_scaling( scale.x, scale.y ) + * rcsc::Matrix2D::make_translation( translate.x, translate.y ) + * rcsc::Matrix2D::make_rotation( rotate ); + + EXPECT_NEAR( m1.transform( v ).dist( m2.transform( v ) ), 0.0, EPS ); +} + +#endif //PARSIAN_UTIL_TEST_MATRIX_2D_CPP_H diff --git a/parsian_util/test/geom/test_polygon_2d.h b/parsian_util/test/geom/test_polygon_2d.h new file mode 100644 index 00000000..192d38c7 --- /dev/null +++ b/parsian_util/test/geom/test_polygon_2d.h @@ -0,0 +1,437 @@ +// +// Created by raziyeh on 3/29/19. +// + +#ifndef PARSIAN_UTIL_TEST_H +#define PARSIAN_UTIL_TEST_H + +#include +#include +#include +#include "config.h" + +#include +#include + +#include +#include + +using namespace rcsc; + +void testEmpty() +{ + Polygon2D empty_polygon; + EXPECT_TRUE(!empty_polygon.contains(rcsc::Vector2D(0.0, 0.0))); +} + +void testPointPolygon(){ + const Vector2D p(+100.0, +100.0); + + std::vector v; + v.push_back(p); + + const rcsc::Polygon2D point_polygon(v); + + EXPECT_TRUE(!point_polygon.contains(rcsc::Vector2D(0.0, 0.0))); + + // strict checks + EXPECT_TRUE(point_polygon.contains(p)); + EXPECT_TRUE(!point_polygon.contains(p, false)); +} + +void testGetBoundingBox(){ + std::vector rect; + rect.emplace_back(rcsc::Vector2D(+200.0, +100.0)); + rect.emplace_back(rcsc::Vector2D(-200.0, +100.0)); + rect.emplace_back(rcsc::Vector2D(-200.0, -100.0)); + rect.emplace_back(rcsc::Vector2D(+200.0, -100.0)); + + const rcsc::Polygon2D rectangle(rect); + // + // getBoundingBox() + // + const rcsc::Rect2D r = rectangle.getBoundingBox(); + EXPECT_NEAR( -200.0 - r.minX(), 0.0, EPS ); + EXPECT_NEAR( +200.0 - r.maxX(), 0.0, EPS ); + EXPECT_NEAR( -100.0 - r.minY(), 0.0, EPS ); + EXPECT_NEAR( +100.0 - r.maxY(), 0.0, EPS ); +} + +void testContains1(){ + std::vector< rcsc::Vector2D > rect; + rect.emplace_back( rcsc::Vector2D( +200.0, +100.0 ) ); + rect.emplace_back( rcsc::Vector2D( -200.0, +100.0 ) ); + rect.emplace_back( rcsc::Vector2D( -200.0, -100.0 ) ); + rect.emplace_back( rcsc::Vector2D( +200.0, -100.0 ) ); + + const rcsc::Polygon2D rectangle( rect ); + + // + // contains + // + ASSERT_TRUE( rectangle.contains( rcsc::Vector2D( 0.0, 0.0 ) ) ); + ASSERT_TRUE( rectangle.contains( rcsc::Vector2D( 50.0, 50.0 ) ) ); + ASSERT_TRUE( rectangle.contains( rcsc::Vector2D( 199.9, 99.9 ) ) ); + ASSERT_TRUE( rectangle.contains( rcsc::Vector2D( -199.9, - 99.9 ) ) ); + ASSERT_TRUE( !rectangle.contains( rcsc::Vector2D( 200.1, 100.1 ) ) ); + ASSERT_TRUE( !rectangle.contains( rcsc::Vector2D( -200.1, -100.1 ) ) ); + ASSERT_TRUE( !rectangle.contains( rcsc::Vector2D( +500.0, +500.0 ) ) ); + ASSERT_TRUE( !rectangle.contains( rcsc::Vector2D( 0.0, +500.0 ) ) ); +} + +void testContains2(){ + // + // contains 2 + // + std::vector< rcsc::Vector2D > tri; + tri.emplace_back( rcsc::Vector2D( -200.0, -100.0 ) ); + tri.emplace_back( rcsc::Vector2D( 0.0, +100.0 ) ); + tri.emplace_back( rcsc::Vector2D( +200.0, -100.0 ) ); + + const rcsc::Polygon2D triangle( tri ); + + ASSERT_TRUE( triangle.contains( rcsc::Vector2D( 0.0, 0.0 ) ) ); + ASSERT_TRUE( !triangle.contains( rcsc::Vector2D( 0.0, -300.0 ) ) ); + ASSERT_TRUE( !triangle.contains( rcsc::Vector2D( 0.1, -300.0 ) ) ); +} + +void testContains3(){ + // + // contains 3 + // + std::vector< rcsc::Vector2D > tri2; + tri2.emplace_back( rcsc::Vector2D( 0.0, 0.0 ) ); + tri2.emplace_back( rcsc::Vector2D( 100.0, 100.0 ) ); + tri2.emplace_back( rcsc::Vector2D( 0.0, 200.0 ) ); + + const rcsc::Polygon2D triangle2( tri2 ); + + ASSERT_TRUE( !triangle2.contains( rcsc::Vector2D( -100.0, 100.0 ) ) ); + ASSERT_TRUE( triangle2.contains( rcsc::Vector2D( 50.0, 100.0 ) ) ); +} + +void testContains4(){ + // + // contains 4 + // + std::vector< rcsc::Vector2D > tri3; + tri3.emplace_back( rcsc::Vector2D( 0.0, 0.0 ) ); + tri3.emplace_back( rcsc::Vector2D( 100.0, 100.0 ) ); + tri3.emplace_back( rcsc::Vector2D( 100.0, 100.0 ) ); + tri3.emplace_back( rcsc::Vector2D( 0.0, 200.0 ) ); + + const rcsc::Polygon2D triangle3( tri3 ); + + ASSERT_TRUE( !triangle3.contains( rcsc::Vector2D( -100.0, 100.0 ) ) ); +} + +void testContains5(){ + // + // contains 5 + // + std::vector< rcsc::Vector2D > tri4; + tri4.emplace_back( rcsc::Vector2D( 0.0, 0.0 ) ); + tri4.emplace_back( rcsc::Vector2D( 100.0, 100.0 ) ); + tri4.emplace_back( rcsc::Vector2D( 100.0, 100.0 ) ); + tri4.emplace_back( rcsc::Vector2D( 100.0, 100.0 ) ); + tri4.emplace_back( rcsc::Vector2D( 0.0, 200.0 ) ); + + const rcsc::Polygon2D triangle4( tri4 ); + + ASSERT_TRUE( !triangle4.contains( rcsc::Vector2D( -100.0, 100.0 ) ) ); +} + +void testContains6(){ + // + // contains 6 + // + std::vector< rcsc::Vector2D > rect; + rect.emplace_back( rcsc::Vector2D( 0, 0 ) ); + rect.emplace_back( rcsc::Vector2D( 10, 0 ) ); + rect.emplace_back( rcsc::Vector2D( 10, 10 ) ); + rect.emplace_back( rcsc::Vector2D( 0, 10 ) ); + + const rcsc::Polygon2D r( rect ); + + ASSERT_TRUE( ! r.contains( rcsc::Vector2D( -100, 0 ) ) ); +} + +void testContains7(){ + // + // contains (grid) + // + std::vector< rcsc::Vector2D > rect; + rect.emplace_back( rcsc::Vector2D( 0, 0 ) ); + rect.emplace_back( rcsc::Vector2D( 10, 0 ) ); + rect.emplace_back( rcsc::Vector2D( 10, 10 ) ); + rect.emplace_back( rcsc::Vector2D( 0, 10 ) ); + + const rcsc::Polygon2D r( rect ); + + int count = 0; + + for ( int x = -100; x <= +100; ++x ) + { + for ( int y = -100; y <= +100; ++y ) + { + if ( 0 <= x && x <= 10 + && 0 <= y && y <= 10 ) + { + continue; + } + + if ( r.contains( rcsc::Vector2D( x, y ) ) ) + { + ++count; + } + } + } + + ASSERT_EQ( 0, count ); +} + +void testContains8(){ + // + // contains + // + std::vector< rcsc::Vector2D > v; + v.emplace_back( rcsc::Vector2D( 100, 100 ) ); + v.emplace_back( rcsc::Vector2D( 200, 100 ) ); + v.emplace_back( rcsc::Vector2D( 200, 500 ) ); + + const rcsc::Polygon2D tri( v ); + + // // + // po1 // + // // + // po2 p5 // + // /| // + // / | // + // / | // + // / | // + // / | // + // / | // + // po3 p7 p1 p6 // + // / | // + // po4 p4---p2--p3 // + // // + // po5 // + + rcsc::Vector2D p1( 150, 150 ); + rcsc::Vector2D p2( 150, 100 ); + rcsc::Vector2D p3( 200, 100 ); + rcsc::Vector2D p4( 100, 100 ); + rcsc::Vector2D p5( 200, 500 ); + rcsc::Vector2D p6( 200, 150 ); + rcsc::Vector2D p7( 200, 150 ); + + rcsc::Vector2D po1( 50, 600 ); + rcsc::Vector2D po2( 50, 500 ); + rcsc::Vector2D po3( 50, 150 ); + rcsc::Vector2D po4( 50, 100 ); + rcsc::Vector2D po5( 50, 0 ); + + + ASSERT_TRUE( tri.contains( p1 ) ); + ASSERT_TRUE( tri.contains( p1, false ) ); + + ASSERT_TRUE( tri.contains( p2 ) ); + ASSERT_TRUE( !tri.contains( p2, false ) ); + + ASSERT_TRUE( tri.contains( p3 ) ); + ASSERT_TRUE( !tri.contains( p3, false ) ); + + ASSERT_TRUE( tri.contains( p4 ) ); + ASSERT_TRUE( !tri.contains( p4, false ) ); + + ASSERT_TRUE( tri.contains( p5 ) ); + ASSERT_TRUE( !tri.contains( p5, false ) ); + + ASSERT_TRUE( tri.contains( p6 ) ); + ASSERT_TRUE( !tri.contains( p6, false ) ); + + ASSERT_TRUE( tri.contains( p7 ) ); + ASSERT_TRUE( !tri.contains( p7, false ) ); + + + ASSERT_TRUE( !tri.contains( po1 ) ); + ASSERT_TRUE( !tri.contains( po1, false ) ); + + ASSERT_TRUE( !tri.contains( po2 ) ); + ASSERT_TRUE( !tri.contains( po2, false ) ); + + ASSERT_TRUE( !tri.contains( po3 ) ); + ASSERT_TRUE( !tri.contains( po3, false ) ); + + ASSERT_TRUE( !tri.contains( po4 ) ); + ASSERT_TRUE( !tri.contains( po4, false ) ); + + ASSERT_TRUE( !tri.contains( po5 ) ); + ASSERT_TRUE( !tri.contains( po5, false ) ); +} + +void testEmptyArea(){ + // + // empty area + // + std::vector< rcsc::Vector2D > a0; + a0.emplace_back( rcsc::Vector2D( 100.0, 100.0 ) ); + a0.emplace_back( rcsc::Vector2D( 100.0, 100.0 ) ); + a0.emplace_back( rcsc::Vector2D( 100.0, 100.0 ) ); + a0.emplace_back( rcsc::Vector2D( 100.0, 100.0 ) ); + a0.emplace_back( rcsc::Vector2D( 100.0, 100.0 ) ); + + const rcsc::Polygon2D area_1( a0 ); + + a0.emplace_back( rcsc::Vector2D( 100.0, 100.0 ) ); + const rcsc::Polygon2D area_2( a0 ); + + + ASSERT_TRUE( !area_1.contains( rcsc::Vector2D( 0.0, 0.0 ) ) ); + ASSERT_TRUE( !area_2.contains( rcsc::Vector2D( 0.0, 0.0 ) ) ); + + // strict checks + ASSERT_TRUE( area_1.contains( rcsc::Vector2D( 100.0, 100.0 ) ) ); + ASSERT_TRUE( !area_1.contains( rcsc::Vector2D( 100.0, 100.0 ), false ) ); + + // strict checks + ASSERT_TRUE( area_2.contains( rcsc::Vector2D( 100.0, 100.0 ) ) ); + ASSERT_TRUE( !area_2.contains( rcsc::Vector2D( 100.0, 100.0 ), false ) ); +} + +void testScissoring(){ + // + // scissoring + // + const rcsc::Rect2D rectangle( rcsc::Vector2D( -100, +100 ), + rcsc::Size2D( /* length of x */ 200, /* length of y */ 200 ) ); + + // // + // (200,200) // + // +---------+ // + // | | // + // -100 | | // + // +100 +----|----+ | // + // | | | | // + // | | | | // + // | +---------+ // + // | (0,0) | // + // | | // + // -100 +---------+ // + // // + + std::vector< rcsc::Vector2D > v; + v.emplace_back( rcsc::Vector2D( 0, 0 ) ); + v.emplace_back( rcsc::Vector2D( 200, 0 ) ); + v.emplace_back( rcsc::Vector2D( 200, 200 ) ); + v.emplace_back( rcsc::Vector2D( 0, 200 ) ); + v.emplace_back( rcsc::Vector2D( 0, 0 ) ); + + const rcsc::Polygon2D polygon( v ); + + const rcsc::Polygon2D result = polygon.getScissoredConnectedPolygon( rectangle ); + + EXPECT_NEAR( 10000.0 - result.area(), 0.0, EPS ); + + const rcsc::Rect2D bbox = result.getBoundingBox(); + + EXPECT_NEAR( 0.0 - bbox.minX(), 0.0, EPS ); + EXPECT_NEAR( 100.0 - bbox.maxX(), 0.0, EPS ); + EXPECT_NEAR( 0.0 - bbox.minY(), 0.0, EPS ); + EXPECT_NEAR( 100.0 - bbox.maxY(), 0.0, EPS ); +} + +void testGetDistance(){ + // + // get_distance + // + std::vector< rcsc::Vector2D > rect; + rect.emplace_back( rcsc::Vector2D( 0, 0 ) ); + rect.emplace_back( rcsc::Vector2D( 10, 0 ) ); + rect.emplace_back( rcsc::Vector2D( 10, 10 ) ); + rect.emplace_back( rcsc::Vector2D( 0, 10 ) ); + + const rcsc::Polygon2D r( rect ); + + // out of polygon + EXPECT_NEAR( 1.0 - r.dist( rcsc::Vector2D( 11.0, 10.0 ) ), 0.0, EPS ); + + // in polygon, check as plane + EXPECT_NEAR( 0.0 - r.dist( rcsc::Vector2D( 5.0, 5.0 ) ), 0.0, EPS ); + + // in polygon, check as polyline + EXPECT_NEAR( 5.0 - r.dist( rcsc::Vector2D( 5.0, 5.0 ), false ), 0.0, EPS ); +} + +void testXYCenter(){ + // + // area, xyCenter + // + std::vector< rcsc::Vector2D > rect; + rect.emplace_back( rcsc::Vector2D( 10, 10 ) ); + rect.emplace_back( rcsc::Vector2D( 20, 10 ) ); + rect.emplace_back( rcsc::Vector2D( 20, 20 ) ); + rect.emplace_back( rcsc::Vector2D( 10, 20 ) ); + + const rcsc::Polygon2D r( rect ); + + EXPECT_NEAR( +100.0 - r.area(), 0.0, EPS ); + + EXPECT_NEAR( rcsc::Vector2D( 15.0, 15.0 ).dist( r.xyCenter() ), 0.0, EPS ); +} + +void testSignedArea2(){ + // + // counter clockwise/clockwise, doubleSignedArea + // + std::vector< rcsc::Vector2D > points; + const rcsc::Polygon2D empty(points); + + points.emplace_back( rcsc::Vector2D( 10, 10 ) ); + const rcsc::Polygon2D point(points); + + points.emplace_back( rcsc::Vector2D( 20, 10 ) ); + const rcsc::Polygon2D line(points); + + points.emplace_back( rcsc::Vector2D( 20, 20 ) ); + const rcsc::Polygon2D triangle(points); + + points.emplace_back( rcsc::Vector2D( 10, 20 ) ); + const rcsc::Polygon2D rectangle(points); + + + ASSERT_EQ( false, empty.isCounterclockwise() ); + ASSERT_EQ( false, empty.isClockwise() ); + + ASSERT_EQ( false, point.isCounterclockwise() ); + ASSERT_EQ( false, point.isClockwise() ); + + ASSERT_EQ( false, line.isCounterclockwise() ); + ASSERT_EQ( false, line.isClockwise() ); + + ASSERT_EQ( true , triangle.isCounterclockwise() ); + ASSERT_EQ( false, triangle.isClockwise() ); + + ASSERT_EQ( true , triangle.isCounterclockwise() ); + ASSERT_EQ( false, triangle.isClockwise() ); + + + std::vector< rcsc::Vector2D > r_points; + r_points.emplace_back( rcsc::Vector2D( 10, 20 ) ); + r_points.emplace_back( rcsc::Vector2D( 20, 20 ) ); + r_points.emplace_back( rcsc::Vector2D( 20, 10 ) ); + const rcsc::Polygon2D r_triangle(r_points); + + r_points.emplace_back( rcsc::Vector2D( 10, 10 ) ); + const rcsc::Polygon2D r_rectangle(r_points); + + ASSERT_EQ( false, r_triangle.isCounterclockwise() ); + ASSERT_EQ( true , r_triangle.isClockwise() ); + + ASSERT_EQ( false, r_rectangle.isCounterclockwise() ); + ASSERT_EQ( true , r_rectangle.isClockwise() ); +} + + +#endif //PARSIAN_UTIL_TEST_H + diff --git a/parsian_util/test/geom/test_rect_2d.h b/parsian_util/test/geom/test_rect_2d.h new file mode 100644 index 00000000..25b81b01 --- /dev/null +++ b/parsian_util/test/geom/test_rect_2d.h @@ -0,0 +1,32 @@ +// +// Created by raziyeh on 3/29/19. +// + +#ifndef PARSIAN_UTIL_TEST_RECT_2D_H +#define PARSIAN_UTIL_TEST_RECT_2D_H + +#include +#include "config.h" + +#include +#include + +using namespace rcsc; + +void testSet() +{ + rcsc::Rect2D r( rcsc::Vector2D( 0.0, 0.0 ), + rcsc::Size2D( 10.0, 10.0 ) ); + rcsc::Rect2D r1 = r; + + r1.setTopLeft( -5.0, -5.0 ); + EXPECT_NEAR( -5.0, r1.left(), EPS ); + EXPECT_NEAR( 5.0, r1.right(), EPS ); + EXPECT_NEAR( -5.0, r1.top(), EPS ); + EXPECT_NEAR( -15.0, r1.bottom(), EPS ); + EXPECT_NEAR( 10.0, r1.size().length(), EPS ); + EXPECT_NEAR( 10.0, r1.size().width(), EPS ); + +} + +#endif //PARSIAN_UTIL_TEST_RECT_2D_H diff --git a/parsian_util/test/geom/test_segment_2d.h b/parsian_util/test/geom/test_segment_2d.h new file mode 100644 index 00000000..e6318841 --- /dev/null +++ b/parsian_util/test/geom/test_segment_2d.h @@ -0,0 +1,510 @@ +// +// Created by raziyeh on 3/29/19. +// + +#ifndef PARSIAN_UTIL_TEST_SEGMENT_2D_H +#define PARSIAN_UTIL_TEST_SEGMENT_2D_H + +#include +#include "config.h" + +#include +#include + +using namespace rcsc; + +void testLength() +{ + // + // check length of segment + // + const Segment2D s1( Vector2D( 0.0, 0.0 ), + Vector2D( 3.0, 4.0 ) ); + + EXPECT_NEAR( 5.0 - s1.length(), 0.0, EPS ); +} + +void testIntersection() +{ + const double delta = 1.0e-6; + const Segment2D segment( Vector2D( 0.0, 0.0 ), + Vector2D( 2.0, 0.0 ) ); + + // + Vector2D result; + Segment2D s( Vector2D( 0.0, 0.0 ), Vector2D( 0.0, 0.0 ) ); +//1 + s.assign( Vector2D( 0.0, 0.0 ), 2.0, AngleDeg( -90.0 ) ); + result = segment.intersection( s ); + ASSERT_TRUE( result.isValid() ); + EXPECT_NEAR( result.dist( segment.origin() ), 0.0, EPS ); + EXPECT_NEAR( result.dist( s.origin() ), 0.0, EPS ); +//2 + s.assign( Vector2D( 0.0, 1.0 ), 2.0, AngleDeg( -90.0 ) ); + result = segment.intersection( s ); + ASSERT_TRUE( result.isValid() ); + EXPECT_NEAR( result.dist( segment.origin() ), 0.0, EPS ); + EXPECT_NEAR( result.dist( s.origin() ), 1.0, EPS ); +//3 + s.assign( Vector2D( 0.0, 2.0 ), 2.0, AngleDeg( -90.0 ) ); + result = segment.intersection( s ); + ASSERT_TRUE( result.isValid() ); + EXPECT_NEAR( result.dist( segment.origin() ), 0.0, EPS ); + EXPECT_NEAR( result.dist( s.origin() ), 2.0, EPS ); +//4 + s.assign( Vector2D( 1.0, 0.0 ), std::sqrt( 2.0 ) * 2.0, AngleDeg( 45.0 ) ); + result = segment.intersection( s ); + ASSERT_TRUE( result.isValid() ); + EXPECT_NEAR( result.dist( segment.origin() ), 1.0, EPS ); + EXPECT_NEAR( result.dist( s.origin() ), 0.0, EPS ); +//5 + s.assign( Vector2D( 0.0, -1.0 ), std::sqrt( 2.0 ) * 2.0, AngleDeg( 45.0 ) ); + result = segment.intersection( s ); + ASSERT_TRUE( result.isValid() ); + EXPECT_NEAR( result.dist( segment.origin() ), 1.0, EPS ); + EXPECT_NEAR( result.dist( s.origin() ), std::sqrt( 2.0 ), EPS ); +//6 + s.assign( Vector2D( -1.0, -2.0 ), std::sqrt( 2.0 ) * 2.0, AngleDeg( 45.0 ) ); + result = segment.intersection( s ); + ASSERT_TRUE( result.isValid() ); + EXPECT_NEAR( result.dist( segment.origin() ), 1.0, EPS ); + EXPECT_NEAR( result.dist( s.origin() ), std::sqrt( 2.0 ) * 2.0, EPS ); +//7 + s.assign( Vector2D( 2.0, 0.0 ), 2.0, AngleDeg( 90.0 ) ); + result = segment.intersection( s ); + ASSERT_TRUE( result.isValid() ); + EXPECT_NEAR( result.dist( segment.origin() ), 2.0, EPS ); + EXPECT_NEAR( result.dist( s.origin() ), 0.0, EPS ); +//8 + s.assign( Vector2D( 2.0, -1.0 ), 2.0, AngleDeg( 90.0 ) ); + result = segment.intersection( s ); + ASSERT_TRUE( result.isValid() ); + EXPECT_NEAR( result.dist( segment.origin() ), 2.0, EPS ); + EXPECT_NEAR( result.dist( s.origin() ), 1.0, EPS ); +//9 + s.assign( Vector2D( 2.0, -2.0 ), 2.0, AngleDeg( 90.0 ) ); + result = segment.intersection( s ); + ASSERT_TRUE( result.isValid() ); + EXPECT_NEAR( result.dist( segment.origin() ), 2.0, EPS ); + EXPECT_NEAR( result.dist( s.origin() ), 2.0, EPS ); +//10 + s.assign( Vector2D( 0.0, -delta * 2.0 ), 2.0, AngleDeg( -90.0 ) ); + result = segment.intersection( s ); + ASSERT_TRUE( result.isValid() ); +//11 + s = Segment2D( Vector2D( -delta * 2.0, 1.0 ), 2.0, AngleDeg( -90.0 ) ); + result = segment.intersection( s ); + ASSERT_TRUE( result.isValid() ); +//12 + s.assign( Vector2D( 0.0, 2.0 ), 2.0 - delta * 2.0, AngleDeg( -90.0 ) ); + result = segment.intersection( s ); + ASSERT_TRUE( result.isValid() ); +//13 + s.assign( Vector2D( 1.0 + delta * 2.0, delta * 2.0 ), std::sqrt( 2.0 ) * 2.0, AngleDeg( 45.0 ) ); + result = segment.intersection( s ); + ASSERT_TRUE( result.isValid() ); +//14 + s.assign( Vector2D( -1.0, -2.0 ), std::sqrt( 2.0 ) * 2.0 - delta * 2.0, AngleDeg( 45.0 ) ); + result = segment.intersection( s ); + ASSERT_TRUE( result.isValid() ); +//15 + s.assign( Vector2D( 2.0, delta * 2.0 ), 2.0, AngleDeg( 90.0 ) ); + result = segment.intersection( s ); + ASSERT_TRUE( result.isValid() ); +//16 + s.assign( Vector2D( 2.0 + delta * 2.0, -1.0 ), 2.0, AngleDeg( 90.0 ) ); + result = segment.intersection( s ); + ASSERT_TRUE( result.isValid() ); +//17 + s.assign( Vector2D( 2.0, -2.0 ), 2.0 - delta * 2.0, AngleDeg( 90.0 ) ); + result = segment.intersection( s ); + ASSERT_TRUE( result.isValid() ); +} + +void testExistIntersectionExceptTerminalPoint() +{ + // + // check existIntersectionExceptTerminalPoint() + // + const rcsc::Segment2D s1( rcsc::Vector2D( 0.0, 0.0 ), + rcsc::Vector2D( 3.0, 4.0 ) ); + + const rcsc::Segment2D s2( rcsc::Vector2D( 0.0, 2.0 ), + rcsc::Vector2D( 5.0, 2.0 ) ); + + ASSERT_TRUE( s1.existIntersectionExceptEndpoint( s2 ) ); + ASSERT_TRUE( s2.existIntersectionExceptEndpoint( s1 ) ); + + ASSERT_TRUE( s1.intersection( s2 ).isValid() ); + ASSERT_TRUE( s2.intersection( s1 ).isValid() ); + + // const rcsc::Segment2D s3( rcsc::Vector2D( 100.0, 200.0 ), + // rcsc::Vector2D( 300.0, 400.0 ) ); + // const rcsc::Segment2D s3( rcsc::Vector2D( s1.origin().x - 1.0, s1.origin().y + 1.0 ), + // rcsc::Vector2D( s1.origin().x + 1.0, s1.origin().y - 1.0 ) ); + const rcsc::Segment2D s3( rcsc::Vector2D( s1.terminal().x - 1.0, s1.terminal().y + 1.0 ), + rcsc::Vector2D( s1.terminal().x + 1.0, s1.terminal().y - 1.0 ) ); + + ASSERT_TRUE( ! s3.existIntersectionExceptEndpoint( s1 ) ); + ASSERT_TRUE( ! s3.existIntersectionExceptEndpoint( s2 ) ); + ASSERT_TRUE( ! s1.existIntersectionExceptEndpoint( s3 ) ); + ASSERT_TRUE( ! s2.existIntersectionExceptEndpoint( s3 ) ); + + ASSERT_TRUE( s3.intersection( s1 ).isValid() ); + ASSERT_TRUE( ! s3.intersection( s2 ).isValid() ); + ASSERT_TRUE( s1.intersection( s3 ).isValid() ); + ASSERT_TRUE( ! s2.intersection( s3 ).isValid() ); + + + // 2 segments on a line + const rcsc::Segment2D s1_2( rcsc::Vector2D( 6.0, 8.0 ), + rcsc::Vector2D( 9.0, 12.0 ) ); + + ASSERT_TRUE( ! s1.existIntersectionExceptEndpoint( s1_2 ) ); + ASSERT_TRUE( ! s1_2.existIntersectionExceptEndpoint( s1 ) ); + + ASSERT_TRUE( ! s1.intersection( s1_2 ).isValid() ); + ASSERT_TRUE( ! s1_2.intersection( s1 ).isValid() ); + + + const rcsc::Segment2D s4( rcsc::Vector2D( -100.0, 4.0 ), + rcsc::Vector2D( +100.0, 4.0 ) ); + + ASSERT_TRUE( s1.existIntersection( s4 ) ); + ASSERT_TRUE( s4.existIntersection( s1 ) ); + + ASSERT_TRUE( s1.intersection( s4 ).isValid() ); + ASSERT_TRUE( s4.intersection( s1 ).isValid() ); +} + +void testExistIntersection() +{ + // + // check existIntersection() + // + const rcsc::Segment2D t1( rcsc::Vector2D( 100, 100 ), + rcsc::Vector2D( 0, 200 ) ); + + const rcsc::Segment2D t2( rcsc::Vector2D( -100, 200 ), + rcsc::Vector2D( 600, 200 ) ); + + ASSERT_TRUE( t1.existIntersection( t2 ) ); + ASSERT_TRUE( t2.existIntersection( t1 ) ); + + ASSERT_TRUE( t1.intersection( t2 ).isValid() ); + ASSERT_TRUE( t2.intersection( t1 ).isValid() ); +} + +void testExistIntersectionAtTerminalPoints() +{ + // existIntersection at terminal points + const rcsc::Segment2D t1( rcsc::Vector2D( -200.0, -100.0 ), + rcsc::Vector2D( 0.0, +100.0 ) ); + + const rcsc::Segment2D t2( rcsc::Vector2D( 0.0, +100.0 ), + rcsc::Vector2D( +200.0, -100.0 ) ); + + const rcsc::Segment2D t_check( rcsc::Vector2D( 0.0, -300.0 ), + rcsc::Vector2D( 0.0, +900.0 ) ); + + ASSERT_TRUE( t1.existIntersection( t_check ) ); + ASSERT_TRUE( t_check.existIntersection( t1 ) ); + + ASSERT_TRUE( t1.intersection( t_check ).isValid() ); + ASSERT_TRUE( t_check.intersection( t1 ).isValid() ); + + // + + ASSERT_TRUE( t2.existIntersection( t_check ) ); + ASSERT_TRUE( t_check.existIntersection( t2 ) ); + + ASSERT_TRUE( t2.intersection( t_check ).isValid() ); + ASSERT_TRUE( t_check.intersection( t2 ).isValid() ); +} + +void testIntersectsAtTerminalPoints() +{ + // intersects at terminal points + const rcsc::Segment2D t1( rcsc::Vector2D( 200, 100 ), + rcsc::Vector2D( 2000, 100 ) ); + + const rcsc::Segment2D t2( rcsc::Vector2D( 200, 100 ), + rcsc::Vector2D( 200, 500 ) ); + + ASSERT_TRUE( t1.existIntersection( t2 ) ); + ASSERT_TRUE( t2.existIntersection( t1 ) ); + + ASSERT_TRUE( t1.intersection( t2 ).isValid() ); + ASSERT_TRUE( t2.intersection( t1 ).isValid() ); +} + +void testIntersectsAtTerminalPointsParallelHorizontal() +{ + // intersects at terminal points (parallel, horizontal) + const rcsc::Segment2D t1( rcsc::Vector2D( +200, +100 ), + rcsc::Vector2D( +500, +100 ) ); + + const rcsc::Segment2D t2( rcsc::Vector2D( +200, +100 ), + rcsc::Vector2D( -100, +100 ) ); + + ASSERT_TRUE( t1.existIntersection( t2 ) ); + ASSERT_TRUE( t2.existIntersection( t1 ) ); + + ASSERT_TRUE( ! t1.intersection( t2 ).isValid() ); + ASSERT_TRUE( ! t2.intersection( t1 ).isValid() ); +} + +void testIntersectsAtTerminalPointsParallelVertical() +{ + // intersects with terminal points (parallel, vertical) + const rcsc::Segment2D t1( rcsc::Vector2D( +100, +200 ), + rcsc::Vector2D( +100, +500 ) ); + + const rcsc::Segment2D t2( rcsc::Vector2D( +100, +200 ), + rcsc::Vector2D( +100, -100 ) ); + + ASSERT_TRUE( t1.existIntersection( t2 ) ); + ASSERT_TRUE( t2.existIntersection( t1 ) ); + + EXPECT_NEAR( t1.dist( t2 ), 0.0, EPS ); + EXPECT_NEAR( t2.dist( t1 ), 0.0, EPS ); + + ASSERT_TRUE( ! t1.intersection( t2 ).isValid() ); + ASSERT_TRUE( ! t2.intersection( t1 ).isValid() ); +} + +void testIntersectWithPointSegment() +{ + // intersect with point segment 1 + { + const rcsc::Segment2D t1( rcsc::Vector2D( 0, 0 ), + rcsc::Vector2D( 0, +500 ) ); + + const rcsc::Segment2D t2( rcsc::Vector2D( +100, +500 ), + rcsc::Vector2D( +100, +500 ) ); + + ASSERT_TRUE( ! t1.existIntersection( t2 ) ); + ASSERT_TRUE( ! t2.existIntersection( t1 ) ); + + ASSERT_TRUE( ! t1.intersection( t2 ).isValid() ); + ASSERT_TRUE( ! t2.intersection( t1 ).isValid() ); + } + + // intersect with point segment 2 + { + const rcsc::Segment2D t1( rcsc::Vector2D( +500, +500 ), + rcsc::Vector2D( +500, +500 ) ); + + const rcsc::Segment2D t2( rcsc::Vector2D( +300, +500 ), + rcsc::Vector2D( +200, +400 ) ); + + + ASSERT_TRUE( ! t1.existIntersection( t2 ) ); + ASSERT_TRUE( ! t2.existIntersection( t1 ) ); + + ASSERT_TRUE( ! t1.intersection( t2 ).isValid() ); + ASSERT_TRUE( ! t2.intersection( t1 ).isValid() ); + } + + // intersect with point segment 3 + { + const rcsc::Segment2D t1( rcsc::Vector2D( +500, +500 ), + rcsc::Vector2D( +500, +500 ) ); + + const rcsc::Segment2D t2( rcsc::Vector2D( +300, +300 ), + rcsc::Vector2D( +300, +300 ) ); + + + ASSERT_TRUE( ! t1.existIntersection( t2 ) ); + ASSERT_TRUE( ! t2.existIntersection( t1 ) ); + + ASSERT_TRUE( t1.existIntersection( t1 ) ); + ASSERT_TRUE( t2.existIntersection( t2 ) ); + + ASSERT_TRUE( ! t1.intersection( t2 ).isValid() ); + ASSERT_TRUE( ! t2.intersection( t1 ).isValid() ); + } + + // intersect with point segment 4 + { + const rcsc::Segment2D t1( rcsc::Vector2D( +500, +500 ), + rcsc::Vector2D( +500, +500 ) ); + + const rcsc::Segment2D t2( rcsc::Vector2D( 0, +500 ), + rcsc::Vector2D( +100, +500 ) ); + + + ASSERT_TRUE( ! t1.existIntersection( t2 ) ); + ASSERT_TRUE( ! t2.existIntersection( t1 ) ); + + ASSERT_TRUE( ! t1.intersection( t2 ).isValid() ); + ASSERT_TRUE( ! t2.intersection( t1 ).isValid() ); + } + + // intersect with point segment 5 + { + const rcsc::Segment2D t1( rcsc::Vector2D( +500, +500 ), + rcsc::Vector2D( +500, +500 ) ); + + const rcsc::Segment2D t2( rcsc::Vector2D( +500, 0 ), + rcsc::Vector2D( +500, +100 ) ); + + ASSERT_TRUE( ! t1.existIntersection( t2 ) ); + ASSERT_TRUE( ! t2.existIntersection( t1 ) ); + + ASSERT_TRUE( ! t1.intersection( t2 ).isValid() ); + ASSERT_TRUE( ! t2.intersection( t1 ).isValid() ); + } +} + +void testNearestPoint() +{ + // + // check nearestPoint() + // + const rcsc::Vector2D s1( -500, 100 ); + const rcsc::Vector2D s2( +500, 100 ); + + const rcsc::Segment2D s( s1, s2 ); + + EXPECT_NEAR( rcsc::Vector2D( 0.0, 100.0 ).dist( s.nearestPoint( rcsc::Vector2D( 0.0, 0.0 ) ) ), + 0.0, + EPS ); + + EXPECT_NEAR( rcsc::Vector2D( 200.0, 100.0 ).dist( s.nearestPoint( rcsc::Vector2D( 200.0, 0.0 ) ) ), + 0.0, + EPS ); + + for ( long i = 0 ; i < 100000 ; i += 10 ) + { + const rcsc::Vector2D p( i, +500 ); + + rcsc::Vector2D c; + + if ( i <= 500 ) + { + c = s.nearestPoint( +p ); + EXPECT_NEAR( rcsc::Vector2D( (+p).x, 100 ).dist( c ), 0.0, EPS ); + + c = s.nearestPoint( -p ); + EXPECT_NEAR( rcsc::Vector2D( (-p).x, 100 ).dist( c ), 0.0, EPS ); + } + else + { + c = s.nearestPoint( +p ); + EXPECT_NEAR( s2.dist( c ), 0.0, EPS ); + + c = s.nearestPoint( -p ); + EXPECT_NEAR( s1.dist( c ), 0.0, EPS ); + } + } +} + +void testDistanceFromPoint() +{ + // + // check distance of segment and point + // + const rcsc::Segment2D seg1( rcsc::Vector2D( -100.0, 0.0 ), + rcsc::Vector2D( 0.0, 0.0 ) ); + const rcsc::Segment2D seg2( rcsc::Vector2D( 0.0, 0.0 ), + rcsc::Vector2D( -100.0, 0.0 ) ); + + const rcsc::Vector2D p( 400.0, 300.0 ); + + EXPECT_NEAR( 500.0 - seg1.dist( p ), 0.0, EPS ); + EXPECT_NEAR( 500.0 - seg2.dist( p ), 0.0, EPS ); +} + +void testDistanceFromPointOnLine() +{ + // distance from point (segment and point are on a line) + const rcsc::Segment2D seg( rcsc::Vector2D( -100, 0.0 ), + rcsc::Vector2D( +100, 0.0 ) ); + + const rcsc::Vector2D p( +150.0, 0.0 ); + + EXPECT_NEAR( 50.0 - seg.dist( p ), 0.0, EPS ); + EXPECT_NEAR( 250.0 - seg.farthestDist( p ), 0.0, EPS ); +} + +void testDistanceFromPointComplex() +{ + // distance from point (complex) + const rcsc::Vector2D s1( -100, 0 ); + const rcsc::Vector2D s2( +100, 0 ); + + const rcsc::Segment2D seg( s1, s2 ); + + const rcsc::Vector2D p1( 0, +150 ); + + EXPECT_NEAR( 150.0 - seg.dist( +p1 ), 0.0, EPS ); + EXPECT_NEAR( 150.0 - seg.dist( -p1 ), 0.0, EPS ); + + const rcsc::Vector2D p2( 300, 0 ); + EXPECT_NEAR( 200.0 - seg.dist( +p2 ), 0.0, EPS ); + EXPECT_NEAR( 200.0 - seg.dist( -p2 ), 0.0, EPS ); + + const rcsc::Vector2D p3( 20000, 0 ); + EXPECT_NEAR( 19900.0 - seg.dist( +p3 ), 0.0, EPS ); + EXPECT_NEAR( 19900.0 - seg.dist( -p3 ), 0.0, EPS ); + + for ( long i = 0 ; i < 100000 ; i += 10 ) + { + const rcsc::Vector2D p( i, +500 ); + + if ( i <= 100 ) + { + EXPECT_NEAR( 500.0 - seg.dist( +p ), 0.0, EPS ); + + EXPECT_NEAR( 500.0 - seg.dist( -p ), 0.0, EPS ); + } + else + { + EXPECT_NEAR( (s2 - p).r() - seg.dist( +p ), 0.0, EPS ); + + EXPECT_NEAR( (s1 - (-p)).r() - seg.dist( -p ), 0.0, EPS ); + } + } +} + +void testDistanceFromSegment() +{ + // + // distance segment and segment + // + const rcsc::Segment2D seg1( rcsc::Vector2D( +100.0, 100.0 ), + rcsc::Vector2D( -100.0, 100.0 ) ); + + const rcsc::Segment2D seg2( rcsc::Vector2D( 0.0, 300.0 ), + rcsc::Vector2D( +100.0, 400.0 ) ); + + EXPECT_NEAR( 200.0 - seg1.dist( seg2 ), 0.0, EPS ); + EXPECT_NEAR( 200.0 - seg2.dist( seg1 ), 0.0, EPS ); +} + +void testOnSegmentStrictly() +{ + { + rcsc::Segment2D s( rcsc::Vector2D( 0.0, 0.0 ), rcsc::Vector2D( 0.0, 10.0 ) ); + ASSERT_TRUE( s.onSegment( rcsc::Vector2D( 0.0, 5.0 ) ) ); + ASSERT_TRUE( ! s.onSegment( rcsc::Vector2D( 1.0e-7, 0.0 ) ) ); + } + + { + rcsc::Segment2D s( rcsc::Vector2D( 0.0, 0.0 ), + rcsc::Vector2D( 10.0, 10.0 ) ); + ASSERT_TRUE( s.onSegment( rcsc::Vector2D( 5.0, 5.0 ) ) ); + ASSERT_TRUE( ! s.onSegment( rcsc::Vector2D( 6.0, 6.0 + 1.0e-7 ) ) ); + } + + { + rcsc::Segment2D s( rcsc::Vector2D( 3.148595, 582.2 ), + rcsc::Vector2D( -1838.235, 23.21145 ) ); + rcsc::Vector2D dir = s.terminal() - s.origin(); + dir.normalize(); + ASSERT_TRUE( ! s.onSegment( s.origin() + dir * 2.462134 ) ); + } +} + +#endif //PARSIAN_UTIL_TEST_SEGMENT_2D_H diff --git a/parsian_util/test/geom/test_triangle_2d.h b/parsian_util/test/geom/test_triangle_2d.h new file mode 100644 index 00000000..d6ba221b --- /dev/null +++ b/parsian_util/test/geom/test_triangle_2d.h @@ -0,0 +1,100 @@ +// +// Created by raziyeh on 3/30/19. +// + +#ifndef PARSIAN_UTIL_TEST_TRIANGLE_2D_H +#define PARSIAN_UTIL_TEST_TRIANGLE_2D_H + +#include +#include "config.h" + +#include +#include + +using namespace rcsc; + +void testSignedArea() +{ + // + // basic checks + // + { + const rcsc::Vector2D p1( 0.0, 0.0 ); + const rcsc::Vector2D p2( 3.0, 0.0 ); + const rcsc::Vector2D p3( 3.0, 4.0 ); + + const rcsc::Triangle2D t1( p1, p2, p3 ); + const rcsc::Triangle2D t2( p3, p2, p1 ); + + EXPECT_NEAR( + 6.0 - t1.signedArea(), 0.0, EPS ); + EXPECT_NEAR( +12.0 - t1.doubleSignedArea(), 0.0, EPS ); + + EXPECT_NEAR( - 6.0 - t2.signedArea(), 0.0, EPS ); + EXPECT_NEAR( -12.0 - t2.doubleSignedArea(), 0.0, EPS ); + } + + + // + // points on a line + // + { + const rcsc::Vector2D p1( -100, 200 ); + const rcsc::Vector2D p2( 600, 200 ); + const rcsc::Vector2D p3( 0, 200 ); + + const rcsc::Triangle2D tri( p1, p2, p3 ); + + // should be EXACTRY equal to 0 + EXPECT_NEAR( tri.doubleSignedArea(), 0.0, EPS ); + } + + + // + // same 2 points + // + { + const rcsc::Vector2D p1( -100, 200 ); + const rcsc::Vector2D p2( + 50, 100 ); + + const rcsc::Triangle2D tri1( p1, p1, p2 ); + const rcsc::Triangle2D tri2( p1, p2, p1 ); + const rcsc::Triangle2D tri3( p2, p1, p1 ); + + // should be EXACTRY equal to 0 + EXPECT_NEAR( tri1.doubleSignedArea(), 0.0, EPS ); + EXPECT_NEAR( tri2.doubleSignedArea(), 0.0, EPS ); + EXPECT_NEAR( tri3.doubleSignedArea(), 0.0, EPS ); + } + + + // + // same 3 points + // + { + const rcsc::Vector2D p( -100, 200 ); + + const rcsc::Triangle2D tri( p, p, p ); + + // should be EXACTRY equal to 0 + EXPECT_NEAR( tri.doubleSignedArea(), 0.0, EPS ); + } +} + + +void testCentroid() +{ + { + rcsc::Vector2D p1( 5.1245, 9.1038 ); + rcsc::Vector2D p2( 3.0, -5.6978 ); + rcsc::Vector2D p3( 3.0, 4.0 ); + + rcsc::Triangle2D tri( p1, p2, p3 ); + + + rcsc::Vector2D c = p1 + p2 + p3; + + EXPECT_NEAR( c.r() - tri.centroid().r() * 3.0, 0.0, EPS ); + } +} + +#endif //PARSIAN_UTIL_TEST_TRIANGLE_2D_H diff --git a/parsian_util/test/geom/test_vector_2d.h b/parsian_util/test/geom/test_vector_2d.h new file mode 100644 index 00000000..f0597157 --- /dev/null +++ b/parsian_util/test/geom/test_vector_2d.h @@ -0,0 +1,116 @@ +// +// Created by raziyeh on 3/30/19. +// + +#ifndef PARSIAN_UTIL_TEST_VECTOR_2D_H +#define PARSIAN_UTIL_TEST_VECTOR_2D_H + +#include +#include "config.h" + +#include +#include + +using namespace rcsc; + + +bool in_distance( const double & x, + const double & y ) +{ + return std::fabs( x - y ) < DISTANCE; +} + +bool in_distance2( const double & x, + const double & y ) +{ + return std::fabs( x - y ) < DISTANCE * DISTANCE; +} + +void testAssign() +{ + // + const Vector2D p0; + ASSERT_TRUE( in_distance( p0.x, 5000.0 ) ); + ASSERT_TRUE( in_distance( p0.y, 5000.0 ) ); + + // + const Vector2D p1( 1.0, -2.0 ); + ASSERT_TRUE( in_distance( p1.x, 1.0 ) ); + ASSERT_TRUE( in_distance( p1.y, -2.0 ) ); + + // + const Vector2D p2( -3.5, 4.5 ); + + // + const Vector2D p3 = p2; + ASSERT_TRUE( in_distance( p3.x, -3.5 ) ); + ASSERT_TRUE( in_distance( p3.y, 4.5 ) ); + + // + Vector2D p4; + p4 = p2; + ASSERT_TRUE( in_distance( p4.x, -3.5 ) ); + ASSERT_TRUE( in_distance( p4.y, 4.5 ) ); +} + +void testDistance() +{ + // + const Vector2D p0; + //ASSERT_TRUE( in_distance( p0.dist( Vector2D::ORIGIN ), 0.0 ) ); + ASSERT_TRUE( in_distance( p0.dist( ERROR_VALUE ), 0.0 ) ); + ASSERT_TRUE( in_distance( p0.dist( Vector2D() ), 0.0 ) ); + //ASSERT_TRUE( in_distance2( p0.dist2( Vector2D::ORIGIN ), 0.0 ) ); + ASSERT_TRUE( in_distance2( p0.dist2( ERROR_VALUE ), 0.0 ) ); + ASSERT_TRUE( in_distance2( p0.dist2( Vector2D() ), 0.0 ) ); + + // + const Vector2D p1( 1.0, -2.0 ); + //ASSERT_TRUE( in_distance( p1.dist( Vector2D::ORIGIN ), std::sqrt( 5.0 ) ) ); + ASSERT_TRUE( in_distance( p1.dist( ZERO ), std::sqrt( 5.0 ) ) ); + //ASSERT_TRUE( in_distance2( p1.dist2( Vector2D::ORIGIN ), 5.0 ) ); + ASSERT_TRUE( in_distance2( p1.dist2( ZERO ), 5.0 ) ); + // + const Vector2D p2( 4.0, 2.0 ); + ASSERT_TRUE( in_distance( p2.dist( p1 ), 5.0 ) ); + ASSERT_TRUE( in_distance( p2.dist2( p1 ), 25.0 ) ); +} + +void testEquals() +{ + // + const Vector2D p0; + //ASSERT_TRUE( p0 == Vector2D::ORIGIN ); + ASSERT_TRUE( p0 == ERROR_VALUE ); + ASSERT_TRUE( p0 == Vector2D() ); + ASSERT_TRUE( p0 != Vector2D( DISTANCE * 2.0, DISTANCE * 2.0 ) ); + + // + const Vector2D p1( 1.0, -2.0 ); + //ASSERT_TRUE( p1 != Vector2D::ORIGIN ); + ASSERT_TRUE( p1 != ZERO ); + ASSERT_TRUE( p1 == p1 ); + ASSERT_TRUE( p1 == Vector2D( 1.0, -2.0 ) ); + ASSERT_TRUE( p1 == Vector2D( 1.0 + DISTANCE3 * 2.0, -2.0 + DISTANCE3 * 2.0 ) ); + ASSERT_TRUE( p1 != Vector2D( 1.0 + DISTANCE2 * 2.0, -2.0 + DISTANCE2 * 2.0 ) ); +} + + +void testRotateVector() +{ + const Vector2D v( 1.0, 1.0 ); + const AngleDeg rot = -30.0; + + std::cerr << '\n'; + + Vector2D v1 = v.rotatedVector( rot ); + std::cerr << "v1=" << v1 << " th=" << v1.th() << std::endl; + EXPECT_NEAR( ( v.th() + rot ).degree(), v1.th().degree(), 1.0e-5 ); + + Vector2D v2( v.x * rot.cos() - v.y * rot.sin(), + v.x * rot.sin() + v.y * rot.cos() ); + std::cerr << "v2=" << v2 << " th=" << v2.th() << std::endl; + EXPECT_NEAR( ( v.th() + rot ).degree(), v2.th().degree(), 1.0e-5 ); +} + +#endif //PARSIAN_UTIL_TEST_VECTOR_2D_H diff --git a/parsian_util/test/geom/test_voronoi_diagram.h b/parsian_util/test/geom/test_voronoi_diagram.h new file mode 100644 index 00000000..28d53dbc --- /dev/null +++ b/parsian_util/test/geom/test_voronoi_diagram.h @@ -0,0 +1,179 @@ +// +// Created by raziyeh on 3/30/19. +// +/* +#ifndef PARSIAN_UTIL_TEST_VORONOI_DIAGRAM_H +#define PARSIAN_UTIL_TEST_VORONOI_DIAGRAM_H + +#include +#include +#include "test_consts.h" + +#include +#include + +using namespace rcsc; + + +void testEmptyVoronoi() +{ + VoronoiDiagram v; + + ASSERT_EQ( static_cast< size_t >( 0 ), v.vertices().size() ); + ASSERT_EQ( static_cast< size_t >( 0 ), v.segments().size() ); + ASSERT_EQ( static_cast< size_t >( 0 ), v.rays().size() ); + + v.compute(); + + ASSERT_EQ( static_cast< size_t >( 0 ), v.vertices().size() ); + ASSERT_EQ( static_cast< size_t >( 0 ), v.segments().size() ); + ASSERT_EQ( static_cast< size_t >( 0 ), v.rays().size() ); +} + +void testVoronoi() +{ + const Vector2D p0( 0.0, 0.0 ); + const Vector2D p1( +10.0, +10.0 ); + const Vector2D p2( -10.0, +10.0 ); + const Vector2D p3( -10.0, -10.0 ); + const Vector2D p4( +10.0, -10.0 ); + + const Vector2D p5( +20.0, 0.0 ); + const Vector2D p6( 0.0, +20.0 ); + const Vector2D p7( -20.0, 0.0 ); + const Vector2D p8( 0.0, -20.0 ); + + // + // input points + // + + // // + // | // + // +20 *p6 // + // | // + // | // + // p2 | p1 // + // +10 * | * // + // | // + // | // + // | // + // p7 | // + // 0 --*---------*---------*-- // + // |p0 p5 // + // | // + // | // + // | // + // -10 * | * // + // p3 | p4 // + // | // + // | // + // | // + // -20 *p8 // + // | // + // // + // -20 -10 0 +10 +20 // + + + std::cerr << "\ninput points=\n " + << p0 << "\n " + << p1 << "\n " + << p2 << "\n " + << p3 << "\n " + << p4 << "\n " + << p5 << "\n " + << p6 << "\n " + << p7 << "\n " + << p8 << std::endl; + + VoronoiDiagram v; + + v.addPoint( p0 ); + v.addPoint( p1 ); + v.addPoint( p2 ); + v.addPoint( p3 ); + v.addPoint( p4 ); + v.addPoint( p5 ); + v.addPoint( p6 ); + v.addPoint( p7 ); + v.addPoint( p8 ); + + v.compute(); + + + // + // result + // + + // \ / // + // \ | / // + // +20 \ *p6 / // + // \ | / // + // \ \ | / / // + // \ p2 \|/ p1 / // + // +10 \ * . * / // + // \ /|\ / // + // \ / | \ / // + // \ / | \ / // + // p7 \ / | \ / // + // 0 --*----.----*----.----*-- // + // / \ |p0 / \ p5 // + // / \ | / \ // + // / \ | / \ // + // / \|/ \ // + // -10 / * . * \ // + // / p3 /|\ p4 \ // + // / / | \ \ // + // / | \ // + // / | \ // + // -20 / *p8 \ // + // / | \ // + // / \ // + // -20 -10 0 +10 +20 // + + + // + // check points + // + int n_points = 0; + for ( VoronoiDiagram::Vector2DCont::const_iterator p = v.vertices().begin(), + end = v.vertices().end(); + p != end; + ++p ) + { + n_points ++; + + } + + + // + // check segments + // + int n_segments = 0; + for ( VoronoiDiagram::Segment2DCont::const_iterator s = v.segments().begin(), + end = v.segments().end(); + s != end; + ++s ) + { + n_segments ++; + + } + ASSERT_EQ( 4, n_segments ); + + + // + // check rays + // + int n_rays = 0; + for ( VoronoiDiagram::Ray2DCont::const_iterator r = v.rays().begin(), + end = v.rays().end(); + r != end; + ++r ) + { + n_rays ++; + } + ASSERT_EQ( 8, n_rays ); +} + + +#endif //PARSIAN_UTIL_TEST_VORONOI_DIAGRAM_H +*/ \ No newline at end of file diff --git a/parsian_util/test/geom/utest.cpp b/parsian_util/test/geom/utest.cpp new file mode 100644 index 00000000..4c6580db --- /dev/null +++ b/parsian_util/test/geom/utest.cpp @@ -0,0 +1,93 @@ +#include +#include +#include +#include "test_polygon_2d.h" +#include "test_matrix_2d.h" +#include "test_rect_2d.h" +#include "test_segment_2d.h" +#include "test_triangle_2d.h" +#include "test_vector_2d.h" +#include "test_voronoi_diagram.h" + +#include +#include + +#include +#include + +using namespace rcsc; + +// Declare a test +TEST(Polygon2DTest, polygon2DTest) { + testEmpty(); + testPointPolygon(); + testGetBoundingBox(); + testContains1(); + testContains2(); + testContains3(); + testContains4(); + testContains5(); + testContains6(); + testContains7(); + testContains8(); + testEmptyArea(); + testScissoring(); + testGetDistance(); + testXYCenter(); + testSignedArea2(); +} + +TEST(TestMatrix2D, testMatrix2D){ + testTranslate(); + testScale(); + testRotate(); + testMultiplication(); +} + +TEST(TestRect2D, testRect2D){ + testSet (); + //TODO add intersection test +} + +TEST(TestSegment2D,testSegment2D){ + testLength(); + testIntersection(); + testExistIntersectionExceptTerminalPoint(); + testExistIntersection(); + testExistIntersectionAtTerminalPoints(); + testIntersectsAtTerminalPoints(); + testIntersectsAtTerminalPointsParallelHorizontal(); + testIntersectsAtTerminalPointsParallelVertical(); + testIntersectWithPointSegment(); + testNearestPoint(); + testDistanceFromPoint(); + testDistanceFromPointOnLine(); + testDistanceFromPointComplex(); + testDistanceFromSegment(); + testOnSegmentStrictly(); +} + +TEST(TestTraingle2D, testTraingle2D){ + testSignedArea(); + testCentroid(); +} + +TEST(TestVector2D, testVector2D){ + testAssign(); + testDistance(); + testEquals(); + testRotateVector(); +} +/* +TEST(TestVoronoiDiagram, testVoronoiDiagram){ + testEmptyVoronoi(); + testVoronoi(); +} +*/ +// Run all the tests that were declared with TEST( +int main(int argc, char **argv){ + testing::InitGoogleTest(&argc, argv); + ros::init(argc, argv, "tester"); + ros::NodeHandle nh; + return RUN_ALL_TESTS(); +} diff --git a/rqt_parsian_gui/CMakeLists.txt b/rqt_parsian_gui/CMakeLists.txt index 6543baa7..7493fe1e 100644 --- a/rqt_parsian_gui/CMakeLists.txt +++ b/rqt_parsian_gui/CMakeLists.txt @@ -85,8 +85,11 @@ set (${PROJECT_NAME}_SRCS src/${PROJECT_NAME}/mode_chooser.cpp src/${PROJECT_NAME}/modeChooserWidget.cpp src/${PROJECT_NAME}/handycontroller.cpp - src/${PROJECT_NAME}/playoffWidget.cpp - src/${PROJECT_NAME}/playoff.cpp + src/${PROJECT_NAME}/playoff/playoffWidget.cpp + src/${PROJECT_NAME}/playoff/tabWidget.cpp + src/${PROJECT_NAME}/playoff/planLabel.cpp + src/${PROJECT_NAME}/playoff/plansView.cpp + src/${PROJECT_NAME}/playoff/playoff.cpp src/${PROJECT_NAME}/taskRunner.cpp src/${PROJECT_NAME}/taskRunnerWidget.cpp src/${PROJECT_NAME}/referee.cpp @@ -99,8 +102,11 @@ set(${PROJECT_NAME}_HDRS include/${PROJECT_NAME}/mode_chooser.h include/${PROJECT_NAME}/modeChooserWidget.h include/${PROJECT_NAME}/handycontroller.h - include/${PROJECT_NAME}/playoffWidget.h - include/${PROJECT_NAME}/playoff.h + include/${PROJECT_NAME}/playoff/playoffWidget.h + include/${PROJECT_NAME}/playoff/tabWidget.h + include/${PROJECT_NAME}/playoff/planLabel.h + include/${PROJECT_NAME}/playoff/plansView.h + include/${PROJECT_NAME}/playoff/playoff.h include/${PROJECT_NAME}/taskRunner.h include/${PROJECT_NAME}/taskRunnerWidget.h include/${PROJECT_NAME}/referee.h diff --git a/rqt_parsian_gui/include/rqt_parsian_gui/graphical/graphical.h b/rqt_parsian_gui/include/rqt_parsian_gui/graphical/graphical.h index fa9065da..8cd9b174 100644 --- a/rqt_parsian_gui/include/rqt_parsian_gui/graphical/graphical.h +++ b/rqt_parsian_gui/include/rqt_parsian_gui/graphical/graphical.h @@ -52,6 +52,8 @@ namespace rqt_parsian_gui { ros::Subscriber draw_sub; ros::Subscriber log_draw_sub; ros::Subscriber color_sub; + ros::Publisher mouse_evetPub; + ros::Timer timer; diff --git a/rqt_parsian_gui/include/rqt_parsian_gui/graphical/soccerview.h b/rqt_parsian_gui/include/rqt_parsian_gui/graphical/soccerview.h index 8e882957..4cbe7c7a 100644 --- a/rqt_parsian_gui/include/rqt_parsian_gui/graphical/soccerview.h +++ b/rqt_parsian_gui/include/rqt_parsian_gui/graphical/soccerview.h @@ -39,6 +39,7 @@ #include #include #include +#include #include #ifndef SOCCERVIEW_H @@ -138,6 +139,7 @@ namespace rqt_parsian_gui { ros::ServiceClient* ballClinet; ros::ServiceClient* robotsClinet; + ros::Publisher* mouse_evetPub; private: void drawFieldLines(FieldDimensions &dimensions); @@ -206,6 +208,8 @@ namespace rqt_parsian_gui { void setBallReplceService(ros::ServiceClient& _client); void setRobotsReplceService(ros::ServiceClient& _client); + void setmousePublisher(ros::Publisher &_publisher); + public slots: diff --git a/rqt_parsian_gui/include/rqt_parsian_gui/playoff.h b/rqt_parsian_gui/include/rqt_parsian_gui/playoff.h deleted file mode 100644 index cb4c14ab..00000000 --- a/rqt_parsian_gui/include/rqt_parsian_gui/playoff.h +++ /dev/null @@ -1,30 +0,0 @@ -// -// Created by parsian-ai on 12/31/17. -// - -#ifndef RQT_PARSIAN_GUI_PLAYOFF_H -#define RQT_PARSIAN_GUI_PLAYOFF_H -#include -#include -#include -#include -#include - -namespace rqt_parsian_gui { - - -class PlayOff : public rqt_gui_cpp::Plugin { - Q_OBJECT -public: - PlayOff(); - virtual void initPlugin(qt_gui_cpp::PluginContext& context); - -private: - ros::NodeHandle n; - ros::NodeHandle n_private; - boost::shared_ptr playOffWidget; - -}; -} // namespace rqt_example_cpp - -#endif //RQT_PARSIAN_GUI_PLAYOFF_H diff --git a/rqt_parsian_gui/include/rqt_parsian_gui/playoff/planLabel.h b/rqt_parsian_gui/include/rqt_parsian_gui/playoff/planLabel.h new file mode 100644 index 00000000..55b09245 --- /dev/null +++ b/rqt_parsian_gui/include/rqt_parsian_gui/playoff/planLabel.h @@ -0,0 +1,62 @@ +#ifndef RQT_PLANLABELWIDGET_H +#define RQT_PLANLABELWIDGET_H + + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + + +namespace rqt_parsian_gui +{ + + class PlanLabel : public QWidget { + Q_OBJECT + public: + explicit PlanLabel(bool _put_options); + virtual ~PlanLabel(); + void create_plan(QString _plan, bool _isActive, bool _isMaster); + void setServerUpdateService(ros::ServiceClient& _client); + QString plan; + bool put_options; + bool isActive; + bool isMaster; + + public slots: + void activatepressed(); + void deacvtivateressed(); + void masterpressed(); + + protected: + + private: + ros::ServiceClient* server_update; + + + //stylesheet + QFile File; + QString FormStyleSheet; + QString resourcePath; + //widgets + QHBoxLayout* main; + QWidget* main_widget; + QGridLayout* main_layout; + QPushButton* activate; + QPushButton* deactivate; + QPushButton* master; + QLabel* planName; + + + }; +} + +#endif //RQT_PLANLABELWIDGET_H diff --git a/rqt_parsian_gui/include/rqt_parsian_gui/playoff/plansView.h b/rqt_parsian_gui/include/rqt_parsian_gui/playoff/plansView.h new file mode 100644 index 00000000..8213e434 --- /dev/null +++ b/rqt_parsian_gui/include/rqt_parsian_gui/playoff/plansView.h @@ -0,0 +1,45 @@ +#ifndef RQT_PLANSVIEWWIDGET_H +#define RQT_PLANSVIEWWIDGET_H + + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + + + +namespace rqt_parsian_gui +{ + + class PlansView : public QScrollArea + { + Q_OBJECT + public: + explicit PlansView(QScrollArea *parent = 0); + void add_contact(QString username, bool isActive, bool isMaster, bool no_option = true); + void setServerUpdateService(ros::ServiceClient& _client); + void sort(); + void clear_all(); + + + private: + ros::ServiceClient* server_update; + //create ZpContactList widget + QList contacts_list; + QVBoxLayout* contacts_list_layout; + QWidget* contact_list_widget; + QWidget* filler; + void resizeEvent(QResizeEvent*); + + }; +} + +#endif //RQT_PLANSVIEWWIDGET_H diff --git a/rqt_parsian_gui/include/rqt_parsian_gui/playoff/playoff.h b/rqt_parsian_gui/include/rqt_parsian_gui/playoff/playoff.h new file mode 100644 index 00000000..58708cc0 --- /dev/null +++ b/rqt_parsian_gui/include/rqt_parsian_gui/playoff/playoff.h @@ -0,0 +1,40 @@ +/* + Copyright 2016 Lucas Walter +*/ + + +#ifndef RQT_PLAYOFF_H +#define RQT_PLAYOFF_H + +#include +#include +#include +#include +#include +#include +#include + + +namespace rqt_parsian_gui +{ + + class PlayOff: public rqt_gui_cpp::Plugin + { + Q_OBJECT + public: + + PlayOff(); + virtual void initPlugin(qt_gui_cpp::PluginContext& context); + virtual void shutdownPlugin(); + void sub(const parsian_msgs::parsian_playoff_clientConstPtr& _msg); + + + private: + ros::NodeHandle n; + ros::NodeHandle n_private; + PlayOffWidget* playoffWidget; + ros::ServiceClient server_update; + ros::Subscriber subscriber; + }; +} +#endif // RQT_PLAYOFF_H diff --git a/rqt_parsian_gui/include/rqt_parsian_gui/playoff/playoffWidget.h b/rqt_parsian_gui/include/rqt_parsian_gui/playoff/playoffWidget.h new file mode 100644 index 00000000..2317e257 --- /dev/null +++ b/rqt_parsian_gui/include/rqt_parsian_gui/playoff/playoffWidget.h @@ -0,0 +1,84 @@ +#ifndef RQT_PLAYOFFWIDGET_H +#define RQT_PLAYOFFWIDGET_H + + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + + + +namespace rqt_parsian_gui +{ + + class PlayOffWidget : public QWidget { + Q_OBJECT + public: + explicit PlayOffWidget(); + virtual ~PlayOffWidget(); + void create_main_widget(); + void setServerUpdateService(ros::ServiceClient& _client); + void subscribe(const parsian_msgs::parsian_playoff_clientConstPtr &msg); + + + + public slots: + void all_pressed(); + void active_pressed(); + void ignored_pressed(); + + void activate_all_pressed(); + void deactivate_all_pressed(); + void demaster_pressed(); + + void subscribe_slot(); + + + signals: + void subscribe_sig(); + + protected: + + private: + ros::ServiceClient* server_update; + QGridLayout* main_layout; + PlansView* all_plansView; + PlansView* active_plansView; + PlansView* ignored_plansView; + QWidget* prev_view; + QWidget* plansView_widg; + QVBoxLayout* plansView_lay; + + TabWidget* tabWidget; + QLabel* last_plan_title; + PlanLabel* last_plan; + QLabel* master_plan_title; + PlanLabel* master_plan; + QPushButton* activate_all; + QPushButton* deactivate_all; + QPushButton* demaster; + + std::vector allPlan; + std::vector activePlan; + std::vector ignoredPlan; + std::string masterPlan; + std::string lastPlan; + + + + + }; +} + +#endif //RQT_PLAYOFFWIDGET_H diff --git a/rqt_parsian_gui/include/rqt_parsian_gui/playoff/tabWidget.h b/rqt_parsian_gui/include/rqt_parsian_gui/playoff/tabWidget.h new file mode 100644 index 00000000..eff3adac --- /dev/null +++ b/rqt_parsian_gui/include/rqt_parsian_gui/playoff/tabWidget.h @@ -0,0 +1,47 @@ +#ifndef RQT_TABWIDGETWIDGET_H +#define RQT_TABWIDGETWIDGET_H + + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + + + +namespace rqt_parsian_gui +{ + + class TabWidget : public QScrollArea + { + Q_OBJECT + public: + explicit TabWidget(QScrollArea *parent = 0); + void add_contact(QString name); + void sort(); + QPushButton* get_contact(QString name); + + + private: + //stylesheet + QFile File; + QString FormStyleSheet; + QString resourcePath; + //create ZpContactList widget + QList contacts_list; + QVBoxLayout* contacts_list_layout; + QWidget* contact_list_widget; + QWidget* filler; + void resizeEvent(QResizeEvent*); + + }; +} + +#endif //RQT_TABWIDGETWIDGET_H diff --git a/rqt_parsian_gui/include/rqt_parsian_gui/playoffWidget.h b/rqt_parsian_gui/include/rqt_parsian_gui/playoffWidget.h deleted file mode 100644 index 09f68b0e..00000000 --- a/rqt_parsian_gui/include/rqt_parsian_gui/playoffWidget.h +++ /dev/null @@ -1,71 +0,0 @@ -// -// Created by parsian-ai on 12/31/17. -// - -#ifndef RQT_PARSIAN_GUI_PLAYOFF_WIDGET_H -#define RQT_PARSIAN_GUI_PLAYOFF_WIDGET_H - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -namespace rqt_parsian_gui { -class PlayOffWidget : public QWidget { - - Q_OBJECT - -public: - PlayOffWidget(ros::NodeHandle & n); - -protected: - - QPushButton *mode; - QPushButton *active; - QPushButton *update; - QPushButton *master; - QPushButton *deactive; - - QColumnView *columns; - - bool debugMode = true; - QList > activeGUI; - QList > masterGUI; - - QStandardItemModel *model; - QItemSelectionModel *selection; - - QList *fileList; - QList *planList; - - parsian_msgs::parsian_update_plans *theplans; - ros::ServiceClient client ; - - QLabel *details[8]; - - parsian_msgs::parsian_plan_GUI *lastPlan; - parsian_msgs::parsian_plan_GUI *chosen; - QItemSelection itemSelected; - -private Q_SLOTS: - void updateModel(); - void updateBtn(bool _debug); - -public Q_SLOTS: - void slt_changeMode(); - void slt_updatePlans(); - void slt_active(); - void slt_deactive(); - void slt_edit(QStandardItem *); - void slt_master(); - void slt_selectionChanged(const QItemSelection &, const QItemSelection &); - -}; - -} -#endif //RQT_PARSIAN_GUI_PLAYOFF_H diff --git a/rqt_parsian_gui/include/rqt_parsian_gui/taskRunnerWidget.h b/rqt_parsian_gui/include/rqt_parsian_gui/taskRunnerWidget.h index 4da5dace..4822c35e 100644 --- a/rqt_parsian_gui/include/rqt_parsian_gui/taskRunnerWidget.h +++ b/rqt_parsian_gui/include/rqt_parsian_gui/taskRunnerWidget.h @@ -6,7 +6,8 @@ // Created by noOne on 10/19/17. // -#define _MAX_NUM_PLAYERS 12 +#define _PLAYER_NUMBER 12 +#define _TASK_NUM 4 #include #include @@ -14,39 +15,51 @@ #include #include #include +#include #include #include #include #include +#include +#include + namespace rqt_parsian_gui { - #define TASK_NUM 3 - static const char* taskNames[TASK_NUM] = {"Ball Placement","GotoPointAvoid","GotoPoint"}; + + static const char* taskNames[_TASK_NUM] = {"GotoPointAvoid","Kick","Receive","OneTouch"}; class TaskRunnerWidget:public QWidget { Q_OBJECT public: ros::Timer timer; + QTimer* wmTimer; TaskRunnerWidget(ros::NodeHandle & n); virtual ~TaskRunnerWidget(); public slots: - void setTask(QAction*); + void comboChange(QString); void setID(QAction * ); + protected: private: int agent_id; - parsian_msgs::grsim_ball_replacement *client ; + bool rightSet,leftSet,setData; + ros::Subscriber worldModelSub; ros::Subscriber mousePosSub; ros::ServiceClient ballReplacementClient; - ros::Publisher *robTaskPub; + ros::Publisher robTaskPub[_PLAYER_NUMBER]; parsian_msgs::parsian_robot_taskPtr task; QAction ** tasks, **ids; QToolButton *toolButton,*agentId; + QComboBox *comboBoxPN , *comboBoxTask; QGridLayout *gridLayout; - void mousePosCallBack(parsian_msgs::vector2DConstPtr); - void timerCb(const ros::TimerEvent& _timer); + void mousePosCallBack(parsian_msgs::mouse_eventConstPtr msg); + //void timerCb(const ros::TimerEvent& _timer); + void m_wmCb(const parsian_msgs::parsian_world_modelConstPtr& _wm); + signals: + void startwmtimer(int); + void stopwmtimer(); }; } diff --git a/rqt_parsian_gui/resource/style_sheet/playoff_planLabel.qss b/rqt_parsian_gui/resource/style_sheet/playoff_planLabel.qss new file mode 100644 index 00000000..54374dc4 --- /dev/null +++ b/rqt_parsian_gui/resource/style_sheet/playoff_planLabel.qss @@ -0,0 +1,100 @@ +QWidget#main_widget { + background-color: #b3ccff; + border: 2px solid gray; +} + +QPushButton#activate_button:disabled { + background-color: #00ff00; + font: bold 20px; + border: 2px solid gray; + color: black; +} + +QPushButton#activate_button:enabled { + background-color: #f2f2f2; + font: bold 20px; + border: 2px solid gray; +} + +QPushButton#activate_button:enabled:hover { + background-color: #f2f2f2; + font: bold 30px; + border: 7px solid gray; +} + +QPushButton#activate_button::checked { + background-color: #b3ffd9; + font: bold 20px; + border: 2px solid gray; +} + + +QPushButton#deactivate_button:disabled { + background-color: #ff3300; + font: bold 20px; + border: 2px solid gray; + color: black; +} + +QPushButton#deactivate_button:enabled { + background-color: #f2f2f2; + font: bold 20px; + border: 2px solid gray; +} + +QPushButton#deactivate_button:enabled:hover { + background-color: #f2f2f2; + font: bold 30px; + border: 7px solid gray; +} + +QPushButton#deactivate_button::checked { + background-color: #b3ffd9; + font: bold 20px; + border: 2px solid gray; +} + + +QPushButton#master_button:disabled { + background-color: #66ffff; + font: bold 20px; + border: 2px solid gray; + color: black; +} + +QPushButton#master_button:enabled { + background-color: #f2f2f2; + font: bold 20px; + border: 2px solid gray; +} + +QPushButton#master_button:enabled:hover { + background-color: #f2f2f2; + font: bold 30px; + border: 7px solid gray; +} + +QPushButton#master_button::checked { + background-color: #b3ffd9; + font: bold 20px; + border: 2px solid gray; +} + +QPushButton#tab:enabled { + background-color: #f2f2f2; + font: bold 20px; + border: 2px solid gray; +} + +QPushButton#tab:enabled:hover { + background-color: #f2f2f2; + font: bold 30px; + border: 7px solid gray; +} + +QPushButton#tab::checked { + background-color: #b3ffd9; + font: bold 20px; + border: 2px solid gray; +} + diff --git a/rqt_parsian_gui/src/rqt_parsian_gui/graphical/graphical.cpp b/rqt_parsian_gui/src/rqt_parsian_gui/graphical/graphical.cpp index a0ed4d0c..55f0c936 100644 --- a/rqt_parsian_gui/src/rqt_parsian_gui/graphical/graphical.cpp +++ b/rqt_parsian_gui/src/rqt_parsian_gui/graphical/graphical.cpp @@ -27,6 +27,7 @@ namespace rqt_parsian_gui { log_draw_sub = n.subscribe("/log/draws", 1000, &GraphicalClient::logdrawCb, this); color_sub = n.subscribe("/team_config", 1000, &GraphicalClient::colorCb, this); timer = n.createTimer(ros::Duration(0.02), &GraphicalClient::timerCb, this); + mouse_evetPub = n.advertise("/mousePos", 1000); parsian_msgs::parsian_team_configPtr team_config{new parsian_msgs::parsian_team_config}; // access standalone command line arguments @@ -80,6 +81,7 @@ namespace rqt_parsian_gui { grsimRobots = n.serviceClient("/GrsimRobotReplacesrv"); view->setBallReplceService(grsimBall); view->setRobotsReplceService(grsimRobots); + view->setmousePublisher(mouse_evetPub); } void GraphicalClient::shutdownPlugin() { diff --git a/rqt_parsian_gui/src/rqt_parsian_gui/graphical/soccerview.cpp b/rqt_parsian_gui/src/rqt_parsian_gui/graphical/soccerview.cpp index de00223c..c67eae45 100644 --- a/rqt_parsian_gui/src/rqt_parsian_gui/graphical/soccerview.cpp +++ b/rqt_parsian_gui/src/rqt_parsian_gui/graphical/soccerview.cpp @@ -83,6 +83,7 @@ GLSoccerView::GLSoccerView(QWidget* parent) : debugs.reset(new parsian_msgs::parsian_draws()); debugs2.reset(new parsian_msgs::parsian_draws()); grayColor = false; + } void GLSoccerView::redraw() @@ -102,6 +103,21 @@ void GLSoccerView::mousePressEvent(QMouseEvent* event) QPointF mp = mouseToFieldPos(event->pos()); + //publish mouseevent + if(leftButton) + { + parsian_msgs::mouse_event msg; + msg.pos.x = mp.x(); msg.pos.y = mp.y(); + msg.isLeftClicked = true; + mouse_evetPub->publish(msg); + } + if(rightButton) + { + parsian_msgs::mouse_event msg; + msg.pos.x = mp.x(); msg.pos.y = mp.y(); + msg.isLeftClicked = false; + mouse_evetPub->publish(msg); + } if(leftButton) setCursor(Qt::ClosedHandCursor); if(midButton) { @@ -590,6 +606,10 @@ void GLSoccerView::setBallReplceService(ros::ServiceClient& _client) { ballClinet = &_client; } +void GLSoccerView::setmousePublisher(ros::Publisher &_publisher) { + mouse_evetPub = &_publisher; +} + double GLSoccerView::distVec(double x1, double y1, double x2, double y2) { return sqrt((x1-x2)*(x1-x2) + (y1-y2)*(y1-y2)); } diff --git a/rqt_parsian_gui/src/rqt_parsian_gui/playoff.cpp b/rqt_parsian_gui/src/rqt_parsian_gui/playoff.cpp deleted file mode 100644 index 50524f6f..00000000 --- a/rqt_parsian_gui/src/rqt_parsian_gui/playoff.cpp +++ /dev/null @@ -1,28 +0,0 @@ - -#include - -namespace rqt_parsian_gui { - -PlayOff::PlayOff() - : rqt_gui_cpp::Plugin() { - // Constructor is called first before initPlugin function, needless to say. - // give QObjects reasonable names - setObjectName("PlayOff"); -} - -void PlayOff::initPlugin(qt_gui_cpp::PluginContext &context) { - n = getNodeHandle(); - n_private = getPrivateNodeHandle(); - - - playOffWidget.reset(new PlayOffWidget(n)); - - // extend the widget with all attributes and children from UI file - - // add widget to the user interface - - context.addWidget(playOffWidget.get()); -} - -} // namespace rqt_parsian_gui -PLUGINLIB_EXPORT_CLASS(rqt_parsian_gui::PlayOff, rqt_gui_cpp::Plugin) diff --git a/rqt_parsian_gui/src/rqt_parsian_gui/playoff/planLabel.cpp b/rqt_parsian_gui/src/rqt_parsian_gui/playoff/planLabel.cpp new file mode 100644 index 00000000..e001553b --- /dev/null +++ b/rqt_parsian_gui/src/rqt_parsian_gui/playoff/planLabel.cpp @@ -0,0 +1,162 @@ +#include + + +namespace rqt_parsian_gui +{ + PlanLabel::PlanLabel(bool _put_options) : QWidget() + { + //getting style sheets + std::string path = ros::package::getPath("rqt_parsian_gui"); + resourcePath = QString::fromStdString(path); + resourcePath += "/resource/style_sheet/playoff_planLabel.qss"; + File.setFileName(resourcePath); + File.open(QFile::ReadOnly); + //ROS_INFO_STREAM("is qt PlanLabel_stylesheet opend:" <setStyleSheet(FormStyleSheet); + File.close(); + + //widget + this->plan = ""; + this->put_options = _put_options; + this->isActive = false; + this->isMaster = false; + this->main = new QHBoxLayout(this); + this->main_widget = new QWidget(this); + this->main_widget->setObjectName("main_widget"); + this->main_layout = new QGridLayout(this->main_widget); + if(this->put_options) + { + this->activate = new QPushButton("A", this); + this->activate->setObjectName("activate_button"); + this->activate->setSizePolicy(QSizePolicy::Preferred, QSizePolicy::Expanding); + connect(this->activate, SIGNAL(pressed()), this, SLOT(activatepressed())); + this->deactivate = new QPushButton("D", this); + this->deactivate->setObjectName("deactivate_button"); + this->deactivate->setSizePolicy(QSizePolicy::Preferred, QSizePolicy::Expanding); + connect(this->deactivate, SIGNAL(pressed()), this, SLOT(deacvtivateressed())); + this->master = new QPushButton("M", this); + this->master->setObjectName("master_button"); + this->master->setSizePolicy(QSizePolicy::Preferred, QSizePolicy::Expanding); + connect(this->master, SIGNAL(pressed()), this, SLOT(masterpressed())); + } + this->planName = new QLabel("No Plan", this); + this->planName->setObjectName("planName"); + + //margin and padding + this->setContentsMargins(0, 0, 0, 0); + this->setFixedHeight(50); + this->main->setContentsMargins(0, 0, 0, 0); + this->main->setMargin(0); + this->main->setSpacing(0); + this->main_widget->setContentsMargins(0, 0, 0, 0); + this->main_layout->setContentsMargins(0, 0, 0, 0); + this->main_layout->setMargin(0); + this->main_layout->setSpacing(0); + + } + + void PlanLabel::create_plan(QString _plan, bool _isActive, bool _isMaster) + { + this->plan = _plan; + this->isActive = _isActive; + this->isMaster = _isMaster; + this->planName->setText(" " + this->plan); + if(this->put_options) + { + this->main_layout->addWidget(this->planName, 0, 0, 1, 5); + this->main_layout->addWidget(this->activate, 0, 5, 1, 1); + this->main_layout->addWidget(this->deactivate, 0, 6, 1, 1); + this->main_layout->addWidget(this->master, 0, 7, 1, 1); + + if(this->isActive) + { + this->activate->setEnabled(false); + this->deactivate->setEnabled(true); + } + else + { + this->activate->setEnabled(true); + this->deactivate->setEnabled(false); + } + + if(this->isMaster) + { + this->master->setEnabled(false); + } + else + { + this->master->setEnabled(true); + } + this->activate->style()->unpolish(this->activate); + this->activate->style()->polish(this->activate); + this->deactivate->style()->unpolish(this->deactivate); + this->deactivate->style()->polish(this->deactivate); + this->master->style()->unpolish(this->master); + this->master->style()->polish(this->master); + } + else + { + this->main_layout->addWidget(this->planName, 0, 0, 1, 8); + } + this->main_widget->setLayout(this->main_layout); + this->main->addWidget(this->main_widget); + this->setLayout(this->main); + + } + + void PlanLabel::setServerUpdateService(ros::ServiceClient& _client) { + server_update = &_client; + } + + void PlanLabel::activatepressed() + { + if(this->server_update == nullptr || this->server_update == NULL) + return; + parsian_msgs::parsian_update_plansRequest req; + parsian_msgs::parsian_update_plansResponse rep; + req.Mode = 1;//ACTIVATE + req.Plans.push_back(this->plan.toStdString()); + server_update->call(req, rep); + } + + void PlanLabel::deacvtivateressed() + { + if(this->server_update == nullptr || this->server_update == NULL) + return; + parsian_msgs::parsian_update_plansRequest req; + parsian_msgs::parsian_update_plansResponse rep; + req.Mode = 2;//DEACTIVATE + req.Plans.push_back(this->plan.toStdString()); + if(server_update->call(req, rep)) + ROS_INFO("it did"); + else + ROS_INFO("it did not"); + } + + void PlanLabel::masterpressed() + { + if(this->server_update == nullptr || this->server_update == NULL) + return; + parsian_msgs::parsian_update_plansRequest req; + parsian_msgs::parsian_update_plansResponse rep; + req.Mode = 3;//MASTER + req.Plans.push_back(this->plan.toStdString()); + server_update->call(req, rep); + } + + PlanLabel::~PlanLabel() + { + delete this->planName; + delete this->master; + delete this->deactivate; + delete this->activate; + delete this->main_layout; + } + + +} + + + + diff --git a/rqt_parsian_gui/src/rqt_parsian_gui/playoff/plansView.cpp b/rqt_parsian_gui/src/rqt_parsian_gui/playoff/plansView.cpp new file mode 100644 index 00000000..cc9703d8 --- /dev/null +++ b/rqt_parsian_gui/src/rqt_parsian_gui/playoff/plansView.cpp @@ -0,0 +1,94 @@ +#include +namespace rqt_parsian_gui +{ + PlansView::PlansView(QScrollArea *parent) : QScrollArea(parent) + { + //layout + contacts_list_layout = new QVBoxLayout(this); + contacts_list_layout->setContentsMargins(0, 0, 0, 0); + contacts_list_layout->setSpacing(0); + contacts_list_layout->setMargin(0); + filler = new QWidget(this); + contacts_list_layout->addWidget(filler); + + //widget + contact_list_widget = new QWidget(this); + contact_list_widget->setObjectName("contactlist"); + contact_list_widget->setLayout(contacts_list_layout); + contact_list_widget->setContentsMargins(0, 0, 0, 0); + //contact_list_widget->setStyleSheet("QWidget#contactlist{background-color: white;}"); + + //scroll bar + this->setWidget(contact_list_widget); + this->setAlignment(Qt::AlignCenter); + this->setWidgetResizable(true); + this->setMinimumWidth(300); + this->setContentsMargins(0, 0, 0, 0); + + + } + + void PlansView::add_contact(QString plan, bool isActive, bool isMaster, bool put_option) + { +// //check if the username already exists +// for(int i{}; i < contacts_list.size(); i++) +// if(contacts_list[i]->plan == plan) +// return; + PlanLabel* new_contact = new PlanLabel(put_option); + new_contact->create_plan(plan, isActive, isMaster); + new_contact->setServerUpdateService(*server_update); + contacts_list_layout->addWidget(new_contact, 0, Qt::AlignTop); + contacts_list.push_back(new_contact); + contacts_list_layout->removeWidget(filler); + if(this->height() - contacts_list.size()*50 > 0) + filler->setFixedHeight(this->height() - contacts_list.size()*50); + else + filler->setFixedHeight(0); + contacts_list_layout->addWidget(filler); + } + void PlansView::clear_all() + { + for(const auto& contact : contacts_list) + { + contacts_list_layout->removeWidget(contact); + contact->hide(); + } + contacts_list_layout->removeWidget(filler); + contacts_list.clear(); + if(this->height() - contacts_list.size()*50 > 0) + filler->setFixedHeight(this->height() - contacts_list.size()*50); + else + filler->setFixedHeight(0); + contacts_list_layout->addWidget(filler); + } + + + void PlansView::sort() + { + for(const auto& contact : contacts_list) + contacts_list_layout->removeWidget(contact); + contacts_list_layout->removeWidget(filler); + std::sort(contacts_list.begin(), contacts_list.end()); + //update GUI + for(const auto& contact : contacts_list) + { + contacts_list_layout->addWidget(contact, 0, Qt::AlignTop); + contact->setServerUpdateService(*server_update); + } + if(this->height() - contacts_list.size()*50 > 0) + filler->setFixedHeight(this->height() - contacts_list.size()*50); + else + filler->setFixedHeight(0); + contacts_list_layout->addWidget(filler); + } + + + void PlansView::resizeEvent(QResizeEvent *) + { + this->sort(); + } + + void PlansView::setServerUpdateService(ros::ServiceClient& _client) { + server_update = &_client; + } +} diff --git a/rqt_parsian_gui/src/rqt_parsian_gui/playoff/playoff.cpp b/rqt_parsian_gui/src/rqt_parsian_gui/playoff/playoff.cpp new file mode 100644 index 00000000..d0bfe5fa --- /dev/null +++ b/rqt_parsian_gui/src/rqt_parsian_gui/playoff/playoff.cpp @@ -0,0 +1,44 @@ +#include + + + +namespace rqt_parsian_gui +{ + + PlayOff::PlayOff() : rqt_gui_cpp::Plugin() + { + setObjectName("PlayOff"); + } + + void PlayOff::initPlugin(qt_gui_cpp::PluginContext& context) + { + + n = getNodeHandle(); + n_private = getPrivateNodeHandle(); + + server_update = n.serviceClient("/update_plans"); + subscriber = n.subscribe("/playoff_client", 1000, boost::bind(& PlayOff::sub, this, _1)); + + + // create QWidget + + playoffWidget = new PlayOffWidget(); + playoffWidget->setServerUpdateService(server_update); + playoffWidget->create_main_widget(); + context.addWidget(playoffWidget); + + + } + + void PlayOff::shutdownPlugin() { + n.shutdown(); + n_private.shutdown(); + } + + void PlayOff::sub(const parsian_msgs::parsian_playoff_clientConstPtr &_msg) { + playoffWidget->subscribe(_msg); + } + +} + +PLUGINLIB_EXPORT_CLASS(rqt_parsian_gui::PlayOff, rqt_gui_cpp::Plugin) diff --git a/rqt_parsian_gui/src/rqt_parsian_gui/playoff/playoffWidget.cpp b/rqt_parsian_gui/src/rqt_parsian_gui/playoff/playoffWidget.cpp new file mode 100644 index 00000000..a6e7de1b --- /dev/null +++ b/rqt_parsian_gui/src/rqt_parsian_gui/playoff/playoffWidget.cpp @@ -0,0 +1,197 @@ +#include + + +namespace rqt_parsian_gui +{ + PlayOffWidget::PlayOffWidget() : QWidget() + { + this->main_layout = new QGridLayout(this); + prev_view = new QWidget(); + all_plansView = new PlansView(); + active_plansView = new PlansView(); + ignored_plansView = new PlansView(); + plansView_widg = new QWidget(); + plansView_lay = new QVBoxLayout(); + tabWidget = new TabWidget(); + last_plan_title = new QLabel(); + last_plan = new PlanLabel(false); + master_plan_title = new QLabel(); + master_plan = new PlanLabel(false); + activate_all = new QPushButton("Activate All"); + deactivate_all = new QPushButton("DeActivate All"); + demaster = new QPushButton("DeMaster"); + + connect(this, SIGNAL(subscribe_sig()), this, SLOT(subscribe_slot())); + } + + void PlayOffWidget::create_main_widget() + { + plansView_widg->setContentsMargins(0, 0, 0, 0); + plansView_lay->setContentsMargins(0, 0, 0, 0); + plansView_lay->setMargin(0); + plansView_lay->setSpacing(0); + plansView_widg->setLayout(plansView_lay); + + tabWidget->add_contact("All"); + tabWidget->add_contact("Active"); + tabWidget->add_contact("Ignored"); + connect(tabWidget->get_contact("All"), SIGNAL(pressed()), this, SLOT(all_pressed())); + connect(tabWidget->get_contact("Active"), SIGNAL(pressed()), this, SLOT(active_pressed())); + connect(tabWidget->get_contact("Ignored"), SIGNAL(pressed()), this, SLOT(ignored_pressed())); + + last_plan_title->setText("last ai plan"); + last_plan_title->setObjectName("plan_title"); + last_plan->create_plan("None", false, false); + + master_plan_title->setText("master plan"); + master_plan_title->setObjectName("plan_title"); + master_plan->create_plan("None", false, false); + + activate_all->setObjectName("mainbutton"); + deactivate_all->setObjectName("mainbutton"); + demaster->setObjectName("mainbutton"); + connect(activate_all, SIGNAL(pressed()), this, SLOT(activate_all_pressed())); + connect(deactivate_all, SIGNAL(pressed()), this, SLOT(deactivate_all_pressed())); + connect(demaster, SIGNAL(pressed()), this, SLOT(demaster_pressed())); + + + this->main_layout->addWidget(last_plan_title, 0, 0, 1, 4); + this->main_layout->addWidget(last_plan, 0, 4, 1, 8); + this->main_layout->addWidget(master_plan_title, 1, 0, 1, 4); + this->main_layout->addWidget(master_plan, 1, 4, 1, 8); + this->main_layout->addWidget(activate_all, 2, 0, 1, 4); + this->main_layout->addWidget(deactivate_all, 2, 4, 1, 4); + this->main_layout->addWidget(demaster, 2, 8, 1, 4); + this->main_layout->addWidget(tabWidget, 3, 0, 7, 4); + this->main_layout->addWidget(plansView_widg, 3, 4, 7, 8); + this->setLayout(this->main_layout); + + prev_view->hide(); + this->main_layout->addWidget(plansView_widg, 3, 4, 7, 8); + prev_view = plansView_widg; + prev_view->show(); + + + } + + + void PlayOffWidget::setServerUpdateService(ros::ServiceClient& _client) { + server_update = &_client; + all_plansView->setServerUpdateService(*server_update); + active_plansView->setServerUpdateService(*server_update); + ignored_plansView->setServerUpdateService(*server_update); + + } + + void PlayOffWidget::all_pressed() + { + prev_view->hide(); + this->main_layout->addWidget(all_plansView, 3, 4, 7, 8); + prev_view = all_plansView; + prev_view->show(); + } + + void PlayOffWidget::active_pressed() + { + prev_view->hide(); + this->main_layout->addWidget(active_plansView, 3, 4, 7, 8); + prev_view = active_plansView; + prev_view->show(); + } + + void PlayOffWidget::ignored_pressed() + { + prev_view->hide(); + this->main_layout->addWidget(ignored_plansView, 3, 4, 7, 8); + prev_view = ignored_plansView; + prev_view->show(); + } + + void PlayOffWidget::activate_all_pressed() + { + if(this->server_update == nullptr) + return; + parsian_msgs::parsian_update_plansRequest req; + parsian_msgs::parsian_update_plansResponse rep; + req.Mode = 5;//ACTIVATE_ALL + server_update->call(req, rep); + } + + void PlayOffWidget::deactivate_all_pressed() + { + if(this->server_update == nullptr) + return; + parsian_msgs::parsian_update_plansRequest req; + parsian_msgs::parsian_update_plansResponse rep; + req.Mode = 6;//DEACTIVATE_ALL + server_update->call(req, rep); + } + + void PlayOffWidget::demaster_pressed() + { + if(this->server_update == nullptr) + return; + parsian_msgs::parsian_update_plansRequest req; + parsian_msgs::parsian_update_plansResponse rep; + req.Mode = 4;//DEMASTER + server_update->call(req, rep); + } + + void PlayOffWidget::subscribe(const parsian_msgs::parsian_playoff_clientConstPtr &msg) + { + this->lastPlan = msg->last_ai_response; + this->masterPlan = msg->master_plan; + this->allPlan = msg->desired_plans; + this->activePlan = msg->active_plans; + this->ignoredPlan = msg->ignored_plans; + + emit subscribe_sig(); + + } + + void PlayOffWidget::subscribe_slot() + { + all_plansView->clear_all(); + active_plansView->clear_all(); + ignored_plansView->clear_all(); + + last_plan->create_plan(QString::fromStdString(lastPlan), false, false); + master_plan->create_plan(QString::fromStdString(masterPlan), false, false); + for(const auto& plan : activePlan) + { + if(plan == masterPlan) + active_plansView->add_contact(QString::fromStdString(plan), true, true, true); + else + active_plansView->add_contact(QString::fromStdString(plan), true, false, true); + } + + for(const auto& plan : allPlan) + { + if(std::find(activePlan.begin(), activePlan.end(), plan) != activePlan.end() && plan == masterPlan) { + all_plansView->add_contact(QString::fromStdString(plan), true, true, true); + } else if(std::find(activePlan.begin(), activePlan.end(), plan) != activePlan.end()) { + all_plansView->add_contact(QString::fromStdString(plan), true, false, true); + }else if(plan == masterPlan){ + all_plansView->add_contact(QString::fromStdString(plan), false, true, true); + }else{ + all_plansView->add_contact(QString::fromStdString(plan), false, false, true); + } + } + + for(const auto& plan : ignoredPlan) + { + ignored_plansView->add_contact(QString::fromStdString(plan), false, false, false); + } + } + + + + + PlayOffWidget::~PlayOffWidget() = default; + + +} + + + + diff --git a/rqt_parsian_gui/src/rqt_parsian_gui/playoff/tabWidget.cpp b/rqt_parsian_gui/src/rqt_parsian_gui/playoff/tabWidget.cpp new file mode 100644 index 00000000..f7416f86 --- /dev/null +++ b/rqt_parsian_gui/src/rqt_parsian_gui/playoff/tabWidget.cpp @@ -0,0 +1,94 @@ +#include +namespace rqt_parsian_gui +{ + TabWidget::TabWidget(QScrollArea *parent) : QScrollArea(parent) + { + + //getting style sheets + std::string path = ros::package::getPath("rqt_parsian_gui"); + resourcePath = QString::fromStdString(path); + resourcePath += "/resource/style_sheet/playoff_planLabel.qss"; + File.setFileName(resourcePath); + File.open(QFile::ReadOnly); + FormStyleSheet = QLatin1String(File.readAll()); + this->setStyleSheet(FormStyleSheet); + File.close(); + //layout + contacts_list_layout = new QVBoxLayout(this); + contacts_list_layout->setContentsMargins(0, 0, 0, 0); + contacts_list_layout->setSpacing(0); + contacts_list_layout->setMargin(0); + filler = new QWidget(this); + contacts_list_layout->addWidget(filler); + + //widget + contact_list_widget = new QWidget(this); + contact_list_widget->setObjectName("contactlist"); + contact_list_widget->setLayout(contacts_list_layout); + contact_list_widget->setContentsMargins(0, 0, 0, 0); + //contact_list_widget->setStyleSheet("QWidget#contactlist{background-color: white;}"); + + //scroll bar + this->setWidget(contact_list_widget); + this->setAlignment(Qt::AlignCenter); + this->setWidgetResizable(true); + this->setMinimumWidth(300); + this->setContentsMargins(0, 0, 0, 0); + + + } + + void TabWidget::add_contact(QString name) + { + //check if the username already exists + for(int i{}; i < contacts_list.size(); i++) + if(contacts_list[i]->text() == name) + return; + QPushButton* new_contact = new QPushButton(name); + new_contact->setFixedHeight(100); + new_contact->setObjectName("tab"); + contacts_list_layout->addWidget(new_contact, 0, Qt::AlignTop); + contacts_list.push_back(new_contact); + contacts_list_layout->removeWidget(filler); + if(this->height() - contacts_list.size()*100 > 0) + filler->setFixedHeight(this->height() - contacts_list.size()*100); + else + filler->setFixedHeight(0); + contacts_list_layout->addWidget(filler); + } + + QPushButton* TabWidget::get_contact(QString name) + { + for(int i{}; i < contacts_list.size(); i++) + if(contacts_list[i]->text() == name) + return contacts_list[i]; + //nothing found(if this line reached means something in code is wrong) + return nullptr; + } + + + void TabWidget::sort() + { + for(const auto& contact : contacts_list) + contacts_list_layout->removeWidget(contact); + contacts_list_layout->removeWidget(filler); + //std::sort(contacts_list.begin(), contacts_list.end()); + //update GUI + for(const auto& contact : contacts_list) + { + contacts_list_layout->addWidget(contact, 0, Qt::AlignTop); + } + if(this->height() - contacts_list.size()*100 > 0) + filler->setFixedHeight(this->height() - contacts_list.size()*100); + else + filler->setFixedHeight(0); + contacts_list_layout->addWidget(filler); + } + + + void TabWidget::resizeEvent(QResizeEvent *) + { + this->sort(); + } + +} diff --git a/rqt_parsian_gui/src/rqt_parsian_gui/playoffWidget.cpp b/rqt_parsian_gui/src/rqt_parsian_gui/playoffWidget.cpp deleted file mode 100644 index d15ab330..00000000 --- a/rqt_parsian_gui/src/rqt_parsian_gui/playoffWidget.cpp +++ /dev/null @@ -1,360 +0,0 @@ -// -// Created by parsian-ai on 12/31/17. -// - -#include "rqt_parsian_gui/playoffWidget.h" - -using namespace rqt_parsian_gui; - -PlayOffWidget::PlayOffWidget(ros::NodeHandle & n) : QWidget() { - - client = n.serviceClient ("/update_plans", true); - theplans = new parsian_msgs::parsian_update_plans(); - lastPlan = new parsian_msgs::parsian_plan_GUI(); - chosen = new parsian_msgs::parsian_plan_GUI(); - - mode = new QPushButton("Game Mode"); - update = new QPushButton("Update (Don't Worry! it will work fine :)"); - active = new QPushButton("Active"); - deactive = new QPushButton("Deactive"); - master = new QPushButton("Master"); - - columns = new QColumnView(); - - active->setEnabled(false); - update->setEnabled(true); - deactive->setEnabled(false); - master->setEnabled(false); - - selection = columns->selectionModel(); - - model = new QStandardItemModel(); - selection = new QItemSelectionModel(model); - - updateModel(); - - columns->setModel(model); - columns->setFont(QFont("Monospace")); - columns->setSelectionModel(selection); - - QList widthList; - widthList.append(300); - widthList.append(200); - widthList.append(100); - columns->setColumnWidths(widthList); - - //*Details*// - - - QFrame *line = new QFrame; - line->setFrameShape(QFrame::HLine); - - - auto buttons = new QHBoxLayout; - auto *main = new QVBoxLayout; - auto *detail = new QVBoxLayout; - - buttons->addWidget(deactive); - buttons->addWidget(active); - buttons->addWidget(master); - - for (auto &i : details) { - i = new QLabel(this); - i->setFont(QFont("Monospace")); - detail->addWidget(i); - } - - - main->addWidget(mode); - main->addWidget(update); - main->addWidget(columns); - main->addLayout(buttons); - main->addWidget(line); // <-- Just A Line - main->addLayout(detail); - - connect(update, SIGNAL(clicked()), this, SLOT(slt_updatePlans())); - connect(mode, SIGNAL(clicked()), this, SLOT(slt_changeMode())); - connect(active, SIGNAL(clicked()), this, SLOT(slt_active())); - connect(master, SIGNAL(clicked()), this, SLOT(slt_master())); - connect(deactive, SIGNAL(clicked()), this, SLOT(slt_deactive())); - connect(model, SIGNAL(itemChanged(QStandardItem *)), this, SLOT(slt_edit(QStandardItem *))); - connect(selection, SIGNAL(selectionChanged(const QItemSelection &, const QItemSelection &)), - this, SLOT(slt_selectionChanged(QItemSelection, QItemSelection))); -// connect(m_loader, Q_SIGNAL(plansUpdated()), this, Q_SLOT(updateModel())); - - setLayout(main); -} - -void PlayOffWidget::updateModel() { - - model->clear(); - - QStandardItem *pkg; - QStandardItem *file; - QStandardItem *plan; - - int pkgCounter = 0; - int fileCounter = 0; - int planCounter = 0; - QStringList last_path_dir; - QString last_pkg; - QStringList response_path_dir; - QString response_pkg ; - int index[theplans->response.allPlans.size()][3] ; // to use in f - - for (size_t i = 0; i < theplans->response.allPlans.size(); i++) { - - - - index[i][0] = pkgCounter; - index[i][1] = fileCounter; - index[i][2] = planCounter; - - if (i != 0) { - last_path_dir = response_path_dir; - last_pkg = response_pkg; - } else { - last_pkg = ""; - } - - response_path_dir = QString::fromStdString(theplans->response.allPlans[i].planFile).split("plans/")[1]. - split("/"); - - response_pkg = "main"; - for (int path_ctr = 0; path_ctr < response_path_dir.size() - 1; path_ctr++) { - response_pkg += "." + response_path_dir[path_ctr]; - } - - if (response_pkg.compare(last_pkg)) { - pkgCounter++; - fileCounter++; - // debugger->debug("PKG",D_ALI); - pkg = new QStandardItem(response_pkg); - model->appendRow(pkg); - //debugger->debug( "FILE",D_ALI); - file = new QStandardItem(response_path_dir.last().left(response_path_dir.last().size() - 5)); - QString temp = response_pkg; - file->setToolTip(""); - pkg->appendRow(file); - } else if (response_path_dir.last().compare(last_path_dir.last())) { - fileCounter++; -// debugger->debug("PLAN",D_ALI); - file = new QStandardItem(response_path_dir.last().left(response_path_dir.last().size() - 5)); - QString temp = response_pkg; - file->setToolTip(""); - pkg->appendRow(file); - } - - planCounter++; - plan = new QStandardItem(QString("%1").arg(i)); - file->appendRow(plan); - file->setEditable(false); - plan->setEditable(false); - pkg->setEditable(false); - lastPlan = &theplans->response.allPlans.at(i); - - } -} - -void PlayOffWidget::slt_changeMode() { - debugMode = !debugMode; - mode->setText((debugMode) ? "Game Mode" : "Debug Mode"); - updateBtn(debugMode); -// qDebug() << "Mode Chaged to " << ((debugMode) ? "Debug" : "Game") << " Mode"; -} - -void PlayOffWidget::updateBtn(bool _debug) { - if (_debug) { - update->setEnabled(true); - columns->setEnabled(true); - active->setEnabled(true); - master->setEnabled(true); - deactive->setEnabled(true); - } else { - active->setEnabled(false); - update->setEnabled(false); - deactive->setEnabled(false); - master->setEnabled(false); - } -} - -void PlayOffWidget::slt_updatePlans() { - - if (client.call(*theplans)) { - ROS_INFO("req to plan server......"); - } else { - ROS_INFO("ERROR req to plan server"); - return; - } - theplans->request.newPlans.clear(); - theplans->request.index.clear(); - updateModel(); -} - -void PlayOffWidget::slt_active() { - - QModelIndexList modelList = itemSelected.indexes(); - Q_FOREACH (QModelIndex model, modelList) { - if (model.parent().row() == -1) { - int i = -1; - while (model.child(++i, 0).data().toString() != "") { - int j = -1; - while (model.child(i, 0).child(++j, 0).data().toString() != "") { - int planID = model.child(i, 0).child(j, 0).data().toUInt(); - theplans->request.newPlans.push_back(theplans->response.allPlans[planID].planFile); - theplans->request.index.push_back(j); - } - } - } else if (model.parent().parent().row() == -1) { - details[0]->setText(QString("Type : File")); - int i = -1; - while (model.child(++i, 0).data().toString() != "") { - int planID = model.child(i, 0).data().toUInt(); - theplans->request.newPlans.push_back(theplans->response.allPlans[planID].planFile); - theplans->request.index.push_back(i); - } - } else if (model.parent().parent().parent().row() == -1) { - unsigned int planID = model.data().toUInt(); - theplans->request.newPlans.push_back(theplans->response.allPlans[planID].planFile); - theplans->request.index.push_back(static_cast < unsigned int && >(model.row())); - } - theplans->request.isActive = static_cast(true); - theplans->request.isMaster = static_cast(false); - active->setEnabled(false); - deactive->setEnabled(true); - master->setEnabled(true); - } - - slt_updatePlans(); -} - -void PlayOffWidget::slt_deactive() { - QModelIndexList modelList = itemSelected.indexes(); - Q_FOREACH (QModelIndex model, modelList) { - if (model.parent().row() == -1) { - int i = -1; - while (model.child(++i, 0).data().toString() != "") { - int j = -1; - while (model.child(i, 0).child(++j, 0).data().toString() != "") { - int planID = model.child(i, 0).child(j, 0).data().toUInt(); - theplans->request.newPlans.push_back(theplans->response.allPlans[planID].planFile); - theplans->request.index.push_back(j); - } - } - } else if (model.parent().parent().row() == -1) { - details[0]->setText(QString("Type : File")); - int i = -1; - while (model.child(++i, 0).data().toString() != "") { - int planID = model.child(i, 0).data().toUInt(); - theplans->request.newPlans.push_back(theplans->response.allPlans[planID].planFile); - theplans->request.index.push_back(i); - } - } else if (model.parent().parent().parent().row() == -1) { - unsigned int planID = model.data().toUInt(); - theplans->request.newPlans.push_back(theplans->response.allPlans[planID].planFile); - theplans->request.index.push_back(static_cast < unsigned int && >(model.row())); - } - theplans->request.isActive = static_cast(false); - theplans->request.isMaster = static_cast(false); - - active->setEnabled(true); - deactive->setEnabled(false); - master->setEnabled(true); - - } - - slt_updatePlans(); -} - -void PlayOffWidget::slt_master() { - QModelIndexList modelList = itemSelected.indexes(); - Q_FOREACH (QModelIndex model, modelList) { - if (model.parent().row() == -1) { - int i = -1; - while (model.child(++i, 0).data().toString() != "") { - int j = -1; - while (model.child(i, 0).child(++j, 0).data().toString() != "") { - int planID = model.child(i, 0).child(j, 0).data().toUInt(); - theplans->request.newPlans.push_back(theplans->response.allPlans[planID].planFile); - theplans->request.index.push_back(j); - } - } - } else if (model.parent().parent().row() == -1) { - details[0]->setText(QString("Type : File")); - int i = -1; - while (model.child(++i, 0).data().toString() != "") { - int planID = model.child(i, 0).data().toUInt(); - theplans->request.newPlans.push_back(theplans->response.allPlans[planID].planFile); - theplans->request.index.push_back(i); - } - } else if (model.parent().parent().parent().row() == -1) { - unsigned int planID = model.data().toUInt(); - theplans->request.newPlans.push_back(theplans->response.allPlans[planID].planFile); - theplans->request.index.push_back(static_cast < unsigned int && >(model.row())); - } - - theplans->request.isActive = static_cast(true); - theplans->request.isMaster = static_cast(true); - - active->setEnabled(false); - deactive->setEnabled(true); - master->setEnabled(false); - - } - slt_updatePlans(); -} - -void PlayOffWidget::slt_edit(QStandardItem *_item) { - // TODO : Make it possible to edit plans via gui. -} - -void PlayOffWidget::slt_selectionChanged(const QItemSelection &selected, const QItemSelection &deselected) { - - for (int i = 0; i < 8; i++) { - details[i]->setText(""); - } - -// chosen = NULL; - itemSelected = selected; - QModelIndexList modelList = selected.indexes(); - Q_FOREACH (QModelIndex model, modelList) { - if (model.parent().row() == -1) { - details[0]->setText(QString("Type : Package")); - active->setEnabled(true); - deactive->setEnabled(true); - master->setEnabled(true); - } else if (model.parent().parent().row() == -1) { - details[0]->setText(QString("Type : File")); - active->setEnabled(true); - deactive->setEnabled(true); - master->setEnabled(true); - - } else if (model.parent().parent().parent().row() == -1) { - - unsigned int planID = model.data().toUInt(); - chosen = &theplans->response.allPlans.at(planID); - - details[0]->setText(QString("Type : Plan")); - details[1]->setText(QString("Agent Size : %1").arg(chosen->agentSize)); - details[2]->setText(QString("Plan Mode : %1").arg(chosen->planMode.c_str())); - details[3]->setText(QString("Chance : %1").arg(chosen->chance)); - details[4]->setText(QString("Last Dist : %1").arg(chosen->lastDist)); - auto final_tag = new QString(); - for (std::string str : chosen->tags) { - *final_tag += QString::fromStdString(str) + " "; - } - details[5]->setText(QString("Tags : %1").arg(*final_tag)); -// details[6]->setText(QString("ShotPos : (%1, %2)").arg(chosen->matching.shotPos.x).arg( -// chosen->matching.shotPos.y)); - //details[7]->setText(QString("ShotZone : %1").arg(CCoach::getShotSpot(chosen->matching.initPos.ball, chosen->matching.shotPos))); - - active->setEnabled(!chosen->isActive); - deactive->setEnabled(chosen->isActive); - master->setEnabled(!chosen->isMaster); -// - } else { - details[0]->setText(QString("Type : SubPlan !!")); - } - } - -} diff --git a/rqt_parsian_gui/src/rqt_parsian_gui/taskRunner.cpp b/rqt_parsian_gui/src/rqt_parsian_gui/taskRunner.cpp index f28262f0..c41ec520 100644 --- a/rqt_parsian_gui/src/rqt_parsian_gui/taskRunner.cpp +++ b/rqt_parsian_gui/src/rqt_parsian_gui/taskRunner.cpp @@ -12,13 +12,14 @@ namespace rqt_parsian_gui void TaskRunner::initPlugin(qt_gui_cpp::PluginContext& context) { - + ROS_INFO_STREAM("Kian"); n = getNodeHandle(); n_private = getPrivateNodeHandle(); - // create QWidget + // create QWidget taskRunnerWidget = new TaskRunnerWidget(n); context.addWidget(taskRunnerWidget); +// ROS_INFO("kasra"); } diff --git a/rqt_parsian_gui/src/rqt_parsian_gui/taskRunnerWidget.cpp b/rqt_parsian_gui/src/rqt_parsian_gui/taskRunnerWidget.cpp index 9079edef..cd4a9362 100644 --- a/rqt_parsian_gui/src/rqt_parsian_gui/taskRunnerWidget.cpp +++ b/rqt_parsian_gui/src/rqt_parsian_gui/taskRunnerWidget.cpp @@ -1,95 +1,177 @@ #include +#include namespace rqt_parsian_gui { TaskRunnerWidget::TaskRunnerWidget(ros::NodeHandle & n) : QWidget() { - task.reset(new parsian_msgs::parsian_robot_task()); - task->select = 255; - client = new parsian_msgs::grsim_ball_replacement(); + gridLayout = new QGridLayout(); - toolButton = new QToolButton(); - toolButton->setText("Choose Task"); - agentId = new QToolButton; - agentId->setText("0"); - agent_id =0; + //toolButton = new QToolButton(); + //toolButton->setText("GoToPointAvoid"); + + //agentId = new QToolButton; + + + comboBoxTask = new QComboBox; + comboBoxTask->addItem("GotoPointAvoid"); + comboBoxTask->addItem("Kick"); + comboBoxTask->addItem("Receive"); + comboBoxTask->addItem("OneTouch"); + + comboBoxPN = new QComboBox; + comboBoxPN->addItem("0"); + comboBoxPN->addItem("1"); + comboBoxPN->addItem("2"); + comboBoxPN->addItem("3"); + comboBoxPN->addItem("4"); + comboBoxPN->addItem("5"); + comboBoxPN->addItem("6"); + comboBoxPN->addItem("7"); + comboBoxPN->addItem("8"); + comboBoxPN->addItem("9"); + comboBoxPN->addItem("10"); + comboBoxPN->addItem("11"); + + + + + + + connect(comboBoxTask,SIGNAL(currentTextChanged(QString)),this,SLOT(comboChange(QString))); + tasks = new QAction* [_TASK_NUM]; + ids = new QAction* [_PLAYER_NUMBER]; - tasks = new QAction* [TASK_NUM]; - ids = new QAction* [_MAX_NUM_PLAYERS]; + //connect(toolButton,SIGNAL(triggered(QAction*)),this,SLOT(setTask(QAction*))); - for (int i = 0; i < TASK_NUM; ++i) { + for (int i = 0; i < _TASK_NUM; ++i) { tasks[i] = new QAction(taskNames[i], this); - connect(toolButton, SIGNAL(triggered(QAction * )), this, SLOT(setTask(QAction * ))); - toolButton->addAction(tasks[i]); + connect(comboBoxTask, SIGNAL(triggered(QAction * )), this, SLOT(setTask(QAction * ))); + comboBoxTask->addAction(tasks[i]); } - //################################################## - for (int i = 0; i < _MAX_NUM_PLAYERS; ++i) { - ids[i] = new QAction(QString::number(i),this); - connect(agentId, SIGNAL(triggered(QAction * )), this, SLOT(setID(QAction * ))); - agentId->addAction(ids[i]); + for(int i{};i <_PLAYER_NUMBER;i++){ + ids[i] = new QAction(QString::number(i),this); + connect(comboBoxPN, SIGNAL(triggered(QAction * )), this, SLOT(setID(QAction * ))); + comboBoxPN->addAction(ids[i]); } - gridLayout->addWidget(toolButton); - gridLayout->addWidget(agentId); + + + + gridLayout->addWidget(comboBoxTask); + gridLayout->addWidget(comboBoxPN); + this->setMaximumHeight(91); this->setLayout(gridLayout); - mousePosSub = n.subscribe("/mousePos",10, &TaskRunnerWidget::mousePosCallBack, this); - ballReplacementClient = n.serviceClient("/GrsimBallReplacesrv",true); - robTaskPub = new ros::Publisher[_MAX_NUM_PLAYERS]; - for (int i = 0; i < _MAX_NUM_PLAYERS; ++i) { + task.reset(new parsian_msgs::parsian_robot_task()); + + //robTaskPub = n.advertise("/agent_0/task",100); + for (int i = 0; i < _PLAYER_NUMBER; ++i) { std::string topic(QString("/agent_%1/task").arg(i).toStdString()); robTaskPub[i] = n.advertise(topic, 100); } - timer = n.createTimer(ros::Duration(0.016), &TaskRunnerWidget::timerCb, this); - } + worldModelSub = n.subscribe("/world_model", 1000, boost::bind(& TaskRunnerWidget::m_wmCb, this, _1)); + mousePosSub = n.subscribe("/mousePos",10, &TaskRunnerWidget::mousePosCallBack,this); + ROS_INFO("Satr"); + - void TaskRunnerWidget::setTask(QAction* action) { - task->select = 255; - toolButton->setText(action->text()); + +} + + + + void TaskRunnerWidget::comboChange(QString) { + task->select = 5; + ROS_INFO("comboChange"); } void TaskRunnerWidget::setID(QAction * action ){ - task->select = 255; - agentId->setText(action->text()); - agent_id = action->text().toInt(); + //agentId->setText(action->text()); + // agent_id = action->text().toInt(); + + +// task->select = 255; + } + + + void TaskRunnerWidget::m_wmCb(const parsian_msgs::parsian_world_modelConstPtr& _wm) { + + ROS_INFO_STREAM(comboBoxTask->currentText().toStdString()); + //ROS_INFO("wm_in"); + if(task != 0) { + if (setData == true) { + robTaskPub[(comboBoxPN->currentText()).toInt()].publish(task); + } + } + } + + + TaskRunnerWidget::~TaskRunnerWidget() { } - void TaskRunnerWidget::mousePosCallBack(parsian_msgs::vector2DConstPtr pos) { - if (QString::fromStdString(taskNames[0]) == toolButton->text()){ // BALL REPLACEMENT REQUEST - client->request.vx = 0; - client->request.vy = 0; - client->request.x = pos->x; - client->request.y = pos->y; - if(ballReplacementClient.call(*client)){ - ROS_INFO_STREAM("YES"); - } else ROS_INFO_STREAM("CALL FAILED"); - - - } else if(QString::fromStdString(taskNames[1]) == toolButton->text()){ - task->gotoPointAvoidTask = *new parsian_msgs::parsian_skill_gotoPointAvoid(); - task->gotoPointAvoidTask.base.targetPos.x=pos->x; - task->gotoPointAvoidTask.base.targetPos.y=pos->y; + void TaskRunnerWidget::mousePosCallBack(parsian_msgs::mouse_eventConstPtr msg) { + ROS_INFO("MOSPOS"); + setData = false; + if(QString::fromStdString(taskNames[0]) == comboBoxTask->currentText()) { + ROS_INFO("GOTOPOINTAVOID"); + task->gotoPointAvoidTask.base.targetPos.x = msg->pos.x; + task->gotoPointAvoidTask.base.targetPos.y = msg->pos.y; + setData = true; task->select = parsian_msgs::parsian_robot_task::GOTOPOINTAVOID; + //robTaskPub.publish(task); } - else if(QString::fromStdString(taskNames[2]) == toolButton->text()){ - task->gotoPointAvoidTask = *new parsian_msgs::parsian_skill_gotoPointAvoid(); - task->gotoPointAvoidTask.base.targetPos.x=pos->x; - task->gotoPointAvoidTask.base.targetPos.y=pos->y; - task->gotoPointAvoidTask.noAvoid = static_cast(true); - task->select = parsian_msgs::parsian_robot_task::GOTOPOINTAVOID; - }else{ + //else if (QString::fromStdString(taskNames[1]) == comboBoxTask->currentText()){ + else if(QString::fromStdString(taskNames[1]) == comboBoxTask->currentText()){ + ROS_INFO("KICK"); + task->kickTask.iskickchargetime = true; + task->kickTask.kickchargetime = 500; + task->kickTask.target.x = msg->pos.x; + task->kickTask.target.y = msg->pos.y; + setData = true; + task->kickTask.avoidPenaltyArea = true; + task->select = parsian_msgs::parsian_robot_task::KICK; + } - } + else if(QString::fromStdString(taskNames[2]) == comboBoxTask->currentText()) { + ROS_INFO("RECEIVE"); + task->receivePassTask.target.x = msg->pos.x; + task->receivePassTask.target.y = msg->pos.y; + task->receivePassTask.receiveRadius = 0.5; + setData = true; + task->select = parsian_msgs::parsian_robot_task::RECIVEPASS; + + } + else if(QString::fromStdString((taskNames[3])) == comboBoxTask->currentText()) { + ROS_INFO("ONETOUCH"); + if(msg->isLeftClicked == false) { + ROS_INFO("RightClicked"); + rightSet = true; + ROS_INFO_STREAM(msg->pos.x); + ROS_INFO_STREAM(msg->pos.y); + task->oneTouchTask.target.x = msg->pos.x; + task->oneTouchTask.target.y = msg->pos.y; + } + else if(msg->isLeftClicked == true) { + ROS_INFO("LeftClicked"); + leftSet = true; + ROS_INFO_STREAM(msg->pos.x); + ROS_INFO_STREAM(msg->pos.y); + task->oneTouchTask.waitPos.x = msg->pos.x; + task->oneTouchTask.waitPos.y = msg->pos.y; + } + if(leftSet && rightSet ){ + setData = true; + } + task->select = parsian_msgs::parsian_robot_task::ONETOUCH; + } + - void TaskRunnerWidget::timerCb(const ros::TimerEvent& _timer){ - if(task->select != 255) - robTaskPub[agent_id].publish(task); } }