diff --git a/.github/workflows/tests-action.yml b/.github/workflows/tests-action.yml index cf5dcd7f9..3fd9baacb 100644 --- a/.github/workflows/tests-action.yml +++ b/.github/workflows/tests-action.yml @@ -28,7 +28,7 @@ jobs: run: | sudo apt-get update sudo apt-get upgrade -y - sudo apt-get install -y cmake g++ qtbase5-dev qtchooser qt5-qmake qtbase5-dev-tools libqt5charts5-dev libsdl2-dev libzmq3-dev libeigen3-dev libgtest-dev ninja-build ccache + sudo apt-get install -y cmake g++ qtbase5-dev qtchooser qt5-qmake qtbase5-dev-tools libqt5charts5-dev libsdl2-dev libzmq3-dev libeigen3-dev libgtest-dev ninja-build ccache libdlib-dev libblas-dev liblapack-dev - name: Build Protobuf if: steps.cache-build-restore.outputs.cache-hit != 'true' diff --git a/.gitmodules b/.gitmodules index dc9f7f60e..26afbd15d 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,9 +1,3 @@ -[submodule "external/framework"] - path = external/framework - url = https://github.com/robotics-erlangen/framework.git -[submodule "external/autoref"] - path = external/autoref - url = https://github.com/robotics-erlangen/autoref.git [submodule "roboteam_autoref"] path = roboteam_autoref url = https://github.com/RoboTeamTwente/roboteam_autoref.git diff --git a/build.sh b/build.sh index 4e5b3fc01..c520bc72a 100755 --- a/build.sh +++ b/build.sh @@ -69,29 +69,6 @@ then pushd roboteam_interface yarn install popd - if [ $SKIP == 0 ]; - then - pushd external - echo -e "${GREEN}Building external${RESET}" - pushd framework - echo -e "${GREEN}Building external/framework${RESET}" - mkdir -p build - pushd build - cmake .. - make simulator-cli -j$(nproc) - popd - popd - pushd autoref - echo -e "${GREEN}Building external/autoref${RESET}" - mkdir -p build - pushd build - cmake .. - make autoref-cli -j$(nproc) - popd - popd - popd - fi - echo -e "${GREEN}Done, exiting builder..${RESET}" else echo -e "${GREEN}Checking submodules${RESET}" git submodule update --init --recursive diff --git a/docker/Dockerfile b/docker/Dockerfile index 479e08a4c..d627178af 100644 --- a/docker/Dockerfile +++ b/docker/Dockerfile @@ -37,6 +37,16 @@ RUN apk add --no-cache bash build-base cmake make musl-dev libtool \ RUN npm install -g yarn WORKDIR /tmp +RUN wget http://dlib.net/files/dlib-19.21.tar.bz2 \ + && tar xvf dlib-19.21.tar.bz2 \ + && cd dlib-19.21 \ + && mkdir build \ + && cd build \ + && cmake .. \ + && make \ + && make install \ + && rm -rf /tmp/dlib-19.21 /tmp/dlib-19.21.tar.bz2 + RUN git clone https://github.com/pantor/ruckig.git /tmp/ruckig \ && cd /tmp/ruckig \ && git checkout dae82835ec043a54ed6bc775f0776e1a7ff99124 \ @@ -80,6 +90,16 @@ RUN apk add --no-cache build-base git cmake make clang \ RUN npm install -g yarn WORKDIR /tmp +RUN wget http://dlib.net/files/dlib-19.21.tar.bz2 \ + && tar xvf dlib-19.21.tar.bz2 \ + && cd dlib-19.21 \ + && mkdir build \ + && cd build \ + && cmake .. \ + && make \ + && make install \ + && rm -rf /tmp/dlib-19.21 /tmp/dlib-19.21.tar.bz2 + RUN git clone https://github.com/pantor/ruckig.git /tmp/ruckig \ && cd /tmp/ruckig \ && git checkout dae82835ec043a54ed6bc775f0776e1a7ff99124 \ diff --git a/docker/builder/docker-compose.yml b/docker/builder/docker-compose.yml index d0d086f03..cd587f6e4 100644 --- a/docker/builder/docker-compose.yml +++ b/docker/builder/docker-compose.yml @@ -1,5 +1,3 @@ -version: '3' - services: roboteam_builder: diff --git a/docker/runner/docker-compose.yml b/docker/runner/docker-compose.yml index 6e1b1d1bc..09a1478af 100644 --- a/docker/runner/docker-compose.yml +++ b/docker/runner/docker-compose.yml @@ -1,5 +1,3 @@ -version: '3' - services: roboteam_primary_ai: @@ -167,10 +165,9 @@ services: profiles: ["simulator","diff","game","autoref"] erforce_autoref_sim: - image: roboteamtwente/roboteam:development + image: roboticserlangen/autoref container_name: RTT_erforce_autoref_sim restart: always - working_dir: "/home/roboteamtwente/external/autoref/build/bin/" command: sh -c "./autoref-cli --vision-port 10020 --tracker-port 10010 --gc-port 10003" network_mode: "host" # Workaround to connect from interface on host to AI websocket, please fix expose: @@ -182,10 +179,9 @@ services: profiles: ["simulator","diff"] erforce_autoref_game: - image: roboteamtwente/roboteam:development + image: roboticserlangen/autoref container_name: RTT_erforce_autoref_game restart: always - working_dir: "/home/roboteamtwente/external/autoref/build/bin/" command: sh -c "./autoref-cli --vision-port 10006 --tracker-port 10010 --gc-port 10003" network_mode: "host" # Workaround to connect from interface on host to AI websocket, please fix expose: @@ -197,10 +193,9 @@ services: profiles: ["game"] erforce_simulator: - image: roboteamtwente/roboteam:development + image: roboticserlangen/simulatorcli container_name: RTT_erforce_simulator restart: always - working_dir: "/home/roboteamtwente/external/framework/build/bin/" command: sh -c "./simulator-cli" network_mode: "host" # Workaround to connect from interface on host to AI websocket, please fix expose: diff --git a/external/autoref b/external/autoref deleted file mode 160000 index 6f15f574e..000000000 --- a/external/autoref +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 6f15f574ea803984ba7169a40c618fad43143596 diff --git a/external/framework b/external/framework deleted file mode 160000 index 0f5ffffa4..000000000 --- a/external/framework +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 0f5ffffa4417be583357c923ee4217183ed5c260 diff --git a/roboteam_ai/src/world/Robot.cpp b/roboteam_ai/src/world/Robot.cpp index 8e5c3ad2e..ddce843ed 100644 --- a/roboteam_ai/src/world/Robot.cpp +++ b/roboteam_ai/src/world/Robot.cpp @@ -62,10 +62,7 @@ void Robot::setWorkingBallSensor(bool _workingBallSensor) noexcept { Robot::work void Robot::setBallSensorSeesBall(bool _seesBall) noexcept { ballSensorSeesBall = _seesBall; } -void Robot::setDribblerSeesBall(bool _seesBall) noexcept { - if (_seesBall) RTT_INFO("Robot " + std::to_string(id) + " has ball") - dribblerSeesBall = _seesBall; -} +void Robot::setDribblerSeesBall(bool _seesBall) noexcept { dribblerSeesBall = _seesBall; } void Robot::setHasBall(bool _hasBall) noexcept { Robot::robotHasBall = _hasBall; } diff --git a/roboteam_observer/observer/CMakeLists.txt b/roboteam_observer/observer/CMakeLists.txt index 0fd32569f..c7d34d1bd 100644 --- a/roboteam_observer/observer/CMakeLists.txt +++ b/roboteam_observer/observer/CMakeLists.txt @@ -1,35 +1,15 @@ -add_library(observer STATIC - src/filters/Scaling.h - src/Observer.cpp - src/parameters/RobotParameterDatabase.cpp - src/filters/referee/RefereeFilter.cpp - src/filters/vision/CameraObjectFilter.cpp - src/filters/vision/robot/CameraRobotFilter.cpp - src/filters/vision/ball/CameraGroundBallFilter.cpp - src/filters/vision/WorldFilter.cpp - src/filters/vision/GeometryFilter.cpp - src/filters/vision/VisionFilter.cpp - src/filters/vision/robot/RobotFilter.cpp - src/filters/vision/ball/BallFilter.cpp - src/filters/vision/PosVelFilter2D.cpp - src/filters/vision/robot/RobotOrientationFilter.cpp - src/filters/vision/PosVelFilter1D.cpp - src/filters/vision/robot/RobotObservation.cpp - src/filters/vision/robot/RobotPos.cpp - src/filters/vision/robot/FilteredRobot.cpp - src/filters/vision/DetectionFrame.cpp - src/filters/vision/ball/BallObservation.cpp - include/observer/filters/vision/KalmanFilter.h - src/filters/vision/ball/GroundBallExtendedKalmanFilter.cpp - src/filters/vision/ball/FilteredBall.cpp - src/filters/vision/ball/BallAssigner.cpp - src/data/RobotParameters.cpp - src/filters/vision/Camera.cpp - src/filters/vision/CameraMap.cpp - src/filters/vision/RobotFeedbackFilter.cpp - ) +file(GLOB_RECURSE OBSERVER_SOURCES + "${CMAKE_CURRENT_SOURCE_DIR}/src/filters/vision/*.cpp" + "${CMAKE_CURRENT_SOURCE_DIR}/src/filters/vision/*.h" + "${CMAKE_CURRENT_SOURCE_DIR}/src/*.cpp" + "${CMAKE_CURRENT_SOURCE_DIR}/src/*.h" + "${CMAKE_CURRENT_SOURCE_DIR}/include/observer/filters/vision/*.h" +) +find_package(dlib REQUIRED) +add_library(observer STATIC ${OBSERVER_SOURCES}) target_link_libraries(observer + PUBLIC dlib::dlib PUBLIC roboteam_networking PRIVATE roboteam_utils PUBLIC Eigen3::Eigen diff --git a/roboteam_observer/observer/include/observer/filters/shot/ChipEstimator.h b/roboteam_observer/observer/include/observer/filters/shot/ChipEstimator.h new file mode 100644 index 000000000..70881b05d --- /dev/null +++ b/roboteam_observer/observer/include/observer/filters/shot/ChipEstimator.h @@ -0,0 +1,37 @@ +#ifndef CHIPESTIMATOR_H +#define CHIPESTIMATOR_H + +#include +#include +#include +#include + +#include "ShotEvent.h" +#include "observer/filters/shot/ChipTrajectory.h" +#include "observer/filters/vision/ball/BallParameters.h" +#include "observer/filters/vision/ball/FilteredBall.h" + +class ChipEstimator { + private: + std::vector ballsSinceShot; + ShotEvent shotEvent; + Eigen::Vector3d bestShotPos; + Eigen::Vector3d bestShotVel; + BallParameters ballParameters; + CameraMap cameraMap; + int pruneIndex = 1; + bool doFirstHop = true; + double bestTOff = 0.0; + + public: + ChipEstimator(const ShotEvent& shotEvent, const BallParameters& ballParameters, const CameraMap& cameraMap); + double getAverageDistance(); + double getAverageDistance(Eigen::Vector3d shotPos, Eigen::Vector3d shotVel, Eigen::Vector2d shotSpin); + void addFilteredBall(const BallObservation& newBall); + std::pair noSpinBall(double tOff); + Eigen::Vector3d getBestShotVel() { return bestShotVel; } + Eigen::Vector3d getBestShotPos() { return bestShotPos; } + Time getFirstBallTime() { return ballsSinceShot[0].timeCaptured; } +}; + +#endif // CHIPESTIMATOR_H \ No newline at end of file diff --git a/roboteam_observer/observer/include/observer/filters/shot/ChipTrajectory.h b/roboteam_observer/observer/include/observer/filters/shot/ChipTrajectory.h new file mode 100644 index 000000000..a2af5af9d --- /dev/null +++ b/roboteam_observer/observer/include/observer/filters/shot/ChipTrajectory.h @@ -0,0 +1,25 @@ +#ifndef CHIP_TRAJECTORY_H +#define CHIP_TRAJECTORY_H + +#include +#include + +#include "observer/filters/shot/ShotEvent.h" +#include "observer/filters/vision/ball/BallParameters.h" + +class ChipTrajectory { + private: + Eigen::Vector3d initialPos; + Eigen::Vector3d initialVel; + Eigen::Vector2d initialSpin; + BallParameters parameters; + + public: + ChipTrajectory(const Eigen::Vector3d& initialPos, const Eigen::Vector3d& initialVel, const Eigen::Vector2d& initialSpin, const BallParameters& ballParameters); + ChipTrajectory(const Eigen::Vector3d& initialPos, const Eigen::Vector3d& initialVel, const BallParameters& ballParameters); + ChipTrajectory(const Eigen::Vector2d& initialPos, const Eigen::Vector3d& initialVel, const BallParameters& ballParameters); + + ShotState getPositionAtTime(double time); +}; + +#endif // CHIP_TRAJECTORY_H \ No newline at end of file diff --git a/roboteam_observer/observer/include/observer/filters/shot/KickEstimator.h b/roboteam_observer/observer/include/observer/filters/shot/KickEstimator.h new file mode 100644 index 000000000..e9ab0e39d --- /dev/null +++ b/roboteam_observer/observer/include/observer/filters/shot/KickEstimator.h @@ -0,0 +1,30 @@ +#ifndef KICKESTIMATOR_H +#define KICKESTIMATOR_H + +#include + +#include "ShotEvent.h" +#include "observer/filters/shot/KickTrajectory.h" +#include "observer/filters/vision/ball/BallParameters.h" +#include "observer/filters/vision/ball/FilteredBall.h" + +class KickEstimator { + private: + std::vector ballsSinceShot; + ShotEvent shotEvent; + BallParameters ballParameters; + int pruneIndex = 1; + + public: + Eigen::Vector3d bestShotPos; + Eigen::Vector3d bestShotVel; + KickEstimator(const ShotEvent& shotEvent, const BallParameters& ballParameters); + double getAverageDistance(); + double getAverageDistance(Eigen::Vector3d shotPos, Eigen::Vector3d shotVel, Eigen::Vector2d shotSpin); + std::pair slidingBall(); + Eigen::Vector2d getKickDir(); + std::pair noSpinBall(); + void addFilteredBall(const BallObservation& newBall); +}; + +#endif // KICKESTIMATOR_H \ No newline at end of file diff --git a/roboteam_observer/observer/include/observer/filters/shot/KickTrajectory.h b/roboteam_observer/observer/include/observer/filters/shot/KickTrajectory.h new file mode 100644 index 000000000..cff679199 --- /dev/null +++ b/roboteam_observer/observer/include/observer/filters/shot/KickTrajectory.h @@ -0,0 +1,32 @@ +#ifndef KICK_TRAJECTORY_H +#define KICK_TRAJECTORY_H + +#include +#include + +#include "observer/filters/shot/ShotEvent.h" +#include "observer/filters/vision/ball/BallParameters.h" + +class KickTrajectory { + private: + double tSwitch; + Eigen::Vector2d posSwitch; + Eigen::Vector2d velSwitch; + Eigen::Vector2d accSlide; + Eigen::Vector2d accSlideSpin; + Eigen::Vector2d accRoll; + Eigen::Vector2d initialPos; + Eigen::Vector2d initialVel; + Eigen::Vector2d initialSpin; + BallParameters parameters; + + public: + KickTrajectory(const Eigen::Vector2d& initialPos, const Eigen::Vector2d& initialVel, const Eigen::Vector2d& initialSpin, const BallParameters& ballParameters); + KickTrajectory(const Eigen::Vector2d& initialPos, const Eigen::Vector2d& initialVel, const BallParameters& ballParameters); + KickTrajectory(const Eigen::Vector3d& initialPos, const Eigen::Vector3d& initialVel, const Eigen::Vector2d& initialSpin, const BallParameters& ballParameters); + + ShotState getPositionAtTime(double time); + double getTimeAtRest(); +}; + +#endif // KICK_TRAJECTORY_H \ No newline at end of file diff --git a/roboteam_observer/observer/include/observer/filters/shot/ShotEvent.h b/roboteam_observer/observer/include/observer/filters/shot/ShotEvent.h new file mode 100644 index 000000000..004fc935a --- /dev/null +++ b/roboteam_observer/observer/include/observer/filters/shot/ShotEvent.h @@ -0,0 +1,24 @@ +#ifndef SHOTEVENT_H +#define SHOTEVENT_H + +#include "observer/filters/vision/CameraMap.h" +#include "observer/filters/vision/DetectionFrame.h" +#include "observer/filters/vision/GeometryFilter.h" +#include "observer/filters/vision/RobotFeedbackFilter.h" +#include "observer/filters/vision/ball/BallFilter.h" +#include "observer/filters/vision/robot/RobotFilter.h" +#include "observer/parameters/RobotParameterDatabase.h" + +struct ShotEvent { + TeamRobotID shootingBot; + RobotPos shottingBotPos; + Eigen::Vector2d ballPosition; + Time time; + std::vector ballsSinceShot; +}; +struct ShotState { + Eigen::Vector3d pos; + Eigen::Vector3d vel; +}; + +#endif // SHOTEVENT_H \ No newline at end of file diff --git a/roboteam_observer/observer/include/observer/filters/vision/GeometryFilter.h b/roboteam_observer/observer/include/observer/filters/vision/GeometryFilter.h index 74aa78c0f..8419b3008 100644 --- a/roboteam_observer/observer/include/observer/filters/vision/GeometryFilter.h +++ b/roboteam_observer/observer/include/observer/filters/vision/GeometryFilter.h @@ -21,6 +21,7 @@ class GeometryFilter { * @return The most recent filtered geometry */ proto::SSL_GeometryData getGeometry() const; + const std::map& getCameras() const { return cameras; } private: std::string lastGeometryString; diff --git a/roboteam_observer/observer/include/observer/filters/vision/VisionFilter.h b/roboteam_observer/observer/include/observer/filters/vision/VisionFilter.h index 7ac10c2c2..393704ebc 100644 --- a/roboteam_observer/observer/include/observer/filters/vision/VisionFilter.h +++ b/roboteam_observer/observer/include/observer/filters/vision/VisionFilter.h @@ -53,7 +53,7 @@ class VisionFilter { GeometryFilter geomFilter; WorldFilter worldFilter; Time lastPacketTime; - TimeExtrapolationPolicy extrapolationPolicy = TimeExtrapolationPolicy::LAST_RECEIVED_PACKET_TIME; + TimeExtrapolationPolicy extrapolationPolicy = TimeExtrapolationPolicy::REALTIME; }; #endif // RTT_ROBOTEAM_OBSERVER_OBSERVER_SRC_FILTERS_VISION_VISIONFILTER_H_ diff --git a/roboteam_observer/observer/include/observer/filters/vision/WorldFilter.h b/roboteam_observer/observer/include/observer/filters/vision/WorldFilter.h index 2ed7658cf..95de10d5c 100644 --- a/roboteam_observer/observer/include/observer/filters/vision/WorldFilter.h +++ b/roboteam_observer/observer/include/observer/filters/vision/WorldFilter.h @@ -5,9 +5,17 @@ #include #include +#include + #include "DetectionFrame.h" +#include "GeometryFilter.h" #include "RobotFeedbackFilter.h" +#include "observer/filters/shot/ChipEstimator.h" +#include "observer/filters/shot/KickEstimator.h" +#include "observer/filters/shot/ShotEvent.h" +#include "observer/filters/vision/CameraMap.h" #include "observer/filters/vision/ball/BallFilter.h" +#include "observer/filters/vision/ball/BallParameters.h" #include "observer/filters/vision/robot/RobotFilter.h" #include "observer/parameters/RobotParameterDatabase.h" @@ -24,9 +32,11 @@ class WorldFilter { public: WorldFilter(); - void process(const std::vector& frames, const std::vector& feedback, const std::vector& camera_ids); - - [[nodiscard]] proto::World getWorldPrediction(const Time& time) const; + void process(const std::vector& frames, const std::vector& feedback, const std::vector& camera_ids, + GeometryFilter& geomFilter); + void kickDetector(FilteredBall bestBall, Time time, const BallParameters& ballParameters); + [[nodiscard]] proto::World getWorldPrediction(const Time& time, const BallParameters& ballParameters); + [[nodiscard]] BallParameters getBallParameters(const std::optional& geometry) const; void updateRobotParameters(const TwoTeamRobotParameters& robotInfo); @@ -35,6 +45,28 @@ class WorldFilter { robotMap blue; robotMap yellow; std::vector balls; + CameraMap cameraMap; + Time lastKickTime = Time::now(); + std::optional kickEstimator; + std::optional chipEstimator; + struct RecentData { + std::vector blue; + std::vector yellow; + std::optional filteredBall; + }; + std::optional mostRecentShot; + + std::deque frameHistory; + void addRecentData(const std::vector& blue, const std::vector& yellow, const std::optional& filteredBall) { + if (frameHistory.size() >= 5) { + frameHistory.pop_front(); + } + frameHistory.push_back({blue, yellow, filteredBall}); + } + bool checkDistance(const std::vector& robots, const std::vector& balls); + bool checkVelocity(const std::vector& balls); + bool checkOrientation(const std::vector& robots, const std::vector& balls); + bool checkIncreasingDistance(const std::vector& robots, const std::vector& balls); RobotParameters blueParams; RobotParameters yellowParams; @@ -50,13 +82,13 @@ class WorldFilter { void processBalls(const DetectionFrame& frame); [[nodiscard]] std::vector getHealthiestRobotsMerged(bool blueBots, Time time) const; [[nodiscard]] std::vector oneCameraHealthyRobots(bool blueBots, int camera_id, Time time) const; - void addRobotPredictionsToMessage(proto::World& world, Time time) const; - void addBallPredictionsToMessage(proto::World& world, Time time) const; + void addRobotPredictionsToMessage(proto::World& world, Time time); + void addBallPredictionsToMessage(proto::World& world, Time time, const BallParameters& ballParameters); // take care, although these method are static, they typically DO modify the current object as they have a robotMap reference static void predictRobots(const DetectionFrame& frame, robotMap& robots); static void updateRobots(robotMap& robots, const std::vector& detectedRobots); - static void updateRobotsNotSeen(const DetectionFrame& frame, robotMap& robots); + static void updateRobotsNotSeen(const DetectionFrame& frame, robotMap& robots, bool blueBots); }; #endif // ROBOTEAM_OBSERVER_KALMANFILTER_H diff --git a/roboteam_observer/observer/include/observer/filters/vision/ball/BallFilter.h b/roboteam_observer/observer/include/observer/filters/vision/ball/BallFilter.h index 1fad5ecb0..ac2d45156 100644 --- a/roboteam_observer/observer/include/observer/filters/vision/ball/BallFilter.h +++ b/roboteam_observer/observer/include/observer/filters/vision/ball/BallFilter.h @@ -26,9 +26,11 @@ class BallFilter { * If the camera is not tracked by the current filter, information from other camera's is used. * @param cameraID to predict the ball for * @param until Time to predict the ball at + * @param yellowRobots Filtered robots used for collision prediction + * @param blueRobots Filtered robots used for collision prediction * @return The prediction */ - [[nodiscard]] GroundBallPrediction predictCam(int cameraID, Time until) const; + [[nodiscard]] GroundBallPrediction predictCam(int cameraID, Time until, std::vector yellowRobots, std::vector blueRobots); /** * Updates the ground filter with a given camera ID with Prediction Observation pairs. @@ -42,13 +44,18 @@ class BallFilter { * @param time * @return The Predicted/Filtered ball */ - [[nodiscard]] FilteredBall mergeBalls(Time time) const; + [[nodiscard]] FilteredBall mergeBalls(Time time, bool consumeObservation = true); /** * @return Gets the health of the ball filter */ [[nodiscard]] double getHealth() const; + /** + * @return The number of observations the filter has seen + */ + [[nodiscard]] double getNumObservations() const; + private: std::map groundFilters; }; diff --git a/roboteam_observer/observer/include/observer/filters/vision/ball/BallObservation.h b/roboteam_observer/observer/include/observer/filters/vision/ball/BallObservation.h index d0c5b808c..ce1663944 100644 --- a/roboteam_observer/observer/include/observer/filters/vision/ball/BallObservation.h +++ b/roboteam_observer/observer/include/observer/filters/vision/ball/BallObservation.h @@ -10,6 +10,8 @@ struct BallObservation { explicit BallObservation(int cameraID, Time timeCaptured, Time timeSent, const proto::SSL_DetectionBall& detectionBall); explicit BallObservation(int cameraID, Time timeCaptured, Time timeSent, Eigen::Vector2d position, Eigen::Vector2d pixelPosition, double confidence, uint32_t totalArea, double height); + explicit BallObservation() + : cameraID(-1), timeCaptured(Time()), timeSent(Time()), position(Eigen::Vector2d::Zero()), pixelPosition(Eigen::Vector2d::Zero()), area(0), confidence(0.0), height(0.0) {} int cameraID; Time timeCaptured; diff --git a/roboteam_observer/observer/include/observer/filters/vision/ball/BallParameters.h b/roboteam_observer/observer/include/observer/filters/vision/ball/BallParameters.h new file mode 100644 index 000000000..8f7c95761 --- /dev/null +++ b/roboteam_observer/observer/include/observer/filters/vision/ball/BallParameters.h @@ -0,0 +1,35 @@ +#pragma once +#include "proto/State.pb.h" + +class BallParameters { + private: + double ballRadius = 0.0215; + double accSlide; + double accRoll; + double inertiaDistribution = 0.5; + double chipDampingXYFirstHop; + double chipDampingXYOtherHops; + double chipDampingZ; + + public: + BallParameters() : accSlide(0), accRoll(0), chipDampingXYFirstHop(0), chipDampingXYOtherHops(0), chipDampingZ(0) {} + + explicit BallParameters(const proto::SSL_GeometryData& geometryData) : BallParameters() { + if (geometryData.models().has_straight_two_phase()) { + accSlide = geometryData.models().straight_two_phase().acc_slide(); + accRoll = geometryData.models().straight_two_phase().acc_roll(); + } + if (geometryData.models().has_chip_fixed_loss()) { + chipDampingXYFirstHop = geometryData.models().chip_fixed_loss().damping_xy_first_hop(); + chipDampingXYOtherHops = geometryData.models().chip_fixed_loss().damping_xy_other_hops(); + chipDampingZ = geometryData.models().chip_fixed_loss().damping_z(); + } + } + double getBallRadius() const { return ballRadius; } + double getAccSlide() const { return accSlide; } + double getAccRoll() const { return accRoll; } + double getInertiaDistribution() const { return inertiaDistribution; } + double getChipDampingXYFirstHop() const { return chipDampingXYFirstHop; } + double getChipDampingXYOtherHops() const { return chipDampingXYOtherHops; } + double getChipDampingZ() const { return chipDampingZ; } +}; \ No newline at end of file diff --git a/roboteam_observer/observer/include/observer/filters/vision/ball/CameraGroundBallFilter.h b/roboteam_observer/observer/include/observer/filters/vision/ball/CameraGroundBallFilter.h index 2290181d5..dc538c27b 100644 --- a/roboteam_observer/observer/include/observer/filters/vision/ball/CameraGroundBallFilter.h +++ b/roboteam_observer/observer/include/observer/filters/vision/ball/CameraGroundBallFilter.h @@ -6,6 +6,7 @@ #include "FilteredBall.h" #include "GroundBallExtendedKalmanFilter.h" #include "observer/filters/vision/CameraObjectFilter.h" +#include "observer/filters/vision/robot/RobotFilter.h" struct CameraGroundBallPrediction { CameraGroundBallPrediction() = default; @@ -18,21 +19,42 @@ struct CameraGroundBallPredictionObservationPair { CameraGroundBallPrediction prediction; std::optional observation; }; +struct BallEstimate { + Eigen::Vector2d position; + Eigen::Vector2d velocity; + double health; + double posUncertainty; + double velocityUncertainty; + + [[nodiscard]] proto::WorldBall asWorldBall() const; + BallEstimate(Eigen::Vector2d position, Eigen::Vector2d velocity, double health, double posUncertainty, double velocityUncertainty) + : position(position), velocity(velocity), health(health), posUncertainty(posUncertainty), velocityUncertainty(velocityUncertainty) {} + BallEstimate() = default; +}; + class CameraGroundBallFilter : public CameraObjectFilter { public: explicit CameraGroundBallFilter(const BallObservation& observation, const Eigen::Vector2d& velocity_estimate = Eigen::Vector2d::Zero()); - [[nodiscard]] FilteredBall getEstimate(Time time) const; + [[nodiscard]] BallEstimate getEstimate(Time time) const; [[nodiscard]] Eigen::Vector2d getVelocityEstimate(Time time) const; - [[nodiscard]] CameraGroundBallPrediction predict(Time time) const; + [[nodiscard]] CameraGroundBallPrediction predict(Time time, std::vector yellowRobots, std::vector blueRobots); bool processDetections(const CameraGroundBallPredictionObservationPair& prediction_observation_pair); + [[nodiscard]] BallObservation getLastObservation() const; + void setLastObservation(const BallObservation& observation); + [[nodiscard]] bool getLastObservationAvailableAndReset(); + private: + bool checkRobots(const std::vector& robots, const Eigen::Vector2d& positionEstimate, const Eigen::Vector2d& velocityEstimate); + bool checkRobotCollision(const FilteredRobot& robot, const Eigen::Vector2d& positionEstimate, const Eigen::Vector2d& velocityEstimate); void predictFilter(const CameraGroundBallPrediction& prediction); void update(const BallObservation& observation); bool updateNotSeen(Time time); GroundBallExtendedKalmanFilter ekf; + BallObservation lastObservation; + bool lastObservationUsed = true; }; #endif // RTT_ROBOTEAM_OBSERVER_OBSERVER_SRC_FILTERS_VISION_BALL_CAMERABALLFILTER_H_ diff --git a/roboteam_observer/observer/include/observer/filters/vision/ball/FilteredBall.h b/roboteam_observer/observer/include/observer/filters/vision/ball/FilteredBall.h index 7d1138937..af9db740e 100644 --- a/roboteam_observer/observer/include/observer/filters/vision/ball/FilteredBall.h +++ b/roboteam_observer/observer/include/observer/filters/vision/ball/FilteredBall.h @@ -2,17 +2,21 @@ #define RTT_ROBOTEAM_OBSERVER_OBSERVER_SRC_FILTERS_VISION_BALL_FILTEREDBALL_H_ #include +#include #include +#include + +#include "BallObservation.h" struct FilteredBall { [[nodiscard]] proto::WorldBall asWorldBall() const; - FilteredBall(Eigen::Vector2d pos, Eigen::Vector2d vel, double health, double posUncertainty, double velocityUncertainty); + FilteredBall(Eigen::Vector2d pos, Eigen::Vector2d vel, Time time, Eigen::Vector2d positionCamera, std::optional currentObservation); FilteredBall() = default; Eigen::Vector2d position; Eigen::Vector2d velocity; - double health; - double posUncertainty; - double velocityUncertainty; + Time time; + Eigen::Vector2d positionCamera; + std::optional currentObservation; }; -#endif // RTT_ROBOTEAM_OBSERVER_OBSERVER_SRC_FILTERS_VISION_BALL_FILTEREDBALL_H_ +#endif // RTT_ROBOTEAM_OBSERVER_OBSERVER_SRC_FILTERS_VISION_BALL_FILTEREDBALL_H_ \ No newline at end of file diff --git a/roboteam_observer/observer/include/observer/filters/vision/robot/RobotPos.h b/roboteam_observer/observer/include/observer/filters/vision/robot/RobotPos.h index 09ef00cc5..6590f5ba4 100644 --- a/roboteam_observer/observer/include/observer/filters/vision/robot/RobotPos.h +++ b/roboteam_observer/observer/include/observer/filters/vision/robot/RobotPos.h @@ -50,6 +50,20 @@ struct TeamRobotID { TeamRobotID(unsigned int robotID, TeamColor color) : robot_id(robotID), team{color} {} bool operator==(const TeamRobotID& other) const { return robot_id == other.robot_id && team == other.team; } bool operator!=(const TeamRobotID& other) const { return !(*this == other); } + bool operator<(const TeamRobotID& other) const { + if (team == other.team) { + return robot_id < other.robot_id; + } + return team == TeamColor::YELLOW; + } + bool operator>(const TeamRobotID& other) const { + if (team == other.team) { + return robot_id > other.robot_id; + } + return team == TeamColor::BLUE; + } + bool operator<=(const TeamRobotID& other) const { return *this < other || *this == other; } + bool operator>=(const TeamRobotID& other) const { return *this > other || *this == other; } RobotID robot_id; TeamColor team; }; diff --git a/roboteam_observer/observer/src/filters/vision/GeometryFilter.cpp b/roboteam_observer/observer/src/filters/vision/GeometryFilter.cpp index 63793f6d8..0bd9b0b83 100644 --- a/roboteam_observer/observer/src/filters/vision/GeometryFilter.cpp +++ b/roboteam_observer/observer/src/filters/vision/GeometryFilter.cpp @@ -24,6 +24,9 @@ bool GeometryFilter::process(const proto::SSL_GeometryData& geometryData) { if (geometryData.has_field()) { combinedGeometry.mutable_field()->CopyFrom(geometryData.field()); } + if (geometryData.has_models()) { + combinedGeometry.mutable_models()->CopyFrom(geometryData.models()); + } return true; } diff --git a/roboteam_observer/observer/src/filters/vision/VisionFilter.cpp b/roboteam_observer/observer/src/filters/vision/VisionFilter.cpp index d56a3b8eb..5a8e37e9a 100644 --- a/roboteam_observer/observer/src/filters/vision/VisionFilter.cpp +++ b/roboteam_observer/observer/src/filters/vision/VisionFilter.cpp @@ -5,8 +5,9 @@ proto::World VisionFilter::process(const std::vector& // TODO for now not extrapolating because grsim sends packets from 1970... Time extroplatedToTime = getExtrapolationTimeForPolicy(); + BallParameters ballParameters = worldFilter.getBallParameters(getGeometry()); - return worldFilter.getWorldPrediction(extroplatedToTime); + return worldFilter.getWorldPrediction(extroplatedToTime, ballParameters); } void VisionFilter::processGeometry(const std::vector& packets) { for (const auto& packet : packets) { @@ -26,7 +27,7 @@ void VisionFilter::processDetections(const std::vector } } } - worldFilter.process(detectionFrames, robotData, camera_ids); + worldFilter.process(detectionFrames, robotData, camera_ids, geomFilter); } void VisionFilter::updateRobotParameters(const TwoTeamRobotParameters& parameters) { worldFilter.updateRobotParameters(parameters); } std::optional VisionFilter::getGeometry() const { @@ -38,8 +39,19 @@ std::optional VisionFilter::getGeometry() const { Time VisionFilter::getExtrapolationTimeForPolicy() const { switch (extrapolationPolicy) { - case TimeExtrapolationPolicy::REALTIME: - return Time::now(); + case TimeExtrapolationPolicy::REALTIME:{ + auto now = Time::now(); + double msDelay = (now-lastPacketTime).asMilliSeconds(); + std::cout<<"Delay since last packet: "< 50.0){ + return lastPacketTime + Time(0.050); + } + return now; + + } case TimeExtrapolationPolicy::LAST_RECEIVED_PACKET_TIME: return lastPacketTime; } diff --git a/roboteam_observer/observer/src/filters/vision/WorldFilter.cpp b/roboteam_observer/observer/src/filters/vision/WorldFilter.cpp index d68005b0d..40fc28720 100644 --- a/roboteam_observer/observer/src/filters/vision/WorldFilter.cpp +++ b/roboteam_observer/observer/src/filters/vision/WorldFilter.cpp @@ -3,16 +3,77 @@ #include "filters/vision/ball/BallAssigner.h" WorldFilter::WorldFilter() {} -proto::World WorldFilter::getWorldPrediction(const Time &time) const { +bool botHasNonsensicalValue(const proto::WorldRobot& bot){ + double xPos = bot.pos().x(); + double yPos = bot.pos().y(); + + for(const double value : {xPos,yPos}){ + if(std::isnan(value) || std::isinf(value) || value < -10 || value > 10){ + return true; + } + } + double xVel = bot.vel().x(); + double yVel = bot.vel().y(); + for(const double value : {xVel,yVel}){ + if(std::isnan(value) || std::isinf(value) || value < -20 || value > 20){ + return true; + } + } + if(bot.yaw() > 2 * M_PI || bot.yaw() < -2 * M_PI){ + return true; + } + if(bot.w() > 20 * M_PI || bot.w() < -20 * M_PI){ + return true; + } + return false; +} + +bool ballHasNonsensicalValue(const proto::WorldBall& ball){ + if(std::fabs(ball.z_vel()) > 0.01){ + return false; //when ball is kicked, we do not do checking + } + double xPos = ball.pos().x(); + double yPos = ball.pos().y(); + + for(const double value : {xPos,yPos}){ + if(std::isnan(value) || std::isinf(value) || value < -10 || value > 10){ + return true; + } + } + double xVel = ball.vel().x(); + double yVel = ball.vel().y(); + for(const double value : {xVel,yVel}){ + if(std::isnan(value) || std::isinf(value) || value < -20 || value > 20){ + return true; + } + } + return false; +} +proto::World WorldFilter::getWorldPrediction(const Time &time, const BallParameters &ballParameters) { proto::World world; addRobotPredictionsToMessage(world, time); - addBallPredictionsToMessage(world, time); + addBallPredictionsToMessage(world, time, ballParameters); world.set_time(time.asNanoSeconds()); return world; } -void WorldFilter::process(const std::vector &frames, const std::vector &feedback, const std::vector &camera_ids) { +BallParameters WorldFilter::getBallParameters(const std::optional &geometry) const { + BallParameters ballParameters; + if (geometry.has_value()) { + ballParameters = BallParameters(geometry.value()); + } + return ballParameters; +} + +void WorldFilter::process(const std::vector &frames, const std::vector &feedback, const std::vector &camera_ids, + GeometryFilter &geomFilter) { + // populate cameraMap + for (const auto &camera : geomFilter.getCameras()) { + if (!cameraMap.hasCamera(camera.first)) { + cameraMap.addCamera(Camera(camera.second)); + } + } // Feedback is processed first, as it is not really dependent on vision packets, // but the vision processing may be helped by the feedback information feedbackFilter.process(feedback); @@ -51,15 +112,16 @@ void WorldFilter::processRobots(const DetectionFrame &frame, bool blueBots) { predictRobots(frame, robots); updateRobots(robots, detectedRobots); - updateRobotsNotSeen(frame, robots); + updateRobotsNotSeen(frame, robots, blueBots); } -void WorldFilter::updateRobotsNotSeen(const DetectionFrame &frame, robotMap &robots) { +void WorldFilter::updateRobotsNotSeen(const DetectionFrame &frame, robotMap &robots, bool blueBots) { for (auto &oneIDFilters : robots) { std::vector &filters = oneIDFilters.second; auto it = filters.begin(); while (it != filters.end()) { if (it->processNotSeen(frame.cameraID, frame.timeCaptured)) { // updates the relevant object filter. If the filter is redundant, we can remove it +// std::cout<<"removing "<<(blueBots? "blue " : "yellow ")< WorldFilter::oneCameraHealthyRobots(bool blueBots, in } return robots; } -void WorldFilter::addRobotPredictionsToMessage(proto::World &world, Time time) const { +void WorldFilter::addRobotPredictionsToMessage(proto::World &world, Time time) { auto feedbackBots = feedbackFilter.getRecentFeedback(); std::vector blueRobots = getHealthiestRobotsMerged(true, time); @@ -164,7 +226,12 @@ void WorldFilter::addRobotPredictionsToMessage(proto::World &world, Time time) c worldBot.mutable_feedbackinfo()->CopyFrom(feedback_it->second); feedbackBots.erase(feedback_it); } - world.mutable_blue()->Add()->CopyFrom(worldBot); + if(botHasNonsensicalValue(worldBot)){ + std::cout<<"Nonsense values detected for blue "<Add()->CopyFrom(worldBot); + } } std::vector yellowRobots = getHealthiestRobotsMerged(false, time); for (const auto &yellowBot : yellowRobots) { @@ -176,7 +243,12 @@ void WorldFilter::addRobotPredictionsToMessage(proto::World &world, Time time) c worldBot.mutable_feedbackinfo()->CopyFrom(feedback_it->second); feedbackBots.erase(feedback_it); } - world.mutable_yellow()->Add()->CopyFrom(worldBot); + if(botHasNonsensicalValue(worldBot)){ + std::cout<<"Nonsense values detected for yellow "<Add()->CopyFrom(worldBot); + } } // Any remaining feedback of robots is put into the lonely category for (const auto &bot : feedbackBots) { @@ -189,8 +261,11 @@ void WorldFilter::addRobotPredictionsToMessage(proto::World &world, Time time) c void WorldFilter::processBalls(const DetectionFrame &frame) { std::vector predictions(balls.size()); // get predictions from cameras + std::vector blueRobots = getHealthiestRobotsMerged(true, frame.timeCaptured); + std::vector yellowRobots = getHealthiestRobotsMerged(false, frame.timeCaptured); + for (std::size_t i = 0; i < balls.size(); ++i) { - predictions[i] = balls[i].predictCam(frame.cameraID, frame.timeCaptured).prediction; + predictions[i] = balls[i].predictCam(frame.cameraID, frame.timeCaptured, yellowRobots, blueRobots).prediction; } // assign observations to relevant filters BallAssignmentResult assignment = BallAssigner::assign_balls(predictions, frame.balls); @@ -209,12 +284,170 @@ void WorldFilter::processBalls(const DetectionFrame &frame) { balls.emplace_back(BallFilter(newBall)); } } -void WorldFilter::addBallPredictionsToMessage(proto::World &world, Time time) const { - if (!balls.empty()) { - auto bestFilter = std::max_element(balls.begin(), balls.end(), [](const BallFilter &best, const BallFilter &filter) { return best.getHealth() < filter.getHealth(); }); - FilteredBall bestBall = bestFilter->mergeBalls(time); +void WorldFilter::kickDetector(FilteredBall bestBall, Time time, const BallParameters &ballParameters) { + // Check if there's no current observation, return early + if (!bestBall.currentObservation.has_value() || bestBall.currentObservation.value().confidence < 0.1) { + return; + } + + // Get the healthiest robots for both teams + std::vector blueRobots = getHealthiestRobotsMerged(true, time); + std::vector yellowRobots = getHealthiestRobotsMerged(false, time); + addRecentData(blueRobots, yellowRobots, bestBall); + + // If not enough frames in history, return early + if (frameHistory.size() < 5) { + return; + } + + // Check if the last kick was too recent, return early + if ((frameHistory.front().filteredBall->time - lastKickTime).asSeconds() < 0.1) { + return; + } + + // Gather all balls and robots within a certain proximity + std::vector allBalls; + std::map> allRobots; + auto firstBallCameraPosition = frameHistory.front().filteredBall->positionCamera; + for (const auto &data : frameHistory) { + allBalls.push_back(data.filteredBall.value()); + for (const auto &robot : data.blue) { + if ((robot.position.position - firstBallCameraPosition).norm() < 1.0) allRobots[robot.id].push_back(robot); + } + for (const auto &robot : data.yellow) { + if ((robot.position.position - firstBallCameraPosition).norm() < 1.0) allRobots[robot.id].push_back(robot); + } + } + + // Remove robots if there's insufficient data + for (auto it = allRobots.begin(); it != allRobots.end();) { + if (it->second.size() < 5) { + it = allRobots.erase(it); + } else { + ++it; + } + } + + for (const auto &[id, filteredRobots] : allRobots) { + if (!checkDistance(filteredRobots, allBalls)) { + continue; + } + if (!checkVelocity(allBalls)) { + continue; + } + if (!checkOrientation(filteredRobots, allBalls)) { + continue; + } + if (!checkIncreasingDistance(filteredRobots, allBalls)) { + continue; + } + + lastKickTime = frameHistory.front().filteredBall->time; + mostRecentShot = ShotEvent{id, filteredRobots[0].position, allBalls[0].positionCamera, lastKickTime, allBalls}; + kickEstimator = KickEstimator(*mostRecentShot, ballParameters); + chipEstimator = ChipEstimator(*mostRecentShot, ballParameters, cameraMap); + std::cout << "Kick detected by robot " << id.robot_id.robotID << " from team " << (id.team == TeamColor::BLUE ? "blue" : "yellow") << std::endl; + break; + } +} + +bool WorldFilter::checkDistance(const std::vector &robots, const std::vector &balls) { + double initialDistance = (robots[0].position.position - balls[0].currentObservation.value().position).norm(); + double maxDistance = initialDistance; + if (initialDistance > 0.17) return false; + + for (size_t i = 1; i < 5; ++i) { + double distance = (robots[i].position.position - balls[i].currentObservation.value().position).norm(); + if (distance < initialDistance) return false; + if (distance > maxDistance) maxDistance = distance; + } + + return maxDistance >= 0.16; +} + +bool WorldFilter::checkVelocity(const std::vector &balls) { + int validVelocities = 0; + for (size_t i = 1; i < 5; ++i) { + auto previousBall = balls[i - 1].currentObservation.value(); + auto currentBall = balls[i].currentObservation.value(); + double velocity = (currentBall.position - previousBall.position).norm() / (currentBall.timeCaptured - previousBall.timeCaptured).asSeconds(); + if (velocity > 0.6) validVelocities++; + } + return validVelocities >= 2; +} + +bool WorldFilter::checkOrientation(const std::vector &robots, const std::vector &balls) { + for (size_t i = 0; i < 5; ++i) { + const auto &robot = robots[i]; + const auto &ballPosition = balls[i].currentObservation.value().position; + Eigen::Vector2d robotToBall = ballPosition - robot.position.position; + double angle = std::atan2(robotToBall.y(), robotToBall.x()); + double angleDiff = std::abs(robot.position.yaw - rtt::Angle(angle)); + if (angleDiff > 0.8) return false; + } + return true; +} + +bool WorldFilter::checkIncreasingDistance(const std::vector &robots, const std::vector &balls) { + double previousDistance = (robots[0].position.position - balls[0].currentObservation.value().position).norm(); + for (size_t i = 1; i < 5; ++i) { + double currentDistance = (robots[i].position.position - balls[i].currentObservation.value().position).norm(); + if (currentDistance < previousDistance) return false; + previousDistance = currentDistance; + } + return true; +} + +void WorldFilter::addBallPredictionsToMessage(proto::World &world, Time time, const BallParameters &ballParameters) { + BallFilter *bestFilter = nullptr; + double bestHealth = -1.0; + for (auto &filter : balls) { + if (filter.getNumObservations() > 3) { + double currentHealth = filter.getHealth(); + if (!bestFilter || currentHealth > bestHealth) { + bestFilter = &filter; + bestHealth = currentHealth; + } + } + } + if (!bestFilter) { + return; + } + FilteredBall bestBall = bestFilter->mergeBalls(time); + + kickDetector(bestBall, time, ballParameters); + if (bestBall.currentObservation.has_value()) { + if (kickEstimator.has_value()) { + kickEstimator->addFilteredBall(bestBall.currentObservation.value()); + if (kickEstimator->getAverageDistance() > 0.1) { + std::cout << "\033[31mRemoved straight kick estimator\033[0m" << std::endl; + kickEstimator = std::nullopt; + } + } + + if (chipEstimator.has_value()) { + chipEstimator->addFilteredBall(bestBall.currentObservation.value()); + if (chipEstimator->getAverageDistance() > 0.1) { + std::cout << "\033[31mRemoved chip estimator\033[0m" << std::endl; + chipEstimator = std::nullopt; + } + } + } + + // if (kickEstimator.has_value() && !chipEstimator.has_value()) { + // std::cout << "\033[32mStraight kick detected\033[0m" << std::endl; + // // kickEstimator = std::nullopt; + // } else if (chipEstimator.has_value() && !kickEstimator.has_value()) { + // std::cout << "\033[32mChip detected\033[0m" << std::endl; + // // chipEstimator = std::nullopt; + // } - world.mutable_ball()->CopyFrom(bestBall.asWorldBall()); + auto protoBall = bestBall.asWorldBall(); + if(ballHasNonsensicalValue(protoBall)){ + std::cout<<"Nonsense values detected for the ball, nuking all ball filters...\n"; + balls.clear(); + }else{ + world.mutable_ball()->CopyFrom(protoBall); } } diff --git a/roboteam_observer/observer/src/filters/vision/ball/BallFilter.cpp b/roboteam_observer/observer/src/filters/vision/ball/BallFilter.cpp index c68e0f25f..733855b5b 100644 --- a/roboteam_observer/observer/src/filters/vision/ball/BallFilter.cpp +++ b/roboteam_observer/observer/src/filters/vision/ball/BallFilter.cpp @@ -3,14 +3,14 @@ #include BallFilter::BallFilter(const BallObservation &observation) : groundFilters{std::make_pair(observation.cameraID, CameraGroundBallFilter(observation))} {} -GroundBallPrediction BallFilter::predictCam(int cameraID, Time until) const { +GroundBallPrediction BallFilter::predictCam(int cameraID, Time until, std::vector yellowRobots, std::vector blueRobots) { auto camera_filter = groundFilters.find(cameraID); if (camera_filter != groundFilters.end()) { - GroundBallPrediction prediction(camera_filter->second.predict(until), true); + GroundBallPrediction prediction(camera_filter->second.predict(until, yellowRobots, blueRobots), true); return prediction; } // no information for this camera available; we merge data from available camera's - FilteredBall estimate = mergeBalls(until); + FilteredBall estimate = mergeBalls(until, false); GroundBallPrediction prediction(CameraGroundBallPrediction(estimate.position, estimate.velocity, until), false); return prediction; } @@ -29,29 +29,36 @@ bool BallFilter::processDetections(const CameraGroundBallPredictionObservationPa } return groundFilters.empty(); } -FilteredBall BallFilter::mergeBalls(Time time) const { +FilteredBall BallFilter::mergeBalls(Time time, bool consumeObservations) { FilteredBall ball; ball.position = Eigen::Vector2d(0, 0); ball.velocity = Eigen::Vector2d(0, 0); - ball.posUncertainty = 0.0; - ball.velocityUncertainty = 0.0; + ball.positionCamera = Eigen::Vector2d(0, 0); + ball.time = time; + auto posUncertainty = 0.0; + auto velocityUncertainty = 0.0; constexpr double mergeFactor = 1.5; - for (const auto &filter : groundFilters) { - FilteredBall estimate = filter.second.getEstimate(time); + for (auto &filter : groundFilters) { + auto estimate = filter.second.getEstimate(time); double weight = 100.0 / estimate.health; double posWeight = std::pow(estimate.posUncertainty * weight, -mergeFactor); double velWeight = std::pow(estimate.velocityUncertainty * weight, -mergeFactor); ball.position += estimate.position * posWeight; ball.velocity += estimate.velocity * velWeight; - ball.posUncertainty += posWeight; - ball.velocityUncertainty += velWeight; + ball.positionCamera += filter.second.getLastObservation().position * posWeight; + posUncertainty += posWeight; + velocityUncertainty += velWeight; + if (!consumeObservations || filter.second.getLastObservationAvailableAndReset()) { + ball.currentObservation = filter.second.getLastObservation(); + } } constexpr double limit = 1e-10; - if (ball.posUncertainty >= limit) { - ball.position /= ball.posUncertainty; + if (posUncertainty >= limit) { + ball.position /= posUncertainty; + ball.positionCamera /= posUncertainty; } - if (ball.velocityUncertainty >= limit) { - ball.velocity /= ball.velocityUncertainty; + if (velocityUncertainty >= limit) { + ball.velocity /= velocityUncertainty; } return ball; @@ -63,5 +70,12 @@ double BallFilter::getHealth() const { } return maxHealth; } +double BallFilter::getNumObservations() const { + double maxTotalObservations = 0.0; + for (const auto &filter : groundFilters) { + maxTotalObservations = fmax(filter.second.numObservations(), maxTotalObservations); + } + return maxTotalObservations; +} GroundBallPrediction::GroundBallPrediction(CameraGroundBallPrediction prediction, bool hadRequestedCamera) : prediction{std::move(prediction)}, hadRequestedCamera{hadRequestedCamera} {} diff --git a/roboteam_observer/observer/src/filters/vision/ball/CameraGroundBallFilter.cpp b/roboteam_observer/observer/src/filters/vision/ball/CameraGroundBallFilter.cpp index 436b0882e..89134cc19 100644 --- a/roboteam_observer/observer/src/filters/vision/ball/CameraGroundBallFilter.cpp +++ b/roboteam_observer/observer/src/filters/vision/ball/CameraGroundBallFilter.cpp @@ -1,8 +1,9 @@ #include +#include #include -void CameraGroundBallFilter::update(const BallObservation &observation) { +void CameraGroundBallFilter::update(const BallObservation& observation) { ekf.update(observation.position); objectSeen(observation.timeCaptured); } @@ -10,45 +11,124 @@ bool CameraGroundBallFilter::updateNotSeen(Time time) { objectInvisible(time); return getHealth() <= 0.0 && consecutiveFramesNotSeen() >= 3; } -bool CameraGroundBallFilter::processDetections(const CameraGroundBallPredictionObservationPair &prediction_observation_pair) { +bool CameraGroundBallFilter::processDetections(const CameraGroundBallPredictionObservationPair& prediction_observation_pair) { predictFilter(prediction_observation_pair.prediction); bool removeFilter = false; if (prediction_observation_pair.observation.has_value()) { + setLastObservation(prediction_observation_pair.observation.value()); update(prediction_observation_pair.observation.value()); } else { removeFilter = updateNotSeen(prediction_observation_pair.prediction.time); } return removeFilter; } -void CameraGroundBallFilter::predictFilter(const CameraGroundBallPrediction &prediction) { - // simple function for now but may become complicated with collisions - ekf.predict(prediction.time); -} +void CameraGroundBallFilter::predictFilter(const CameraGroundBallPrediction& prediction) { ekf.predict(prediction.time); } Eigen::Vector2d CameraGroundBallFilter::getVelocityEstimate(Time time) const { return ekf.getVelocityEstimate(time); } -CameraGroundBallFilter::CameraGroundBallFilter(const BallObservation &observation, const Eigen::Vector2d &velocity_estimate) +CameraGroundBallFilter::CameraGroundBallFilter(const BallObservation& observation, const Eigen::Vector2d& velocity_estimate) : CameraObjectFilter(0.2, 1 / 60.0, 15, 3, observation.timeCaptured) { Eigen::Vector4d startState = {observation.position.x(), observation.position.y(), velocity_estimate.x(), velocity_estimate.y()}; + constexpr double INITIAL_POS_COV = 0.001; // [m] uncertainty in initial ball position + constexpr double INITIAL_VEL_COV = 4.0; // [m/s] uncertainty in initial ball velocity, assuming higher due to zero information Eigen::Matrix4d startCovariance = Eigen::Matrix4d::Zero(); - constexpr double BALL_POSITION_INITIAL_COV = 0.05; //[m] uncertainty in initial ball position - constexpr double BALL_VELOCITY_INITIAL_COV = 4.0; //[m/s] - - startCovariance(0, 0) = BALL_POSITION_INITIAL_COV; - startCovariance(1, 1) = BALL_POSITION_INITIAL_COV; - startCovariance(2, 2) = BALL_VELOCITY_INITIAL_COV; - startCovariance(3, 3) = BALL_VELOCITY_INITIAL_COV; + startCovariance.diagonal() << INITIAL_POS_COV, INITIAL_POS_COV, INITIAL_VEL_COV, INITIAL_VEL_COV; constexpr double BALL_MODEL_ERROR = 1.0; constexpr double BALL_MEASUREMENT_ERROR = 0.002; //[m] estimated average position uncertainty in ball detections ekf = GroundBallExtendedKalmanFilter(startState, startCovariance, BALL_MODEL_ERROR, BALL_MEASUREMENT_ERROR, observation.timeCaptured); } -CameraGroundBallPrediction CameraGroundBallFilter::predict(Time time) const { - const auto &estimate = ekf.getStateEstimate(time); + +bool CameraGroundBallFilter::checkRobotCollision(const FilteredRobot& robot, const Eigen::Vector2d& positionEstimate, const Eigen::Vector2d& velocityEstimate) { + rtt::Vector2 robotPos = rtt::Vector2(robot.position.position.x(), robot.position.position.y()); + rtt::Angle robotYaw = rtt::Angle(robot.position.yaw); + rtt::Vector2 ballPos = rtt::Vector2(positionEstimate.x(), positionEstimate.y()); + rtt::Vector2 ballVel = rtt::Vector2(velocityEstimate.x(), velocityEstimate.y()); + rtt::Vector2 robotVel = rtt::Vector2(robot.velocity.velocity.x(), robot.velocity.velocity.y()); + + if (!rtt::RobotShape(robotPos, 0.07, 0.09, robotYaw).contains(ballPos)) { + return false; + } + + auto ballVelUsed = ballVel - robotVel; + auto robotShape = rtt::RobotShape(robotPos, 0.07 + 0.0213, 0.09 + 0.0213, robotYaw); + auto frontLine = robotShape.kicker(); + auto ballVelLine = rtt::LineSegment(ballPos, ballPos - ballVelUsed.stretchToLength(0.09 * 5)); + + if (frontLine.intersects(ballVelLine)) { + auto collisionAngle = (robotYaw - rtt::Angle(ballVelUsed.scale(-1))); + + if (std::abs(collisionAngle) > 0.5 * M_PI) { + return false; + } + + auto outVelAbs = ballVelUsed.length() - (ballVelUsed.length() * std::cos(collisionAngle) * 1); + auto outVel = rtt::Vector2(robotYaw).scale(-1).rotate(collisionAngle).scale(-1).stretchToLength(outVelAbs); + outVel += robotVel; + Eigen::Vector2d outVelEigen(outVel.x, outVel.y); + ekf.setVelocity(outVelEigen); + return true; + } + + auto botCircle = rtt::Circle(robotPos, 0.09 + 0.0213); + auto intersects = botCircle.intersects(ballVelLine); + + if (!intersects.empty()) { + auto intersect = intersects[0]; + auto collisionAngle = (rtt::Vector2(robotPos - intersect).toAngle() - ballVelUsed.toAngle()); + + if (std::abs(collisionAngle) > 0.5 * M_PI) { + return false; + } + + auto outVelAbs = ballVelUsed.length() - (ballVelUsed.length() * std::cos(collisionAngle) * 0.6); + auto outVel = rtt::Vector2(robotPos - intersect).rotate(collisionAngle).scale(-1).stretchToLength(outVelAbs); + outVel += robotVel; + Eigen::Vector2d outVelEigen(outVel.x, outVel.y); + ekf.setVelocity(outVelEigen); + return true; + } + + return false; +} + +bool CameraGroundBallFilter::checkRobots(const std::vector& robots, const Eigen::Vector2d& positionEstimate, const Eigen::Vector2d& velocityEstimate) { + for (const auto& robot : robots) { + if (checkRobotCollision(robot, positionEstimate, velocityEstimate)) { + return true; + } + } + return false; +} + +CameraGroundBallPrediction CameraGroundBallFilter::predict(Time time, std::vector yellowRobots, std::vector blueRobots) { + auto positionEstimate = ekf.getPositionEstimate(time); + auto velocityEstimate = ekf.getVelocityEstimate(time); + + if (!checkRobots(yellowRobots, positionEstimate, velocityEstimate)) { + checkRobots(blueRobots, positionEstimate, velocityEstimate); + } + + const auto& estimate = ekf.getStateEstimate(time); CameraGroundBallPrediction prediction(estimate.head<2>(), estimate.tail<2>(), time); return prediction; } -FilteredBall CameraGroundBallFilter::getEstimate(Time time) const { - FilteredBall ball(ekf.getPositionEstimate(time), ekf.getVelocityEstimate(time), getHealth(), ekf.getPositionUncertainty().norm(), ekf.getVelocityUncertainty().norm()); + +BallEstimate CameraGroundBallFilter::getEstimate(Time time) const { + BallEstimate ball(ekf.getPositionEstimate(time), ekf.getVelocityEstimate(time), getHealth(), ekf.getPositionUncertainty().norm(), ekf.getVelocityUncertainty().norm()); return ball; } CameraGroundBallPrediction::CameraGroundBallPrediction(Eigen::Vector2d pos, Eigen::Vector2d vel, Time time) : position{std::move(pos)}, velocity{std::move(vel)}, time{time} {} + +BallObservation CameraGroundBallFilter::getLastObservation() const { return lastObservation; } +void CameraGroundBallFilter::setLastObservation(const BallObservation& observation) { + lastObservationUsed = false; + lastObservation = observation; +} + +bool CameraGroundBallFilter::getLastObservationAvailableAndReset() { + if (lastObservationUsed) { + return false; + } + lastObservationUsed = true; + return true; +} \ No newline at end of file diff --git a/roboteam_observer/observer/src/filters/vision/ball/FilteredBall.cpp b/roboteam_observer/observer/src/filters/vision/ball/FilteredBall.cpp index 70080a4c5..57894a921 100644 --- a/roboteam_observer/observer/src/filters/vision/ball/FilteredBall.cpp +++ b/roboteam_observer/observer/src/filters/vision/ball/FilteredBall.cpp @@ -17,5 +17,5 @@ proto::WorldBall FilteredBall::asWorldBall() const { return proto_ball; } -FilteredBall::FilteredBall(Eigen::Vector2d pos, Eigen::Vector2d vel, double health, double posUncertainty, double velocityUncertainty) - : position{std::move(pos)}, velocity{std::move(vel)}, health{health}, posUncertainty{posUncertainty}, velocityUncertainty{velocityUncertainty} {} +FilteredBall::FilteredBall(Eigen::Vector2d pos, Eigen::Vector2d vel, Time time, Eigen::Vector2d positionCamera, std::optional currentObservation) + : position(std::move(pos)), velocity(std::move(vel)), time(time), positionCamera(std::move(positionCamera)), currentObservation(std::move(currentObservation)) {} \ No newline at end of file diff --git a/roboteam_observer/observer/src/filters/vision/shot/ChipEstimator.cpp b/roboteam_observer/observer/src/filters/vision/shot/ChipEstimator.cpp new file mode 100644 index 000000000..13821bd9e --- /dev/null +++ b/roboteam_observer/observer/src/filters/vision/shot/ChipEstimator.cpp @@ -0,0 +1,116 @@ +#include "filters/shot/ChipEstimator.h" + +#include + +#include + +ChipEstimator::ChipEstimator(const ShotEvent& shotEvent, const BallParameters& ballParameters, const CameraMap& cameraMap) + : shotEvent(shotEvent), ballParameters(ballParameters), cameraMap(cameraMap) { + for (const auto& ball : shotEvent.ballsSinceShot) { + if (ball.currentObservation.has_value()) { + if ((ball.currentObservation.value().position - shotEvent.ballPosition).norm() < 0.05) { + continue; + } + ballsSinceShot.push_back(ball.currentObservation.value()); + } + } + bestShotPos = Eigen::Vector3d(shotEvent.ballPosition.x(), shotEvent.ballPosition.y(), 0); +} + +void ChipEstimator::addFilteredBall(const BallObservation& newBall) { + ballsSinceShot.push_back(newBall); + if (ballsSinceShot.size() > 50) { + ballsSinceShot.erase(ballsSinceShot.begin() + pruneIndex); + pruneIndex++; + if (pruneIndex > 40) { + pruneIndex = 1; + } + } + // only predict based on the first hop, bit janky but it _might_ work + if (doFirstHop && ballsSinceShot.size() > 8) { + auto tOff = 0.05; + auto inc = tOff / 2; + while (inc > 1e-3) { + auto resNeg = noSpinBall(tOff - 1e-5); + auto resPos = noSpinBall(tOff + 1e-5); + if (resNeg.second < resPos.second) { + tOff -= inc; + } else { + tOff += inc; + } + inc /= 2; + } + auto res = noSpinBall(tOff); + bestShotVel = res.first; + bestTOff = tOff; + + auto tFly = 2 * bestShotVel.z() / 9.81; + if ((newBall.timeCaptured - ballsSinceShot[0].timeCaptured).asSeconds() > tFly) { + doFirstHop = false; + std::cout << "\033[33mFirst hop done\033[0m" << std::endl; + } + } +} + +std::pair ChipEstimator::noSpinBall(double tOff) { + int numRecords = ballsSinceShot.size(); + auto tZero = ballsSinceShot[0].timeCaptured; + double pz = 0.02; + + Eigen::MatrixXd matA(numRecords * 2, 3); + Eigen::VectorXd b(numRecords * 2); + + for (int i = 0; i < numRecords; i++) { + const auto& ballRecord = ballsSinceShot[i]; + + auto g = ballRecord.position; + double t = (ballRecord.timeCaptured - tZero).asSeconds() + tOff; + auto f = cameraMap[ballRecord.cameraID].position(); + + matA.row(i * 2) << f.z() * t, 0, (g.x() - f.x()) * t; + matA.row((i * 2) + 1) << 0, f.z() * t, (g.y() - f.y()) * t; + + b(i * 2) = ((0.5 * 9.81 * t * t * (g.x() - f.x())) + (g.x() * f.z())) - (bestShotPos.x() * f.z()) - ((g.x() - f.x()) * pz); + b((i * 2) + 1) = ((0.5 * 9.81 * t * t * (g.y() - f.y())) + (g.y() * f.z())) - (bestShotPos.y() * f.z()) - ((g.y() - f.y()) * pz); + } + + Eigen::VectorXd x; + try { + x = matA.colPivHouseholderQr().solve(b); + } catch (const std::exception& e) { + } + auto bestVel = Eigen::Vector3d(x(0), x(1), x(2)); + auto l1Error = (matA * x - b).lpNorm<1>(); + return std::make_pair(bestVel, l1Error); +} + +double ChipEstimator::getAverageDistance(Eigen::Vector3d shotPos, Eigen::Vector3d shotVel, Eigen::Vector2d shotSpin) { + ChipTrajectory chipTrajectory = ChipTrajectory(shotPos, shotVel, shotSpin, ballParameters); + + auto tZero = ballsSinceShot[0].timeCaptured; + double totalDistance = 0; + + for (std::size_t i = 0; i < ballsSinceShot.size(); i++) { + double time = (ballsSinceShot[i].timeCaptured - tZero).asSeconds() + bestTOff; + auto realPos = chipTrajectory.getPositionAtTime(time).pos; + auto camera = cameraMap[ballsSinceShot[i].cameraID].position(); + + auto scale = camera.z() / (camera.z() - realPos.z()); + auto xPos = (realPos.x() - camera.x()) * scale + camera.x(); + auto yPos = (realPos.y() - camera.y()) * scale + camera.y(); + auto projectedPos = Eigen::Vector2d(xPos, yPos); + double distance = (projectedPos - ballsSinceShot[i].position).norm(); + totalDistance += distance; + } + + return totalDistance / ballsSinceShot.size(); +} + +double ChipEstimator::getAverageDistance() { + if (ballsSinceShot.size() < 9) { + // TODO: CHECK HOW MANY WE NEED + // std::cout << "Not enough balls to estimate chip" << std::endl; + return 0; + } + return getAverageDistance(bestShotPos, bestShotVel, Eigen::Vector2d(0, 0)); +} \ No newline at end of file diff --git a/roboteam_observer/observer/src/filters/vision/shot/ChipTrajectory.cpp b/roboteam_observer/observer/src/filters/vision/shot/ChipTrajectory.cpp new file mode 100644 index 000000000..a57b3ba64 --- /dev/null +++ b/roboteam_observer/observer/src/filters/vision/shot/ChipTrajectory.cpp @@ -0,0 +1,50 @@ +#include "observer/filters/shot/ChipTrajectory.h" + +#include + +ChipTrajectory::ChipTrajectory(const Eigen::Vector3d& initialPos, const Eigen::Vector3d& initialVel, const Eigen::Vector2d& initialSpin, const BallParameters& ballParameters) + : initialPos(initialPos), initialVel(initialVel), initialSpin(initialSpin), parameters(ballParameters) {} + +ChipTrajectory::ChipTrajectory(const Eigen::Vector3d& initialPos, const Eigen::Vector3d& initialVel, const BallParameters& ballParameters) + : initialPos(initialPos), initialVel(initialVel), initialSpin(Eigen::Vector2d::Zero()), parameters(ballParameters) {} + +ChipTrajectory::ChipTrajectory(const Eigen::Vector2d& initialPos, const Eigen::Vector3d& initialVel, const BallParameters& ballParameters) + : initialPos(initialPos.x(), initialPos.y(), 0), initialVel(initialVel), initialSpin(Eigen::Vector2d::Zero()), parameters(ballParameters) {} + +ShotState ChipTrajectory::getPositionAtTime(double time) { + Eigen::Vector3d posNow = initialPos; + Eigen::Vector3d velNow = initialVel; + double tNow = 0.0; + Eigen::Vector2d spin = initialSpin; + // Only continue if hops are at least 0.01m, smaller hops is just rolling + while ((velNow.z() * velNow.z()) / (2.0 * 9.81) > 0.01) { + double tFly = 2.0 * velNow.z() / 9.81; + if ((tNow + tFly) > time) { + double t = time - tNow; + posNow += velNow * t; + posNow.z() -= 0.5 * 9.81 * t * t; + velNow.z() -= 9.81 * t; + return ShotState{posNow, velNow}; + } + posNow += velNow * tFly; + posNow.z() = 0; + if (spin.norm() > 0) { + velNow.x() = velNow.x() * parameters.getChipDampingXYOtherHops(); + velNow.y() = velNow.y() * parameters.getChipDampingXYOtherHops(); + } else { + velNow.x() = velNow.x() * parameters.getChipDampingXYFirstHop(); + velNow.y() = velNow.y() * parameters.getChipDampingXYFirstHop(); + } + velNow.z() = velNow.z() * parameters.getChipDampingZ(); + + tNow += tFly; + spin = velNow.head<2>() * (1.0 / parameters.getBallRadius()); + } + velNow.z() = 0; + double t = time - tNow; + double tStop = -velNow.norm() / parameters.getAccRoll(); + if (t > tStop) t = tStop; + posNow += velNow * t + velNow.normalized() * (0.5 * t * t * parameters.getAccRoll()); + velNow += velNow.normalized() * (t * parameters.getAccRoll()); + return ShotState{posNow, velNow}; +} \ No newline at end of file diff --git a/roboteam_observer/observer/src/filters/vision/shot/KickEstimator.cpp b/roboteam_observer/observer/src/filters/vision/shot/KickEstimator.cpp new file mode 100644 index 000000000..92b51addd --- /dev/null +++ b/roboteam_observer/observer/src/filters/vision/shot/KickEstimator.cpp @@ -0,0 +1,158 @@ +#include "filters/shot/KickEstimator.h" + +#include + +#include + +KickEstimator::KickEstimator(const ShotEvent& shotEvent, const BallParameters& ballParameters) : shotEvent(shotEvent), ballParameters(ballParameters) { + for (const auto& ball : shotEvent.ballsSinceShot) { + if (ball.currentObservation.has_value()) { + ballsSinceShot.push_back(ball.currentObservation.value()); + } + } +} + +void KickEstimator::addFilteredBall(const BallObservation& newBall) { + ballsSinceShot.push_back(newBall); + if (ballsSinceShot.size() > 50) { + ballsSinceShot.erase(ballsSinceShot.begin() + pruneIndex); + pruneIndex++; + if (pruneIndex > 40) { + pruneIndex = 1; + } + } + // TODO: MAYBE IMPLEMENT BALL WITH INITIAL SPIN AS WELL? + std::pair slidingPosVel = slidingBall(); + // Set estimate if there is non yet + if (bestShotPos.norm() == 0 && bestShotVel.norm() == 0) { + bestShotPos = slidingPosVel.first; + bestShotVel = slidingPosVel.second; + } + std::pair nonLinPosVel = noSpinBall(); + double slidingDistance = getAverageDistance(slidingPosVel.first, slidingPosVel.second, Eigen::Vector2d(0, 0)); + double noSpinDistance = getAverageDistance(nonLinPosVel.first, nonLinPosVel.second, Eigen::Vector2d(0, 0)); + if (slidingDistance < noSpinDistance) { + bestShotPos = slidingPosVel.first; + bestShotVel = slidingPosVel.second; + } else { + bestShotPos = nonLinPosVel.first; + bestShotVel = nonLinPosVel.second; + } +} + +std::pair KickEstimator::noSpinBall() { + auto dir = getKickDir(); + auto objectiveFunction = [&](const dlib::matrix& params) -> double { + Eigen::Vector3d shotPos(params(0), params(1), 0); + auto shotVelInt = dir * params(2); + Eigen::Vector3d shotVel(shotVelInt.x(), shotVelInt.y(), 0); + Eigen::Vector2d shotSpin(0, 0); + return getAverageDistance(shotPos, shotVel, shotSpin); + }; + + // Initial guess for the parameters + dlib::matrix initialParams(3); + initialParams(0) = bestShotPos.x(); + initialParams(1) = bestShotPos.y(); + initialParams(2) = bestShotVel.norm(); + + dlib::matrix x_lower(3), x_upper(3); + x_lower(0) = bestShotPos.x() - 1; + x_lower(1) = bestShotPos.y() - 1; + x_lower(2) = bestShotVel.norm() - 1; + x_upper(0) = bestShotPos.x() + 1; + x_upper(1) = bestShotPos.y() + 1; + x_upper(2) = bestShotVel.norm() + 1; + + for (long i = 0; i < initialParams.size(); ++i) { + if (initialParams(i) < x_lower(i)) { + initialParams(i) = x_lower(i); + } else if (initialParams(i) > x_upper(i)) { + initialParams(i) = x_upper(i); + } + } + try { + dlib::find_min_bobyqa(objectiveFunction, initialParams, 7, x_lower, x_upper, 0.2, 1e-3, 400); + } catch (const dlib::bobyqa_failure& e) { + } + + auto shotPos = Eigen::Vector3d(initialParams(0), initialParams(1), 0); + auto shotVel = Eigen::Vector3d(dir.x() * initialParams(2), dir.y() * initialParams(2), 0); + return std::make_pair(shotPos, shotVel); +} + +Eigen::Vector2d KickEstimator::getKickDir() { + int numRecords = ballsSinceShot.size(); + std::vector groundPos; + for (int i = 0; i < numRecords; i++) { + groundPos.push_back(ballsSinceShot[i].position); + } + + double sumX = 0.0, sumY = 0.0, sumX2 = 0.0, sumXY = 0.0; + for (int i = 0; i < numRecords; ++i) { + double x = ballsSinceShot[i].position.x(); + double y = ballsSinceShot[i].position.y(); + sumX += x; + sumY += y; + sumX2 += x * x; + sumXY += x * y; + } + + // Calculate the slope (m) and intercept (b) + double m = (numRecords * sumXY - sumX * sumY) / (numRecords * sumX2 - sumX * sumX); + // double b = (sumY - m * sumX) / numRecords; + + Eigen::Vector2d dir(1, m); + dir.normalize(); + return dir; +} + +std::pair KickEstimator::slidingBall() { + int numRecords = ballsSinceShot.size(); + auto tZero = ballsSinceShot[0].timeCaptured; + double acc = ballParameters.getAccSlide(); + auto dir = getKickDir(); + + Eigen::MatrixXd matA(numRecords * 2, 3); + Eigen::VectorXd b(numRecords * 2); + + for (int i = 0; i < numRecords; i++) { + const auto& ballRecord = ballsSinceShot[i]; + + auto g = ballRecord.position; + double t = (ballRecord.timeCaptured - tZero).asSeconds(); + + matA.row(i * 2) << 1, 0, dir.x() * t; + matA.row(i * 2 + 1) << 0, 1, dir.y() * t; + + b(i * 2) = g.x() - (0.5 * dir.x() * t * t * acc); + b(i * 2 + 1) = g.y() - (0.5 * dir.y() * t * t * acc); + } + + Eigen::VectorXd x; + try { + x = matA.colPivHouseholderQr().solve(b); + } catch (const std::exception& e) { + return std::make_pair(Eigen::Vector3d(0, 0, 0), Eigen::Vector3d(0, 0, 0)); + } + dir *= x(2) / dir.norm(); + auto shotPos = Eigen::Vector3d(x(0), x(1), 0); + auto shotVel = Eigen::Vector3d(dir.x(), dir.y(), 0); + return std::make_pair(shotPos, shotVel); +} + +double KickEstimator::getAverageDistance(Eigen::Vector3d shotPos, Eigen::Vector3d shotVel, Eigen::Vector2d shotSpin) { + KickTrajectory kickTrajectory = KickTrajectory(shotPos, shotVel, shotSpin, ballParameters); + auto tZero = ballsSinceShot[0].timeCaptured; + double totalDistance = 0; + for (std::size_t i = 0; i < ballsSinceShot.size(); i++) { + double time = (ballsSinceShot[i].timeCaptured - tZero).asSeconds(); + auto trajState = kickTrajectory.getPositionAtTime(time); + auto pos2d = trajState.pos.head<2>(); + double distance = (pos2d - ballsSinceShot[i].position).norm(); + totalDistance += distance; + } + return totalDistance / ballsSinceShot.size(); +} + +double KickEstimator::getAverageDistance() { return getAverageDistance(bestShotPos, bestShotVel, Eigen::Vector2d(0, 0)); } \ No newline at end of file diff --git a/roboteam_observer/observer/src/filters/vision/shot/KickTrajectory.cpp b/roboteam_observer/observer/src/filters/vision/shot/KickTrajectory.cpp new file mode 100644 index 000000000..0d607ade7 --- /dev/null +++ b/roboteam_observer/observer/src/filters/vision/shot/KickTrajectory.cpp @@ -0,0 +1,59 @@ +#include "observer/filters/shot/KickTrajectory.h" + +#include +#include + +KickTrajectory::KickTrajectory(const Eigen::Vector2d& initialPos, const Eigen::Vector2d& initialVel, const Eigen::Vector2d& initialSpin, const BallParameters& ballParameters) + : initialPos(initialPos), initialVel(initialVel), initialSpin(initialSpin), parameters(ballParameters) { + Eigen::Vector2d contactVelocity = initialVel - initialSpin * parameters.getBallRadius(); + // Ball is rolling + if (contactVelocity.norm() < 0.01) { + accSlide = initialVel.normalized() * parameters.getAccRoll(); + accSlideSpin = accSlide / parameters.getBallRadius(); + tSwitch = 0.0; + // Ball is sliding + } else { + accSlide = contactVelocity.normalized() * parameters.getAccSlide(); + accSlideSpin = accSlide / (parameters.getBallRadius() * parameters.getInertiaDistribution()); + double f = 1.0 / (1.0 + 1.0 / parameters.getInertiaDistribution()); + Eigen::Vector2d slideVel = (initialSpin * parameters.getBallRadius() - initialVel) * f; + + if (std::fabs(accSlide.x()) > 0 && std::fabs(accSlide.x()) > std::fabs(accSlide.y())) { + tSwitch = slideVel.x() / accSlide.x(); + } else if (std::fabs(accSlide.y()) > 0) { + tSwitch = slideVel.y() / accSlide.y(); + } else { + tSwitch = 0.0; + } + tSwitch = std::max(0.0, tSwitch); + } + velSwitch = initialVel + (accSlide * tSwitch); + posSwitch = initialPos + (initialVel * tSwitch) + accSlide * (0.5 * tSwitch * tSwitch); + accRoll = velSwitch.normalized() * parameters.getAccRoll(); +} + +KickTrajectory::KickTrajectory(const Eigen::Vector2d& initialPos, const Eigen::Vector2d& initialVel, const BallParameters& ballParameters) + : KickTrajectory(initialPos, initialVel, Eigen::Vector2d::Zero(), ballParameters) {} + +KickTrajectory::KickTrajectory(const Eigen::Vector3d& initialPos, const Eigen::Vector3d& initialVel, const Eigen::Vector2d& initialSpin, const BallParameters& ballParameters) + : KickTrajectory(Eigen::Vector2d(initialPos.head<2>()), Eigen::Vector2d(initialVel.head<2>()), initialSpin, ballParameters) {} + +ShotState KickTrajectory::getPositionAtTime(double time) { + if (time < tSwitch) { + auto pos = initialPos + initialVel * time + accSlide * (0.5 * time * time); + auto vel = initialVel + accSlide * time; + return ShotState({pos.x(), pos.y(), 0}, {vel.x(), vel.y(), 0}); + } + auto t2 = time - tSwitch; + if (time > getTimeAtRest()) { + t2 = getTimeAtRest() - tSwitch; + } + auto pos = posSwitch + velSwitch * t2 + accRoll * (0.5 * t2 * t2); + auto vel = velSwitch + accRoll * t2; + return ShotState({pos.x(), pos.y(), 0}, {vel.x(), vel.y(), 0}); +} + +double KickTrajectory::getTimeAtRest() { + auto tStop = -velSwitch.norm() / parameters.getAccRoll(); + return tSwitch + tStop; +} \ No newline at end of file diff --git a/roboteam_observer/observer/src/parameters/RobotParameterDatabase.cpp b/roboteam_observer/observer/src/parameters/RobotParameterDatabase.cpp index 08c1d380c..d6299529c 100644 --- a/roboteam_observer/observer/src/parameters/RobotParameterDatabase.cpp +++ b/roboteam_observer/observer/src/parameters/RobotParameterDatabase.cpp @@ -28,7 +28,7 @@ TwoTeamRobotParameters RobotParameterDatabase::getParams() const { RobotParameters RobotParameterDatabase::getTeamParameters(const std::string &teamName) { // These teamnames should be the same as set in - // https://github.com/RoboCup-SSL/ssl-game-controller/blob/master/src/components/settings/team/TeamName.vue + // https://github.com/RoboCup-SSL/ssl-game-controller/blob/master/internal/app/engine/config.go // TODO: add teams we play against here. if (teamName == "RoboTeam Twente") { return RobotParameters::from_rtt2020(); diff --git a/roboteam_observer/src/Handler.cpp b/roboteam_observer/src/Handler.cpp index 14ebf7892..0cc77c3e3 100644 --- a/roboteam_observer/src/Handler.cpp +++ b/roboteam_observer/src/Handler.cpp @@ -116,7 +116,7 @@ void Handler::start(std::string visionip, std::string refereeip, int visionport, } } }, - 100); + 80); } bool Handler::initializeNetworkers() { this->worldPublisher = std::make_unique(); @@ -202,7 +202,9 @@ void Handler::startReplay(rtt::LogFileReader& reader) { auto check = observer.process(visionPackets, refereePackets, feedbackPackets); numMessagesProcessed++; - std::cout << "Num Messages processed: " << numMessagesProcessed << "\n"; + if(numMessagesProcessed % 1000 == 0){ + std::cout << "Num Messages processed: " << numMessagesProcessed << "\n"; + } } }