PhotonVision C++ dev-v2025.0.0-beta-8-22-gfa2034d3
All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Concepts
VisionSystemSim.h
Go to the documentation of this file.
1/*
2 * MIT License
3 *
4 * Copyright (c) PhotonVision
5 *
6 * Permission is hereby granted, free of charge, to any person obtaining a copy
7 * of this software and associated documentation files (the "Software"), to deal
8 * in the Software without restriction, including without limitation the rights
9 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
10 * copies of the Software, and to permit persons to whom the Software is
11 * furnished to do so, subject to the following conditions:
12 *
13 * The above copyright notice and this permission notice shall be included in
14 * all copies or substantial portions of the Software.
15 *
16 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
17 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
18 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
19 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
20 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
21 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
22 * SOFTWARE.
23 */
24
25#pragma once
26
27#include <string>
28#include <unordered_map>
29#include <utility>
30#include <vector>
31
32#include <frc/Timer.h>
33#include <frc/interpolation/TimeInterpolatableBuffer.h>
34#include <frc/smartdashboard/Field2d.h>
35#include <frc/smartdashboard/FieldObject2d.h>
36#include <frc/smartdashboard/SmartDashboard.h>
37
39
40namespace photon {
42 public:
43 explicit VisionSystemSim(std::string visionSystemName) {
44 std::string tableName = "VisionSystemSim-" + visionSystemName;
45 frc::SmartDashboard::PutData(tableName + "/Sim Field", &dbgField);
46 }
47 std::optional<PhotonCameraSim*> GetCameraSim(std::string name) {
48 auto it = camSimMap.find(name);
49 if (it != camSimMap.end()) {
50 return std::make_optional(it->second);
51 } else {
52 return std::nullopt;
53 }
54 }
55 std::vector<PhotonCameraSim*> GetCameraSims() {
56 std::vector<PhotonCameraSim*> retVal;
57 for (auto const& cam : camSimMap) {
58 retVal.emplace_back(cam.second);
59 }
60 return retVal;
61 }
62 void AddCamera(PhotonCameraSim* cameraSim,
63 const frc::Transform3d& robotToCamera) {
64 auto found =
65 camSimMap.find(std::string{cameraSim->GetCamera()->GetCameraName()});
66 if (found == camSimMap.end()) {
67 camSimMap[std::string{cameraSim->GetCamera()->GetCameraName()}] =
68 cameraSim;
69 camTrfMap.insert(std::make_pair(
70 std::move(cameraSim),
71 frc::TimeInterpolatableBuffer<frc::Pose3d>{bufferLength}));
72 camTrfMap.at(cameraSim).AddSample(frc::Timer::GetFPGATimestamp(),
73 frc::Pose3d{} + robotToCamera);
74 }
75 }
76 void ClearCameras() {
77 camSimMap.clear();
78 camTrfMap.clear();
79 }
80 bool RemoveCamera(PhotonCameraSim* cameraSim) {
81 int numOfElementsRemoved =
82 camSimMap.erase(std::string{cameraSim->GetCamera()->GetCameraName()});
83 if (numOfElementsRemoved == 1) {
84 return true;
85 } else {
86 return false;
87 }
88 }
89 std::optional<frc::Transform3d> GetRobotToCamera(PhotonCameraSim* cameraSim) {
90 return GetRobotToCamera(cameraSim, frc::Timer::GetFPGATimestamp());
91 }
92 std::optional<frc::Transform3d> GetRobotToCamera(PhotonCameraSim* cameraSim,
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);
98 if (!sample) {
99 return std::nullopt;
100 } else {
101 return std::make_optional(
102 frc::Transform3d{frc::Pose3d{}, sample.value_or(frc::Pose3d{})});
103 }
104 } else {
105 return std::nullopt;
106 }
107 }
108 std::optional<frc::Pose3d> GetCameraPose(PhotonCameraSim* cameraSim) {
109 return GetCameraPose(cameraSim, frc::Timer::GetFPGATimestamp());
110 }
111 std::optional<frc::Pose3d> GetCameraPose(PhotonCameraSim* cameraSim,
112 units::second_t time) {
113 auto robotToCamera = GetRobotToCamera(cameraSim, time);
114 if (!robotToCamera) {
115 return std::nullopt;
116 } else {
117 return std::make_optional(GetRobotPose(time) + robotToCamera.value());
118 }
119 }
121 const frc::Transform3d& robotToCamera) {
122 if (camTrfMap.find(cameraSim) != camTrfMap.end()) {
123 camTrfMap.at(cameraSim).AddSample(frc::Timer::GetFPGATimestamp(),
124 frc::Pose3d{} + robotToCamera);
125 return true;
126 } else {
127 return false;
128 }
129 }
131 for (const auto& pair : camTrfMap) {
132 ResetCameraTransforms(pair.first);
133 }
134 }
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{})};
141 trfBuffer.Clear();
142 AdjustCamera(cameraSim, lastTrf);
143 return true;
144 } else {
145 return false;
146 }
147 }
148 std::vector<VisionTargetSim> GetVisionTargets() {
149 std::vector<VisionTargetSim> all{};
150 for (const auto& entry : targetSets) {
151 for (const auto& target : entry.second) {
152 all.emplace_back(target);
153 }
154 }
155 return all;
156 }
157 std::vector<VisionTargetSim> GetVisionTargets(std::string type) {
158 return targetSets[type];
159 }
160 void AddVisionTargets(const std::vector<VisionTargetSim>& targets) {
161 AddVisionTargets("targets", targets);
162 }
163 void AddVisionTargets(std::string type,
164 const std::vector<VisionTargetSim>& targets) {
165 if (!targetSets.contains(type)) {
166 targetSets[type] = std::vector<VisionTargetSim>{};
167 }
168 for (const auto& tgt : targets) {
169 targetSets[type].emplace_back(tgt);
170 }
171 }
172 void AddAprilTags(const frc::AprilTagFieldLayout& layout) {
173 std::vector<VisionTargetSim> targets;
174 for (const frc::AprilTag& tag : layout.GetTags()) {
175 targets.emplace_back(VisionTargetSim{layout.GetTagPose(tag.ID).value(),
176 photon::kAprilTag36h11, tag.ID});
177 }
178 AddVisionTargets("apriltag", targets);
179 }
180 void ClearVisionTargets() { targetSets.clear(); }
181 void ClearAprilTags() { RemoveVisionTargets("apriltag"); }
182 void RemoveVisionTargets(std::string type) { targetSets.erase(type); }
183 std::vector<VisionTargetSim> RemoveVisionTargets(
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);
192 }
193 }
194 }
195 return removedList;
196 }
197 frc::Pose3d GetRobotPose() {
198 return GetRobotPose(frc::Timer::GetFPGATimestamp());
199 }
200 frc::Pose3d GetRobotPose(units::second_t timestamp) {
201 return robotPoseBuffer.Sample(timestamp).value_or(frc::Pose3d{});
202 }
203 void ResetRobotPose(const frc::Pose2d& robotPose) {
204 ResetRobotPose(frc::Pose3d{robotPose});
205 }
206 void ResetRobotPose(const frc::Pose3d& robotPose) {
207 robotPoseBuffer.Clear();
208 robotPoseBuffer.AddSample(frc::Timer::GetFPGATimestamp(), robotPose);
209 }
210 frc::Field2d& GetDebugField() { return dbgField; }
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());
217 }
218 dbgField.GetObject(set.first)->SetPoses(posesToAdd);
219 }
220
221 units::second_t now = frc::Timer::GetFPGATimestamp();
222 robotPoseBuffer.AddSample(now, robotPose);
223 dbgField.SetRobotPose(robotPose.ToPose2d());
224
225 std::vector<VisionTargetSim> allTargets{};
226 for (const auto& set : targetSets) {
227 for (const auto& target : set.second) {
228 allTargets.emplace_back(target);
229 }
230 }
231
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();
238 if (!optTimestamp) {
239 continue;
240 } else {
241 processed = true;
242 }
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;
247
248 frc::Pose3d lateRobotPose = GetRobotPose(timestampCapture);
249 frc::Pose3d lateCameraPose =
250 lateRobotPose + GetRobotToCamera(camSim, timestampCapture).value();
251 cameraPoses2d.push_back(lateCameraPose.ToPose2d());
252
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) {
258 continue;
259 }
260 visTgtPoses2d.push_back(lateCameraPose.TransformBy(trf).ToPose2d());
261 }
262 }
263 if (processed) {
264 dbgField.GetObject("visibleTargetPoses")->SetPoses(visTgtPoses2d);
265 }
266 if (cameraPoses2d.size() != 0) {
267 dbgField.GetObject("cameras")->SetPoses(cameraPoses2d);
268 }
269 }
270
271 private:
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>>
276 camTrfMap;
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{};
281};
282} // namespace photon
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