WIP: Observer improvements #131
Open
Conversation
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This prevents the ball appearing on random spots, because it's only there for a very short moment
…is 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
Add kick estimator
Everything _seems_ to be working, now all that is left is to actual use the results
rolfvdhulst
requested changes
Jul 11, 2024
Comment on lines
+1
to
+7
| 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" | ||
| ) |
There was a problem hiding this comment.
Never do this, this always gives problems down the line because you're compiling some file that you're not aware of. It's always better to be explicit.
roboteam_observer/observer/include/observer/filters/shot/ChipEstimator.h
Outdated
Show resolved
Hide resolved
roboteam_observer/observer/include/observer/filters/shot/KickEstimator.h
Outdated
Show resolved
Hide resolved
Comment on lines
+22
to
+28
| KickEstimator(const ShotEvent& shotEvent, const BallParameters& ballParameters); | ||
| double getAverageDistance(); | ||
| double getAverageDistance(Eigen::Vector3d shotPos, Eigen::Vector3d shotVel, Eigen::Vector2d shotSpin); | ||
| std::pair<Eigen::Vector3d, Eigen::Vector3d> slidingBall(); | ||
| Eigen::Vector2d getKickDir(); | ||
| std::pair<Eigen::Vector3d, Eigen::Vector3d> noSpinBall(); | ||
| void addFilteredBall(const BallObservation& newBall); |
There was a problem hiding this comment.
These functions need some documentation (probably not now), because their usage is not clear from their naming to me.
Comment on lines
+19
to
+26
| ballsSinceShot.push_back(newBall); | ||
| if (ballsSinceShot.size() > 50) { | ||
| ballsSinceShot.erase(ballsSinceShot.begin() + pruneIndex); | ||
| pruneIndex++; | ||
| if (pruneIndex > 40) { | ||
| pruneIndex = 1; | ||
| } | ||
| } |
There was a problem hiding this comment.
This seems hacky, and I do not understand it well...
Comment on lines
+7
to
+16
| 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); | ||
| } |
There was a problem hiding this comment.
Check that these are set; other behavior relies on these being set.
Comment on lines
+40
to
+91
| bool CameraGroundBallFilter::checkRobotCollision(const FilteredRobot& robot, const Eigen::Vector2d& positionEstimate, const Eigen::Vector2d& velocityEstimate) { | ||
| rtt::Vector2 robotPos = rtt::Vector2(robot.position.position.x(), robot.position.position.y()); | ||
| rtt::Angle robotYaw = rtt::Angle(robot.position.yaw); | ||
| rtt::Vector2 ballPos = rtt::Vector2(positionEstimate.x(), positionEstimate.y()); | ||
| rtt::Vector2 ballVel = rtt::Vector2(velocityEstimate.x(), velocityEstimate.y()); | ||
| rtt::Vector2 robotVel = rtt::Vector2(robot.velocity.velocity.x(), robot.velocity.velocity.y()); | ||
|
|
||
| if (!rtt::RobotShape(robotPos, 0.07, 0.09, robotYaw).contains(ballPos)) { | ||
| return false; | ||
| } | ||
|
|
||
| auto ballVelUsed = ballVel - robotVel; | ||
| auto robotShape = rtt::RobotShape(robotPos, 0.07 + 0.0213, 0.09 + 0.0213, robotYaw); | ||
| auto frontLine = robotShape.kicker(); | ||
| auto ballVelLine = rtt::LineSegment(ballPos, ballPos - ballVelUsed.stretchToLength(0.09 * 5)); | ||
|
|
||
| if (frontLine.intersects(ballVelLine)) { | ||
| auto collisionAngle = (robotYaw - rtt::Angle(ballVelUsed.scale(-1))); | ||
|
|
||
| if (std::abs(collisionAngle) > 0.5 * M_PI) { | ||
| return false; | ||
| } | ||
|
|
||
| auto outVelAbs = ballVelUsed.length() - (ballVelUsed.length() * std::cos(collisionAngle) * 1); | ||
| auto outVel = rtt::Vector2(robotYaw).scale(-1).rotate(collisionAngle).scale(-1).stretchToLength(outVelAbs); | ||
| outVel += robotVel; | ||
| Eigen::Vector2d outVelEigen(outVel.x, outVel.y); | ||
| ekf.setVelocity(outVelEigen); | ||
| return true; | ||
| } | ||
|
|
||
| auto botCircle = rtt::Circle(robotPos, 0.09 + 0.0213); | ||
| auto intersects = botCircle.intersects(ballVelLine); | ||
|
|
||
| if (!intersects.empty()) { | ||
| auto intersect = intersects[0]; | ||
| auto collisionAngle = (rtt::Vector2(robotPos - intersect).toAngle() - ballVelUsed.toAngle()); | ||
|
|
||
| if (std::abs(collisionAngle) > 0.5 * M_PI) { | ||
| return false; | ||
| } | ||
|
|
||
| auto outVelAbs = ballVelUsed.length() - (ballVelUsed.length() * std::cos(collisionAngle) * 0.6); | ||
| auto outVel = rtt::Vector2(robotPos - intersect).rotate(collisionAngle).scale(-1).stretchToLength(outVelAbs); | ||
| outVel += robotVel; | ||
| Eigen::Vector2d outVelEigen(outVel.x, outVel.y); | ||
| ekf.setVelocity(outVelEigen); | ||
| return true; | ||
| } | ||
|
|
||
| return false; | ||
| } |
There was a problem hiding this comment.
Maybe want to create a function in utils that tells you if they intersect and if so if they intersect on the kicker or the hull.
There was a problem hiding this comment.
There is already a function that checks intersection, so this should not be much extra work.
Comment on lines
+106
to
+108
| if (!checkRobots(yellowRobots, positionEstimate, velocityEstimate)) { | ||
| checkRobots(blueRobots, positionEstimate, velocityEstimate); | ||
| } |
There was a problem hiding this comment.
This might give 'wrong results', because multiple robots might collide with the ball trajectory. It would be better to check for the closest one.
| checkRobots(blueRobots, positionEstimate, velocityEstimate); | ||
| } | ||
|
|
||
| const auto& estimate = ekf.getStateEstimate(time); |
There was a problem hiding this comment.
Needs a comment that the checkRobots() function adjusts the EKF. now it seems like you're doing a useless computation.
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
Add this suggestion to a batch that can be applied as a single commit.
This suggestion is invalid because no changes were made to the code.
Suggestions cannot be applied while the pull request is closed.
Suggestions cannot be applied while viewing a subset of changes.
Only one suggestion per line can be applied in a batch.
Add this suggestion to a batch that can be applied as a single commit.
Applying suggestions on deleted lines is not supported.
You must change the existing code in this line in order to create a valid suggestion.
Outdated suggestions cannot be applied.
This suggestion has been applied or marked resolved.
Suggestions cannot be applied from pending reviews.
Suggestions cannot be applied on multi-line comments.
Suggestions cannot be applied while the pull request is queued to merge.
Suggestion cannot be applied right now. Please check back later.
Only output balls visible for 3+ frames
Added ball-robot collision for quicker tracking after collision
Added shot detection, kick+chip trajectories and estimator to know what happened