void LocalisationAdapter::writeResultToBlackboard(void) { AbsCoord robotPos = L->getRobotPose(); AbsCoord ballPosition = L->getBallPosition(); RRCoord ballPosRR = ballPosition.convertToRobotRelative(robotPos); AbsCoord ballPosRRC = ballPosition.convertToRobotRelativeCartesian(robotPos); AbsCoord ballVelocity = L->getBallVelocity(); double robotPosUncertainty = L->getRobotPosUncertainty(); double robotHeadingUncertainty = L->getRobotHeadingUncertainty(); double ballPosUncertainty = L->getBallPosUncertainty(); double ballVelEigenvalue = L->getBallVelocityUncertainty(); AbsCoord nextBall(ballPosition.x() + ballVelocity.x(), ballPosition.y() + ballVelocity.y(), 0.0f); AbsCoord nextBallRRC = nextBall.convertToRobotRelativeCartesian(robotPos); AbsCoord ballVelRRC(nextBallRRC.x() - ballPosRRC.x(), nextBallRRC.y() - ballPosRRC.y(), 0.0f); Pose pose = readFrom(motion, pose); XYZ_Coord ballNeckRelative = pose.robotRelativeToNeckCoord(ballPosRR, BALL_RADIUS); uint32_t ballLostCount = L->getBallLostCount(); SharedLocalisationUpdateBundle sharedLocalisationBundle = L->getSharedUpdateData(); acquireLock(serialization); writeTo(localisation, robotPos, robotPos); writeTo(localisation, ballLostCount, ballLostCount); writeTo(localisation, ballPos, ballPosition); writeTo(localisation, ballPosRR, ballPosRR); writeTo(localisation, ballPosRRC, ballPosRRC); writeTo(localisation, ballVelRRC, ballVelRRC); writeTo(localisation, ballVel, ballVelocity); writeTo(localisation, robotPosUncertainty, robotPosUncertainty); writeTo(localisation, robotHeadingUncertainty, robotHeadingUncertainty); writeTo(localisation, ballPosUncertainty, ballPosUncertainty); writeTo(localisation, ballVelEigenvalue, ballVelEigenvalue); writeTo(localisation, teamBall, TeamBallInfo()); // TODO: make this a different value? writeTo(localisation, ballNeckRelative, ballNeckRelative); writeTo(localisation, sharedLocalisationBundle, sharedLocalisationBundle); writeTo(localisation, havePendingOutgoingSharedBundle, true); writeTo(localisation, havePendingIncomingSharedBundle, std::vector<bool>(5, false)); writeTo(localisation, robotObstacles, robotFilter->filteredRobots); releaseLock(serialization); }