From e4a42049d83759cb79c64f1e749986dd60fc7ad6 Mon Sep 17 00:00:00 2001 From: Jorn Date: Wed, 26 Jun 2024 17:23:26 +0200 Subject: [PATCH 01/26] Only output balls that have 3+ observations This prevents the ball appearing on random spots, because it's only there for a very short moment --- .../include/observer/filters/vision/ball/BallFilter.h | 5 +++++ .../observer/src/filters/vision/WorldFilter.cpp | 9 ++++++--- .../observer/src/filters/vision/ball/BallFilter.cpp | 7 +++++++ 3 files changed, 18 insertions(+), 3 deletions(-) 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..c20f533a4 100644 --- a/roboteam_observer/observer/include/observer/filters/vision/ball/BallFilter.h +++ b/roboteam_observer/observer/include/observer/filters/vision/ball/BallFilter.h @@ -49,6 +49,11 @@ class BallFilter { */ [[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/src/filters/vision/WorldFilter.cpp b/roboteam_observer/observer/src/filters/vision/WorldFilter.cpp index d68005b0d..ba6253c30 100644 --- a/roboteam_observer/observer/src/filters/vision/WorldFilter.cpp +++ b/roboteam_observer/observer/src/filters/vision/WorldFilter.cpp @@ -210,11 +210,14 @@ void WorldFilter::processBalls(const DetectionFrame &frame) { } } 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(); }); + std::vector filteredBalls; + std::copy_if(balls.begin(), balls.end(), std::back_inserter(filteredBalls), [](const BallFilter &filter) { return filter.getNumObservations() > 3; }); - FilteredBall bestBall = bestFilter->mergeBalls(time); + if (!filteredBalls.empty()) { + auto bestFilter = + std::max_element(filteredBalls.begin(), filteredBalls.end(), [](const BallFilter &best, const BallFilter &filter) { return best.getHealth() < filter.getHealth(); }); + FilteredBall bestBall = bestFilter->mergeBalls(time); world.mutable_ball()->CopyFrom(bestBall.asWorldBall()); } } diff --git a/roboteam_observer/observer/src/filters/vision/ball/BallFilter.cpp b/roboteam_observer/observer/src/filters/vision/ball/BallFilter.cpp index c68e0f25f..7baa835c3 100644 --- a/roboteam_observer/observer/src/filters/vision/ball/BallFilter.cpp +++ b/roboteam_observer/observer/src/filters/vision/ball/BallFilter.cpp @@ -63,5 +63,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} {} From 202cb4f4c640c50b970e1d4942b05821498453cf Mon Sep 17 00:00:00 2001 From: Jorn Date: Thu, 27 Jun 2024 09:34:20 +0200 Subject: [PATCH 02/26] Avoid copying balls --- .../src/filters/vision/WorldFilter.cpp | 19 ++++++++++++------- 1 file changed, 12 insertions(+), 7 deletions(-) diff --git a/roboteam_observer/observer/src/filters/vision/WorldFilter.cpp b/roboteam_observer/observer/src/filters/vision/WorldFilter.cpp index ba6253c30..59357db36 100644 --- a/roboteam_observer/observer/src/filters/vision/WorldFilter.cpp +++ b/roboteam_observer/observer/src/filters/vision/WorldFilter.cpp @@ -210,13 +210,18 @@ void WorldFilter::processBalls(const DetectionFrame &frame) { } } void WorldFilter::addBallPredictionsToMessage(proto::World &world, Time time) const { - std::vector filteredBalls; - std::copy_if(balls.begin(), balls.end(), std::back_inserter(filteredBalls), [](const BallFilter &filter) { return filter.getNumObservations() > 3; }); - - if (!filteredBalls.empty()) { - auto bestFilter = - std::max_element(filteredBalls.begin(), filteredBalls.end(), [](const BallFilter &best, const BallFilter &filter) { return best.getHealth() < filter.getHealth(); }); - + const BallFilter *bestFilter = nullptr; + double bestHealth = -1.0; + for (const auto &filter : balls) { + if (filter.getNumObservations() > 3) { + double currentHealth = filter.getHealth(); + if (!bestFilter || currentHealth > bestHealth) { + bestFilter = &filter; + bestHealth = currentHealth; + } + } + } + if (bestFilter) { FilteredBall bestBall = bestFilter->mergeBalls(time); world.mutable_ball()->CopyFrom(bestBall.asWorldBall()); } From 6bf3738fdc80f8e868741ec7221f853fcfc4acf8 Mon Sep 17 00:00:00 2001 From: Jorn Date: Fri, 28 Jun 2024 17:37:33 +0200 Subject: [PATCH 03/26] Implement ball-robot collision for ball estimation for the Kalman. This helps in a quicker correct velocity+position for the ball after a collision, and prevents the ball from traveling through a robot for a bit. Still need to add the usage of robotparameters --- .../observer/filters/vision/ball/BallFilter.h | 4 +- .../vision/ball/CameraGroundBallFilter.h | 5 +- .../ball/GroundBallExtendedKalmanFilter.h | 1 + .../src/filters/vision/WorldFilter.cpp | 5 +- .../src/filters/vision/ball/BallFilter.cpp | 4 +- .../vision/ball/CameraGroundBallFilter.cpp | 99 ++++++++++++++++--- .../ball/GroundBallExtendedKalmanFilter.cpp | 5 + .../src/parameters/RobotParameterDatabase.cpp | 2 +- 8 files changed, 103 insertions(+), 22 deletions(-) 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 c20f533a4..11bef8b3e 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. 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..31ab052f5 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; @@ -24,11 +25,13 @@ class CameraGroundBallFilter : public CameraObjectFilter { [[nodiscard]] FilteredBall 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); 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); diff --git a/roboteam_observer/observer/include/observer/filters/vision/ball/GroundBallExtendedKalmanFilter.h b/roboteam_observer/observer/include/observer/filters/vision/ball/GroundBallExtendedKalmanFilter.h index 6939e731e..5721dc3a7 100644 --- a/roboteam_observer/observer/include/observer/filters/vision/ball/GroundBallExtendedKalmanFilter.h +++ b/roboteam_observer/observer/include/observer/filters/vision/ball/GroundBallExtendedKalmanFilter.h @@ -39,6 +39,7 @@ class GroundBallExtendedKalmanFilter { [[nodiscard]] Eigen::Vector4d state() const; [[nodiscard]] Eigen::Matrix4d covariance() const; + void resetCovariance(); private: void setProccessNoise(double dt); diff --git a/roboteam_observer/observer/src/filters/vision/WorldFilter.cpp b/roboteam_observer/observer/src/filters/vision/WorldFilter.cpp index 59357db36..1cbcb8155 100644 --- a/roboteam_observer/observer/src/filters/vision/WorldFilter.cpp +++ b/roboteam_observer/observer/src/filters/vision/WorldFilter.cpp @@ -189,8 +189,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); diff --git a/roboteam_observer/observer/src/filters/vision/ball/BallFilter.cpp b/roboteam_observer/observer/src/filters/vision/ball/BallFilter.cpp index 7baa835c3..615ca12c6 100644 --- a/roboteam_observer/observer/src/filters/vision/ball/BallFilter.cpp +++ b/roboteam_observer/observer/src/filters/vision/ball/BallFilter.cpp @@ -3,10 +3,10 @@ #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 diff --git a/roboteam_observer/observer/src/filters/vision/ball/CameraGroundBallFilter.cpp b/roboteam_observer/observer/src/filters/vision/ball/CameraGroundBallFilter.cpp index 436b0882e..9413837d3 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,7 +11,7 @@ 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()) { @@ -20,32 +21,98 @@ bool CameraGroundBallFilter::processDetections(const CameraGroundBallPredictionO } 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); + ekf.resetCovariance(); + 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); + ekf.resetCovariance(); + 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()); return ball; diff --git a/roboteam_observer/observer/src/filters/vision/ball/GroundBallExtendedKalmanFilter.cpp b/roboteam_observer/observer/src/filters/vision/ball/GroundBallExtendedKalmanFilter.cpp index 8bf3a9fa7..7bed13044 100644 --- a/roboteam_observer/observer/src/filters/vision/ball/GroundBallExtendedKalmanFilter.cpp +++ b/roboteam_observer/observer/src/filters/vision/ball/GroundBallExtendedKalmanFilter.cpp @@ -109,6 +109,11 @@ Eigen::Vector4d GroundBallExtendedKalmanFilter::state() const { return X; } Eigen::Matrix4d GroundBallExtendedKalmanFilter::covariance() const { return P; } +void GroundBallExtendedKalmanFilter::resetCovariance() { + P.setZero(); + P.diagonal() << 0.001, 0.001, 0.001, 0.001; +} + void GroundBallExtendedKalmanFilter::predict(Time timeStamp) { assert(timeStamp >= lastUpdateTime); double frame_dt = (timeStamp - lastUpdateTime).asSeconds(); 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(); From 7d726aceada3937d48d7dbe35d5f137e60e5ec4b Mon Sep 17 00:00:00 2001 From: Jorn Date: Sun, 30 Jun 2024 14:42:56 +0200 Subject: [PATCH 04/26] remove reset covariance --- .../filters/vision/ball/GroundBallExtendedKalmanFilter.h | 1 - .../src/filters/vision/ball/CameraGroundBallFilter.cpp | 2 -- .../filters/vision/ball/GroundBallExtendedKalmanFilter.cpp | 5 ----- 3 files changed, 8 deletions(-) diff --git a/roboteam_observer/observer/include/observer/filters/vision/ball/GroundBallExtendedKalmanFilter.h b/roboteam_observer/observer/include/observer/filters/vision/ball/GroundBallExtendedKalmanFilter.h index 5721dc3a7..6939e731e 100644 --- a/roboteam_observer/observer/include/observer/filters/vision/ball/GroundBallExtendedKalmanFilter.h +++ b/roboteam_observer/observer/include/observer/filters/vision/ball/GroundBallExtendedKalmanFilter.h @@ -39,7 +39,6 @@ class GroundBallExtendedKalmanFilter { [[nodiscard]] Eigen::Vector4d state() const; [[nodiscard]] Eigen::Matrix4d covariance() const; - void resetCovariance(); private: void setProccessNoise(double dt); diff --git a/roboteam_observer/observer/src/filters/vision/ball/CameraGroundBallFilter.cpp b/roboteam_observer/observer/src/filters/vision/ball/CameraGroundBallFilter.cpp index 9413837d3..b5a70aeb6 100644 --- a/roboteam_observer/observer/src/filters/vision/ball/CameraGroundBallFilter.cpp +++ b/roboteam_observer/observer/src/filters/vision/ball/CameraGroundBallFilter.cpp @@ -64,7 +64,6 @@ bool CameraGroundBallFilter::checkRobotCollision(const FilteredRobot& robot, con outVel += robotVel; Eigen::Vector2d outVelEigen(outVel.x, outVel.y); ekf.setVelocity(outVelEigen); - ekf.resetCovariance(); return true; } @@ -84,7 +83,6 @@ bool CameraGroundBallFilter::checkRobotCollision(const FilteredRobot& robot, con outVel += robotVel; Eigen::Vector2d outVelEigen(outVel.x, outVel.y); ekf.setVelocity(outVelEigen); - ekf.resetCovariance(); return true; } diff --git a/roboteam_observer/observer/src/filters/vision/ball/GroundBallExtendedKalmanFilter.cpp b/roboteam_observer/observer/src/filters/vision/ball/GroundBallExtendedKalmanFilter.cpp index 7bed13044..8bf3a9fa7 100644 --- a/roboteam_observer/observer/src/filters/vision/ball/GroundBallExtendedKalmanFilter.cpp +++ b/roboteam_observer/observer/src/filters/vision/ball/GroundBallExtendedKalmanFilter.cpp @@ -109,11 +109,6 @@ Eigen::Vector4d GroundBallExtendedKalmanFilter::state() const { return X; } Eigen::Matrix4d GroundBallExtendedKalmanFilter::covariance() const { return P; } -void GroundBallExtendedKalmanFilter::resetCovariance() { - P.setZero(); - P.diagonal() << 0.001, 0.001, 0.001, 0.001; -} - void GroundBallExtendedKalmanFilter::predict(Time timeStamp) { assert(timeStamp >= lastUpdateTime); double frame_dt = (timeStamp - lastUpdateTime).asSeconds(); From bf57def7bdecb7fed02a1699c4b308f8bc7bade8 Mon Sep 17 00:00:00 2001 From: Jorn Date: Tue, 2 Jul 2024 10:10:22 +0200 Subject: [PATCH 05/26] Add comparison operators to TeamRobotID for set inclusion --- .../observer/filters/vision/robot/RobotPos.h | 14 ++++++++++++++ 1 file changed, 14 insertions(+) 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; }; From 0f30585495bb16575495a93abc0411409c219a60 Mon Sep 17 00:00:00 2001 From: Jorn Date: Tue, 2 Jul 2024 10:11:49 +0200 Subject: [PATCH 06/26] Use CameraMap with data from GeoemtryFilter --- roboteam_ai/src/world/Robot.cpp | 5 +---- .../include/observer/filters/vision/GeometryFilter.h | 1 + .../include/observer/filters/vision/WorldFilter.h | 5 ++++- .../observer/src/filters/vision/VisionFilter.cpp | 2 +- .../observer/src/filters/vision/WorldFilter.cpp | 9 ++++++++- 5 files changed, 15 insertions(+), 7 deletions(-) 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/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/WorldFilter.h b/roboteam_observer/observer/include/observer/filters/vision/WorldFilter.h index 2ed7658cf..67ca1d6b3 100644 --- a/roboteam_observer/observer/include/observer/filters/vision/WorldFilter.h +++ b/roboteam_observer/observer/include/observer/filters/vision/WorldFilter.h @@ -6,7 +6,9 @@ #include #include "DetectionFrame.h" +#include "GeometryFilter.h" #include "RobotFeedbackFilter.h" +#include "observer/filters/vision/CameraMap.h" #include "observer/filters/vision/ball/BallFilter.h" #include "observer/filters/vision/robot/RobotFilter.h" #include "observer/parameters/RobotParameterDatabase.h" @@ -24,7 +26,8 @@ class WorldFilter { public: WorldFilter(); - void process(const std::vector& frames, const std::vector& feedback, const std::vector& camera_ids); + void process(const std::vector& frames, const std::vector& feedback, const std::vector& camera_ids, + GeometryFilter& geomFilter); [[nodiscard]] proto::World getWorldPrediction(const Time& time) const; diff --git a/roboteam_observer/observer/src/filters/vision/VisionFilter.cpp b/roboteam_observer/observer/src/filters/vision/VisionFilter.cpp index d56a3b8eb..b8499af6b 100644 --- a/roboteam_observer/observer/src/filters/vision/VisionFilter.cpp +++ b/roboteam_observer/observer/src/filters/vision/VisionFilter.cpp @@ -26,7 +26,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 { diff --git a/roboteam_observer/observer/src/filters/vision/WorldFilter.cpp b/roboteam_observer/observer/src/filters/vision/WorldFilter.cpp index 1cbcb8155..a3a355523 100644 --- a/roboteam_observer/observer/src/filters/vision/WorldFilter.cpp +++ b/roboteam_observer/observer/src/filters/vision/WorldFilter.cpp @@ -12,7 +12,14 @@ proto::World WorldFilter::getWorldPrediction(const Time &time) const { return world; } -void WorldFilter::process(const std::vector &frames, const std::vector &feedback, const std::vector &camera_ids) { +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); From ce8c12ad65705223803a016d63c80d2d1d2b8591 Mon Sep 17 00:00:00 2001 From: Jorn Date: Tue, 2 Jul 2024 13:46:16 +0200 Subject: [PATCH 07/26] Update BallFilter to contain more information --- .../observer/filters/vision/ball/BallFilter.h | 2 +- .../filters/vision/ball/BallObservation.h | 2 ++ .../vision/ball/CameraGroundBallFilter.h | 21 +++++++++++++- .../filters/vision/ball/FilteredBall.h | 14 +++++---- .../src/filters/vision/ball/BallFilter.cpp | 29 ++++++++++++------- .../vision/ball/CameraGroundBallFilter.cpp | 19 ++++++++++-- .../src/filters/vision/ball/FilteredBall.cpp | 4 +-- 7 files changed, 69 insertions(+), 22 deletions(-) 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 11bef8b3e..0070b52ea 100644 --- a/roboteam_observer/observer/include/observer/filters/vision/ball/BallFilter.h +++ b/roboteam_observer/observer/include/observer/filters/vision/ball/BallFilter.h @@ -44,7 +44,7 @@ class BallFilter { * @param time * @return The Predicted/Filtered ball */ - [[nodiscard]] FilteredBall mergeBalls(Time time) const; + [[nodiscard]] FilteredBall mergeBalls(Time time); /** * @return Gets the health of the ball filter 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/CameraGroundBallFilter.h b/roboteam_observer/observer/include/observer/filters/vision/ball/CameraGroundBallFilter.h index 31ab052f5..dc538c27b 100644 --- a/roboteam_observer/observer/include/observer/filters/vision/ball/CameraGroundBallFilter.h +++ b/roboteam_observer/observer/include/observer/filters/vision/ball/CameraGroundBallFilter.h @@ -19,16 +19,33 @@ 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, 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); @@ -36,6 +53,8 @@ class CameraGroundBallFilter : public CameraObjectFilter { 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/src/filters/vision/ball/BallFilter.cpp b/roboteam_observer/observer/src/filters/vision/ball/BallFilter.cpp index 615ca12c6..02c56c39b 100644 --- a/roboteam_observer/observer/src/filters/vision/ball/BallFilter.cpp +++ b/roboteam_observer/observer/src/filters/vision/ball/BallFilter.cpp @@ -29,29 +29,36 @@ bool BallFilter::processDetections(const CameraGroundBallPredictionObservationPa } return groundFilters.empty(); } -FilteredBall BallFilter::mergeBalls(Time time) const { +FilteredBall BallFilter::mergeBalls(Time time) { 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 (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; diff --git a/roboteam_observer/observer/src/filters/vision/ball/CameraGroundBallFilter.cpp b/roboteam_observer/observer/src/filters/vision/ball/CameraGroundBallFilter.cpp index b5a70aeb6..89134cc19 100644 --- a/roboteam_observer/observer/src/filters/vision/ball/CameraGroundBallFilter.cpp +++ b/roboteam_observer/observer/src/filters/vision/ball/CameraGroundBallFilter.cpp @@ -15,6 +15,7 @@ bool CameraGroundBallFilter::processDetections(const CameraGroundBallPredictionO 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); @@ -111,9 +112,23 @@ CameraGroundBallPrediction CameraGroundBallFilter::predict(Time time, std::vecto 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 From 37ee6f6529a7d342bee214fc661c765322f4114e Mon Sep 17 00:00:00 2001 From: Jorn Date: Tue, 2 Jul 2024 16:35:20 +0200 Subject: [PATCH 08/26] Implemented kick detection, result not used yet --- .../observer/filters/vision/WorldFilter.h | 26 +++- .../observer/filters/vision/ball/BallFilter.h | 2 +- .../src/filters/vision/WorldFilter.cpp | 130 +++++++++++++++++- .../src/filters/vision/ball/BallFilter.cpp | 6 +- roboteam_observer/src/Handler.cpp | 2 +- 5 files changed, 151 insertions(+), 15 deletions(-) diff --git a/roboteam_observer/observer/include/observer/filters/vision/WorldFilter.h b/roboteam_observer/observer/include/observer/filters/vision/WorldFilter.h index 67ca1d6b3..7dd7006d8 100644 --- a/roboteam_observer/observer/include/observer/filters/vision/WorldFilter.h +++ b/roboteam_observer/observer/include/observer/filters/vision/WorldFilter.h @@ -5,6 +5,8 @@ #include #include +#include + #include "DetectionFrame.h" #include "GeometryFilter.h" #include "RobotFeedbackFilter.h" @@ -28,8 +30,8 @@ class WorldFilter { void process(const std::vector& frames, const std::vector& feedback, const std::vector& camera_ids, GeometryFilter& geomFilter); - - [[nodiscard]] proto::World getWorldPrediction(const Time& time) const; + void kickDetector(FilteredBall bestBall, Time time); + [[nodiscard]] proto::World getWorldPrediction(const Time& time); void updateRobotParameters(const TwoTeamRobotParameters& robotInfo); @@ -38,6 +40,24 @@ class WorldFilter { robotMap blue; robotMap yellow; std::vector balls; + CameraMap cameraMap; + Time lastKickTime = Time::now(); + struct RecentData { + std::vector blue; + std::vector yellow; + std::optional filteredBall; + }; + 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; @@ -54,7 +74,7 @@ class WorldFilter { [[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 addBallPredictionsToMessage(proto::World& world, Time time); // 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); 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 0070b52ea..ac2d45156 100644 --- a/roboteam_observer/observer/include/observer/filters/vision/ball/BallFilter.h +++ b/roboteam_observer/observer/include/observer/filters/vision/ball/BallFilter.h @@ -44,7 +44,7 @@ class BallFilter { * @param time * @return The Predicted/Filtered ball */ - [[nodiscard]] FilteredBall mergeBalls(Time time); + [[nodiscard]] FilteredBall mergeBalls(Time time, bool consumeObservation = true); /** * @return Gets the health of the ball filter diff --git a/roboteam_observer/observer/src/filters/vision/WorldFilter.cpp b/roboteam_observer/observer/src/filters/vision/WorldFilter.cpp index a3a355523..9c9a42b62 100644 --- a/roboteam_observer/observer/src/filters/vision/WorldFilter.cpp +++ b/roboteam_observer/observer/src/filters/vision/WorldFilter.cpp @@ -3,7 +3,7 @@ #include "filters/vision/ball/BallAssigner.h" WorldFilter::WorldFilter() {} -proto::World WorldFilter::getWorldPrediction(const Time &time) const { +proto::World WorldFilter::getWorldPrediction(const Time &time) { proto::World world; addRobotPredictionsToMessage(world, time); addBallPredictionsToMessage(world, time); @@ -219,10 +219,123 @@ void WorldFilter::processBalls(const DetectionFrame &frame) { balls.emplace_back(BallFilter(newBall)); } } -void WorldFilter::addBallPredictionsToMessage(proto::World &world, Time time) const { - const BallFilter *bestFilter = nullptr; + +void WorldFilter::kickDetector(FilteredBall bestBall, Time time) { + // 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; + } + + // Kick detected, update last kick time and print the result + lastKickTime = frameHistory.front().filteredBall->time; + 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) { + BallFilter *bestFilter = nullptr; double bestHealth = -1.0; - for (const auto &filter : balls) { + for (auto &filter : balls) { if (filter.getNumObservations() > 3) { double currentHealth = filter.getHealth(); if (!bestFilter || currentHealth > bestHealth) { @@ -231,8 +344,11 @@ void WorldFilter::addBallPredictionsToMessage(proto::World &world, Time time) co } } } - if (bestFilter) { - FilteredBall bestBall = bestFilter->mergeBalls(time); - world.mutable_ball()->CopyFrom(bestBall.asWorldBall()); + if (!bestFilter) { + return; } + FilteredBall bestBall = bestFilter->mergeBalls(time); + kickDetector(bestBall, time); + + world.mutable_ball()->CopyFrom(bestBall.asWorldBall()); } diff --git a/roboteam_observer/observer/src/filters/vision/ball/BallFilter.cpp b/roboteam_observer/observer/src/filters/vision/ball/BallFilter.cpp index 02c56c39b..733855b5b 100644 --- a/roboteam_observer/observer/src/filters/vision/ball/BallFilter.cpp +++ b/roboteam_observer/observer/src/filters/vision/ball/BallFilter.cpp @@ -10,7 +10,7 @@ GroundBallPrediction BallFilter::predictCam(int cameraID, Time until, std::vecto 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,7 +29,7 @@ bool BallFilter::processDetections(const CameraGroundBallPredictionObservationPa } return groundFilters.empty(); } -FilteredBall BallFilter::mergeBalls(Time time) { +FilteredBall BallFilter::mergeBalls(Time time, bool consumeObservations) { FilteredBall ball; ball.position = Eigen::Vector2d(0, 0); ball.velocity = Eigen::Vector2d(0, 0); @@ -48,7 +48,7 @@ FilteredBall BallFilter::mergeBalls(Time time) { ball.positionCamera += filter.second.getLastObservation().position * posWeight; posUncertainty += posWeight; velocityUncertainty += velWeight; - if (filter.second.getLastObservationAvailableAndReset()) { + if (!consumeObservations || filter.second.getLastObservationAvailableAndReset()) { ball.currentObservation = filter.second.getLastObservation(); } } diff --git a/roboteam_observer/src/Handler.cpp b/roboteam_observer/src/Handler.cpp index 14ebf7892..17b2c0dba 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(); From 00a8e119bf6f423d133372de96076152bed45783 Mon Sep 17 00:00:00 2001 From: Jorn Date: Tue, 2 Jul 2024 16:47:45 +0200 Subject: [PATCH 09/26] Add kickEvent --- .../include/observer/filters/vision/WorldFilter.h | 9 +++++++++ .../observer/src/filters/vision/WorldFilter.cpp | 4 ++-- 2 files changed, 11 insertions(+), 2 deletions(-) diff --git a/roboteam_observer/observer/include/observer/filters/vision/WorldFilter.h b/roboteam_observer/observer/include/observer/filters/vision/WorldFilter.h index 7dd7006d8..1bff2c90f 100644 --- a/roboteam_observer/observer/include/observer/filters/vision/WorldFilter.h +++ b/roboteam_observer/observer/include/observer/filters/vision/WorldFilter.h @@ -47,6 +47,15 @@ class WorldFilter { std::vector yellow; std::optional filteredBall; }; + struct KickEvent { + TeamRobotID kickingBot; + RobotPos kickingBotPos; + Eigen::Vector2d ballPosition; + Time time; + std::vector ballsSinceKick; + }; + std::optional mostRecentKick; + std::deque frameHistory; void addRecentData(const std::vector& blue, const std::vector& yellow, const std::optional& filteredBall) { if (frameHistory.size() >= 5) { diff --git a/roboteam_observer/observer/src/filters/vision/WorldFilter.cpp b/roboteam_observer/observer/src/filters/vision/WorldFilter.cpp index 9c9a42b62..714bde92b 100644 --- a/roboteam_observer/observer/src/filters/vision/WorldFilter.cpp +++ b/roboteam_observer/observer/src/filters/vision/WorldFilter.cpp @@ -278,9 +278,9 @@ void WorldFilter::kickDetector(FilteredBall bestBall, Time time) { continue; } - // Kick detected, update last kick time and print the result lastKickTime = frameHistory.front().filteredBall->time; - std::cout << "Kick detected by robot " << id.robot_id.robotID << " from team " << (id.team == TeamColor::BLUE ? "blue" : "yellow") << std::endl; + mostRecentKick = KickEvent{id, filteredRobots[0].position, allBalls[0].positionCamera, lastKickTime, allBalls}; + // std::cout << "Kick detected by robot " << id.robot_id.robotID << " from team " << (id.team == TeamColor::BLUE ? "blue" : "yellow") << std::endl; break; } } From 2a41997e0d98c581a850a1b513217ec7305c6483 Mon Sep 17 00:00:00 2001 From: Jorn Date: Wed, 3 Jul 2024 17:57:34 +0200 Subject: [PATCH 10/26] Use ball model data from vision --- roboteam_observer/observer/CMakeLists.txt | 40 +++++-------------- .../include/observer/filters/kick/KickEvent.h | 20 ++++++++++ .../observer/filters/vision/WorldFilter.h | 15 +++---- .../filters/vision/ball/BallParameters.h | 35 ++++++++++++++++ .../src/filters/vision/GeometryFilter.cpp | 3 ++ .../src/filters/vision/VisionFilter.cpp | 3 +- .../src/filters/vision/WorldFilter.cpp | 16 ++++++-- 7 files changed, 87 insertions(+), 45 deletions(-) create mode 100644 roboteam_observer/observer/include/observer/filters/kick/KickEvent.h create mode 100644 roboteam_observer/observer/include/observer/filters/vision/ball/BallParameters.h diff --git a/roboteam_observer/observer/CMakeLists.txt b/roboteam_observer/observer/CMakeLists.txt index 0fd32569f..42c57afa6 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" +) +add_library(observer STATIC ${OBSERVER_SOURCES}) +find_package(dlib REQUIRED) 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/kick/KickEvent.h b/roboteam_observer/observer/include/observer/filters/kick/KickEvent.h new file mode 100644 index 000000000..819cafc50 --- /dev/null +++ b/roboteam_observer/observer/include/observer/filters/kick/KickEvent.h @@ -0,0 +1,20 @@ +#ifndef KICKEVENT_H +#define KICKEVENT_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 KickEvent { + TeamRobotID kickingBot; + RobotPos kickingBotPos; + Eigen::Vector2d ballPosition; + Time time; + std::vector ballsSinceKick; +}; + +#endif // KICKEVENT_H \ No newline at end of file diff --git a/roboteam_observer/observer/include/observer/filters/vision/WorldFilter.h b/roboteam_observer/observer/include/observer/filters/vision/WorldFilter.h index 1bff2c90f..e9187f424 100644 --- a/roboteam_observer/observer/include/observer/filters/vision/WorldFilter.h +++ b/roboteam_observer/observer/include/observer/filters/vision/WorldFilter.h @@ -12,6 +12,7 @@ #include "RobotFeedbackFilter.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" @@ -30,8 +31,9 @@ class WorldFilter { void process(const std::vector& frames, const std::vector& feedback, const std::vector& camera_ids, GeometryFilter& geomFilter); - void kickDetector(FilteredBall bestBall, Time time); - [[nodiscard]] proto::World getWorldPrediction(const Time& time); + 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); @@ -47,13 +49,6 @@ class WorldFilter { std::vector yellow; std::optional filteredBall; }; - struct KickEvent { - TeamRobotID kickingBot; - RobotPos kickingBotPos; - Eigen::Vector2d ballPosition; - Time time; - std::vector ballsSinceKick; - }; std::optional mostRecentKick; std::deque frameHistory; @@ -83,7 +78,7 @@ class WorldFilter { [[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); + 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); 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..60ffe1a29 --- /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() : ballRadius(0), accSlide(0), accRoll(0), inertiaDistribution(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/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 b8499af6b..9a535ec2d 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) { diff --git a/roboteam_observer/observer/src/filters/vision/WorldFilter.cpp b/roboteam_observer/observer/src/filters/vision/WorldFilter.cpp index 714bde92b..1fcf4be43 100644 --- a/roboteam_observer/observer/src/filters/vision/WorldFilter.cpp +++ b/roboteam_observer/observer/src/filters/vision/WorldFilter.cpp @@ -3,15 +3,23 @@ #include "filters/vision/ball/BallAssigner.h" WorldFilter::WorldFilter() {} -proto::World WorldFilter::getWorldPrediction(const Time &time) { +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; } +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 @@ -220,7 +228,7 @@ void WorldFilter::processBalls(const DetectionFrame &frame) { } } -void WorldFilter::kickDetector(FilteredBall bestBall, Time 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; @@ -332,7 +340,7 @@ bool WorldFilter::checkIncreasingDistance(const std::vector &robo return true; } -void WorldFilter::addBallPredictionsToMessage(proto::World &world, Time time) { +void WorldFilter::addBallPredictionsToMessage(proto::World &world, Time time, const BallParameters &ballParameters) { BallFilter *bestFilter = nullptr; double bestHealth = -1.0; for (auto &filter : balls) { From ab26d7df9226ed770bc6170fa4d05103e0b1ddbe Mon Sep 17 00:00:00 2001 From: Jorn Date: Fri, 5 Jul 2024 19:43:31 +0200 Subject: [PATCH 11/26] Implement Chip/Kick Trajectories Add kick estimator --- .../observer/filters/shot/ChipTrajectory.h | 25 +++ .../observer/filters/shot/KickEstimator.h | 30 ++++ .../observer/filters/shot/KickTrajectory.h | 31 ++++ .../{kick/KickEvent.h => shot/ShotEvent.h} | 18 +- .../observer/filters/vision/WorldFilter.h | 5 +- .../filters/vision/ball/BallParameters.h | 2 +- .../src/filters/vision/WorldFilter.cpp | 19 ++- .../filters/vision/shot/ChipTrajectory.cpp | 52 ++++++ .../src/filters/vision/shot/KickEstimator.cpp | 160 ++++++++++++++++++ .../filters/vision/shot/KickTrajectory.cpp | 60 +++++++ 10 files changed, 390 insertions(+), 12 deletions(-) create mode 100644 roboteam_observer/observer/include/observer/filters/shot/ChipTrajectory.h create mode 100644 roboteam_observer/observer/include/observer/filters/shot/KickEstimator.h create mode 100644 roboteam_observer/observer/include/observer/filters/shot/KickTrajectory.h rename roboteam_observer/observer/include/observer/filters/{kick/KickEvent.h => shot/ShotEvent.h} (62%) create mode 100644 roboteam_observer/observer/src/filters/vision/shot/ChipTrajectory.cpp create mode 100644 roboteam_observer/observer/src/filters/vision/shot/KickEstimator.cpp create mode 100644 roboteam_observer/observer/src/filters/vision/shot/KickTrajectory.cpp 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..98ef37e21 --- /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/vision/ball/BallParameters.h" +#include "observer/filters/shot/ShotEvent.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..89c9d131c --- /dev/null +++ b/roboteam_observer/observer/include/observer/filters/shot/KickEstimator.h @@ -0,0 +1,30 @@ +#ifndef KICKESTIMATOR_H +#define KICKESTIMATOR_H +#include +#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..931ded808 --- /dev/null +++ b/roboteam_observer/observer/include/observer/filters/shot/KickTrajectory.h @@ -0,0 +1,31 @@ +#ifndef KICK_TRAJECTORY_H +#define KICK_TRAJECTORY_H + +#include +#include +#include "observer/filters/vision/ball/BallParameters.h" +#include "observer/filters/shot/ShotEvent.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/kick/KickEvent.h b/roboteam_observer/observer/include/observer/filters/shot/ShotEvent.h similarity index 62% rename from roboteam_observer/observer/include/observer/filters/kick/KickEvent.h rename to roboteam_observer/observer/include/observer/filters/shot/ShotEvent.h index 819cafc50..004fc935a 100644 --- a/roboteam_observer/observer/include/observer/filters/kick/KickEvent.h +++ b/roboteam_observer/observer/include/observer/filters/shot/ShotEvent.h @@ -1,5 +1,5 @@ -#ifndef KICKEVENT_H -#define KICKEVENT_H +#ifndef SHOTEVENT_H +#define SHOTEVENT_H #include "observer/filters/vision/CameraMap.h" #include "observer/filters/vision/DetectionFrame.h" @@ -9,12 +9,16 @@ #include "observer/filters/vision/robot/RobotFilter.h" #include "observer/parameters/RobotParameterDatabase.h" -struct KickEvent { - TeamRobotID kickingBot; - RobotPos kickingBotPos; +struct ShotEvent { + TeamRobotID shootingBot; + RobotPos shottingBotPos; Eigen::Vector2d ballPosition; Time time; - std::vector ballsSinceKick; + std::vector ballsSinceShot; +}; +struct ShotState { + Eigen::Vector3d pos; + Eigen::Vector3d vel; }; -#endif // KICKEVENT_H \ No newline at end of file +#endif // SHOTEVENT_H \ No newline at end of file diff --git a/roboteam_observer/observer/include/observer/filters/vision/WorldFilter.h b/roboteam_observer/observer/include/observer/filters/vision/WorldFilter.h index e9187f424..78b9b1ea1 100644 --- a/roboteam_observer/observer/include/observer/filters/vision/WorldFilter.h +++ b/roboteam_observer/observer/include/observer/filters/vision/WorldFilter.h @@ -10,6 +10,8 @@ #include "DetectionFrame.h" #include "GeometryFilter.h" #include "RobotFeedbackFilter.h" +#include "observer/filters/shot/ShotEvent.h" +#include "observer/filters/shot/KickEstimator.h" #include "observer/filters/vision/CameraMap.h" #include "observer/filters/vision/ball/BallFilter.h" #include "observer/filters/vision/ball/BallParameters.h" @@ -44,12 +46,13 @@ class WorldFilter { std::vector balls; CameraMap cameraMap; Time lastKickTime = Time::now(); + std::optional kickEstimator; struct RecentData { std::vector blue; std::vector yellow; std::optional filteredBall; }; - std::optional mostRecentKick; + std::optional mostRecentShot; std::deque frameHistory; void addRecentData(const std::vector& blue, const std::vector& yellow, const std::optional& filteredBall) { diff --git a/roboteam_observer/observer/include/observer/filters/vision/ball/BallParameters.h b/roboteam_observer/observer/include/observer/filters/vision/ball/BallParameters.h index 60ffe1a29..8f7c95761 100644 --- a/roboteam_observer/observer/include/observer/filters/vision/ball/BallParameters.h +++ b/roboteam_observer/observer/include/observer/filters/vision/ball/BallParameters.h @@ -12,7 +12,7 @@ class BallParameters { double chipDampingZ; public: - BallParameters() : ballRadius(0), accSlide(0), accRoll(0), inertiaDistribution(0), chipDampingXYFirstHop(0), chipDampingXYOtherHops(0), chipDampingZ(0) {} + 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()) { diff --git a/roboteam_observer/observer/src/filters/vision/WorldFilter.cpp b/roboteam_observer/observer/src/filters/vision/WorldFilter.cpp index 1fcf4be43..02199d4a2 100644 --- a/roboteam_observer/observer/src/filters/vision/WorldFilter.cpp +++ b/roboteam_observer/observer/src/filters/vision/WorldFilter.cpp @@ -287,8 +287,10 @@ void WorldFilter::kickDetector(FilteredBall bestBall, Time time, const BallParam } lastKickTime = frameHistory.front().filteredBall->time; - mostRecentKick = KickEvent{id, filteredRobots[0].position, allBalls[0].positionCamera, lastKickTime, allBalls}; - // std::cout << "Kick detected by robot " << id.robot_id.robotID << " from team " << (id.team == TeamColor::BLUE ? "blue" : "yellow") << std::endl; + 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; } } @@ -356,7 +358,18 @@ void WorldFilter::addBallPredictionsToMessage(proto::World &world, Time time, co return; } FilteredBall bestBall = bestFilter->mergeBalls(time); - kickDetector(bestBall, 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; + } else { + std::cout << "[KICK] Average distance: " << kickEstimator->getAverageDistance() << std::endl; + } + } + } world.mutable_ball()->CopyFrom(bestBall.asWorldBall()); } 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..ebeeab113 --- /dev/null +++ b/roboteam_observer/observer/src/filters/vision/shot/ChipTrajectory.cpp @@ -0,0 +1,52 @@ +#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..adbb24f8a --- /dev/null +++ b/roboteam_observer/observer/src/filters/vision/shot/KickEstimator.cpp @@ -0,0 +1,160 @@ +#include "filters/shot/KickEstimator.h" +#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(), 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::cout << std::endl; +} + + +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, + 50); + } 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 (int 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..a371a6cb3 --- /dev/null +++ b/roboteam_observer/observer/src/filters/vision/shot/KickTrajectory.cpp @@ -0,0 +1,60 @@ +#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 From 7be63957d6fab38e98868de895d13cb73dcf0855 Mon Sep 17 00:00:00 2001 From: Jorn Date: Fri, 5 Jul 2024 22:21:04 +0200 Subject: [PATCH 12/26] Add chip estimator Everything _seems_ to be working, now all that is left is to actual use the results --- .../observer/filters/shot/ChipEstimator.h | 39 ++++++ .../observer/filters/shot/ChipTrajectory.h | 4 +- .../observer/filters/shot/KickEstimator.h | 1 + .../observer/filters/shot/KickTrajectory.h | 3 +- .../observer/filters/vision/WorldFilter.h | 4 +- .../src/filters/vision/WorldFilter.cpp | 20 ++- .../src/filters/vision/shot/ChipEstimator.cpp | 114 ++++++++++++++++++ .../filters/vision/shot/ChipTrajectory.cpp | 12 +- .../src/filters/vision/shot/KickEstimator.cpp | 16 +-- .../filters/vision/shot/KickTrajectory.cpp | 11 +- 10 files changed, 194 insertions(+), 30 deletions(-) create mode 100644 roboteam_observer/observer/include/observer/filters/shot/ChipEstimator.h create mode 100644 roboteam_observer/observer/src/filters/vision/shot/ChipEstimator.cpp 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..89ca5af67 --- /dev/null +++ b/roboteam_observer/observer/include/observer/filters/shot/ChipEstimator.h @@ -0,0 +1,39 @@ +#ifndef CHIPESTIMATOR_H +#define CHIPESTIMATOR_H + +#include + +#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 index 98ef37e21..a2af5af9d 100644 --- a/roboteam_observer/observer/include/observer/filters/shot/ChipTrajectory.h +++ b/roboteam_observer/observer/include/observer/filters/shot/ChipTrajectory.h @@ -3,8 +3,9 @@ #include #include -#include "observer/filters/vision/ball/BallParameters.h" + #include "observer/filters/shot/ShotEvent.h" +#include "observer/filters/vision/ball/BallParameters.h" class ChipTrajectory { private: @@ -14,7 +15,6 @@ class ChipTrajectory { 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); diff --git a/roboteam_observer/observer/include/observer/filters/shot/KickEstimator.h b/roboteam_observer/observer/include/observer/filters/shot/KickEstimator.h index 89c9d131c..2493611ca 100644 --- a/roboteam_observer/observer/include/observer/filters/shot/KickEstimator.h +++ b/roboteam_observer/observer/include/observer/filters/shot/KickEstimator.h @@ -1,6 +1,7 @@ #ifndef KICKESTIMATOR_H #define KICKESTIMATOR_H #include + #include #include "ShotEvent.h" diff --git a/roboteam_observer/observer/include/observer/filters/shot/KickTrajectory.h b/roboteam_observer/observer/include/observer/filters/shot/KickTrajectory.h index 931ded808..cff679199 100644 --- a/roboteam_observer/observer/include/observer/filters/shot/KickTrajectory.h +++ b/roboteam_observer/observer/include/observer/filters/shot/KickTrajectory.h @@ -3,8 +3,9 @@ #include #include -#include "observer/filters/vision/ball/BallParameters.h" + #include "observer/filters/shot/ShotEvent.h" +#include "observer/filters/vision/ball/BallParameters.h" class KickTrajectory { private: diff --git a/roboteam_observer/observer/include/observer/filters/vision/WorldFilter.h b/roboteam_observer/observer/include/observer/filters/vision/WorldFilter.h index 78b9b1ea1..5e67644af 100644 --- a/roboteam_observer/observer/include/observer/filters/vision/WorldFilter.h +++ b/roboteam_observer/observer/include/observer/filters/vision/WorldFilter.h @@ -10,8 +10,9 @@ #include "DetectionFrame.h" #include "GeometryFilter.h" #include "RobotFeedbackFilter.h" -#include "observer/filters/shot/ShotEvent.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" @@ -47,6 +48,7 @@ class WorldFilter { CameraMap cameraMap; Time lastKickTime = Time::now(); std::optional kickEstimator; + std::optional chipEstimator; struct RecentData { std::vector blue; std::vector yellow; diff --git a/roboteam_observer/observer/src/filters/vision/WorldFilter.cpp b/roboteam_observer/observer/src/filters/vision/WorldFilter.cpp index 02199d4a2..e4e44737b 100644 --- a/roboteam_observer/observer/src/filters/vision/WorldFilter.cpp +++ b/roboteam_observer/observer/src/filters/vision/WorldFilter.cpp @@ -289,7 +289,7 @@ void WorldFilter::kickDetector(FilteredBall bestBall, Time time, const BallParam 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); + 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; } @@ -365,11 +365,25 @@ void WorldFilter::addBallPredictionsToMessage(proto::World &world, Time time, co if (kickEstimator->getAverageDistance() > 0.1) { std::cout << "\033[31mRemoved straight kick estimator\033[0m" << std::endl; kickEstimator = std::nullopt; - } else { - std::cout << "[KICK] Average distance: " << kickEstimator->getAverageDistance() << std::endl; + } + } + + 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()); } 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..44dc59f06 --- /dev/null +++ b/roboteam_observer/observer/src/filters/vision/shot/ChipEstimator.cpp @@ -0,0 +1,114 @@ +#include "filters/shot/ChipEstimator.h" + +#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 index ebeeab113..a57b3ba64 100644 --- a/roboteam_observer/observer/src/filters/vision/shot/ChipTrajectory.cpp +++ b/roboteam_observer/observer/src/filters/vision/shot/ChipTrajectory.cpp @@ -1,17 +1,15 @@ #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) { -} + : 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) { -} + : 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) { -} + : initialPos(initialPos.x(), initialPos.y(), 0), initialVel(initialVel), initialSpin(Eigen::Vector2d::Zero()), parameters(ballParameters) {} ShotState ChipTrajectory::getPositionAtTime(double time) { Eigen::Vector3d posNow = initialPos; @@ -24,7 +22,7 @@ ShotState ChipTrajectory::getPositionAtTime(double time) { if ((tNow + tFly) > time) { double t = time - tNow; posNow += velNow * t; - posNow.z() += - 0.5 * 9.81 * t * t; + posNow.z() -= 0.5 * 9.81 * t * t; velNow.z() -= 9.81 * t; return ShotState{posNow, velNow}; } diff --git a/roboteam_observer/observer/src/filters/vision/shot/KickEstimator.cpp b/roboteam_observer/observer/src/filters/vision/shot/KickEstimator.cpp index adbb24f8a..a8727813c 100644 --- a/roboteam_observer/observer/src/filters/vision/shot/KickEstimator.cpp +++ b/roboteam_observer/observer/src/filters/vision/shot/KickEstimator.cpp @@ -1,4 +1,5 @@ #include "filters/shot/KickEstimator.h" + #include KickEstimator::KickEstimator(const ShotEvent& shotEvent, const BallParameters& ballParameters) : shotEvent(shotEvent), ballParameters(ballParameters) { @@ -12,7 +13,7 @@ KickEstimator::KickEstimator(const ShotEvent& shotEvent, const BallParameters& b void KickEstimator::addFilteredBall(const BallObservation& newBall) { ballsSinceShot.push_back(newBall); if (ballsSinceShot.size() > 50) { - ballsSinceShot.erase(ballsSinceShot.begin(), ballsSinceShot.begin() + pruneIndex); + ballsSinceShot.erase(ballsSinceShot.begin() + pruneIndex); pruneIndex++; if (pruneIndex > 40) { pruneIndex = 1; @@ -35,17 +36,15 @@ void KickEstimator::addFilteredBall(const BallObservation& newBall) { bestShotPos = nonLinPosVel.first; bestShotVel = nonLinPosVel.second; } - std::cout << std::endl; } - 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); + Eigen::Vector2d shotSpin(0, 0); return getAverageDistance(shotPos, shotVel, shotSpin); }; @@ -71,10 +70,7 @@ std::pair KickEstimator::noSpinBall() { } } try { - dlib::find_min_bobyqa(objectiveFunction, initialParams, 7, - x_lower, x_upper, 0.2, - 1e-3, - 50); + dlib::find_min_bobyqa(objectiveFunction, initialParams, 7, x_lower, x_upper, 0.2, 1e-3, 50); } catch (const dlib::bobyqa_failure& e) { } @@ -138,7 +134,7 @@ std::pair KickEstimator::slidingBall() { 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 shotPos = Eigen::Vector3d(x(0), x(1), 0); auto shotVel = Eigen::Vector3d(dir.x(), dir.y(), 0); return std::make_pair(shotPos, shotVel); } @@ -147,7 +143,7 @@ double KickEstimator::getAverageDistance(Eigen::Vector3d shotPos, Eigen::Vector3 KickTrajectory kickTrajectory = KickTrajectory(shotPos, shotVel, shotSpin, ballParameters); auto tZero = ballsSinceShot[0].timeCaptured; double totalDistance = 0; - for (int i = 0; i < ballsSinceShot.size(); i++) { + 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>(); diff --git a/roboteam_observer/observer/src/filters/vision/shot/KickTrajectory.cpp b/roboteam_observer/observer/src/filters/vision/shot/KickTrajectory.cpp index a371a6cb3..0d607ade7 100644 --- a/roboteam_observer/observer/src/filters/vision/shot/KickTrajectory.cpp +++ b/roboteam_observer/observer/src/filters/vision/shot/KickTrajectory.cpp @@ -1,6 +1,7 @@ #include "observer/filters/shot/KickTrajectory.h" -#include + #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) { @@ -10,7 +11,7 @@ KickTrajectory::KickTrajectory(const Eigen::Vector2d& initialPos, const Eigen::V accSlide = initialVel.normalized() * parameters.getAccRoll(); accSlideSpin = accSlide / parameters.getBallRadius(); tSwitch = 0.0; - // Ball is sliding + // Ball is sliding } else { accSlide = contactVelocity.normalized() * parameters.getAccSlide(); accSlideSpin = accSlide / (parameters.getBallRadius() * parameters.getInertiaDistribution()); @@ -32,12 +33,10 @@ KickTrajectory::KickTrajectory(const Eigen::Vector2d& initialPos, const Eigen::V } KickTrajectory::KickTrajectory(const Eigen::Vector2d& initialPos, const Eigen::Vector2d& initialVel, const BallParameters& ballParameters) - : KickTrajectory(initialPos, initialVel, Eigen::Vector2d::Zero(), 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) { -} + : KickTrajectory(Eigen::Vector2d(initialPos.head<2>()), Eigen::Vector2d(initialVel.head<2>()), initialSpin, ballParameters) {} ShotState KickTrajectory::getPositionAtTime(double time) { if (time < tSwitch) { From fc5ec5223925c3cf7fa79c73019eafc964389232 Mon Sep 17 00:00:00 2001 From: Jorn Date: Tue, 9 Jul 2024 10:48:30 +0200 Subject: [PATCH 13/26] add package needed for optimization --- .github/workflows/tests-action.yml | 2 +- docker/Dockerfile | 4 +++- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/.github/workflows/tests-action.yml b/.github/workflows/tests-action.yml index d4a39d014..5936eabd9 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 - name: Build Protobuf if: steps.cache-build-restore.outputs.cache-hit != 'true' diff --git a/docker/Dockerfile b/docker/Dockerfile index 690d2a44a..094a10af9 100644 --- a/docker/Dockerfile +++ b/docker/Dockerfile @@ -33,7 +33,8 @@ RUN apk add --no-cache bash build-base cmake make musl-dev libtool \ libzmq zeromq-dev libzmq-static eigen-dev gtest-dev \ libtbb-dev curl unzip wget zlib-dev zip nodejs npm \ openssl-dev sdl2-dev libudev-zero libudev-zero-dev libusb libusb-dev \ - qt5-qtbase-dev qt5-qtcharts-dev qt5-qtdeclarative-dev qt5-qtsvg-dev + qt5-qtbase-dev qt5-qtcharts-dev qt5-qtdeclarative-dev qt5-qtsvg-dev \ + libdlib-dev RUN npm install -g yarn WORKDIR /root/protobuf @@ -67,6 +68,7 @@ RUN apk add --no-cache \ libtbb-dev zlib-dev \ openssl-dev sdl2-dev \ qt5-qtbase-dev qt5-qtcharts-dev qt5-qtdeclarative-dev qt5-qtsvg-dev \ + libdlib-dev \ sudo nodejs npm RUN npm install -g yarn From 185f774fd822c0609f7c8210649060e46c080694 Mon Sep 17 00:00:00 2001 From: Becky Date: Wed, 10 Jul 2024 12:27:34 +0200 Subject: [PATCH 14/26] Fix docker --- docker/Dockerfile | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/docker/Dockerfile b/docker/Dockerfile index 094a10af9..c220c7811 100644 --- a/docker/Dockerfile +++ b/docker/Dockerfile @@ -34,7 +34,7 @@ RUN apk add --no-cache bash build-base cmake make musl-dev libtool \ libtbb-dev curl unzip wget zlib-dev zip nodejs npm \ openssl-dev sdl2-dev libudev-zero libudev-zero-dev libusb libusb-dev \ qt5-qtbase-dev qt5-qtcharts-dev qt5-qtdeclarative-dev qt5-qtsvg-dev \ - libdlib-dev + libdlib-dev libblas-dev liblapack-dev RUN npm install -g yarn WORKDIR /root/protobuf @@ -68,7 +68,7 @@ RUN apk add --no-cache \ libtbb-dev zlib-dev \ openssl-dev sdl2-dev \ qt5-qtbase-dev qt5-qtcharts-dev qt5-qtdeclarative-dev qt5-qtsvg-dev \ - libdlib-dev \ + libdlib-dev libblas-dev liblapack-dev \ sudo nodejs npm RUN npm install -g yarn From e1ea04fd84e7b6887e48022bd60557e76054878f Mon Sep 17 00:00:00 2001 From: Becky Date: Wed, 10 Jul 2024 14:37:46 +0200 Subject: [PATCH 15/26] Fix docker and tests perhaps --- .github/workflows/tests-action.yml | 2 +- docker/Dockerfile | 13 +++++++++++-- docker/builder/docker-compose.yml | 2 -- docker/runner/docker-compose.yml | 2 -- 4 files changed, 12 insertions(+), 7 deletions(-) diff --git a/.github/workflows/tests-action.yml b/.github/workflows/tests-action.yml index 5936eabd9..8f062868d 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 libdlib-dev + 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/docker/Dockerfile b/docker/Dockerfile index c220c7811..c5d5e0bd1 100644 --- a/docker/Dockerfile +++ b/docker/Dockerfile @@ -34,9 +34,19 @@ RUN apk add --no-cache bash build-base cmake make musl-dev libtool \ libtbb-dev curl unzip wget zlib-dev zip nodejs npm \ openssl-dev sdl2-dev libudev-zero libudev-zero-dev libusb libusb-dev \ qt5-qtbase-dev qt5-qtcharts-dev qt5-qtdeclarative-dev qt5-qtsvg-dev \ - libdlib-dev libblas-dev liblapack-dev 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 + WORKDIR /root/protobuf RUN wget https://github.com/protocolbuffers/protobuf/releases/download/v3.20.3/protobuf-cpp-3.20.3.zip -O protobuf.zip && unzip protobuf.zip && rm protobuf.zip WORKDIR /root/protobuf/protobuf-3.20.3 @@ -68,7 +78,6 @@ RUN apk add --no-cache \ libtbb-dev zlib-dev \ openssl-dev sdl2-dev \ qt5-qtbase-dev qt5-qtcharts-dev qt5-qtdeclarative-dev qt5-qtsvg-dev \ - libdlib-dev libblas-dev liblapack-dev \ sudo nodejs npm RUN npm install -g yarn 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..d31dd21d8 100644 --- a/docker/runner/docker-compose.yml +++ b/docker/runner/docker-compose.yml @@ -1,5 +1,3 @@ -version: '3' - services: roboteam_primary_ai: From 619a88d09973377adcc6b4fb6ab977980a9d7d22 Mon Sep 17 00:00:00 2001 From: Becky Date: Wed, 10 Jul 2024 16:29:58 +0200 Subject: [PATCH 16/26] Add dlib to release as well --- docker/Dockerfile | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/docker/Dockerfile b/docker/Dockerfile index c5d5e0bd1..050792b96 100644 --- a/docker/Dockerfile +++ b/docker/Dockerfile @@ -81,6 +81,17 @@ RUN apk add --no-cache \ sudo nodejs npm 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 + COPY --from=development --chown=root:root /usr/local/lib/libprotobuf.so.31 /usr/local/lib/libprotobuf.so.31 COPY --from=development --chown=root:root /usr/local/bin/protoc /usr/local/bin/protoc @@ -96,4 +107,6 @@ ENV LD_LIBRARY_PATH=$HOME/lib/ # Copy just build binaries (roboteam software) from volume to container home # Note: cannot COPY from outside context, build from parent folder +# print all dirs +RUN ls -l ../ COPY --chown=$USER:$USER ../build/release/ $HOME/ \ No newline at end of file From fa73ae8562ca5a5fef0a0997088662e68921dfc4 Mon Sep 17 00:00:00 2001 From: Becky Date: Wed, 10 Jul 2024 17:00:36 +0200 Subject: [PATCH 17/26] Docker fix for real --- docker/Dockerfile | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docker/Dockerfile b/docker/Dockerfile index 050792b96..c85e784a7 100644 --- a/docker/Dockerfile +++ b/docker/Dockerfile @@ -33,7 +33,7 @@ RUN apk add --no-cache bash build-base cmake make musl-dev libtool \ libzmq zeromq-dev libzmq-static eigen-dev gtest-dev \ libtbb-dev curl unzip wget zlib-dev zip nodejs npm \ openssl-dev sdl2-dev libudev-zero libudev-zero-dev libusb libusb-dev \ - qt5-qtbase-dev qt5-qtcharts-dev qt5-qtdeclarative-dev qt5-qtsvg-dev \ + qt5-qtbase-dev qt5-qtcharts-dev qt5-qtdeclarative-dev qt5-qtsvg-dev RUN npm install -g yarn WORKDIR /tmp From a919d2a1fc65d1f88ea8559f9ccb126b85abfb40 Mon Sep 17 00:00:00 2001 From: Becky Date: Wed, 10 Jul 2024 17:21:41 +0200 Subject: [PATCH 18/26] remove external autoref/framework as submodule, use them as image instead --- .gitmodules | 6 ------ build.sh | 23 ----------------------- docker/runner/docker-compose.yml | 9 +++------ external/autoref | 1 - external/framework | 1 - 5 files changed, 3 insertions(+), 37 deletions(-) delete mode 160000 external/autoref delete mode 160000 external/framework 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/runner/docker-compose.yml b/docker/runner/docker-compose.yml index d31dd21d8..09a1478af 100644 --- a/docker/runner/docker-compose.yml +++ b/docker/runner/docker-compose.yml @@ -165,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: @@ -180,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: @@ -195,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 From 6b2832dcebf19482b8b2b2a6f7ae3d2dc9d76062 Mon Sep 17 00:00:00 2001 From: Becky Date: Wed, 10 Jul 2024 17:41:30 +0200 Subject: [PATCH 19/26] Oeps --- docker/Dockerfile | 1 - 1 file changed, 1 deletion(-) diff --git a/docker/Dockerfile b/docker/Dockerfile index c85e784a7..8b57ea14c 100644 --- a/docker/Dockerfile +++ b/docker/Dockerfile @@ -107,6 +107,5 @@ ENV LD_LIBRARY_PATH=$HOME/lib/ # Copy just build binaries (roboteam software) from volume to container home # Note: cannot COPY from outside context, build from parent folder -# print all dirs RUN ls -l ../ COPY --chown=$USER:$USER ../build/release/ $HOME/ \ No newline at end of file From 6b2bb9b42e2bb5e946302c7fad9279a91e5eae15 Mon Sep 17 00:00:00 2001 From: Becky Date: Wed, 10 Jul 2024 17:41:42 +0200 Subject: [PATCH 20/26] Oeps --- docker/Dockerfile | 1 - 1 file changed, 1 deletion(-) diff --git a/docker/Dockerfile b/docker/Dockerfile index 8b57ea14c..8f14a53c2 100644 --- a/docker/Dockerfile +++ b/docker/Dockerfile @@ -107,5 +107,4 @@ ENV LD_LIBRARY_PATH=$HOME/lib/ # Copy just build binaries (roboteam software) from volume to container home # Note: cannot COPY from outside context, build from parent folder -RUN ls -l ../ COPY --chown=$USER:$USER ../build/release/ $HOME/ \ No newline at end of file From 0c09dca3005698b02aa76b5cf3169fd87361c405 Mon Sep 17 00:00:00 2001 From: rolfvdhulst Date: Thu, 11 Jul 2024 15:37:52 +0200 Subject: [PATCH 21/26] Finetune iteration count nonlinear kick estimator --- .../observer/src/filters/vision/shot/KickEstimator.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/roboteam_observer/observer/src/filters/vision/shot/KickEstimator.cpp b/roboteam_observer/observer/src/filters/vision/shot/KickEstimator.cpp index a8727813c..eed2725d4 100644 --- a/roboteam_observer/observer/src/filters/vision/shot/KickEstimator.cpp +++ b/roboteam_observer/observer/src/filters/vision/shot/KickEstimator.cpp @@ -70,7 +70,7 @@ std::pair KickEstimator::noSpinBall() { } } try { - dlib::find_min_bobyqa(objectiveFunction, initialParams, 7, x_lower, x_upper, 0.2, 1e-3, 50); + dlib::find_min_bobyqa(objectiveFunction, initialParams, 7, x_lower, x_upper, 0.2, 1e-3, 400); } catch (const dlib::bobyqa_failure& e) { } From 5dd52fa330044b23edd7333ddf1b02696c4ea1c4 Mon Sep 17 00:00:00 2001 From: Jorn Date: Mon, 15 Jul 2024 15:10:23 +0200 Subject: [PATCH 22/26] Move find_package before add_library --- roboteam_observer/observer/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/roboteam_observer/observer/CMakeLists.txt b/roboteam_observer/observer/CMakeLists.txt index 42c57afa6..c7d34d1bd 100644 --- a/roboteam_observer/observer/CMakeLists.txt +++ b/roboteam_observer/observer/CMakeLists.txt @@ -6,8 +6,8 @@ file(GLOB_RECURSE OBSERVER_SOURCES "${CMAKE_CURRENT_SOURCE_DIR}/include/observer/filters/vision/*.h" ) -add_library(observer STATIC ${OBSERVER_SOURCES}) find_package(dlib REQUIRED) +add_library(observer STATIC ${OBSERVER_SOURCES}) target_link_libraries(observer PUBLIC dlib::dlib PUBLIC roboteam_networking From c1c3112368d3799f83e24f6fb9020c3cc9aad9f6 Mon Sep 17 00:00:00 2001 From: Jorn Date: Mon, 15 Jul 2024 15:12:37 +0200 Subject: [PATCH 23/26] Move dlib to cpp --- .../observer/include/observer/filters/shot/ChipEstimator.h | 2 -- .../observer/include/observer/filters/shot/KickEstimator.h | 1 - .../observer/src/filters/vision/shot/ChipEstimator.cpp | 2 ++ .../observer/src/filters/vision/shot/KickEstimator.cpp | 2 ++ 4 files changed, 4 insertions(+), 3 deletions(-) diff --git a/roboteam_observer/observer/include/observer/filters/shot/ChipEstimator.h b/roboteam_observer/observer/include/observer/filters/shot/ChipEstimator.h index 89ca5af67..70881b05d 100644 --- a/roboteam_observer/observer/include/observer/filters/shot/ChipEstimator.h +++ b/roboteam_observer/observer/include/observer/filters/shot/ChipEstimator.h @@ -1,8 +1,6 @@ #ifndef CHIPESTIMATOR_H #define CHIPESTIMATOR_H -#include - #include #include #include diff --git a/roboteam_observer/observer/include/observer/filters/shot/KickEstimator.h b/roboteam_observer/observer/include/observer/filters/shot/KickEstimator.h index 2493611ca..e9ab0e39d 100644 --- a/roboteam_observer/observer/include/observer/filters/shot/KickEstimator.h +++ b/roboteam_observer/observer/include/observer/filters/shot/KickEstimator.h @@ -1,6 +1,5 @@ #ifndef KICKESTIMATOR_H #define KICKESTIMATOR_H -#include #include diff --git a/roboteam_observer/observer/src/filters/vision/shot/ChipEstimator.cpp b/roboteam_observer/observer/src/filters/vision/shot/ChipEstimator.cpp index 44dc59f06..13821bd9e 100644 --- a/roboteam_observer/observer/src/filters/vision/shot/ChipEstimator.cpp +++ b/roboteam_observer/observer/src/filters/vision/shot/ChipEstimator.cpp @@ -1,5 +1,7 @@ #include "filters/shot/ChipEstimator.h" +#include + #include ChipEstimator::ChipEstimator(const ShotEvent& shotEvent, const BallParameters& ballParameters, const CameraMap& cameraMap) diff --git a/roboteam_observer/observer/src/filters/vision/shot/KickEstimator.cpp b/roboteam_observer/observer/src/filters/vision/shot/KickEstimator.cpp index eed2725d4..92b51addd 100644 --- a/roboteam_observer/observer/src/filters/vision/shot/KickEstimator.cpp +++ b/roboteam_observer/observer/src/filters/vision/shot/KickEstimator.cpp @@ -1,5 +1,7 @@ #include "filters/shot/KickEstimator.h" +#include + #include KickEstimator::KickEstimator(const ShotEvent& shotEvent, const BallParameters& ballParameters) : shotEvent(shotEvent), ballParameters(ballParameters) { From 5eebae1f21da3075ab7c64f021f23f0932d74e4f Mon Sep 17 00:00:00 2001 From: rolfvdhulst Date: Wed, 17 Jul 2024 23:31:33 +0200 Subject: [PATCH 24/26] Detect NaN/infinities and reset filters when necessary --- .../observer/filters/vision/WorldFilter.h | 4 +- .../src/filters/vision/WorldFilter.cpp | 76 +++++++++++++++++-- roboteam_observer/src/Handler.cpp | 4 +- 3 files changed, 75 insertions(+), 9 deletions(-) diff --git a/roboteam_observer/observer/include/observer/filters/vision/WorldFilter.h b/roboteam_observer/observer/include/observer/filters/vision/WorldFilter.h index 5e67644af..95de10d5c 100644 --- a/roboteam_observer/observer/include/observer/filters/vision/WorldFilter.h +++ b/roboteam_observer/observer/include/observer/filters/vision/WorldFilter.h @@ -82,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 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/src/filters/vision/WorldFilter.cpp b/roboteam_observer/observer/src/filters/vision/WorldFilter.cpp index e4e44737b..40fc28720 100644 --- a/roboteam_observer/observer/src/filters/vision/WorldFilter.cpp +++ b/roboteam_observer/observer/src/filters/vision/WorldFilter.cpp @@ -3,6 +3,52 @@ #include "filters/vision/ball/BallAssigner.h" WorldFilter::WorldFilter() {} +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); @@ -66,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); @@ -179,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) { @@ -191,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) { @@ -358,6 +415,7 @@ void WorldFilter::addBallPredictionsToMessage(proto::World &world, Time time, co return; } FilteredBall bestBall = bestFilter->mergeBalls(time); + kickDetector(bestBall, time, ballParameters); if (bestBall.currentObservation.has_value()) { if (kickEstimator.has_value()) { @@ -385,5 +443,11 @@ void WorldFilter::addBallPredictionsToMessage(proto::World &world, Time time, co // // 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/src/Handler.cpp b/roboteam_observer/src/Handler.cpp index 17b2c0dba..0cc77c3e3 100644 --- a/roboteam_observer/src/Handler.cpp +++ b/roboteam_observer/src/Handler.cpp @@ -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"; + } } } From 382256f4ffe5ff20e8069a5006a0a10860a04df0 Mon Sep 17 00:00:00 2001 From: rolfvdhulst Date: Wed, 17 Jul 2024 23:37:04 +0200 Subject: [PATCH 25/26] Change time extrapolation and add a cap to prevent long extrapolations --- .../include/observer/filters/vision/VisionFilter.h | 2 +- .../observer/src/filters/vision/VisionFilter.cpp | 12 ++++++++++-- 2 files changed, 11 insertions(+), 3 deletions(-) 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/src/filters/vision/VisionFilter.cpp b/roboteam_observer/observer/src/filters/vision/VisionFilter.cpp index 9a535ec2d..f350d5b18 100644 --- a/roboteam_observer/observer/src/filters/vision/VisionFilter.cpp +++ b/roboteam_observer/observer/src/filters/vision/VisionFilter.cpp @@ -39,8 +39,16 @@ 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; } From e71ae8bfc22cf0117f9c8d600d7086089e527812 Mon Sep 17 00:00:00 2001 From: rolfvdhulst Date: Wed, 17 Jul 2024 23:41:01 +0200 Subject: [PATCH 26/26] Check for negative time differences in extrapolation --- roboteam_observer/observer/src/filters/vision/VisionFilter.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/roboteam_observer/observer/src/filters/vision/VisionFilter.cpp b/roboteam_observer/observer/src/filters/vision/VisionFilter.cpp index f350d5b18..5a8e37e9a 100644 --- a/roboteam_observer/observer/src/filters/vision/VisionFilter.cpp +++ b/roboteam_observer/observer/src/filters/vision/VisionFilter.cpp @@ -43,6 +43,9 @@ Time VisionFilter::getExtrapolationTimeForPolicy() const { auto now = Time::now(); double msDelay = (now-lastPacketTime).asMilliSeconds(); std::cout<<"Delay since last packet: "< 50.0){ return lastPacketTime + Time(0.050); }