28#include <unordered_map>
33#include <frc/interpolation/TimeInterpolatableBuffer.h>
34#include <frc/smartdashboard/Field2d.h>
35#include <frc/smartdashboard/FieldObject2d.h>
36#include <frc/smartdashboard/SmartDashboard.h>
44 std::string tableName =
"VisionSystemSim-" + visionSystemName;
45 frc::SmartDashboard::PutData(tableName +
"/Sim Field", &dbgField);
48 auto it = camSimMap.find(name);
49 if (it != camSimMap.end()) {
50 return std::make_optional(it->second);
56 std::vector<PhotonCameraSim*> retVal;
57 for (
auto const& cam : camSimMap) {
58 retVal.emplace_back(cam.second);
63 const frc::Transform3d& robotToCamera) {
66 if (found == camSimMap.end()) {
69 camTrfMap.insert(std::make_pair(
71 frc::TimeInterpolatableBuffer<frc::Pose3d>{bufferLength}));
72 camTrfMap.at(cameraSim).AddSample(frc::Timer::GetFPGATimestamp(),
73 frc::Pose3d{} + robotToCamera);
81 int numOfElementsRemoved =
83 if (numOfElementsRemoved == 1) {
93 units::second_t time) {
94 if (camTrfMap.find(cameraSim) != camTrfMap.end()) {
95 frc::TimeInterpolatableBuffer<frc::Pose3d> trfBuffer =
96 camTrfMap.at(cameraSim);
97 std::optional<frc::Pose3d> sample = trfBuffer.Sample(time);
101 return std::make_optional(
102 frc::Transform3d{frc::Pose3d{}, sample.value_or(frc::Pose3d{})});
109 return GetCameraPose(cameraSim, frc::Timer::GetFPGATimestamp());
112 units::second_t time) {
114 if (!robotToCamera) {
117 return std::make_optional(
GetRobotPose(time) + robotToCamera.value());
121 const frc::Transform3d& robotToCamera) {
122 if (camTrfMap.find(cameraSim) != camTrfMap.end()) {
123 camTrfMap.at(cameraSim).AddSample(frc::Timer::GetFPGATimestamp(),
124 frc::Pose3d{} + robotToCamera);
131 for (
const auto& pair : camTrfMap) {
136 units::second_t now = frc::Timer::GetFPGATimestamp();
137 if (camTrfMap.find(cameraSim) != camTrfMap.end()) {
138 auto trfBuffer = camTrfMap.at(cameraSim);
139 frc::Transform3d lastTrf{frc::Pose3d{},
140 trfBuffer.Sample(now).value_or(frc::Pose3d{})};
149 std::vector<VisionTargetSim> all{};
150 for (
const auto& entry : targetSets) {
151 for (
const auto& target : entry.second) {
152 all.emplace_back(target);
158 return targetSets[type];
164 const std::vector<VisionTargetSim>& targets) {
165 if (!targetSets.contains(type)) {
166 targetSets[type] = std::vector<VisionTargetSim>{};
168 for (
const auto& tgt : targets) {
169 targetSets[type].emplace_back(tgt);
173 std::vector<VisionTargetSim> targets;
174 for (
const frc::AprilTag& tag : layout.GetTags()) {
175 targets.emplace_back(
VisionTargetSim{layout.GetTagPose(tag.ID).value(),
184 const std::vector<VisionTargetSim>& targets) {
185 std::vector<VisionTargetSim> removedList;
186 for (
auto& entry : targetSets) {
187 for (
auto target : entry.second) {
188 auto it = std::find(targets.begin(), targets.end(), target);
189 if (it != targets.end()) {
190 removedList.emplace_back(target);
191 entry.second.erase(it);
201 return robotPoseBuffer.Sample(timestamp).value_or(frc::Pose3d{});
207 robotPoseBuffer.Clear();
208 robotPoseBuffer.AddSample(frc::Timer::GetFPGATimestamp(), robotPose);
211 void Update(
const frc::Pose2d& robotPose) {
Update(frc::Pose3d{robotPose}); }
212 void Update(
const frc::Pose3d& robotPose) {
213 for (
auto& set : targetSets) {
214 std::vector<frc::Pose2d> posesToAdd{};
215 for (
auto& target : set.second) {
216 posesToAdd.emplace_back(target.GetPose().ToPose2d());
218 dbgField.GetObject(set.first)->SetPoses(posesToAdd);
221 units::second_t now = frc::Timer::GetFPGATimestamp();
222 robotPoseBuffer.AddSample(now, robotPose);
223 dbgField.SetRobotPose(robotPose.ToPose2d());
225 std::vector<VisionTargetSim> allTargets{};
226 for (
const auto& set : targetSets) {
227 for (
const auto& target : set.second) {
228 allTargets.emplace_back(target);
232 std::vector<frc::Pose2d> visTgtPoses2d{};
233 std::vector<frc::Pose2d> cameraPoses2d{};
234 bool processed{
false};
235 for (
const auto& entry : camSimMap) {
236 auto camSim = entry.second;
237 auto optTimestamp = camSim->ConsumeNextEntryTime();
243 uint64_t timestampNt = optTimestamp.value();
244 units::second_t latency = camSim->prop.EstLatency();
245 units::second_t timestampCapture =
246 units::microsecond_t{
static_cast<double>(timestampNt)} - latency;
248 frc::Pose3d lateRobotPose =
GetRobotPose(timestampCapture);
249 frc::Pose3d lateCameraPose =
251 cameraPoses2d.push_back(lateCameraPose.ToPose2d());
253 auto camResult = camSim->Process(latency, lateCameraPose, allTargets);
254 camSim->SubmitProcessedFrame(camResult, timestampNt);
255 for (
const auto& target : camResult.GetTargets()) {
256 auto trf = target.GetBestCameraToTarget();
257 if (trf == kEmptyTrf) {
260 visTgtPoses2d.push_back(lateCameraPose.TransformBy(trf).ToPose2d());
264 dbgField.GetObject(
"visibleTargetPoses")->SetPoses(visTgtPoses2d);
266 if (cameraPoses2d.size() != 0) {
267 dbgField.GetObject(
"cameras")->SetPoses(cameraPoses2d);
272 std::unordered_map<std::string, PhotonCameraSim*> camSimMap{};
273 static constexpr units::second_t bufferLength{1.5_s};
274 std::unordered_map<PhotonCameraSim*,
275 frc::TimeInterpolatableBuffer<frc::Pose3d>>
277 frc::TimeInterpolatableBuffer<frc::Pose3d> robotPoseBuffer{bufferLength};
278 std::unordered_map<std::string, std::vector<VisionTargetSim>> targetSets{};
279 frc::Field2d dbgField{};
280 const frc::Transform3d kEmptyTrf{};
const std::string_view GetCameraName() const
Returns the name of the camera.
Definition PhotonCameraSim.h:50
PhotonCamera * GetCamera()
Definition PhotonCameraSim.h:57
Definition VisionSystemSim.h:41
void ResetRobotPose(const frc::Pose2d &robotPose)
Definition VisionSystemSim.h:203
std::optional< PhotonCameraSim * > GetCameraSim(std::string name)
Definition VisionSystemSim.h:47
std::optional< frc::Transform3d > GetRobotToCamera(PhotonCameraSim *cameraSim, units::second_t time)
Definition VisionSystemSim.h:92
std::optional< frc::Pose3d > GetCameraPose(PhotonCameraSim *cameraSim)
Definition VisionSystemSim.h:108
void ResetRobotPose(const frc::Pose3d &robotPose)
Definition VisionSystemSim.h:206
std::vector< VisionTargetSim > RemoveVisionTargets(const std::vector< VisionTargetSim > &targets)
Definition VisionSystemSim.h:183
void AddAprilTags(const frc::AprilTagFieldLayout &layout)
Definition VisionSystemSim.h:172
void Update(const frc::Pose3d &robotPose)
Definition VisionSystemSim.h:212
void AddVisionTargets(std::string type, const std::vector< VisionTargetSim > &targets)
Definition VisionSystemSim.h:163
void ResetCameraTransforms()
Definition VisionSystemSim.h:130
void RemoveVisionTargets(std::string type)
Definition VisionSystemSim.h:182
bool ResetCameraTransforms(PhotonCameraSim *cameraSim)
Definition VisionSystemSim.h:135
frc::Field2d & GetDebugField()
Definition VisionSystemSim.h:210
void Update(const frc::Pose2d &robotPose)
Definition VisionSystemSim.h:211
bool AdjustCamera(PhotonCameraSim *cameraSim, const frc::Transform3d &robotToCamera)
Definition VisionSystemSim.h:120
void AddCamera(PhotonCameraSim *cameraSim, const frc::Transform3d &robotToCamera)
Definition VisionSystemSim.h:62
std::vector< PhotonCameraSim * > GetCameraSims()
Definition VisionSystemSim.h:55
VisionSystemSim(std::string visionSystemName)
Definition VisionSystemSim.h:43
void ClearVisionTargets()
Definition VisionSystemSim.h:180
std::vector< VisionTargetSim > GetVisionTargets()
Definition VisionSystemSim.h:148
void AddVisionTargets(const std::vector< VisionTargetSim > &targets)
Definition VisionSystemSim.h:160
std::vector< VisionTargetSim > GetVisionTargets(std::string type)
Definition VisionSystemSim.h:157
frc::Pose3d GetRobotPose()
Definition VisionSystemSim.h:197
void ClearAprilTags()
Definition VisionSystemSim.h:181
void ClearCameras()
Definition VisionSystemSim.h:76
frc::Pose3d GetRobotPose(units::second_t timestamp)
Definition VisionSystemSim.h:200
std::optional< frc::Pose3d > GetCameraPose(PhotonCameraSim *cameraSim, units::second_t time)
Definition VisionSystemSim.h:111
std::optional< frc::Transform3d > GetRobotToCamera(PhotonCameraSim *cameraSim)
Definition VisionSystemSim.h:89
bool RemoveCamera(PhotonCameraSim *cameraSim)
Definition VisionSystemSim.h:80
Definition VisionTargetSim.h:34
Definition VisionEstimation.h:32
static const TargetModel kAprilTag36h11
Definition TargetModel.h:114