Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-19 00:41:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'main' into 2027

Browse Source
This commit is contained in:
Peter Johnson
2025-01-16 23:17:59 -08:00
parent 1cad4f64a4 a9f3fc6b2c
commit d9f8fded09
92 changed files with 2748 additions and 534 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
.bazelrc
View File

@@ -16,9 +16,15 @@ import shared/bazel/compiler_flags/base_linux_flags.rc
import shared/bazel/compiler_flags/linux_flags.rc
import shared/bazel/compiler_flags/osx_flags.rc
import shared/bazel/compiler_flags/roborio_flags.rc
import shared/bazel/compiler_flags/systemcore_flags.rc
import shared/bazel/compiler_flags/windows_flags.rc
import shared/bazel/compiler_flags/coverage_flags.rc
# Alias toolchain names to what wpilibsuite uses for CI/Artifact naming
build:athena --config=roborio
build:linuxarm32 --config=raspibookworm32
build:linuxarm64 --config=bookworm64
build:build_java --test_tag_filters=allwpilib-build-java --build_tag_filters=allwpilib-build-java
build:build_cpp --test_tag_filters=+allwpilib-build-cpp --build_tag_filters=+allwpilib-build-cpp
build:no_example --test_tag_filters=-wpi-example --build_tag_filters=-wpi-example

5
.github/workflows/fix_compile_commands.py vendored
View File

@@ -18,9 +18,9 @@ def main():
for obj in data:
out_args = []
# Filter out -isystem flags that cause false positives
iter_args = iter(obj["arguments"])
for arg in iter_args:
# Filter out -isystem flags that cause false positives
if arg == "-isystem":
next_arg = next(iter_args)
@@ -28,6 +28,9 @@ def main():
# error: conflicting types for '_mm_prefetch' [clang-diagnostic-error]
if not next_arg.startswith("/usr/lib/gcc/"):
out_args += ["-isystem", next_arg]
# Replace GCC warning argument with one Clang recognizes
elif arg == "-Wno-maybe-uninitialized":
out_args.append("-Wno-uninitialized")
else:
out_args.append(arg)

2
LICENSE.md
View File

@@ -1,4 +1,4 @@
Copyright (c) 2009-2024 FIRST and other WPILib contributors
Copyright (c) 2009-2025 FIRST and other WPILib contributors
All rights reserved.
Redistribution and use in source and binary forms, with or without

567
ThirdPartyNotices.txt
View File

@@ -15,7 +15,8 @@ licenses, and/or restrictions:
Program Locations
------- ---------
Google Test gtest
Google Test thirdparty/googletest/include
thirdparty/googletest/src
LLVM wpiutil/src/main/native/thirdparty/llvm
wpiutil/src/test/native/cpp/llvm/
JSON for Modern C++ wpiutil/src/main/native/thirdparty/json
@@ -33,17 +34,28 @@ popper.js wpinet/src/main/native/resources/popper-*
units wpimath/src/main/native/include/units/
Eigen wpimath/src/main/native/thirdparty/eigen/include/
StackWalker wpiutil/src/main/native/windows/StackWalker.*
GHC filesystem wpiutil/src/main/native/thirdparty/include/wpi/ghc/
Team 254 Library wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/SplineParameterizer.java
wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryParameterizer.java
wpilibc/src/main/native/include/spline/SplineParameterizer.h
wpilibc/src/main/native/include/trajectory/TrajectoryParameterizer.h
wpilibc/src/main/native/cpp/trajectory/TrajectoryParameterizer.cpp
Team 254 Library wpimath/src/main/java/edu/wpi/first/math/spline/SplineParameterizer.java
wpimath/src/main/java/edu/wpi/first/math/trajectory/TrajectoryParameterizer.java
wpimath/src/main/native/include/frc/spline/SplineParameterizer.h
wpimath/src/main/native/include/frc/trajectory/TrajectoryParameterizer.h
wpimath/src/main/native/cpp/trajectory/TrajectoryParameterizer.cpp
Portable File Dialogs wpigui/src/main/native/include/portable-file-dialogs.h
V8 export-template wpiutil/src/main/native/include/wpi/SymbolExports.h
GCEM wpimath/src/main/native/thirdparty/gcem/include/
Sleipnir wpimath/src/main/native/thirdparty/sleipnir
Debugging wpiutil/src/main/native/thirdparty/debugging
argparse wpiutil/src/main/native/thirdparty/argparse/include/wpi/argparse.h
apriltag apriltag/src/main/native/thirdparty/apriltag
glfw thirdparty/imgui_suite/glfw
Dear ImGui thirdparty/imgui_suite/imgui
implot thirdparty/imgui_suite/implot
memory wpiutil/src/main/native/thirdparty/memory
nanopb wpiutil/src/main/native/thirdparty/nanopb
protobuf wpiutil/src/main/native/thirdparty/protobuf
mrcal wpical/src/main/native/thirdparty/mrcal
libdogleg wpical/src/main/native/thirdparty/libdogleg
Additionally, glfw, memory, and nanopb were all modified for use in WPILib.
==============================================================================
Google Test License
@@ -462,7 +474,7 @@ limitations under the License.
==============================================================================
MPacks License
MPack License
==============================================================================
The MIT License (MIT)
@@ -1078,33 +1090,6 @@ and/or modify it under the terms of the Do What the **** You Want
to Public License, Version 2, as published by the WTFPL Task Force.
See http://www.wtfpl.net/ for more details.
======================
Boost Software License
======================
Boost Software License - Version 1.0 - August 17th, 2003
Permission is hereby granted, free of charge, to any person or organization
obtaining a copy of the software and accompanying documentation covered by
this license (the "Software") to use, reproduce, display, distribute,
execute, and transmit the Software, and to prepare derivative works of the
Software, and to permit third-parties to whom the Software is furnished to
do so, all subject to the following:
The copyright notices in the Software and this entire statement, including
the above license grant, this restriction and the following disclaimer,
must be included in all copies of the Software, in whole or in part, and
all derivative works of the Software, unless such copies or derivative
works are solely in the form of machine-executable object code generated by
a source language processor.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE, TITLE AND NON-INFRINGEMENT. IN NO EVENT
SHALL THE COPYRIGHT HOLDERS OR ANYONE DISTRIBUTING THE SOFTWARE BE LIABLE
FOR ANY DAMAGES OR OTHER LIABILITY, WHETHER IN CONTRACT, TORT OR OTHERWISE,
ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
DEALINGS IN THE SOFTWARE.
======
fmtlib
======
@@ -1136,29 +1121,6 @@ of this Software are embedded into a machine-executable object form of such
source code, you may redistribute such embedded portions in such object form
without including the above copyright and permission notices.
==============
GHC filesystem
==============
Copyright (c) 2018, Steffen Schümann <s.schuemann@pobox.com>
Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:
The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
SOFTWARE.
==================
V8 export-template
==================
@@ -1251,3 +1213,492 @@ AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
SOFTWARE.
================
argparse License
================
Copyright (c) 2018 Pranav Srinivas Kumar <pranav.srinivas.kumar@gmail.com>
Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:
The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
================
apriltag License
================
BSD 2-Clause License
Copyright (C) 2013-2016, The Regents of The University of Michigan.
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
1. Redistributions of source code must retain the above copyright notice, this
list of conditions and the following disclaimer.
2. Redistributions in binary form must reproduce the above copyright notice,
this list of conditions and the following disclaimer in the documentation
and/or other materials provided with the distribution.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
The views and conclusions contained in the software and documentation are those
of the authors and should not be interpreted as representing official policies,
either expressed or implied, of the Regents of The University of Michigan.
============
gl3w License
============
This is free and unencumbered software released into the public domain.
Anyone is free to copy, modify, publish, use, compile, sell, or
distribute this software, either in source code form or as a compiled
binary, for any purpose, commercial or non-commercial, and by any
means.
In jurisdictions that recognize copyright laws, the author or authors
of this software dedicate any and all copyright interest in the
software to the public domain. We make this dedication for the benefit
of the public at large and to the detriment of our heirs and
successors. We intend this dedication to be an overt act of
relinquishment in perpetuity of all present and future rights to this
software under copyright law.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
IN NO EVENT SHALL THE AUTHORS BE LIABLE FOR ANY CLAIM, DAMAGES OR
OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
OTHER DEALINGS IN THE SOFTWARE.
============
glfw License
============
Copyright (c) 2002-2006 Marcus Geelnard
Copyright (c) 2006-2019 Camilla Löwy
This software is provided 'as-is', without any express or implied
warranty. In no event will the authors be held liable for any damages
arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it
freely, subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not
claim that you wrote the original software. If you use this software
in a product, an acknowledgment in the product documentation would
be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not
be misrepresented as being the original software.
3. This notice may not be removed or altered from any source
distribution.
==================
Dear ImGui License
==================
The MIT License (MIT)
Copyright (c) 2014-2024 Omar Cornut
Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:
The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
SOFTWARE.
==============
implot License
==============
MIT License
Copyright (c) 2020 Evan Pezent
Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:
The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
SOFTWARE.
==============
memory License
==============
Copyright (C) 2015-2023 Jonathan Müller and foonathan/memory contributors
This software is provided 'as-is', without any express or
implied warranty. In no event will the authors be held
liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute
it freely, subject to the following restrictions:
1. The origin of this software must not be misrepresented;
you must not claim that you wrote the original software.
If you use this software in a product, an acknowledgment
in the product documentation would be appreciated but
is not required.
2. Altered source versions must be plainly marked as such,
and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any
source distribution.
==============
nanopb License
==============
Copyright (c) 2011 Petteri Aimonen <jpa at nanopb.mail.kapsi.fi>
This software is provided 'as-is', without any express or
implied warranty. In no event will the authors be held liable
for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any
purpose, including commercial applications, and to alter it and
redistribute it freely, subject to the following restrictions:
1. The origin of this software must not be misrepresented; you
must not claim that you wrote the original software. If you use
this software in a product, an acknowledgment in the product
documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and
must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source
distribution.
================
protobuf License
================
Copyright 2008 Google Inc. All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are
met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above
copyright notice, this list of conditions and the following disclaimer
in the documentation and/or other materials provided with the
distribution.
* Neither the name of Google Inc. nor the names of its
contributors may be used to endorse or promote products derived from
this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
Code generated by the Protocol Buffer compiler is owned by the owner
of the input file used when generating it. This code is not
standalone and requires a support library to be linked with it. This
support library is itself covered by the above license.
===========
stb License
===========
This software is available under 2 licenses -- choose whichever you prefer.
------------------------------------------------------------------------------
ALTERNATIVE A - MIT License
Copyright (c) 2017 Sean Barrett
Permission is hereby granted, free of charge, to any person obtaining a copy of
this software and associated documentation files (the "Software"), to deal in
the Software without restriction, including without limitation the rights to
use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies
of the Software, and to permit persons to whom the Software is furnished to do
so, subject to the following conditions:
The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
SOFTWARE.
------------------------------------------------------------------------------
ALTERNATIVE B - Public Domain (www.unlicense.org)
This is free and unencumbered software released into the public domain.
Anyone is free to copy, modify, publish, use, compile, sell, or distribute this
software, either in source code form or as a compiled binary, for any purpose,
commercial or non-commercial, and by any means.
In jurisdictions that recognize copyright laws, the author or authors of this
software dedicate any and all copyright interest in the software to the public
domain. We make this dedication for the benefit of the public at large and to
the detriment of our heirs and successors. We intend this dedication to be an
overt act of relinquishment in perpetuity of all present and future rights to
this software under copyright law.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION
WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
=============
mrcal License
=============
Apache License
Version 2.0, January 2004
http://www.apache.org/licenses/
TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION
1. Definitions.
"License" shall mean the terms and conditions for use, reproduction,
and distribution as defined by Sections 1 through 9 of this document.
"Licensor" shall mean the copyright owner or entity authorized by
the copyright owner that is granting the License.
"Legal Entity" shall mean the union of the acting entity and all
other entities that control, are controlled by, or are under common
control with that entity. For the purposes of this definition,
"control" means (i) the power, direct or indirect, to cause the
direction or management of such entity, whether by contract or
otherwise, or (ii) ownership of fifty percent (50%) or more of the
outstanding shares, or (iii) beneficial ownership of such entity.
"You" (or "Your") shall mean an individual or Legal Entity
exercising permissions granted by this License.
"Source" form shall mean the preferred form for making modifications,
including but not limited to software source code, documentation
source, and configuration files.
"Object" form shall mean any form resulting from mechanical
transformation or translation of a Source form, including but
not limited to compiled object code, generated documentation,
and conversions to other media types.
"Work" shall mean the work of authorship, whether in Source or
Object form, made available under the License, as indicated by a
copyright notice that is included in or attached to the work
(an example is provided in the Appendix below).
"Derivative Works" shall mean any work, whether in Source or Object
form, that is based on (or derived from) the Work and for which the
editorial revisions, annotations, elaborations, or other modifications
represent, as a whole, an original work of authorship. For the purposes
of this License, Derivative Works shall not include works that remain
separable from, or merely link (or bind by name) to the interfaces of,
the Work and Derivative Works thereof.
"Contribution" shall mean any work of authorship, including
the original version of the Work and any modifications or additions
to that Work or Derivative Works thereof, that is intentionally
submitted to Licensor for inclusion in the Work by the copyright owner
or by an individual or Legal Entity authorized to submit on behalf of
the copyright owner. For the purposes of this definition, "submitted"
means any form of electronic, verbal, or written communication sent
to the Licensor or its representatives, including but not limited to
communication on electronic mailing lists, source code control systems,
and issue tracking systems that are managed by, or on behalf of, the
Licensor for the purpose of discussing and improving the Work, but
excluding communication that is conspicuously marked or otherwise
designated in writing by the copyright owner as "Not a Contribution."
"Contributor" shall mean Licensor and any individual or Legal Entity
on behalf of whom a Contribution has been received by Licensor and
subsequently incorporated within the Work.
2. Grant of Copyright License. Subject to the terms and conditions of
this License, each Contributor hereby grants to You a perpetual,
worldwide, non-exclusive, no-charge, royalty-free, irrevocable
copyright license to reproduce, prepare Derivative Works of,
publicly display, publicly perform, sublicense, and distribute the
Work and such Derivative Works in Source or Object form.
3. Grant of Patent License. Subject to the terms and conditions of
this License, each Contributor hereby grants to You a perpetual,
worldwide, non-exclusive, no-charge, royalty-free, irrevocable
(except as stated in this section) patent license to make, have made,
use, offer to sell, sell, import, and otherwise transfer the Work,
where such license applies only to those patent claims licensable
by such Contributor that are necessarily infringed by their
Contribution(s) alone or by combination of their Contribution(s)
with the Work to which such Contribution(s) was submitted. If You
institute patent litigation against any entity (including a
cross-claim or counterclaim in a lawsuit) alleging that the Work
or a Contribution incorporated within the Work constitutes direct
or contributory patent infringement, then any patent licenses
granted to You under this License for that Work shall terminate
as of the date such litigation is filed.
4. Redistribution. You may reproduce and distribute copies of the
Work or Derivative Works thereof in any medium, with or without
modifications, and in Source or Object form, provided that You
meet the following conditions:
(a) You must give any other recipients of the Work or
Derivative Works a copy of this License; and
(b) You must cause any modified files to carry prominent notices
stating that You changed the files; and
(c) You must retain, in the Source form of any Derivative Works
that You distribute, all copyright, patent, trademark, and
attribution notices from the Source form of the Work,
excluding those notices that do not pertain to any part of
the Derivative Works; and
(d) If the Work includes a "NOTICE" text file as part of its
distribution, then any Derivative Works that You distribute must
include a readable copy of the attribution notices contained
within such NOTICE file, excluding those notices that do not
pertain to any part of the Derivative Works, in at least one
of the following places: within a NOTICE text file distributed
as part of the Derivative Works; within the Source form or
documentation, if provided along with the Derivative Works; or,
within a display generated by the Derivative Works, if and
wherever such third-party notices normally appear. The contents
of the NOTICE file are for informational purposes only and
do not modify the License. You may add Your own attribution
notices within Derivative Works that You distribute, alongside
or as an addendum to the NOTICE text from the Work, provided
that such additional attribution notices cannot be construed
as modifying the License.
You may add Your own copyright statement to Your modifications and
may provide additional or different license terms and conditions
for use, reproduction, or distribution of Your modifications, or
for any such Derivative Works as a whole, provided Your use,
reproduction, and distribution of the Work otherwise complies with
the conditions stated in this License.
5. Submission of Contributions. Unless You explicitly state otherwise,
any Contribution intentionally submitted for inclusion in the Work
by You to the Licensor shall be under the terms and conditions of
this License, without any additional terms or conditions.
Notwithstanding the above, nothing herein shall supersede or modify
the terms of any separate license agreement you may have executed
with Licensor regarding such Contributions.
6. Trademarks. This License does not grant permission to use the trade
names, trademarks, service marks, or product names of the Licensor,
except as required for reasonable and customary use in describing the
origin of the Work and reproducing the content of the NOTICE file.
7. Disclaimer of Warranty. Unless required by applicable law or
agreed to in writing, Licensor provides the Work (and each
Contributor provides its Contributions) on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
implied, including, without limitation, any warranties or conditions
of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A
PARTICULAR PURPOSE. You are solely responsible for determining the
appropriateness of using or redistributing the Work and assume any
risks associated with Your exercise of permissions under this License.
8. Limitation of Liability. In no event and under no legal theory,
whether in tort (including negligence), contract, or otherwise,
unless required by applicable law (such as deliberate and grossly
negligent acts) or agreed to in writing, shall any Contributor be
liable to You for damages, including any direct, indirect, special,
incidental, or consequential damages of any character arising as a
result of this License or out of the use or inability to use the
Work (including but not limited to damages for loss of goodwill,
work stoppage, computer failure or malfunction, or any and all
other commercial damages or losses), even if such Contributor
has been advised of the possibility of such damages.
9. Accepting Warranty or Additional Liability. While redistributing
the Work or Derivative Works thereof, You may choose to offer,
and charge a fee for, acceptance of support, warranty, indemnity,
or other liability obligations and/or rights consistent with this
License. However, in accepting such obligations, You may act only
on Your own behalf and on Your sole responsibility, not on behalf
of any other Contributor, and only if You agree to indemnify,
defend, and hold each Contributor harmless for any liability
incurred by, or claims asserted against, such Contributor by reason
of your accepting any such warranty or additional liability.
END OF TERMS AND CONDITIONS
APPENDIX: How to apply the Apache License to your work.
To apply the Apache License to your work, attach the following
boilerplate notice, with the fields enclosed by brackets "[]"
replaced with your own identifying information. (Don't include
the brackets!) The text should be enclosed in the appropriate
comment syntax for the file format. We also recommend that a
file or class name and description of purpose be included on the
same "printed page" as the copyright notice for easier
identification within third-party archives.
Copyright (c) 2017-2023 California Institute of Technology ("Caltech"). U.S.
Government sponsorship acknowledged. All rights reserved.
Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at
http://www.apache.org/licenses/LICENSE-2.0
Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.
=================
libdogleg License
=================
Copyright 2011 Oblong Industries 2017 Dima Kogan <dima@secretsauce.net>
This program is free software: you can redistribute it and/or modify it under the terms of the GNU Lesser General Public License as published by the Free Software Foundation, either version 3 of the License, or (at your option) any later version.
This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License for more details.
The full text of the license is available at http://www.gnu.org/licenses

28
WORKSPACE
View File

@@ -35,8 +35,8 @@ maven_install(
# Download toolchains
http_archive(
name = "rules_bzlmodrio_toolchains",
sha256 = "fe267e2af53c1def1e962700a9aeda9e8fdfa9fb46b72167c615ec0e25447dd6",
url = "https://github.com/wpilibsuite/rules_bzlmodRio_toolchains/releases/download/2025-1/rules_bzlmodRio_toolchains-2025-1.tar.gz",
sha256 = "ff25b5f9445cbd43759be4c6582b987d1065cf817c593eedc7ada1a699298c84",
url = "https://github.com/wpilibsuite/rules_bzlmodRio_toolchains/releases/download/2025-1.bcr2/rules_bzlmodRio_toolchains-2025-1.bcr2.tar.gz",
)
load("@rules_bzlmodrio_toolchains//:maven_deps.bzl", "setup_legacy_setup_toolchains_dependencies")
@@ -50,8 +50,8 @@ load_toolchains()
#
http_archive(
name = "rules_bzlmodrio_jdk",
sha256 = "a00d5fa971fbcad8a17b1968cdc5350688397035e90b0cb94e040d375ecd97b4",
url = "https://github.com/wpilibsuite/rules_bzlmodRio_jdk/releases/download/17.0.8.1-1/rules_bzlmodRio_jdk-17.0.8.1-1.tar.gz",
sha256 = "81869fe9860e39b17e4a9bc1d33c1ca2faede7e31d9538ed0712406f753a2163",
url = "https://github.com/wpilibsuite/rules_bzlmodRio_jdk/releases/download/17.0.12-7/rules_bzlmodRio_jdk-17.0.12-7.tar.gz",
)
load("@rules_bzlmodrio_jdk//:maven_deps.bzl", "setup_legacy_setup_jdk_dependencies")
@@ -62,9 +62,15 @@ register_toolchains(
"@local_roborio//:macos",
"@local_roborio//:linux",
"@local_roborio//:windows",
"@local_raspi_32//:macos",
"@local_raspi_32//:linux",
"@local_raspi_32//:windows",
"@local_systemcore//:macos",
"@local_systemcore//:linux",
"@local_systemcore//:windows",
"@local_raspi_bullseye_32//:macos",
"@local_raspi_bullseye_32//:linux",
"@local_raspi_bullseye_32//:windows",
"@local_raspi_bookworm_32//:macos",
"@local_raspi_bookworm_32//:linux",
"@local_raspi_bookworm_32//:windows",
"@local_bullseye_32//:macos",
"@local_bullseye_32//:linux",
"@local_bullseye_32//:windows",
@@ -83,8 +89,8 @@ setup_legacy_setup_jdk_dependencies()
http_archive(
name = "bzlmodrio-ni",
sha256 = "197fceac88bf44fb8427d5e000b0083118d3346172dd2ad31eccf83a5e61b3ce",
url = "https://github.com/wpilibsuite/bzlmodRio-ni/releases/download/2025.0.0/bzlmodRio-ni-2025.0.0.tar.gz",
sha256 = "fff62c3cb3e83f9a0d0a01f1739477c9ca5e9a6fac05be1ad59dafcd385801f7",
url = "https://github.com/wpilibsuite/bzlmodRio-ni/releases/download/2025.2.0/bzlmodRio-ni-2025.2.0.tar.gz",
)
load("@bzlmodrio-ni//:maven_cpp_deps.bzl", "setup_legacy_bzlmodrio_ni_cpp_dependencies")
@@ -93,8 +99,8 @@ setup_legacy_bzlmodrio_ni_cpp_dependencies()
http_archive(
name = "bzlmodrio-opencv",
sha256 = "4f4a607956ca8555618736c3058dd96e09d02df19e95088c1e352d2319fd70c7",
url = "https://github.com/wpilibsuite/bzlmodRio-opencv/releases/download/2025.4.10.0-2/bzlmodRio-opencv-2025.4.10.0-2.tar.gz",
sha256 = "ba3f4910ce9cc0e08abff732aeb5835b1bcfd864ca5296edeadcf2935f7e81b9",
url = "https://github.com/wpilibsuite/bzlmodRio-opencv/releases/download/2025.4.10.0-3.bcr1/bzlmodRio-opencv-2025.4.10.0-3.bcr1.tar.gz",
)
load("@bzlmodrio-opencv//:maven_cpp_deps.bzl", "setup_legacy_bzlmodrio_opencv_cpp_dependencies")

14
apriltag/convert_apriltag_layouts.py
View File

@@ -7,10 +7,11 @@ AprilTagFields expects.
The input CSV has the following format:
* Columns: ID, X, Y, Z, Rotation
* Columns: ID, X, Y, Z, Z Rotation, Y Rotation
* ID is a positive integer
* X, Y, and Z are decimal inches
* Rotation is yaw in degrees
* Z Rotation is yaw in degrees
* Y Rotation is pitch in degrees
The values come from a table in the layout marking diagram (e.g.,
https://firstfrc.blob.core.windows.net/frc2024/FieldAssets/2024LayoutMarkingDiagram.pdf).
@@ -48,13 +49,14 @@ def main():
x = float(row[1])
y = float(row[2])
z = float(row[3])
rotation = float(row[4])
zRotation = float(row[4])
yRotation = float(row[5])
# Turn yaw into quaternion
q = geometry.Rotation3d(
units.radians(0.0),
units.radians(0.0),
units.degreesToRadians(rotation),
units.radians(0),
units.degreesToRadians(yRotation),
units.degreesToRadians(zRotation),
).getQuaternion()
json_data["tags"].append(

6
apriltag/src/main/java/edu/wpi/first/apriltag/AprilTagFields.java
View File

@@ -13,13 +13,15 @@ public enum AprilTagFields {
/** 2023 Charged Up. */
k2023ChargedUp("2023-chargedup.json"),
/** 2024 Crescendo. */
k2024Crescendo("2024-crescendo.json");
k2024Crescendo("2024-crescendo.json"),
/** 2025 Reefscape. */
k2025Reefscape("2025-reefscape.json");
/** Base resource directory. */
public static final String kBaseResourceDir = "/edu/wpi/first/apriltag/";
/** Alias to the current game. */
public static final AprilTagFields kDefaultField = k2024Crescendo;
public static final AprilTagFields kDefaultField = k2025Reefscape;
/** Resource filename. */
public final String m_resourceFile;

4
apriltag/src/main/native/cpp/AprilTagFieldLayout.cpp
View File

@@ -133,6 +133,7 @@ namespace frc {
std::string_view GetResource_2022_rapidreact_json();
std::string_view GetResource_2023_chargedup_json();
std::string_view GetResource_2024_crescendo_json();
std::string_view GetResource_2025_reefscape_json();
} // namespace frc
@@ -148,6 +149,9 @@ AprilTagFieldLayout AprilTagFieldLayout::LoadField(AprilTagField field) {
case AprilTagField::k2024Crescendo:
fieldString = GetResource_2024_crescendo_json();
break;
case AprilTagField::k2025Reefscape:
fieldString = GetResource_2025_reefscape_json();
break;
case AprilTagField::kNumFields:
throw std::invalid_argument("Invalid Field");
}

4
apriltag/src/main/native/include/frc/apriltag/AprilTagFields.h
View File

@@ -20,6 +20,10 @@ enum class AprilTagField {
k2023ChargedUp,
/// 2024 Crescendo.
k2024Crescendo,
/// 2025 Reefscape.
k2025Reefscape,
/// Alias to the current game.
kDefaultField = k2025Reefscape,
// This is a placeholder for denoting the last supported field. This should
// always be the last entry in the enum and should not be used by users

17
apriltag/src/main/native/resources/edu/wpi/first/apriltag/2024-crescendo.csv
View File

@@ -1,17 +0,0 @@
ID,X,Y,Z,Rotation
1,593.68,9.68,53.38,120
2,637.21,34.79,53.38,120
3,652.73,196.17,57.13,180
4,652.73,218.42,57.13,180
5,578.77,323.00,53.38,270
6,72.5,323.00,53.38,270
7,-1.50,218.42,57.13,0
8,-1.50,196.17,57.13,0
9,14.02,34.79,53.38,60
10,57.54,9.68,53.38,60
11,468.69,146.19,52.00,300
12,468.69,177.10,52.00,60
13,441.74,161.62,52.00,180
14,209.48,161.62,52.00,0
15,182.73,177.10,52.00,120
16,182.73,146.19,52.00,240
1 ID X Y Z Rotation
2 1 593.68 9.68 53.38 120
3 2 637.21 34.79 53.38 120
4 3 652.73 196.17 57.13 180
5 4 652.73 218.42 57.13 180
6 5 578.77 323.00 53.38 270
7 6 72.5 323.00 53.38 270
8 7 -1.50 218.42 57.13 0
9 8 -1.50 196.17 57.13 0
10 9 14.02 34.79 53.38 60
11 10 57.54 9.68 53.38 60
12 11 468.69 146.19 52.00 300
13 12 468.69 177.10 52.00 60
14 13 441.74 161.62 52.00 180
15 14 209.48 161.62 52.00 0
16 15 182.73 177.10 52.00 120
17 16 182.73 146.19 52.00 240

23
apriltag/src/main/native/resources/edu/wpi/first/apriltag/2025-reefscape.csv Normal file
View File

@@ -0,0 +1,23 @@
ID,X,Y,Z,Z-Rotation,X-Rotation
1,657.37,25.8,58.5,126,0
2,657.37,291.2,58.5,234,0
3,455.15,317.15,51.25,270,0
4,365.2,241.64,73.54,0,30
5,365.2,75.39,73.54,0,30
6,530.49,130.17,12.13,300,0
7,546.87,158.5,12.13,0,0
8,530.49,186.83,12.13,60,0
9,497.77,186.83,12.13,120,0
10,481.39,158.5,12.13,180,0
11,497.77,130.17,12.13,240,0
12,33.51,25.8,58.5,54,0
13,33.51,291.2,58.5,306,0
14,325.68,241.64,73.54,180,30
15,325.68,75.39,73.54,180,30
16,235.73,-0.15,51.25,90,0
17,160.39,130.17,12.13,240,0
18,144,158.5,12.13,180,0
19,160.39,186.83,12.13,120,0
20,193.1,186.83,12.13,60,0
21,209.49,158.5,12.13,0,0
22,193.1,130.17,12.13,300,0
1 ID X Y Z Z-Rotation X-Rotation
2 1 657.37 25.8 58.5 126 0
3 2 657.37 291.2 58.5 234 0
4 3 455.15 317.15 51.25 270 0
5 4 365.2 241.64 73.54 0 30
6 5 365.2 75.39 73.54 0 30
7 6 530.49 130.17 12.13 300 0
8 7 546.87 158.5 12.13 0 0
9 8 530.49 186.83 12.13 60 0
10 9 497.77 186.83 12.13 120 0
11 10 481.39 158.5 12.13 180 0
12 11 497.77 130.17 12.13 240 0
13 12 33.51 25.8 58.5 54 0
14 13 33.51 291.2 58.5 306 0
15 14 325.68 241.64 73.54 180 30
16 15 325.68 75.39 73.54 180 30
17 16 235.73 -0.15 51.25 90 0
18 17 160.39 130.17 12.13 240 0
19 18 144 158.5 12.13 180 0
20 19 160.39 186.83 12.13 120 0
21 20 193.1 186.83 12.13 60 0
22 21 209.49 158.5 12.13 0 0
23 22 193.1 130.17 12.13 300 0

404
apriltag/src/main/native/resources/edu/wpi/first/apriltag/2025-reefscape.json Normal file
View File

@@ -0,0 +1,404 @@
{
"tags": [
{
"ID": 1,
"pose": {
"translation": {
"x": 16.697198,
"y": 0.65532,
"z": 1.4859
},
"rotation": {
"quaternion": {
"W": 0.4539904997395468,
"X": 0.0,
"Y": 0.0,
"Z": 0.8910065241883678
}
}
}
},
{
"ID": 2,
"pose": {
"translation": {
"x": 16.697198,
"y": 7.3964799999999995,
"z": 1.4859
},
"rotation": {
"quaternion": {
"W": -0.45399049973954675,
"X": -0.0,
"Y": 0.0,
"Z": 0.8910065241883679
}
}
}
},
{
"ID": 3,
"pose": {
"translation": {
"x": 11.560809999999998,
"y": 8.05561,
"z": 1.30175
},
"rotation": {
"quaternion": {
"W": -0.7071067811865475,
"X": -0.0,
"Y": 0.0,
"Z": 0.7071067811865476
}
}
}
},
{
"ID": 4,
"pose": {
"translation": {
"x": 9.276079999999999,
"y": 6.137656,
"z": 1.8679160000000001
},
"rotation": {
"quaternion": {
"W": 0.9659258262890683,
"X": 0.0,
"Y": 0.25881904510252074,
"Z": 0.0
}
}
}
},
{
"ID": 5,
"pose": {
"translation": {
"x": 9.276079999999999,
"y": 1.914906,
"z": 1.8679160000000001
},
"rotation": {
"quaternion": {
"W": 0.9659258262890683,
"X": 0.0,
"Y": 0.25881904510252074,
"Z": 0.0
}
}
}
},
{
"ID": 6,
"pose": {
"translation": {
"x": 13.474446,
"y": 3.3063179999999996,
"z": 0.308102
},
"rotation": {
"quaternion": {
"W": -0.8660254037844387,
"X": -0.0,
"Y": 0.0,
"Z": 0.49999999999999994
}
}
}
},
{
"ID": 7,
"pose": {
"translation": {
"x": 13.890498,
"y": 4.0259,
"z": 0.308102
},
"rotation": {
"quaternion": {
"W": 1.0,
"X": 0.0,
"Y": 0.0,
"Z": 0.0
}
}
}
},
{
"ID": 8,
"pose": {
"translation": {
"x": 13.474446,
"y": 4.745482,
"z": 0.308102
},
"rotation": {
"quaternion": {
"W": 0.8660254037844387,
"X": 0.0,
"Y": 0.0,
"Z": 0.49999999999999994
}
}
}
},
{
"ID": 9,
"pose": {
"translation": {
"x": 12.643358,
"y": 4.745482,
"z": 0.308102
},
"rotation": {
"quaternion": {
"W": 0.5000000000000001,
"X": 0.0,
"Y": 0.0,
"Z": 0.8660254037844386
}
}
}
},
{
"ID": 10,
"pose": {
"translation": {
"x": 12.227305999999999,
"y": 4.0259,
"z": 0.308102
},
"rotation": {
"quaternion": {
"W": 6.123233995736766e-17,
"X": 0.0,
"Y": 0.0,
"Z": 1.0
}
}
}
},
{
"ID": 11,
"pose": {
"translation": {
"x": 12.643358,
"y": 3.3063179999999996,
"z": 0.308102
},
"rotation": {
"quaternion": {
"W": -0.4999999999999998,
"X": -0.0,
"Y": 0.0,
"Z": 0.8660254037844387
}
}
}
},
{
"ID": 12,
"pose": {
"translation": {
"x": 0.851154,
"y": 0.65532,
"z": 1.4859
},
"rotation": {
"quaternion": {
"W": 0.8910065241883679,
"X": 0.0,
"Y": 0.0,
"Z": 0.45399049973954675
}
}
}
},
{
"ID": 13,
"pose": {
"translation": {
"x": 0.851154,
"y": 7.3964799999999995,
"z": 1.4859
},
"rotation": {
"quaternion": {
"W": -0.8910065241883678,
"X": -0.0,
"Y": 0.0,
"Z": 0.45399049973954686
}
}
}
},
{
"ID": 14,
"pose": {
"translation": {
"x": 8.272272,
"y": 6.137656,
"z": 1.8679160000000001
},
"rotation": {
"quaternion": {
"W": 5.914589856893349e-17,
"X": -0.25881904510252074,
"Y": 1.5848095757158825e-17,
"Z": 0.9659258262890683
}
}
}
},
{
"ID": 15,
"pose": {
"translation": {
"x": 8.272272,
"y": 1.914906,
"z": 1.8679160000000001
},
"rotation": {
"quaternion": {
"W": 5.914589856893349e-17,
"X": -0.25881904510252074,
"Y": 1.5848095757158825e-17,
"Z": 0.9659258262890683
}
}
}
},
{
"ID": 16,
"pose": {
"translation": {
"x": 5.9875419999999995,
"y": -0.0038099999999999996,
"z": 1.30175
},
"rotation": {
"quaternion": {
"W": 0.7071067811865476,
"X": 0.0,
"Y": 0.0,
"Z": 0.7071067811865476
}
}
}
},
{
"ID": 17,
"pose": {
"translation": {
"x": 4.073905999999999,
"y": 3.3063179999999996,
"z": 0.308102
},
"rotation": {
"quaternion": {
"W": -0.4999999999999998,
"X": -0.0,
"Y": 0.0,
"Z": 0.8660254037844387
}
}
}
},
{
"ID": 18,
"pose": {
"translation": {
"x": 3.6576,
"y": 4.0259,
"z": 0.308102
},
"rotation": {
"quaternion": {
"W": 6.123233995736766e-17,
"X": 0.0,
"Y": 0.0,
"Z": 1.0
}
}
}
},
{
"ID": 19,
"pose": {
"translation": {
"x": 4.073905999999999,
"y": 4.745482,
"z": 0.308102
},
"rotation": {
"quaternion": {
"W": 0.5000000000000001,
"X": 0.0,
"Y": 0.0,
"Z": 0.8660254037844386
}
}
}
},
{
"ID": 20,
"pose": {
"translation": {
"x": 4.904739999999999,
"y": 4.745482,
"z": 0.308102
},
"rotation": {
"quaternion": {
"W": 0.8660254037844387,
"X": 0.0,
"Y": 0.0,
"Z": 0.49999999999999994
}
}
}
},
{
"ID": 21,
"pose": {
"translation": {
"x": 5.321046,
"y": 4.0259,
"z": 0.308102
},
"rotation": {
"quaternion": {
"W": 1.0,
"X": 0.0,
"Y": 0.0,
"Z": 0.0
}
}
}
},
{
"ID": 22,
"pose": {
"translation": {
"x": 4.904739999999999,
"y": 3.3063179999999996,
"z": 0.308102
},
"rotation": {
"quaternion": {
"W": -0.8660254037844387,
"X": -0.0,
"Y": 0.0,
"Z": 0.49999999999999994
}
}
}
}
],
"field": {
"length": 17.548,
"width": 8.052
}
}

19
cscore/src/main/java/edu/wpi/first/cscore/CvSink.java
View File

@@ -6,6 +6,7 @@ package edu.wpi.first.cscore;
import edu.wpi.first.util.PixelFormat;
import edu.wpi.first.util.RawFrame;
import edu.wpi.first.util.TimestampSource;
import java.nio.ByteBuffer;
import org.opencv.core.CvType;
import org.opencv.core.Mat;
@@ -220,4 +221,22 @@ public class CvSink extends ImageSink {
}
return rv;
}
/**
* Get the last time a frame was grabbed. This uses the same time base as wpi::Now().
*
* @return Time in 1 us increments.
*/
public long getLastFrameTime() {
return m_frame.getTimestamp();
}
/**
* Get the time source for the timestamp the last frame was grabbed at.
*
* @return Time source
*/
public TimestampSource getLastFrameTimeSource() {
return m_frame.getTimestampSource();
}
}

8
cscore/src/main/native/cpp/Frame.cpp
View File

@@ -16,18 +16,22 @@
using namespace cs;
Frame::Frame(SourceImpl& source, std::string_view error, Time time)
Frame::Frame(SourceImpl& source, std::string_view error, Time time,
WPI_TimestampSource timeSrc)
: m_impl{source.AllocFrameImpl().release()} {
m_impl->refcount = 1;
m_impl->error = error;
m_impl->time = time;
m_impl->timeSource = timeSrc;
}
Frame::Frame(SourceImpl& source, std::unique_ptr<Image> image, Time time)
Frame::Frame(SourceImpl& source, std::unique_ptr<Image> image, Time time,
WPI_TimestampSource timeSrc)
: m_impl{source.AllocFrameImpl().release()} {
m_impl->refcount = 1;
m_impl->error.resize(0);
m_impl->time = time;
m_impl->timeSource = timeSrc;
m_impl->images.push_back(image.release());
}

10
cscore/src/main/native/cpp/Frame.h
View File

@@ -39,6 +39,7 @@ class Frame {
wpi::recursive_mutex mutex;
std::atomic_int refcount{0};
Time time{0};
WPI_TimestampSource timeSource{WPI_TIMESRC_UNKNOWN};
SourceImpl& source;
std::string error;
wpi::SmallVector<Image*, 4> images;
@@ -48,9 +49,11 @@ class Frame {
public:
Frame() noexcept = default;
Frame(SourceImpl& source, std::string_view error, Time time);
Frame(SourceImpl& source, std::string_view error, Time time,
WPI_TimestampSource timeSrc);
Frame(SourceImpl& source, std::unique_ptr<Image> image, Time time);
Frame(SourceImpl& source, std::unique_ptr<Image> image, Time time,
WPI_TimestampSource timeSrc);
Frame(const Frame& frame) noexcept : m_impl{frame.m_impl} {
if (m_impl) {
@@ -75,6 +78,9 @@ class Frame {
}
Time GetTime() const { return m_impl ? m_impl->time : 0; }
WPI_TimestampSource GetTimeSource() const {
return m_impl ? m_impl->timeSource : WPI_TIMESRC_UNKNOWN;
}
std::string_view GetError() const {
if (!m_impl) {

2
cscore/src/main/native/cpp/RawSinkImpl.cpp
View File

@@ -120,6 +120,8 @@ uint64_t RawSinkImpl::GrabFrameImpl(WPI_RawFrame& rawFrame,
rawFrame.pixelFormat = newImage->pixelFormat;
rawFrame.size = newImage->size();
std::copy(newImage->data(), newImage->data() + rawFrame.size, rawFrame.data);
rawFrame.timestamp = incomingFrame.GetTime();
rawFrame.timestampSrc = incomingFrame.GetTimeSource();
return incomingFrame.GetTime();
}

19
cscore/src/main/native/cpp/SourceImpl.cpp
View File

@@ -29,7 +29,7 @@ SourceImpl::SourceImpl(std::string_view name, wpi::Logger& logger,
m_notifier(notifier),
m_telemetry(telemetry),
m_name{name} {
m_frame = Frame{*this, std::string_view{}, 0};
m_frame = Frame{*this, std::string_view{}, 0, WPI_TIMESRC_UNKNOWN};
}
SourceImpl::~SourceImpl() {
@@ -95,7 +95,8 @@ Frame SourceImpl::GetNextFrame(double timeout, Frame::Time lastFrameTime) {
if (!m_frameCv.wait_for(
lock, std::chrono::milliseconds(static_cast<int>(timeout * 1000)),
[=, this] { return m_frame.GetTime() != lastFrameTime; })) {
m_frame = Frame{*this, "timed out getting frame", wpi::Now()};
m_frame = Frame{*this, "timed out getting frame", wpi::Now(),
WPI_TIMESRC_UNKNOWN};
}
return m_frame;
}
@@ -103,7 +104,7 @@ Frame SourceImpl::GetNextFrame(double timeout, Frame::Time lastFrameTime) {
void SourceImpl::Wakeup() {
{
std::scoped_lock lock{m_frameMutex};
m_frame = Frame{*this, std::string_view{}, 0};
m_frame = Frame{*this, std::string_view{}, 0, WPI_TIMESRC_UNKNOWN};
}
m_frameCv.notify_all();
}
@@ -463,7 +464,8 @@ std::unique_ptr<Image> SourceImpl::AllocImage(
}
void SourceImpl::PutFrame(VideoMode::PixelFormat pixelFormat, int width,
int height, std::string_view data, Frame::Time time) {
int height, std::string_view data, Frame::Time time,
WPI_TimestampSource timeSrc) {
if (pixelFormat == VideoMode::PixelFormat::kBGRA) {
// Write BGRA as BGR to save a copy
auto image =
@@ -480,10 +482,11 @@ void SourceImpl::PutFrame(VideoMode::PixelFormat pixelFormat, int width,
fmt::ptr(data.data()), data.size());
std::memcpy(image->data(), data.data(), data.size());
PutFrame(std::move(image), time);
PutFrame(std::move(image), time, timeSrc);
}
void SourceImpl::PutFrame(std::unique_ptr<Image> image, Frame::Time time) {
void SourceImpl::PutFrame(std::unique_ptr<Image> image, Frame::Time time,
WPI_TimestampSource timeSrc) {
// Update telemetry
m_telemetry.RecordSourceFrames(*this, 1);
m_telemetry.RecordSourceBytes(*this, static_cast<int>(image->size()));
@@ -491,7 +494,7 @@ void SourceImpl::PutFrame(std::unique_ptr<Image> image, Frame::Time time) {
// Update frame
{
std::scoped_lock lock{m_frameMutex};
m_frame = Frame{*this, std::move(image), time};
m_frame = Frame{*this, std::move(image), time, timeSrc};
}
// Signal listeners
@@ -502,7 +505,7 @@ void SourceImpl::PutError(std::string_view msg, Frame::Time time) {
// Update frame
{
std::scoped_lock lock{m_frameMutex};
m_frame = Frame{*this, msg, time};
m_frame = Frame{*this, msg, time, WPI_TIMESRC_UNKNOWN};
}
// Signal listeners

7
cscore/src/main/native/cpp/SourceImpl.h
View File

@@ -13,6 +13,7 @@
#include <vector>
#include <wpi/Logger.h>
#include <wpi/RawFrame.h>
#include <wpi/condition_variable.h>
#include <wpi/json_fwd.h>
#include <wpi/mutex.h>
@@ -141,8 +142,10 @@ class SourceImpl : public PropertyContainer {
std::string_view valueStr) override;
void PutFrame(VideoMode::PixelFormat pixelFormat, int width, int height,
std::string_view data, Frame::Time time);
void PutFrame(std::unique_ptr<Image> image, Frame::Time time);
std::string_view data, Frame::Time time,
WPI_TimestampSource timeSrc = WPI_TIMESRC_FRAME_DEQUEUE);
void PutFrame(std::unique_ptr<Image> image, Frame::Time time,
WPI_TimestampSource timeSrc = WPI_TIMESRC_FRAME_DEQUEUE);
void PutError(std::string_view msg, Frame::Time time);
// Notification functions for corresponding atomics

26
cscore/src/main/native/include/cscore_cv.h
View File

@@ -8,6 +8,7 @@
#include <functional>
#include <opencv2/core/mat.hpp>
#include <wpi/RawFrame.h>
#include "cscore_oo.h"
#include "cscore_raw.h"
@@ -172,6 +173,23 @@ class CvSink : public ImageSink {
uint64_t GrabFrameDirectLastTime(cv::Mat& image, uint64_t lastFrameTime,
double timeout = 0.225);
/**
* Get the last time a frame was grabbed. This uses the same time base as
* wpi::Now().
*
* @return Time in 1 us increments.
*/
[[nodiscard]]
uint64_t LastFrameTime();
/**
* Get the time source for the timestamp the last frame was grabbed at.
*
* @return Time source
*/
[[nodiscard]]
WPI_TimestampSource LastFrameTimeSource();
private:
constexpr int GetCvFormat(WPI_PixelFormat pixelFormat);
@@ -405,6 +423,14 @@ inline uint64_t CvSink::GrabFrameDirectLastTime(cv::Mat& image,
return timestamp;
}
inline uint64_t CvSink::LastFrameTime() {
return rawFrame.timestamp;
}
inline WPI_TimestampSource CvSink::LastFrameTimeSource() {
return static_cast<WPI_TimestampSource>(rawFrame.timestampSrc);
}
} // namespace cs
#endif // CSCORE_CSCORE_CV_H_

45
cscore/src/main/native/linux/UsbCameraImpl.cpp
View File

@@ -555,8 +555,51 @@ void UsbCameraImpl::CameraThreadMain() {
good = false;
}
if (good) {
Frame::Time frameTime{wpi::Now()};
WPI_TimestampSource timeSource{WPI_TIMESRC_FRAME_DEQUEUE};
// check the timestamp time
auto tsFlags = buf.flags & V4L2_BUF_FLAG_TIMESTAMP_MASK;
SDEBUG4("Flags {}", tsFlags);
if (tsFlags == V4L2_BUF_FLAG_TIMESTAMP_UNKNOWN) {
SDEBUG4("Got unknown time for frame - default to wpi::Now");
} else if (tsFlags == V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC) {
SDEBUG4("Got valid monotonic time for frame");
// we can't go directly to frametime, since the rest of cscore
// expects us to use wpi::Now, which is in an arbitrary timebase
// (see timestamp.cpp). Best I can do is (approximately) translate
// between timebases
// grab current time in the same timebase as buf.timestamp
struct timespec ts;
if (clock_gettime(CLOCK_MONOTONIC, &ts) == 0) {
int64_t nowTime = {ts.tv_sec * 1'000'000 + ts.tv_nsec / 1000};
int64_t bufTime = {buf.timestamp.tv_sec * 1'000'000 +
buf.timestamp.tv_usec};
// And offset frameTime by the latency
int64_t offset{nowTime - bufTime};
frameTime -= offset;
// Figure out the timestamp's source
int tsrcFlags = buf.flags & V4L2_BUF_FLAG_TSTAMP_SRC_MASK;
if (tsrcFlags == V4L2_BUF_FLAG_TSTAMP_SRC_EOF) {
timeSource = WPI_TIMESRC_V4L_EOF;
} else if (tsrcFlags == V4L2_BUF_FLAG_TSTAMP_SRC_SOE) {
timeSource = WPI_TIMESRC_V4L_SOE;
} else {
timeSource = WPI_TIMESRC_UNKNOWN;
}
SDEBUG4("Frame was {} uS old, flags {}, source {}", offset,
tsrcFlags, static_cast<int>(timeSource));
} else {
// Can't do anything if we can't access the clock, leave default
}
} else if (tsFlags == V4L2_BUF_FLAG_TIMESTAMP_COPY) {
SDEBUG4("Got valid copy time for frame - default to wpi::Now");
}
PutFrame(static_cast<VideoMode::PixelFormat>(m_mode.pixelFormat),
width, height, image, wpi::Now()); // TODO: time
width, height, image, frameTime, timeSource);
}
}

7
epilogue-runtime/src/main/java/edu/wpi/first/epilogue/logging/EpilogueBackend.java
View File

@@ -198,10 +198,9 @@ public interface EpilogueBackend {
*
* @param identifier the identifier of the data field
* @param value the new value of the data field
* @param <U> the dimension of the unit
*/
default <U extends Unit> void log(String identifier, Measure<U> value) {
log(identifier, value, value.baseUnit());
default void log(String identifier, Measure<?> value) {
log(identifier, value.baseUnitMagnitude());
}
/**
@@ -213,7 +212,7 @@ public interface EpilogueBackend {
* @param <U> the dimension of the unit
*/
default <U extends Unit> void log(String identifier, Measure<U> value, U unit) {
log(identifier + " (" + unit.symbol() + ")", value.in(unit));
log(identifier, value.in(unit));
}
/**

16
epilogue-runtime/src/main/java/edu/wpi/first/epilogue/logging/LazyBackend.java
View File

@@ -117,7 +117,7 @@ public class LazyBackend implements EpilogueBackend {
return;
}
m_previousValues.put(identifier, value);
m_previousValues.put(identifier, value.clone());
m_backend.log(identifier, value);
}
@@ -130,7 +130,7 @@ public class LazyBackend implements EpilogueBackend {
return;
}
m_previousValues.put(identifier, value);
m_previousValues.put(identifier, value.clone());
m_backend.log(identifier, value);
}
@@ -143,7 +143,7 @@ public class LazyBackend implements EpilogueBackend {
return;
}
m_previousValues.put(identifier, value);
m_previousValues.put(identifier, value.clone());
m_backend.log(identifier, value);
}
@@ -156,7 +156,7 @@ public class LazyBackend implements EpilogueBackend {
return;
}
m_previousValues.put(identifier, value);
m_previousValues.put(identifier, value.clone());
m_backend.log(identifier, value);
}
@@ -169,7 +169,7 @@ public class LazyBackend implements EpilogueBackend {
return;
}
m_previousValues.put(identifier, value);
m_previousValues.put(identifier, value.clone());
m_backend.log(identifier, value);
}
@@ -182,7 +182,7 @@ public class LazyBackend implements EpilogueBackend {
return;
}
m_previousValues.put(identifier, value);
m_previousValues.put(identifier, value.clone());
m_backend.log(identifier, value);
}
@@ -208,7 +208,7 @@ public class LazyBackend implements EpilogueBackend {
return;
}
m_previousValues.put(identifier, value);
m_previousValues.put(identifier, value.clone());
m_backend.log(identifier, value);
}
@@ -234,7 +234,7 @@ public class LazyBackend implements EpilogueBackend {
return;
}
m_previousValues.put(identifier, value);
m_previousValues.put(identifier, value.clone());
m_backend.log(identifier, value, struct);
}
}

45
epilogue-runtime/src/test/java/edu/wpi/first/epilogue/logging/CustomStruct.java Normal file
View File

@@ -0,0 +1,45 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.epilogue.logging;
import edu.wpi.first.util.struct.Struct;
import edu.wpi.first.util.struct.StructSerializable;
import java.nio.ByteBuffer;
public record CustomStruct(int x) implements StructSerializable {
public static final Serializer struct = new Serializer();
public static final class Serializer implements Struct<CustomStruct> {
@Override
public Class<CustomStruct> getTypeClass() {
return CustomStruct.class;
}
@Override
public String getTypeName() {
return "CustomStruct";
}
@Override
public int getSize() {
return kSizeInt32;
}
@Override
public String getSchema() {
return "int32 x;";
}
@Override
public CustomStruct unpack(ByteBuffer bb) {
return new CustomStruct(bb.getInt());
}
@Override
public void pack(ByteBuffer bb, CustomStruct value) {
bb.putInt(value.x);
}
}
}

132
epilogue-runtime/src/test/java/edu/wpi/first/epilogue/logging/LazyBackendTest.java
View File

@@ -4,6 +4,7 @@
package edu.wpi.first.epilogue.logging;
import static org.junit.jupiter.api.Assertions.assertArrayEquals;
import static org.junit.jupiter.api.Assertions.assertEquals;
import static org.junit.jupiter.api.Assertions.assertSame;
@@ -53,4 +54,135 @@ class LazyBackendTest {
backend.getEntries());
}
}
@Test
void inPlaceByteArray() {
var backend = new TestBackend();
var lazy = new LazyBackend(backend);
byte[] arr = new byte[] {0};
lazy.log("arr", arr);
arr[0] = 1;
lazy.log("arr", arr);
assertEquals(2, backend.getEntries().size());
assertArrayEquals(new byte[] {0}, (byte[]) backend.getEntries().get(0).value());
assertArrayEquals(new byte[] {1}, (byte[]) backend.getEntries().get(1).value());
}
@Test
void inPlaceIntArray() {
var backend = new TestBackend();
var lazy = new LazyBackend(backend);
int[] arr = new int[] {0};
lazy.log("arr", arr);
arr[0] = 1;
lazy.log("arr", arr);
assertEquals(2, backend.getEntries().size());
assertArrayEquals(new int[] {0}, (int[]) backend.getEntries().get(0).value());
assertArrayEquals(new int[] {1}, (int[]) backend.getEntries().get(1).value());
}
@Test
void inPlaceLongArray() {
var backend = new TestBackend();
var lazy = new LazyBackend(backend);
long[] arr = new long[] {0};
lazy.log("arr", arr);
arr[0] = 1;
lazy.log("arr", arr);
assertEquals(2, backend.getEntries().size());
assertArrayEquals(new long[] {0}, (long[]) backend.getEntries().get(0).value());
assertArrayEquals(new long[] {1}, (long[]) backend.getEntries().get(1).value());
}
@Test
void inPlaceFloatArray() {
var backend = new TestBackend();
var lazy = new LazyBackend(backend);
float[] arr = new float[] {0};
lazy.log("arr", arr);
arr[0] = 1;
lazy.log("arr", arr);
assertEquals(2, backend.getEntries().size());
assertArrayEquals(new float[] {0}, (float[]) backend.getEntries().get(0).value());
assertArrayEquals(new float[] {1}, (float[]) backend.getEntries().get(1).value());
}
@Test
void inPlaceDoubleArray() {
var backend = new TestBackend();
var lazy = new LazyBackend(backend);
double[] arr = new double[] {0};
lazy.log("arr", arr);
arr[0] = 1;
lazy.log("arr", arr);
assertEquals(2, backend.getEntries().size());
assertArrayEquals(new double[] {0}, (double[]) backend.getEntries().get(0).value());
assertArrayEquals(new double[] {1}, (double[]) backend.getEntries().get(1).value());
}
@Test
void inPlaceBooleanArray() {
var backend = new TestBackend();
var lazy = new LazyBackend(backend);
boolean[] arr = new boolean[] {false};
lazy.log("arr", arr);
arr[0] = true;
lazy.log("arr", arr);
assertEquals(2, backend.getEntries().size());
assertArrayEquals(new boolean[] {false}, (boolean[]) backend.getEntries().get(0).value());
assertArrayEquals(new boolean[] {true}, (boolean[]) backend.getEntries().get(1).value());
}
@Test
void inPlaceStringArray() {
var backend = new TestBackend();
var lazy = new LazyBackend(backend);
String[] arr = new String[] {"0"};
lazy.log("arr", arr);
arr[0] = "1";
lazy.log("arr", arr);
assertEquals(2, backend.getEntries().size());
assertArrayEquals(new String[] {"0"}, (String[]) backend.getEntries().get(0).value());
assertArrayEquals(new String[] {"1"}, (String[]) backend.getEntries().get(1).value());
}
@Test
void inPlaceStructArray() {
var backend = new TestBackend();
var lazy = new LazyBackend(backend);
CustomStruct[] arr = new CustomStruct[] {new CustomStruct(0)};
lazy.log("arr", arr, CustomStruct.struct);
arr[0] = new CustomStruct(1);
lazy.log("arr", arr, CustomStruct.struct);
assertEquals(2, backend.getEntries().size());
assertArrayEquals(
new byte[] {0x00, 0x00, 0x00, 0x00}, (byte[]) backend.getEntries().get(0).value());
assertArrayEquals(
new byte[] {0x01, 0x00, 0x00, 0x00}, (byte[]) backend.getEntries().get(1).value());
}
}

26
epilogue-runtime/src/test/java/edu/wpi/first/epilogue/logging/TestBackend.java
View File

@@ -55,32 +55,32 @@ public class TestBackend implements EpilogueBackend {
@Override
public void log(String identifier, byte[] value) {
m_entries.add(new LogEntry<>(identifier, value));
m_entries.add(new LogEntry<>(identifier, value.clone()));
}
@Override
public void log(String identifier, int[] value) {
m_entries.add(new LogEntry<>(identifier, value));
m_entries.add(new LogEntry<>(identifier, value.clone()));
}
@Override
public void log(String identifier, long[] value) {
m_entries.add(new LogEntry<>(identifier, value));
m_entries.add(new LogEntry<>(identifier, value.clone()));
}
@Override
public void log(String identifier, float[] value) {
m_entries.add(new LogEntry<>(identifier, value));
m_entries.add(new LogEntry<>(identifier, value.clone()));
}
@Override
public void log(String identifier, double[] value) {
m_entries.add(new LogEntry<>(identifier, value));
m_entries.add(new LogEntry<>(identifier, value.clone()));
}
@Override
public void log(String identifier, boolean[] value) {
m_entries.add(new LogEntry<>(identifier, value));
m_entries.add(new LogEntry<>(identifier, value.clone()));
}
@Override
@@ -90,19 +90,27 @@ public class TestBackend implements EpilogueBackend {
@Override
public void log(String identifier, String[] value) {
m_entries.add(new LogEntry<>(identifier, value));
m_entries.add(new LogEntry<>(identifier, value.clone()));
}
@Override
public <S> void log(String identifier, S value, Struct<S> struct) {
var serialized = StructBuffer.create(struct).write(value).array();
var buffer = StructBuffer.create(struct).write(value).position(0);
var serialized = new byte[buffer.capacity()];
for (int i = 0; i < buffer.capacity(); i++) {
serialized[i] = buffer.get();
}
m_entries.add(new LogEntry<>(identifier, serialized));
}
@Override
public <S> void log(String identifier, S[] value, Struct<S> struct) {
var serialized = StructBuffer.create(struct).writeArray(value).array();
var buffer = StructBuffer.create(struct).writeArray(value).position(0);
var serialized = new byte[buffer.capacity()];
for (int i = 0; i < buffer.capacity(); i++) {
serialized[i] = buffer.get();
}
m_entries.add(new LogEntry<>(identifier, serialized));
}

5
fieldImages/src/main/java/edu/wpi/first/fields/Fields.java
View File

@@ -16,12 +16,13 @@ public enum Fields {
k2021Slalom("2021-slalompath.json"),
k2022RapidReact("2022-rapidreact.json"),
k2023ChargedUp("2023-chargedup.json"),
k2024Crescendo("2024-crescendo.json");
k2024Crescendo("2024-crescendo.json"),
k2025Reefscape("2025-reefscape.json");
public static final String kBaseResourceDir = "/edu/wpi/first/fields/";
/** Alias to the current game. */
public static final Fields kDefaultField = k2024Crescendo;
public static final Fields kDefaultField = k2025Reefscape;
public final String m_resourceFile;

3
fieldImages/src/main/native/cpp/fields.cpp
View File

@@ -16,10 +16,13 @@
#include "fields/2022-rapidreact.h"
#include "fields/2023-chargedup.h"
#include "fields/2024-crescendo.h"
#include "fields/2025-reefscape.h"
using namespace fields;
static const Field kFields[] = {
{"2025 Reefscape", GetResource_2025_reefscape_json,
GetResource_2025_field_png},
{"2024 Crescendo", GetResource_2024_crescendo_json,
GetResource_2024_field_png},
{"2023 Charged Up", GetResource_2023_chargedup_json,

12
fieldImages/src/main/native/include/fields/2025-reefscape.h Normal file
View File

@@ -0,0 +1,12 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#pragma once
#include <string_view>
namespace fields {
std::string_view GetResource_2025_reefscape_json();
std::string_view GetResource_2025_field_png();
} // namespace fields

BIN
fieldImages/src/main/native/resources/edu/wpi/first/fields/2025-field.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 921 KiB

19
fieldImages/src/main/native/resources/edu/wpi/first/fields/2025-reefscape.json Normal file
View File

@@ -0,0 +1,19 @@
{
"game": "Reefscape",
"field-image": "2025-field.png",
"field-corners": {
"top-left": [
534,
291
],
"bottom-right": [
3466,
1638
]
},
"field-size": [
57.573,
26.417
],
"field-unit": "foot"
}

18
glass/src/lib/native/cpp/Window.cpp
View File

@@ -57,12 +57,22 @@ void Window::Display() {
ImGui::PushStyleVar(ImGuiStyleVar_WindowPadding, m_padding);
}
std::string label;
std::string* name = &m_name;
if (m_name.empty()) {
label = fmt::format("{}###{}", m_defaultName, m_id);
} else {
label = fmt::format("{}###{}", m_name, m_id);
name = &m_defaultName;
}
std::string label = fmt::format("{}###{}", *name, m_id);
// Accounts for size of title, collapse button, and close button
float minWidth =
ImGui::CalcTextSize(name->c_str()).x + ImGui::GetFontSize() * 2 +
ImGui::GetStyle().ItemInnerSpacing.x * 3 +
ImGui::GetStyle().FramePadding.x * 2 + ImGui::GetStyle().WindowBorderSize;
// Accounts for size of hamburger button
if (m_renamePopupEnabled || m_view->HasSettings()) {
minWidth += ImGui::GetFontSize() + ImGui::GetStyle().FramePadding.x;
}
ImGui::SetNextWindowSizeConstraints({minWidth, 0}, ImVec2{FLT_MAX, FLT_MAX});
if (Begin(label.c_str(), &m_visible, m_flags)) {
if (m_renamePopupEnabled || m_view->HasSettings()) {

2
glass/src/lib/native/cpp/hardware/LEDDisplay.cpp
View File

@@ -33,7 +33,7 @@ void glass::DisplayLEDDisplay(LEDDisplayModel* model, int index) {
int& order = storage.GetInt("order", LEDConfig::RowMajor);
int& start = storage.GetInt("start", LEDConfig::UpperLeft);
ImGui::PushItemWidth(ImGui::GetFontSize() * 6);
ImGui::PushItemWidth(ImGui::GetFontSize() * 7);
ImGui::LabelText("Length", "%d", length);
ImGui::LabelText("Running", "%s", running ? "Yes" : "No");
ImGui::InputInt("Columns", &numColumns);

37
glass/src/lib/native/cpp/other/Field2D.cpp
View File

@@ -373,13 +373,12 @@ void FieldInfo::DisplaySettings() {
}
ImGui::EndCombo();
}
if (m_builtin.empty() && ImGui::Button("Load image...")) {
if (m_builtin.empty() && ImGui::Button("Load JSON/image...")) {
m_fileOpener = std::make_unique<pfd::open_file>(
"Choose field image", "",
std::vector<std::string>{"Image File",
"Choose field JSON/image", "",
std::vector<std::string>{"PathWeaver JSON File", "*.json", "Image File",
"*.jpg *.jpeg *.png *.bmp *.psd *.tga *.gif "
"*.hdr *.pic *.ppm *.pgm",
"PathWeaver JSON File", "*.json"});
"*.hdr *.pic *.ppm *.pgm"});
}
if (ImGui::Button("Reset image")) {
Reset();
@@ -586,17 +585,29 @@ FieldFrameData FieldInfo::GetFrameData(ImVec2 min, ImVec2 max) const {
max.x -= (m_imageWidth - m_right) * scale;
max.y -= (m_imageHeight - m_bottom) * scale;
} else if ((max.x - min.x) > 40 && (max.y - min.y > 40)) {
// scale padding to be proportional to aspect ratio
float width = max.x - min.x;
float height = max.y - min.y;
float padX, padY;
if (width > height) {
padX = 20 * width / height;
padY = 20;
} else {
padX = 20;
padY = 20 * height / width;
}
// ensure there's some padding
min.x += 20;
max.x -= 20;
min.y += 20;
max.y -= 20;
min.x += padX;
max.x -= padX;
min.y += padY;
max.y -= padY;
// also pad the image so it's the same size as the box
ffd.imageMin.x += 20;
ffd.imageMax.x -= 20;
ffd.imageMin.y += 20;
ffd.imageMax.y -= 20;
ffd.imageMin.x += padX;
ffd.imageMax.x -= padX;
ffd.imageMin.y += padY;
ffd.imageMax.y -= padY;
}
ffd.min = min;

6
hal/src/generate/ResourceType.txt
View File

@@ -122,6 +122,6 @@ kResourceType_ChoreoTrigger = 120
kResourceType_PathWeaverTrajectory = 121
kResourceType_Koors40 = 122
kResourceType_ThriftyNova = 123
kResourceType_PWFSEN36005 = 124
kResourceType_LaserShark = 125
kResourceType_RevServoHub = 126
kResourceType_RevServoHub = 124
kResourceType_PWFSEN36005 = 125
kResourceType_LaserShark = 126

12
hal/src/generated/main/java/edu/wpi/first/hal/FRCNetComm.java generated
View File

@@ -267,12 +267,12 @@ public final class FRCNetComm {
public static final int kResourceType_Koors40 = 122;
/** kResourceType_ThriftyNova = 123. */
public static final int kResourceType_ThriftyNova = 123;
/** kResourceType_PWFSEN36005 = 124. */
public static final int kResourceType_PWFSEN36005 = 124;
/** kResourceType_LaserShark = 125. */
public static final int kResourceType_LaserShark = 125;
/** kResourceType_RevServoHub = 126. */
public static final int kResourceType_RevServoHub = 126;
/** kResourceType_RevServoHub = 124. */
public static final int kResourceType_RevServoHub = 124;
/** kResourceType_PWFSEN36005 = 125. */
public static final int kResourceType_PWFSEN36005 = 125;
/** kResourceType_LaserShark = 126. */
public static final int kResourceType_LaserShark = 126;
}
/**

6
hal/src/generated/main/native/include/hal/FRCUsageReporting.h generated
View File

@@ -175,9 +175,9 @@ namespace HALUsageReporting {
kResourceType_PathWeaverTrajectory = 121,
kResourceType_Koors40 = 122,
kResourceType_ThriftyNova = 123,
kResourceType_PWFSEN36005 = 124,
kResourceType_LaserShark = 125,
kResourceType_RevServoHub = 126,
kResourceType_RevServoHub = 124,
kResourceType_PWFSEN36005 = 125,
kResourceType_LaserShark = 126,
};
enum tInstances : int32_t {
kLanguage_LabVIEW = 1,

6
hal/src/generated/main/native/include/hal/UsageReporting.h generated
View File

@@ -144,9 +144,9 @@ typedef enum
kResourceType_PathWeaverTrajectory = 121,
kResourceType_Koors40 = 122,
kResourceType_ThriftyNova = 123,
kResourceType_PWFSEN36005 = 124,
kResourceType_LaserShark = 125,
kResourceType_RevServoHub = 126,
kResourceType_RevServoHub = 124,
kResourceType_PWFSEN36005 = 125,
kResourceType_LaserShark = 126,
// kResourceType_MaximumID = 255,
} tResourceType;

5
hal/src/main/java/edu/wpi/first/hal/AddressableLEDJNI.java
View File

@@ -53,7 +53,8 @@ public class AddressableLEDJNI extends JNIWrapper {
/**
* Sets the bit timing.
*
* <p>By default, the driver is set up to drive WS2812Bs, so nothing needs to be set for those.
* <p>By default, the driver is set up to drive WS2812B and WS2815, so nothing needs to be set for
* those.
*
* @param handle the Addressable LED handle
* @param highTime0NanoSeconds high time for 0 bit (default 400ns)
@@ -72,7 +73,7 @@ public class AddressableLEDJNI extends JNIWrapper {
/**
* Sets the sync time.
*
* <p>The sync time is the time to hold output so LEDs enable. Default set for WS2812B.
* <p>The sync time is the time to hold output so LEDs enable. Default set for WS2812B and WS2815.
*
* @param handle the Addressable LED handle
* @param syncTimeMicroSeconds the sync time (default 280us)

6
hal/src/main/native/include/hal/AddressableLED.h
View File

@@ -77,8 +77,8 @@ void HAL_WriteAddressableLEDData(HAL_AddressableLEDHandle handle,
/**
* Sets the bit timing.
*
* <p>By default, the driver is set up to drive WS2812Bs, so nothing needs to
* be set for those.
* <p>By default, the driver is set up to drive WS2812B and WS2815, so nothing
* needs to be set for those.
*
* @param[in] handle the Addressable LED handle
* @param[in] highTime0NanoSeconds high time for 0 bit (default 400ns)
@@ -98,7 +98,7 @@ void HAL_SetAddressableLEDBitTiming(HAL_AddressableLEDHandle handle,
* Sets the sync time.
*
* <p>The sync time is the time to hold output so LEDs enable. Default set for
* WS2812B.
* WS2812B and WS2815.
*
* @param[in] handle the Addressable LED handle
* @param[in] syncTimeMicroSeconds the sync time (default 280us)

10
shared/bazel/compiler_flags/systemcore_flags.rc Normal file
View File

@@ -0,0 +1,10 @@
build:systemcore --config=base_linux
build:systemcore --platforms=@rules_bzlmodrio_toolchains//platforms/systemcore
build:systemcore --build_tag_filters=-no-bookworm
build:systemcore --features=compiler_param_file
build:systemcore --platform_suffix=systemcore
build:systemcore --incompatible_enable_cc_toolchain_resolution
build:systemcore --cxxopt=-Wno-error=deprecated-declarations

10
simulation/halsim_gui/src/main/native/cpp/DriverStationGui.cpp
View File

@@ -1102,7 +1102,15 @@ static void DriverStationExecute() {
}
ImGui::SetNextWindowPos(ImVec2{5, 20}, ImGuiCond_FirstUseEver);
ImGui::Begin("Robot State", nullptr, ImGuiWindowFlags_AlwaysAutoResize);
const char* title = "Robot State";
// Accounts for size of title and collapse button
float minWidth = ImGui::CalcTextSize(title).x + ImGui::GetFontSize() +
ImGui::GetStyle().ItemInnerSpacing.x * 2 +
ImGui::GetStyle().FramePadding.x * 2 +
ImGui::GetStyle().WindowBorderSize;
ImGui::SetNextWindowSizeConstraints(ImVec2{minWidth, 0},
ImVec2{FLT_MAX, FLT_MAX});
ImGui::Begin(title, nullptr, ImGuiWindowFlags_AlwaysAutoResize);
if (ImGui::Selectable("Disconnected", !isAttached)) {
HALSIM_SetDriverStationEnabled(false);
HALSIM_SetDriverStationDsAttached(false);

2
upstream_utils/eigen_patches/0001-Disable-warnings.patch
View File

@@ -1,7 +1,7 @@
From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
From: Tyler Veness <calcmogul@gmail.com>
Date: Wed, 18 May 2022 09:14:24 -0700
Subject: [PATCH 1/2] Disable warnings
Subject: [PATCH 1/3] Disable warnings
---
Eigen/src/Core/util/DisableStupidWarnings.h | 6 ++++++

2
upstream_utils/eigen_patches/0002-Intellisense-fix.patch
View File

@@ -1,7 +1,7 @@
From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
From: Peter Johnson <johnson.peter@gmail.com>
Date: Fri, 20 Jan 2023 23:41:56 -0800
Subject: [PATCH 2/2] Intellisense fix
Subject: [PATCH 2/3] Intellisense fix
---
Eigen/src/Core/util/ConfigureVectorization.h | 7 +++++++

305
upstream_utils/eigen_patches/0003-Make-assignment-constexpr.patch Normal file
View File

@@ -0,0 +1,305 @@
From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
From: Tyler Veness <calcmogul@gmail.com>
Date: Sun, 12 Jan 2025 21:04:07 -0800
Subject: [PATCH 3/3] Make assignment constexpr
---
Eigen/src/Core/AssignEvaluator.h | 165 +++++++++++--------
Eigen/src/Core/EigenBase.h | 2 +-
Eigen/src/Core/functors/AssignmentFunctors.h | 2 +-
3 files changed, 102 insertions(+), 67 deletions(-)
diff --git a/Eigen/src/Core/AssignEvaluator.h b/Eigen/src/Core/AssignEvaluator.h
index f7f0b238b8ca70bbc9100262479cc1dbebab9979..9c2436afa7fe98692a036e6ef255ed104a5bf388 100644
--- a/Eigen/src/Core/AssignEvaluator.h
+++ b/Eigen/src/Core/AssignEvaluator.h
@@ -263,7 +263,7 @@ struct copy_using_evaluator_innervec_CompleteUnrolling {
DstAlignment = Kernel::AssignmentTraits::DstAlignment
};
- EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE void run(Kernel& kernel) {
+ EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE constexpr void run(Kernel& kernel) {
kernel.template assignPacketByOuterInner<DstAlignment, SrcAlignment, PacketType>(outer, inner);
enum { NextIndex = Index + unpacket_traits<PacketType>::size };
copy_using_evaluator_innervec_CompleteUnrolling<Kernel, NextIndex, Stop>::run(kernel);
@@ -431,17 +431,25 @@ struct dense_assignment_loop<Kernel, LinearVectorizedTraversal, NoUnrolling> {
template <typename Kernel>
struct dense_assignment_loop<Kernel, LinearVectorizedTraversal, CompleteUnrolling> {
EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE EIGEN_CONSTEXPR void run(Kernel& kernel) {
- typedef typename Kernel::DstEvaluatorType::XprType DstXprType;
- typedef typename Kernel::PacketType PacketType;
-
- enum {
- size = DstXprType::SizeAtCompileTime,
- packetSize = unpacket_traits<PacketType>::size,
- alignedSize = (int(size) / packetSize) * packetSize
- };
-
- copy_using_evaluator_linearvec_CompleteUnrolling<Kernel, 0, alignedSize>::run(kernel);
- copy_using_evaluator_LinearTraversal_CompleteUnrolling<Kernel, alignedSize, size>::run(kernel);
+ if (internal::is_constant_evaluated()) {
+ for (Index outer = 0; outer < kernel.outerSize(); ++outer) {
+ for (Index inner = 0; inner < kernel.innerSize(); ++inner) {
+ kernel.assignCoeffByOuterInner(outer, inner);
+ }
+ }
+ } else {
+ typedef typename Kernel::DstEvaluatorType::XprType DstXprType;
+ typedef typename Kernel::PacketType PacketType;
+
+ enum {
+ size = DstXprType::SizeAtCompileTime,
+ packetSize = unpacket_traits<PacketType>::size,
+ alignedSize = (int(size) / packetSize) * packetSize
+ };
+
+ copy_using_evaluator_linearvec_CompleteUnrolling<Kernel, 0, alignedSize>::run(kernel);
+ copy_using_evaluator_LinearTraversal_CompleteUnrolling<Kernel, alignedSize, size>::run(kernel);
+ }
}
};
@@ -465,9 +473,17 @@ struct dense_assignment_loop<Kernel, InnerVectorizedTraversal, NoUnrolling> {
template <typename Kernel>
struct dense_assignment_loop<Kernel, InnerVectorizedTraversal, CompleteUnrolling> {
- EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE void run(Kernel& kernel) {
- typedef typename Kernel::DstEvaluatorType::XprType DstXprType;
- copy_using_evaluator_innervec_CompleteUnrolling<Kernel, 0, DstXprType::SizeAtCompileTime>::run(kernel);
+ EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE constexpr void run(Kernel& kernel) {
+ if (internal::is_constant_evaluated()) {
+ for (Index outer = 0; outer < kernel.outerSize(); ++outer) {
+ for (Index inner = 0; inner < kernel.innerSize(); ++inner) {
+ kernel.assignCoeffByOuterInner(outer, inner);
+ }
+ }
+ } else {
+ typedef typename Kernel::DstEvaluatorType::XprType DstXprType;
+ copy_using_evaluator_innervec_CompleteUnrolling<Kernel, 0, DstXprType::SizeAtCompileTime>::run(kernel);
+ }
}
};
@@ -498,8 +514,16 @@ struct dense_assignment_loop<Kernel, LinearTraversal, NoUnrolling> {
template <typename Kernel>
struct dense_assignment_loop<Kernel, LinearTraversal, CompleteUnrolling> {
EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE EIGEN_CONSTEXPR void run(Kernel& kernel) {
- typedef typename Kernel::DstEvaluatorType::XprType DstXprType;
- copy_using_evaluator_LinearTraversal_CompleteUnrolling<Kernel, 0, DstXprType::SizeAtCompileTime>::run(kernel);
+ if (internal::is_constant_evaluated()) {
+ for (Index outer = 0; outer < kernel.outerSize(); ++outer) {
+ for (Index inner = 0; inner < kernel.innerSize(); ++inner) {
+ kernel.assignCoeffByOuterInner(outer, inner);
+ }
+ }
+ } else {
+ typedef typename Kernel::DstEvaluatorType::XprType DstXprType;
+ copy_using_evaluator_LinearTraversal_CompleteUnrolling<Kernel, 0, DstXprType::SizeAtCompileTime>::run(kernel);
+ }
}
};
@@ -510,41 +534,49 @@ struct dense_assignment_loop<Kernel, LinearTraversal, CompleteUnrolling> {
template <typename Kernel>
struct dense_assignment_loop<Kernel, SliceVectorizedTraversal, NoUnrolling> {
EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE EIGEN_CONSTEXPR void run(Kernel& kernel) {
- typedef typename Kernel::Scalar Scalar;
- typedef typename Kernel::PacketType PacketType;
- enum {
- packetSize = unpacket_traits<PacketType>::size,
- requestedAlignment = int(Kernel::AssignmentTraits::InnerRequiredAlignment),
- alignable =
- packet_traits<Scalar>::AlignedOnScalar || int(Kernel::AssignmentTraits::DstAlignment) >= sizeof(Scalar),
- dstIsAligned = int(Kernel::AssignmentTraits::DstAlignment) >= int(requestedAlignment),
- dstAlignment = alignable ? int(requestedAlignment) : int(Kernel::AssignmentTraits::DstAlignment)
- };
- const Scalar* dst_ptr = kernel.dstDataPtr();
- if ((!bool(dstIsAligned)) && (std::uintptr_t(dst_ptr) % sizeof(Scalar)) > 0) {
- // the pointer is not aligned-on scalar, so alignment is not possible
- return dense_assignment_loop<Kernel, DefaultTraversal, NoUnrolling>::run(kernel);
- }
- const Index packetAlignedMask = packetSize - 1;
- const Index innerSize = kernel.innerSize();
- const Index outerSize = kernel.outerSize();
- const Index alignedStep = alignable ? (packetSize - kernel.outerStride() % packetSize) & packetAlignedMask : 0;
- Index alignedStart =
- ((!alignable) || bool(dstIsAligned)) ? 0 : internal::first_aligned<requestedAlignment>(dst_ptr, innerSize);
-
- for (Index outer = 0; outer < outerSize; ++outer) {
- const Index alignedEnd = alignedStart + ((innerSize - alignedStart) & ~packetAlignedMask);
- // do the non-vectorizable part of the assignment
- for (Index inner = 0; inner < alignedStart; ++inner) kernel.assignCoeffByOuterInner(outer, inner);
-
- // do the vectorizable part of the assignment
- for (Index inner = alignedStart; inner < alignedEnd; inner += packetSize)
- kernel.template assignPacketByOuterInner<dstAlignment, Unaligned, PacketType>(outer, inner);
-
- // do the non-vectorizable part of the assignment
- for (Index inner = alignedEnd; inner < innerSize; ++inner) kernel.assignCoeffByOuterInner(outer, inner);
-
- alignedStart = numext::mini((alignedStart + alignedStep) % packetSize, innerSize);
+ if (internal::is_constant_evaluated()) {
+ for (Index outer = 0; outer < kernel.outerSize(); ++outer) {
+ for (Index inner = 0; inner < kernel.innerSize(); ++inner) {
+ kernel.assignCoeffByOuterInner(outer, inner);
+ }
+ }
+ } else {
+ typedef typename Kernel::Scalar Scalar;
+ typedef typename Kernel::PacketType PacketType;
+ enum {
+ packetSize = unpacket_traits<PacketType>::size,
+ requestedAlignment = int(Kernel::AssignmentTraits::InnerRequiredAlignment),
+ alignable =
+ packet_traits<Scalar>::AlignedOnScalar || int(Kernel::AssignmentTraits::DstAlignment) >= sizeof(Scalar),
+ dstIsAligned = int(Kernel::AssignmentTraits::DstAlignment) >= int(requestedAlignment),
+ dstAlignment = alignable ? int(requestedAlignment) : int(Kernel::AssignmentTraits::DstAlignment)
+ };
+ const Scalar* dst_ptr = kernel.dstDataPtr();
+ if ((!bool(dstIsAligned)) && (std::uintptr_t(dst_ptr) % sizeof(Scalar)) > 0) {
+ // the pointer is not aligned-on scalar, so alignment is not possible
+ return dense_assignment_loop<Kernel, DefaultTraversal, NoUnrolling>::run(kernel);
+ }
+ const Index packetAlignedMask = packetSize - 1;
+ const Index innerSize = kernel.innerSize();
+ const Index outerSize = kernel.outerSize();
+ const Index alignedStep = alignable ? (packetSize - kernel.outerStride() % packetSize) & packetAlignedMask : 0;
+ Index alignedStart =
+ ((!alignable) || bool(dstIsAligned)) ? 0 : internal::first_aligned<requestedAlignment>(dst_ptr, innerSize);
+
+ for (Index outer = 0; outer < outerSize; ++outer) {
+ const Index alignedEnd = alignedStart + ((innerSize - alignedStart) & ~packetAlignedMask);
+ // do the non-vectorizable part of the assignment
+ for (Index inner = 0; inner < alignedStart; ++inner) kernel.assignCoeffByOuterInner(outer, inner);
+
+ // do the vectorizable part of the assignment
+ for (Index inner = alignedStart; inner < alignedEnd; inner += packetSize)
+ kernel.template assignPacketByOuterInner<dstAlignment, Unaligned, PacketType>(outer, inner);
+
+ // do the non-vectorizable part of the assignment
+ for (Index inner = alignedEnd; inner < innerSize; ++inner) kernel.assignCoeffByOuterInner(outer, inner);
+
+ alignedStart = numext::mini((alignedStart + alignedStep) % packetSize, innerSize);
+ }
}
}
};
@@ -594,9 +626,9 @@ class generic_dense_assignment_kernel {
typedef copy_using_evaluator_traits<DstEvaluatorTypeT, SrcEvaluatorTypeT, Functor> AssignmentTraits;
typedef typename AssignmentTraits::PacketType PacketType;
- EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE generic_dense_assignment_kernel(DstEvaluatorType& dst,
- const SrcEvaluatorType& src,
- const Functor& func, DstXprType& dstExpr)
+ EIGEN_DEVICE_FUNC
+ EIGEN_STRONG_INLINE constexpr generic_dense_assignment_kernel(DstEvaluatorType& dst, const SrcEvaluatorType& src,
+ const Functor& func, DstXprType& dstExpr)
: m_dst(dst), m_src(src), m_functor(func), m_dstExpr(dstExpr) {
#ifdef EIGEN_DEBUG_ASSIGN
AssignmentTraits::debug();
@@ -614,7 +646,7 @@ class generic_dense_assignment_kernel {
EIGEN_DEVICE_FUNC const SrcEvaluatorType& srcEvaluator() const EIGEN_NOEXCEPT { return m_src; }
/// Assign src(row,col) to dst(row,col) through the assignment functor.
- EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void assignCoeff(Index row, Index col) {
+ EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE constexpr void assignCoeff(Index row, Index col) {
m_functor.assignCoeff(m_dst.coeffRef(row, col), m_src.coeff(row, col));
}
@@ -624,7 +656,7 @@ class generic_dense_assignment_kernel {
}
/// \sa assignCoeff(Index,Index)
- EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void assignCoeffByOuterInner(Index outer, Index inner) {
+ EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE constexpr void assignCoeffByOuterInner(Index outer, Index inner) {
Index row = rowIndexByOuterInner(outer, inner);
Index col = colIndexByOuterInner(outer, inner);
assignCoeff(row, col);
@@ -648,7 +680,7 @@ class generic_dense_assignment_kernel {
assignPacket<StoreMode, LoadMode, Packet>(row, col);
}
- EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE Index rowIndexByOuterInner(Index outer, Index inner) {
+ EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE constexpr Index rowIndexByOuterInner(Index outer, Index inner) {
typedef typename DstEvaluatorType::ExpressionTraits Traits;
return int(Traits::RowsAtCompileTime) == 1 ? 0
: int(Traits::ColsAtCompileTime) == 1 ? inner
@@ -656,7 +688,7 @@ class generic_dense_assignment_kernel {
: inner;
}
- EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE Index colIndexByOuterInner(Index outer, Index inner) {
+ EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE constexpr Index colIndexByOuterInner(Index outer, Index inner) {
typedef typename DstEvaluatorType::ExpressionTraits Traits;
return int(Traits::ColsAtCompileTime) == 1 ? 0
: int(Traits::RowsAtCompileTime) == 1 ? inner
@@ -708,8 +740,8 @@ EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void resize_if_allowed(DstXprType& dst, co
}
template <typename DstXprType, typename SrcXprType, typename T1, typename T2>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void resize_if_allowed(DstXprType& dst, const SrcXprType& src,
- const internal::assign_op<T1, T2>& /*func*/) {
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE constexpr void resize_if_allowed(DstXprType& dst, const SrcXprType& src,
+ const internal::assign_op<T1, T2>& /*func*/) {
Index dstRows = src.rows();
Index dstCols = src.cols();
if (((dst.rows() != dstRows) || (dst.cols() != dstCols))) dst.resize(dstRows, dstCols);
@@ -790,7 +822,7 @@ struct Assignment;
// not has to bother about these annoying details.
template <typename Dst, typename Src>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void call_assignment(Dst& dst, const Src& src) {
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE constexpr void call_assignment(Dst& dst, const Src& src) {
call_assignment(dst, src, internal::assign_op<typename Dst::Scalar, typename Src::Scalar>());
}
template <typename Dst, typename Src>
@@ -807,7 +839,7 @@ EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR void call_assignment(
}
template <typename Dst, typename Src, typename Func>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void call_assignment(
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE constexpr void call_assignment(
Dst& dst, const Src& src, const Func& func, std::enable_if_t<!evaluator_assume_aliasing<Src>::value, void*> = 0) {
call_assignment_no_alias(dst, src, func);
}
@@ -891,9 +923,12 @@ EIGEN_DEVICE_FUNC void check_for_aliasing(const Dst& dst, const Src& src);
// both partial specialization+SFINAE without ambiguous specialization
template <typename DstXprType, typename SrcXprType, typename Functor, typename Weak>
struct Assignment<DstXprType, SrcXprType, Functor, Dense2Dense, Weak> {
- EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE void run(DstXprType& dst, const SrcXprType& src, const Functor& func) {
+ EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE constexpr void run(DstXprType& dst, const SrcXprType& src,
+ const Functor& func) {
#ifndef EIGEN_NO_DEBUG
- internal::check_for_aliasing(dst, src);
+ if (!internal::is_constant_evaluated()) {
+ internal::check_for_aliasing(dst, src);
+ }
#endif
call_dense_assignment_loop(dst, src, func);
diff --git a/Eigen/src/Core/EigenBase.h b/Eigen/src/Core/EigenBase.h
index 6d167006a094181fa3693b19f6b9daeb6f2afb79..894bfc13b15eb994abd90f100da15de5bd8b22b7 100644
--- a/Eigen/src/Core/EigenBase.h
+++ b/Eigen/src/Core/EigenBase.h
@@ -50,7 +50,7 @@ struct EigenBase {
/** \returns a const reference to the derived object */
EIGEN_DEVICE_FUNC constexpr const Derived& derived() const { return *static_cast<const Derived*>(this); }
- EIGEN_DEVICE_FUNC inline Derived& const_cast_derived() const {
+ EIGEN_DEVICE_FUNC inline constexpr Derived& const_cast_derived() const {
return *static_cast<Derived*>(const_cast<EigenBase*>(this));
}
EIGEN_DEVICE_FUNC inline const Derived& const_derived() const { return *static_cast<const Derived*>(this); }
diff --git a/Eigen/src/Core/functors/AssignmentFunctors.h b/Eigen/src/Core/functors/AssignmentFunctors.h
index 09d1da8ca2bcb41384520f46e2b793ba8b28a798..3687bb20db4dfe1a2f6cf1342b4fcbd8f91f1f68 100644
--- a/Eigen/src/Core/functors/AssignmentFunctors.h
+++ b/Eigen/src/Core/functors/AssignmentFunctors.h
@@ -23,7 +23,7 @@ namespace internal {
*/
template <typename DstScalar, typename SrcScalar>
struct assign_op {
- EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void assignCoeff(DstScalar& a, const SrcScalar& b) const { a = b; }
+ EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE constexpr void assignCoeff(DstScalar& a, const SrcScalar& b) const { a = b; }
template <int Alignment, typename Packet>
EIGEN_STRONG_INLINE void assignPacket(DstScalar* a, const Packet& b) const {

500
wpical/src/main/native/cpp/WPIcal.cpp
View File

@@ -9,12 +9,14 @@
#include <fstream>
#include <iostream>
#include <map>
#include <memory>
#include <numbers>
#include <string>
#include <vector>
#include <GLFW/glfw3.h>
#include <fmt/format.h>
#include <imgui.h>
#include <portable-file-dialogs.h>
#include <tagpose.h>
@@ -58,6 +60,112 @@ void drawCheck() {
ImGui::NewLine();
}
void processFileSelector(std::unique_ptr<pfd::open_file>& selector,
std::string& selected_file) {
if (selector && selector->ready(0)) {
auto selectedFiles = selector->result();
if (!selectedFiles.empty()) {
selected_file = selectedFiles[0];
}
selector.reset();
}
}
void processFilesSelector(std::unique_ptr<pfd::open_file>& selector,
std::vector<std::string>& selected_files) {
if (selector && selector->ready(0)) {
auto selectedFiles = selector->result();
if (!selectedFiles.empty()) {
selected_files = selectedFiles;
}
selector.reset();
}
}
void processDirectorySelector(std::unique_ptr<pfd::select_folder>& selector,
std::string& selected_directory) {
if (selector && selector->ready(0)) {
auto selectedFiles = selector->result();
if (!selectedFiles.empty()) {
selected_directory = selectedFiles;
}
selector.reset();
}
}
void openFileButton(const char* text, std::string& selected_file,
std::unique_ptr<pfd::open_file>& selector,
const std::string& file_type,
const std::string& file_extensions) {
if (ImGui::Button(text)) {
selector = std::make_unique<pfd::open_file>(
"Select File", "", std::vector<std::string>{file_type, file_extensions},
pfd::opt::none);
}
}
void openFilesButton(const char* text, std::vector<std::string>& selected_files,
std::unique_ptr<pfd::open_file>& selector,
const std::string& file_type,
const std::string& file_extensions) {
if (ImGui::Button(text)) {
selector = std::make_unique<pfd::open_file>(
"Select File", "", std::vector<std::string>{file_type, file_extensions},
pfd::opt::multiselect);
}
}
void openDirectoryButton(const char* text,
std::unique_ptr<pfd::select_folder>& selector,
std::string& selected_directory) {
if (ImGui::Button(text)) {
selector = std::make_unique<pfd::select_folder>("Select Directory", "");
}
}
std::string getFileName(std::string path) {
size_t lastSlash = path.find_last_of("/\\");
size_t lastDot = path.find_last_of(".");
return path.substr(lastSlash + 1, lastDot - lastSlash - 1);
}
static bool EmitEntryTarget(int tag_id, std::string& file) {
if (!file.empty()) {
auto text = fmt::format("{}: {}", tag_id, file);
ImGui::TextUnformatted(text.c_str());
} else {
ImGui::Text("Tag ID %i: <none (DROP HERE)>", tag_id);
}
bool rv = false;
if (ImGui::BeginDragDropTarget()) {
if (const ImGuiPayload* payload =
ImGui::AcceptDragDropPayload("FieldCalibration")) {
file = *(std::string*)payload->Data;
rv = true;
}
ImGui::EndDragDropTarget();
}
return rv;
}
void saveCalibration(wpi::json& field, std::string& output_directory,
std::string output_name, bool& isCalibrating) {
if (!field.empty() && !output_directory.empty()) {
std::cout << "Saving calibration to " << output_directory << std::endl;
std::ofstream out(output_directory + "/" + output_name + ".json");
out << field.dump(4);
out.close();
std::ofstream fmap(output_directory + "/" + output_name + ".fmap");
fmap << fmap::convertfmap(field).dump(4);
fmap.close();
field.clear();
output_directory.clear();
isCalibrating = false;
}
}
static void DisplayGui() {
ImGui::GetStyle().WindowRounding = 0;
@@ -82,19 +190,28 @@ static void DisplayGui() {
ImGui::EndMenuBar();
static std::unique_ptr<pfd::open_file> camera_intrinsics_selector;
static std::string selected_camera_intrinsics;
static std::unique_ptr<pfd::open_file> field_map_selector;
static std::string selected_field_map;
static std::unique_ptr<pfd::open_file> output_calibration_json_selector;
static std::unique_ptr<pfd::open_file> combination_calibrations_selector;
static std::unique_ptr<pfd::select_folder>
field_calibration_directory_selector;
static std::string selected_field_calibration_directory;
static std::unique_ptr<pfd::select_folder> download_directory_selector;
static std::string selected_download_directory;
static std::string calibration_json_path;
static wpi::json field_calibration_json;
static wpi::json field_combination_json;
static std::string selected_camera_intrinsics;
static std::string selected_field_map;
static std::string selected_field_calibration_directory;
static std::string selected_download_directory;
static std::string output_calibration_json_path;
static std::vector<std::string> selected_combination_calibrations;
static std::map<int, std::string> combiner_map;
static int current_combiner_tag_id = 0;
static bool isCalibrating = false;
cameracalibration::CameraModel cameraModel = {
.intrinsic_matrix = Eigen::Matrix<double, 3, 3>::Identity(),
@@ -114,15 +231,16 @@ static void DisplayGui() {
static int focusedTag = 1;
static int referenceTag = 1;
static int maxFRCTag = 22;
static Fieldmap currentCalibrationMap;
static Fieldmap currentReferenceMap;
static Fieldmap currentCombinerMap;
// camera matrix selector button
if (ImGui::Button("Upload Camera Intrinsics")) {
camera_intrinsics_selector = std::make_unique<pfd::open_file>(
"Select Camera Intrinsics JSON", "",
std::vector<std::string>{"JSON", "*.json"}, pfd::opt::none);
}
openFileButton("Select Camera Intrinsics JSON", selected_camera_intrinsics,
camera_intrinsics_selector, "JSON Files", "*.json");
processFileSelector(camera_intrinsics_selector, selected_camera_intrinsics);
ImGui::SameLine();
ImGui::Text("Or");
@@ -134,50 +252,25 @@ static void DisplayGui() {
ImGui::OpenPopup("Camera Calibration");
}
if (camera_intrinsics_selector) {
auto selectedFiles = camera_intrinsics_selector->result();
if (!selectedFiles.empty()) {
selected_camera_intrinsics = selectedFiles[0];
}
camera_intrinsics_selector.reset();
}
if (!selected_camera_intrinsics.empty()) {
drawCheck();
}
// field json selector button
if (ImGui::Button("Select Field Map JSON")) {
field_map_selector = std::make_unique<pfd::open_file>(
"Select Json File", "",
std::vector<std::string>{"JSON Files", "*.json"}, pfd::opt::none);
}
if (field_map_selector) {
auto selectedFiles = field_map_selector->result();
if (!selectedFiles.empty()) {
selected_field_map = selectedFiles[0];
}
field_map_selector.reset();
}
openFileButton("Select Field Map JSON", selected_field_map,
field_map_selector, "JSON Files", "*.json");
processFileSelector(field_map_selector, selected_field_map);
if (!selected_field_map.empty()) {
drawCheck();
}
// field calibration directory selector button
if (ImGui::Button("Select Field Calibration Folder")) {
field_calibration_directory_selector = std::make_unique<pfd::select_folder>(
"Select Field Calibration Folder", "");
}
if (field_calibration_directory_selector) {
auto selectedFiles = field_calibration_directory_selector->result();
if (!selectedFiles.empty()) {
selected_field_calibration_directory = selectedFiles;
}
field_calibration_directory_selector.reset();
}
openDirectoryButton("Select Field Calibration Directory",
field_calibration_directory_selector,
selected_field_calibration_directory);
processDirectorySelector(field_calibration_directory_selector,
selected_field_calibration_directory);
if (!selected_field_calibration_directory.empty()) {
drawCheck();
@@ -187,62 +280,46 @@ static void DisplayGui() {
ImGui::SetNextItemWidth(ImGui::GetFontSize() * 12);
ImGui::InputInt("Pinned Tag", &pinnedTag);
if (pinnedTag < 1) {
pinnedTag = 1;
} else if (pinnedTag > 16) {
pinnedTag = 16;
}
// calibrate button
if (ImGui::Button("Calibrate!!!")) {
if (!selected_field_calibration_directory.empty() &&
!selected_camera_intrinsics.empty() && !selected_field_map.empty() &&
pinnedTag > 0 && pinnedTag <= 16) {
int calibrationOutput = fieldcalibration::calibrate(
selected_field_calibration_directory.c_str(), field_calibration_json,
selected_camera_intrinsics, selected_field_map.c_str(), pinnedTag,
showDebug);
if (calibrationOutput == 1) {
ImGui::OpenPopup("Field Calibration Error");
}
if (selected_download_directory.empty() &&
!field_calibration_json.empty() && !download_directory_selector) {
download_directory_selector =
std::make_unique<pfd::select_folder>("Select Download Folder", "");
if (download_directory_selector) {
auto selectedFiles = download_directory_selector->result();
if (!selectedFiles.empty()) {
selected_download_directory = selectedFiles;
}
download_directory_selector.reset();
}
calibration_json_path = selected_download_directory + "/output.json";
int calibrationOutput = fieldcalibration::calibrate(
selected_field_calibration_directory.c_str(), calibration_json_path,
selected_camera_intrinsics, selected_field_map.c_str(), pinnedTag,
showDebug);
if (calibrationOutput == 1) {
ImGui::OpenPopup("Fmap Conversion failed");
} else if (calibrationOutput == 0) {
std::ifstream caljsonpath(calibration_json_path);
try {
wpi::json fmap = fmap::convertfmap(wpi::json::parse(caljsonpath));
std::ofstream out(selected_download_directory + "/output.fmap");
out << fmap.dump(4);
out.close();
ImGui::SetNextWindowSize(ImVec2(600, 400), ImGuiCond_Always);
ImGui::OpenPopup("Visualize Calibration");
} catch (...) {
ImGui::OpenPopup("Field Calibration Error");
}
}
}
}
processDirectorySelector(download_directory_selector,
selected_download_directory);
saveCalibration(field_calibration_json, selected_download_directory,
"field_calibration", isCalibrating);
if (ImGui::Button("Visualize")) {
ImGui::SetNextWindowSize(ImVec2(600, 400), ImGuiCond_Always);
ImGui::OpenPopup("Visualize Calibration");
}
if (ImGui::Button("Combine Calibrations")) {
ImGui::SetNextWindowSize(ImVec2(600, 400), ImGuiCond_Always);
ImGui::OpenPopup("Combine Calibrations");
}
if (selected_field_calibration_directory.empty() ||
selected_camera_intrinsics.empty() || selected_field_map.empty()) {
ImGui::TextWrapped(
"Some inputs are empty! please enter your camera calibration video, "
"field map, and field calibration directory");
} else if (!(pinnedTag > 0 && pinnedTag <= 16)) {
ImGui::TextWrapped("Make sure the pinned tag is a valid april tag (1-16)");
} else if (!(pinnedTag > 0 && pinnedTag <= maxFRCTag)) {
ImGui::TextWrapped(
"The pinned tag is not within the normal range for FRC fields (1-22), "
"If you proceed, You may experience a bad calibration.");
} else {
ImGui::TextWrapped("Calibration Ready");
}
@@ -269,7 +346,7 @@ static void DisplayGui() {
ImGui::EndPopup();
}
if (ImGui::BeginPopupModal("Fmap Conversion failed", NULL,
if (ImGui::BeginPopupModal("Fmap Conversion Error", NULL,
ImGuiWindowFlags_AlwaysAutoResize)) {
ImGui::TextWrapped(
"Fmap conversion failed - you can still use the calibration output on "
@@ -323,21 +400,11 @@ static void DisplayGui() {
}
if (mrcal) {
if (ImGui::Button("Select Camera Calibration Video")) {
camera_intrinsics_selector = std::make_unique<pfd::open_file>(
"Select Camera Calibration Video", "",
std::vector<std::string>{"Video Files",
"*.mp4 *.mov *.m4v *.mkv *.avi"},
pfd::opt::none);
}
if (camera_intrinsics_selector) {
auto selectedFiles = camera_intrinsics_selector->result();
if (!selectedFiles.empty()) {
selected_camera_intrinsics = selectedFiles[0];
}
camera_intrinsics_selector.reset();
}
openFileButton("Select Camera Calibration Video",
selected_camera_intrinsics, camera_intrinsics_selector,
"Video Files", "*.mp4 *.mov *.m4v *.mkv *.avi");
processFileSelector(camera_intrinsics_selector,
selected_camera_intrinsics);
ImGui::SetNextItemWidth(ImGui::GetFontSize() * 12);
ImGui::InputDouble("Square Width (in)", &squareWidth);
@@ -382,21 +449,11 @@ static void DisplayGui() {
}
}
} else {
if (ImGui::Button("Select Camera Calibration Video")) {
camera_intrinsics_selector = std::make_unique<pfd::open_file>(
"Select Camera Calibration Video", "",
std::vector<std::string>{"Video Files",
"*.mp4 *.mov *.m4v *.mkv *.avi"},
pfd::opt::none);
}
if (camera_intrinsics_selector) {
auto selectedFiles = camera_intrinsics_selector->result();
if (!selectedFiles.empty()) {
selected_camera_intrinsics = selectedFiles[0];
}
camera_intrinsics_selector.reset();
}
openFileButton("Select Camera Calibration Video",
selected_camera_intrinsics, camera_intrinsics_selector,
"Video Files", "*.mp4 *.mov *.m4v *.mkv *.avi");
processFileSelector(camera_intrinsics_selector,
selected_camera_intrinsics);
ImGui::SetNextItemWidth(ImGui::GetFontSize() * 12);
ImGui::InputDouble("Square Width (in)", &squareWidth);
@@ -449,26 +506,19 @@ static void DisplayGui() {
// visualize calibration popup
if (ImGui::BeginPopupModal("Visualize Calibration", NULL,
ImGuiWindowFlags_AlwaysAutoResize)) {
if (ImGui::Button("Load Calibrated Field")) {
calibration_json_path =
std::make_unique<pfd::open_file>(
"Select Json File", "",
std::vector<std::string>{"JSON Files", "*.json"}, pfd::opt::none)
->result()[0];
}
openFileButton("Select Calibration JSON", output_calibration_json_path,
output_calibration_json_selector, "JSON", "*.json");
processFileSelector(output_calibration_json_selector,
output_calibration_json_path);
if (!calibration_json_path.empty()) {
if (!output_calibration_json_path.empty()) {
ImGui::SameLine();
drawCheck();
}
if (ImGui::Button("Load Reference Field")) {
selected_field_map =
std::make_unique<pfd::open_file>(
"Select Json File", "",
std::vector<std::string>{"JSON Files", "*.json"}, pfd::opt::none)
->result()[0];
}
openFileButton("Select Ideal Field Map", selected_field_map,
field_map_selector, "JSON", "*.json");
processFileSelector(field_map_selector, selected_field_map);
if (!selected_field_map.empty()) {
ImGui::SameLine();
@@ -480,70 +530,76 @@ static void DisplayGui() {
ImGui::SetNextItemWidth(ImGui::GetFontSize() * 12);
ImGui::InputInt("Reference Tag", &referenceTag);
if (focusedTag < 1) {
focusedTag = 1;
} else if (focusedTag > 16) {
focusedTag = 16;
}
if (referenceTag < 1) {
referenceTag = 1;
} else if (referenceTag > 16) {
referenceTag = 16;
}
if (!calibration_json_path.empty() && !selected_field_map.empty()) {
std::ifstream calJson(calibration_json_path);
if (!output_calibration_json_path.empty() && !selected_field_map.empty()) {
std::ifstream calJson(output_calibration_json_path);
std::ifstream refJson(selected_field_map);
currentCalibrationMap = Fieldmap(wpi::json::parse(calJson));
currentReferenceMap = Fieldmap(wpi::json::parse(refJson));
double xDiff = currentReferenceMap.getTag(focusedTag).xPos -
currentCalibrationMap.getTag(focusedTag).xPos;
double yDiff = currentReferenceMap.getTag(focusedTag).yPos -
currentCalibrationMap.getTag(focusedTag).yPos;
double zDiff = currentReferenceMap.getTag(focusedTag).zPos -
currentCalibrationMap.getTag(focusedTag).zPos;
double yawDiff = currentReferenceMap.getTag(focusedTag).yawRot -
currentCalibrationMap.getTag(focusedTag).yawRot;
double pitchDiff = currentReferenceMap.getTag(focusedTag).pitchRot -
currentCalibrationMap.getTag(focusedTag).pitchRot;
double rollDiff = currentReferenceMap.getTag(focusedTag).rollRot -
currentCalibrationMap.getTag(focusedTag).rollRot;
if (currentCalibrationMap.getNumTags() !=
currentReferenceMap.getNumTags()) {
ImGui::TextWrapped(
"The number of tags in the calibration output and the ideal field "
"map "
"do not match. Please ensure that the calibration output and ideal "
"field "
"map have the same number of tags.");
} else if (currentReferenceMap.hasTag(focusedTag) &&
currentReferenceMap.hasTag(referenceTag)) {
double xDiff = currentReferenceMap.getTag(focusedTag).xPos -
currentCalibrationMap.getTag(focusedTag).xPos;
double yDiff = currentReferenceMap.getTag(focusedTag).yPos -
currentCalibrationMap.getTag(focusedTag).yPos;
double zDiff = currentReferenceMap.getTag(focusedTag).zPos -
currentCalibrationMap.getTag(focusedTag).zPos;
double yawDiff = currentReferenceMap.getTag(focusedTag).yawRot -
currentCalibrationMap.getTag(focusedTag).yawRot;
double pitchDiff = currentReferenceMap.getTag(focusedTag).pitchRot -
currentCalibrationMap.getTag(focusedTag).pitchRot;
double rollDiff = currentReferenceMap.getTag(focusedTag).rollRot -
currentCalibrationMap.getTag(focusedTag).rollRot;
double xRef = currentCalibrationMap.getTag(referenceTag).xPos -
currentCalibrationMap.getTag(focusedTag).xPos;
double yRef = currentCalibrationMap.getTag(referenceTag).yPos -
currentCalibrationMap.getTag(focusedTag).yPos;
double zRef = currentCalibrationMap.getTag(referenceTag).zPos -
currentCalibrationMap.getTag(focusedTag).zPos;
double xRef = currentCalibrationMap.getTag(referenceTag).xPos -
currentCalibrationMap.getTag(focusedTag).xPos;
double yRef = currentCalibrationMap.getTag(referenceTag).yPos -
currentCalibrationMap.getTag(focusedTag).yPos;
double zRef = currentCalibrationMap.getTag(referenceTag).zPos -
currentCalibrationMap.getTag(focusedTag).zPos;
ImGui::TextWrapped("X Difference: %s (m)", std::to_string(xDiff).c_str());
ImGui::TextWrapped("Y Difference: %s (m)", std::to_string(yDiff).c_str());
ImGui::TextWrapped("Z Difference: %s (m)", std::to_string(zDiff).c_str());
ImGui::TextWrapped("X Difference: %s (m)",
std::to_string(xDiff).c_str());
ImGui::TextWrapped("Y Difference: %s (m)",
std::to_string(yDiff).c_str());
ImGui::TextWrapped("Z Difference: %s (m)",
std::to_string(zDiff).c_str());
ImGui::TextWrapped(
"Yaw Difference %s°",
std::to_string(
Fieldmap::minimizeAngle(yawDiff * (180.0 / std::numbers::pi)))
.c_str());
ImGui::TextWrapped(
"Pitch Difference %s°",
std::to_string(
Fieldmap::minimizeAngle(pitchDiff * (180.0 / std::numbers::pi)))
.c_str());
ImGui::TextWrapped(
"Roll Difference %s°",
std::to_string(
Fieldmap::minimizeAngle(rollDiff * (180.0 / std::numbers::pi)))
.c_str());
ImGui::TextWrapped(
"Yaw Difference %s°",
std::to_string(
Fieldmap::minimizeAngle(yawDiff * (180.0 / std::numbers::pi)))
.c_str());
ImGui::TextWrapped(
"Pitch Difference %s°",
std::to_string(
Fieldmap::minimizeAngle(pitchDiff * (180.0 / std::numbers::pi)))
.c_str());
ImGui::TextWrapped(
"Roll Difference %s°",
std::to_string(
Fieldmap::minimizeAngle(rollDiff * (180.0 / std::numbers::pi)))
.c_str());
ImGui::NewLine();
ImGui::NewLine();
ImGui::TextWrapped("X Reference: %s (m)", std::to_string(xRef).c_str());
ImGui::TextWrapped("Y Reference: %s (m)", std::to_string(yRef).c_str());
ImGui::TextWrapped("Z Reference: %s (m)", std::to_string(zRef).c_str());
ImGui::TextWrapped("X Reference: %s (m)", std::to_string(xRef).c_str());
ImGui::TextWrapped("Y Reference: %s (m)", std::to_string(yRef).c_str());
ImGui::TextWrapped("Z Reference: %s (m)", std::to_string(zRef).c_str());
} else {
ImGui::TextWrapped(
"Please select tags that are in the ideal field map and "
"calibration map");
}
}
if (ImGui::Button("Close")) {
@@ -552,6 +608,78 @@ static void DisplayGui() {
ImGui::EndPopup();
}
if (ImGui::BeginPopupModal("Combine Calibrations", NULL,
ImGuiWindowFlags_AlwaysAutoResize)) {
openFileButton("Select Ideal Map", selected_field_map, field_map_selector,
"JSON", "*.json");
processFileSelector(field_map_selector, selected_field_map);
if (!selected_field_map.empty()) {
drawCheck();
std::ifstream json(selected_field_map);
currentReferenceMap = Fieldmap(wpi::json::parse(json));
currentCombinerMap = currentReferenceMap;
}
openFilesButton("Select Field Calibrations",
selected_combination_calibrations,
combination_calibrations_selector, "JSON", "*.json");
processFilesSelector(combination_calibrations_selector,
selected_combination_calibrations);
if (!selected_field_map.empty() &&
!selected_combination_calibrations.empty()) {
for (std::string& file : selected_combination_calibrations) {
ImGui::Selectable(getFileName(file).c_str(), false,
ImGuiSelectableFlags_DontClosePopups);
if (ImGui::BeginDragDropSource()) {
ImGui::SetDragDropPayload("FieldCalibration", &file, sizeof(file));
ImGui::TextUnformatted(file.c_str());
ImGui::EndDragDropSource();
}
}
for (auto& [key, val] : combiner_map) {
EmitEntryTarget(key, val);
}
ImGui::InputInt("Tag ID", &current_combiner_tag_id);
ImGui::SameLine();
if (ImGui::Button("Add", ImVec2(0, 0)) &&
currentCombinerMap.hasTag(current_combiner_tag_id)) {
combiner_map.emplace(current_combiner_tag_id, "");
}
ImGui::SameLine();
if (ImGui::Button("Remove", ImVec2(0, 0))) {
combiner_map.erase(current_combiner_tag_id);
}
}
ImGui::Separator();
if (ImGui::Button("Close", ImVec2(0, 0))) {
ImGui::CloseCurrentPopup();
}
ImGui::SameLine();
if (ImGui::Button("Download", ImVec2(0, 0))) {
for (auto& [key, val] : combiner_map) {
std::ifstream json(val);
Fieldmap map(wpi::json::parse(json));
currentCombinerMap.replaceTag(key, map.getTag(key));
}
field_combination_json = currentCombinerMap.toJson();
}
if (selected_download_directory.empty() &&
!field_combination_json.empty() && !download_directory_selector) {
download_directory_selector =
std::make_unique<pfd::select_folder>("Select Download Folder", "");
}
processDirectorySelector(download_directory_selector,
selected_download_directory);
saveCalibration(field_combination_json, selected_download_directory,
"combined_calibration", isCalibrating);
ImGui::EndPopup();
}
ImGui::End();
}

19
wpical/src/main/native/cpp/fieldcalibration.cpp
View File

@@ -23,7 +23,6 @@
#include <opencv2/highgui.hpp>
#include <opencv2/imgproc.hpp>
#include <opencv2/videoio.hpp>
#include <wpi/json.h>
#include "apriltag.h"
#include "tag36h11.h"
@@ -433,7 +432,7 @@ inline bool process_video_file(
}
int fieldcalibration::calibrate(std::string input_dir_path,
std::string output_file_path,
wpi::json& output_json,
std::string camera_model_path,
std::string ideal_map_path, int pinned_tag_id,
bool show_debug_window) {
@@ -466,6 +465,19 @@ int fieldcalibration::calibrate(std::string input_dir_path,
return 1;
}
bool pinned_tag_found = false;
// Check if pinned tag is in ideal map
for (const auto& [tag_id, tag_json] : ideal_map) {
if (tag_id == pinned_tag_id) {
pinned_tag_found = true;
break;
}
}
if (!pinned_tag_found) {
return 1;
}
// Apriltag detector
apriltag_detector_t* tag_detector = apriltag_detector_create();
tag_detector->nthreads = 8;
@@ -592,8 +604,7 @@ int fieldcalibration::calibrate(std::string input_dir_path,
{"length", static_cast<double>(json.at("field").at("length"))},
{"width", static_cast<double>(json.at("field").at("width"))}};
std::ofstream output_file(output_file_path);
output_file << observed_map_json.dump(4) << std::endl;
output_json = observed_map_json;
return 0;
}

18
wpical/src/main/native/cpp/tagpose.cpp
View File

@@ -5,8 +5,10 @@
#include <tagpose.h>
namespace tag {
Pose::Pose(double xpos, double ypos, double zpos, double w, double x, double y,
double z, double field_length_meters, double field_width_meters) {
Pose::Pose(int tag_id, double xpos, double ypos, double zpos, double w,
double x, double y, double z, double field_length_meters,
double field_width_meters) {
tagId = tag_id;
xPos = xpos;
yPos = ypos;
zPos = zpos;
@@ -26,4 +28,16 @@ Pose::Pose(double xpos, double ypos, double zpos, double w, double x, double y,
pitchRot = eulerAngles[1];
yawRot = eulerAngles[2];
}
wpi::json Pose::toJson() {
return {{"ID", tagId},
{"pose",
{{"translation", {{"x", xPos}, {"y", yPos}, {"z", zPos}}},
{"rotation",
{{"quaternion",
{{"W", quaternion.w()},
{"X", quaternion.x()},
{"Y", quaternion.y()},
{"Z", quaternion.z()}}}}}}}};
}
} // namespace tag

4
wpical/src/main/native/include/fieldcalibration.h
View File

@@ -6,10 +6,12 @@
#include <string>
#include <wpi/json.h>
#include "cameracalibration.h"
namespace fieldcalibration {
int calibrate(std::string input_dir_path, std::string output_file_path,
int calibrate(std::string input_dir_path, wpi::json& output_json,
std::string camera_model_path, std::string ideal_map_path,
int pinned_tag_id, bool show_debug_window);
} // namespace fieldcalibration

37
wpical/src/main/native/include/fieldmap.h
View File

@@ -5,7 +5,7 @@
#pragma once
#include <cmath>
#include <vector>
#include <map>
#include <tagpose.h>
#include <wpi/json.h>
@@ -19,6 +19,7 @@ class Fieldmap {
double field_width_meters =
static_cast<double>(json.at("field").at("width"));
for (const auto& tag : json.at("tags").items()) {
double tag_id = static_cast<int>(tag.value().at("ID"));
double tagXPos =
static_cast<double>(tag.value().at("pose").at("translation").at("x"));
double tagYPos =
@@ -34,15 +35,30 @@ class Fieldmap {
double tagZQuat = static_cast<double>(
tag.value().at("pose").at("rotation").at("quaternion").at("Z"));
tagVec.emplace_back(tagXPos, tagYPos, tagZPos, tagWQuat, tagXQuat,
tagYQuat, tagZQuat, field_length_meters,
field_width_meters);
tagMap.emplace(
tag_id, tag::Pose(tag_id, tagXPos, tagYPos, tagZPos, tagWQuat,
tagXQuat, tagYQuat, tagZQuat, field_length_meters,
field_width_meters));
}
fieldLength = field_length_meters;
fieldWidth = field_width_meters;
}
const tag::Pose& getTag(size_t tag) const { return tagVec[tag - 1]; }
const tag::Pose& getTag(size_t tag) const { return tagMap.at(tag); }
int getNumTags() const { return tagVec.size(); }
int getNumTags() const { return tagMap.size(); }
bool hasTag(int tag) { return tagMap.find(tag) != tagMap.end(); }
wpi::json toJson() {
wpi::json json;
for (auto& [key, val] : tagMap) {
json["tags"].push_back(val.toJson());
}
json["field"]["length"] = fieldLength;
json["field"]["width"] = fieldWidth;
return json;
}
static double minimizeAngle(double angle) {
angle = std::fmod(angle, 360);
@@ -54,6 +70,13 @@ class Fieldmap {
return angle;
}
void replaceTag(int tag_id, tag::Pose pose) {
tagMap.erase(tag_id);
tagMap.emplace(tag_id, pose);
}
private:
std::vector<tag::Pose> tagVec;
double fieldLength;
double fieldWidth;
std::map<int, tag::Pose> tagMap;
};

8
wpical/src/main/native/include/tagpose.h
View File

@@ -6,15 +6,19 @@
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <wpi/json.h>
namespace tag {
class Pose {
public:
Pose(double xpos, double ypos, double zpos, double w, double x, double y,
double z, double field_length_meters, double field_width_meters);
Pose(int tag_id, double xpos, double ypos, double zpos, double w, double x,
double y, double z, double field_length_meters,
double field_width_meters);
int tagId;
double xPos, yPos, zPos, yawRot, rollRot, pitchRot;
Eigen::Quaterniond quaternion;
Eigen::Matrix3d rotationMatrix;
Eigen::Matrix4d transformMatrixFmap;
wpi::json toJson();
};
} // namespace tag

27
wpical/src/test/native/cpp/test_calibrate.cpp
View File

@@ -19,6 +19,9 @@ cameracalibration::CameraModel cameraModel = {
.intrinsic_matrix = Eigen::Matrix<double, 3, 3>::Identity(),
.distortion_coefficients = Eigen::Matrix<double, 8, 1>::Zero(),
.avg_reprojection_error = 0.0};
wpi::json output_json;
#ifdef __linux__
const std::string fileSuffix = ".avi";
const std::string videoLocation = "/altfieldvideo";
@@ -58,7 +61,7 @@ TEST(Camera_CalibrationTest, MRcal_Atypical) {
TEST(Field_CalibrationTest, Typical) {
int ret = fieldcalibration::calibrate(
projectRootPath + videoLocation, calSavePath,
projectRootPath + videoLocation, output_json,
calSavePath + "/cameracalibration.json",
projectRootPath + "/2024-crescendo.json", 3, false);
EXPECT_EQ(ret, 0);
@@ -66,7 +69,7 @@ TEST(Field_CalibrationTest, Typical) {
TEST(Field_CalibrationTest, Atypical_Bad_Camera_Model_Directory) {
int ret = fieldcalibration::calibrate(
projectRootPath + videoLocation, calSavePath,
projectRootPath + videoLocation, output_json,
projectRootPath + videoLocation + "/long" + fileSuffix,
projectRootPath + "/2024-crescendo.json", 3, false);
EXPECT_EQ(ret, 1);
@@ -74,7 +77,7 @@ TEST(Field_CalibrationTest, Atypical_Bad_Camera_Model_Directory) {
TEST(Field_CalibrationTest, Atypical_Bad_Ideal_JSON) {
int ret = fieldcalibration::calibrate(
projectRootPath + videoLocation, calSavePath,
projectRootPath + videoLocation, output_json,
calSavePath + "/cameracalibration.json",
calSavePath + "/cameracalibration.json", 3, false);
EXPECT_EQ(ret, 1);
@@ -82,8 +85,24 @@ TEST(Field_CalibrationTest, Atypical_Bad_Ideal_JSON) {
TEST(Field_CalibrationTest, Atypical_Bad_Input_Directory) {
int ret = fieldcalibration::calibrate(
projectRootPath + "", calSavePath,
projectRootPath + "", output_json,
calSavePath + "/cameracalibration.json",
projectRootPath + "/2024-crescendo.json", 3, false);
EXPECT_EQ(ret, 1);
}
TEST(Field_CalibrationTest, Atypical_Bad_Pinned_Tag) {
int ret = fieldcalibration::calibrate(
projectRootPath + videoLocation, output_json,
calSavePath + "/cameracalibration.json",
projectRootPath + "/2024-crescendo.json", 42, false);
EXPECT_EQ(ret, 1);
}
TEST(Field_CalibrationTest, Atypical_Bad_Pinned_Tag_Negative) {
int ret = fieldcalibration::calibrate(
projectRootPath + videoLocation, output_json,
calSavePath + "/cameracalibration.json",
projectRootPath + "/2024-crescendo.json", -1, false);
EXPECT_EQ(ret, 1);
}

6
wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandScheduler.java
View File

@@ -180,6 +180,9 @@ public final class CommandScheduler implements Sendable, AutoCloseable {
* using those requirements have been scheduled as interruptible. If this is the case, they will
* be interrupted and the command will be scheduled.
*
* <p>WARNING: using this function directly can often lead to unexpected behavior and should be
* avoided. Instead Triggers should be used to schedule Commands.
*
* @param command the command to schedule. If null, no-op.
*/
private void schedule(Command command) {
@@ -230,6 +233,9 @@ public final class CommandScheduler implements Sendable, AutoCloseable {
/**
* Schedules multiple commands for execution. Does nothing for commands already scheduled.
*
* <p>WARNING: using this function directly can often lead to unexpected behavior and should be
* avoided. Instead Triggers should be used to schedule Commands.
*
* @param commands the commands to schedule. No-op on null.
*/
public void schedule(Command... commands) {

8
wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/Commands.java
View File

@@ -198,15 +198,11 @@ public final class Commands {
*
* @param supplier the command supplier
* @return the command
* @deprecated The ProxyCommand supplier constructor has been deprecated in favor of directly
* proxying a {@link DeferredCommand}, see ProxyCommand documentation for more details. As a
* replacement, consider using `defer(supplier).asProxy()`.
* @see ProxyCommand
* @see DeferredCommand
*/
@Deprecated(since = "2025", forRemoval = true)
@SuppressWarnings("removal")
public static Command deferredProxy(Supplier<Command> supplier) {
return new ProxyCommand(supplier);
return defer(() -> supplier.get().asProxy(), Set.of());
}
// Command Groups

15
wpilibNewCommands/src/main/native/cpp/frc2/command/Commands.cpp
View File

@@ -7,6 +7,7 @@
#include <utility>
#include <vector>
#include <wpi/FunctionExtras.h>
#include <wpi/deprecated.h>
#include "frc2/command/ConditionalCommand.h"
@@ -73,15 +74,21 @@ CommandPtr cmd::Print(std::string_view msg) {
return PrintCommand(msg).ToPtr();
}
WPI_IGNORE_DEPRECATED
CommandPtr cmd::DeferredProxy(wpi::unique_function<Command*()> supplier) {
return ProxyCommand(std::move(supplier)).ToPtr();
return Defer(
[supplier = std::move(supplier)]() mutable {
// There is no non-owning version of AsProxy(), so use the non-owning
// ProxyCommand constructor instead.
return ProxyCommand{supplier()}.ToPtr();
},
{});
}
CommandPtr cmd::DeferredProxy(wpi::unique_function<CommandPtr()> supplier) {
return ProxyCommand(std::move(supplier)).ToPtr();
return Defer([supplier = std::move(
supplier)]() mutable { return supplier().AsProxy(); },
{});
}
WPI_UNIGNORE_DEPRECATED
CommandPtr cmd::Wait(units::second_t duration) {
return WaitCommand(duration).ToPtr();

16
wpilibNewCommands/src/main/native/include/frc2/command/CommandScheduler.h
View File

@@ -88,6 +88,10 @@ class CommandScheduler final : public wpi::Sendable,
* interruptible. If this is the case, they will be interrupted and the
* command will be scheduled.
*
* @warning Using this function directly can often lead to unexpected behavior
* and should be avoided. Instead Triggers should be used to schedule
* Commands.
*
* @param command the command to schedule
*/
void Schedule(const CommandPtr& command);
@@ -112,6 +116,10 @@ class CommandScheduler final : public wpi::Sendable,
*
* The pointer must remain valid through the entire lifecycle of the command.
*
* @warning Using this function directly can often lead to unexpected behavior
* and should be avoided. Instead Triggers should be used to schedule
* Commands.
*
* @param command the command to schedule
*/
void Schedule(Command* command);
@@ -120,6 +128,10 @@ class CommandScheduler final : public wpi::Sendable,
* Schedules multiple commands for execution. Does nothing for commands
* already scheduled.
*
* @warning Using this function directly can often lead to unexpected behavior
* and should be avoided. Instead Triggers should be used to schedule
* Commands.
*
* @param commands the commands to schedule
*/
void Schedule(std::span<Command* const> commands);
@@ -128,6 +140,10 @@ class CommandScheduler final : public wpi::Sendable,
* Schedules multiple commands for execution. Does nothing for commands
* already scheduled.
*
* @warning Using this function directly can often lead to unexpected behavior
* and should be avoided. Instead Triggers should be used to schedule
* Commands.
*
* @param commands the commands to schedule
*/
void Schedule(std::initializer_list<Command*> commands);

13
wpilibNewCommands/src/main/native/include/frc2/command/Commands.h
View File

@@ -169,15 +169,11 @@ CommandPtr Defer(wpi::unique_function<CommandPtr()> supplier,
/**
* Constructs a command that schedules the command returned from the supplier
* when initialized, and ends when it is no longer scheduled. The supplier is
* called when the command is initialized. As a replacement, consider using
* `Defer(supplier).AsProxy()`.
* called when the command is initialized.
*
* @param supplier the command supplier
*/
WPI_IGNORE_DEPRECATED
[[nodiscard]] [[deprecated(
"The ProxyCommand supplier constructor has been deprecated. Use "
"Defer(supplier).AsProxy() instead.")]]
[[nodiscard]]
CommandPtr DeferredProxy(wpi::unique_function<Command*()> supplier);
/**
@@ -187,11 +183,8 @@ CommandPtr DeferredProxy(wpi::unique_function<Command*()> supplier);
*
* @param supplier the command supplier
*/
[[nodiscard]] [[deprecated(
"The ProxyCommand supplier constructor has been deprecated. Use "
"Defer(supplier).AsProxy() instead.")]]
[[nodiscard]]
CommandPtr DeferredProxy(wpi::unique_function<CommandPtr()> supplier);
WPI_UNIGNORE_DEPRECATED
// Command Groups
namespace impl {

7
wpilibc/src/main/native/cpp/Joystick.cpp
View File

@@ -5,12 +5,8 @@
#include "frc/Joystick.h"
#include <cmath>
#include <numbers>
#include <hal/FRCUsageReporting.h>
#include <units/dimensionless.h>
#include <units/math.h>
#include <wpi/deprecated.h>
#include "frc/event/BooleanEvent.h"
@@ -123,6 +119,5 @@ double Joystick::GetMagnitude() const {
}
units::radian_t Joystick::GetDirection() const {
return units::math::atan2(units::dimensionless::scalar_t{GetX()},
units::dimensionless::scalar_t{-GetY()});
return units::radian_t{std::atan2(GetX(), -GetY())};
}

21
wpilibc/src/main/native/cpp/simulation/DCMotorSim.cpp
View File

@@ -42,6 +42,15 @@ void DCMotorSim::SetState(units::radian_t angularPosition,
SetState(Vectord<2>{angularPosition, angularVelocity});
}
void DCMotorSim::SetAngle(units::radian_t angularPosition) {
SetState(angularPosition, GetAngularVelocity());
}
void DCMotorSim::SetAngularVelocity(
units::radians_per_second_t angularVelocity) {
SetState(GetAngularPosition(), angularVelocity);
}
units::radian_t DCMotorSim::GetAngularPosition() const {
return units::radian_t{GetOutput(0)};
}
@@ -76,3 +85,15 @@ void DCMotorSim::SetInputVoltage(units::volt_t voltage) {
SetInput(Vectord<1>{voltage.value()});
ClampInput(frc::RobotController::GetBatteryVoltage().value());
}
const DCMotor& DCMotorSim::GetGearbox() const {
return m_gearbox;
}
double DCMotorSim::GetGearing() const {
return m_gearing;
}
units::kilogram_square_meter_t DCMotorSim::GetJ() const {
return m_j;
}

12
wpilibc/src/main/native/include/frc/AddressableLED.h
View File

@@ -19,10 +19,10 @@
namespace frc {
/**
* A class for driving addressable LEDs, such as WS2812Bs and NeoPixels.
* A class for driving addressable LEDs, such as WS2812B, WS2815, and NeoPixels.
*
* By default, the timing supports WS2812B LEDs, but is configurable using
* SetBitTiming()
* By default, the timing supports WS2812B and WS2815 LEDs, but is configurable
* using SetBitTiming()
*
* <p>Only 1 LED driver is currently supported by the roboRIO. However,
* multiple LED strips can be connected in series and controlled from the
@@ -130,8 +130,8 @@ class AddressableLED {
/**
* Sets the bit timing.
*
* <p>By default, the driver is set up to drive WS2812Bs, so nothing needs to
* be set for those.
* <p>By default, the driver is set up to drive WS2812B and WS2815, so nothing
* needs to be set for those.
*
* @param highTime0 high time for 0 bit (default 400ns)
* @param lowTime0 low time for 0 bit (default 900ns)
@@ -146,7 +146,7 @@ class AddressableLED {
* Sets the sync time.
*
* <p>The sync time is the time to hold output so LEDs enable. Default set for
* WS2812B.
* WS2812B and WS2815.
*
* @param syncTime the sync time (default 280us)
*/

20
wpilibc/src/main/native/include/frc/simulation/DCMotorSim.h
View File

@@ -45,6 +45,20 @@ class DCMotorSim : public LinearSystemSim<2, 1, 2> {
void SetState(units::radian_t angularPosition,
units::radians_per_second_t angularVelocity);
/**
* Sets the DC motor's angular position.
*
* @param angularPosition The new position in radians.
*/
void SetAngle(units::radian_t angularPosition);
/**
* Sets the DC motor's angular velocity.
*
* @param angularVelocity The new velocity in radians per second.
*/
void SetAngularVelocity(units::radians_per_second_t angularVelocity);
/**
* Returns the DC motor position.
*
@@ -97,17 +111,17 @@ class DCMotorSim : public LinearSystemSim<2, 1, 2> {
/**
* Returns the gearbox.
*/
const DCMotor& Gearbox() const { return m_gearbox; }
const DCMotor& GetGearbox() const;
/**
* Returns the gearing;
*/
double Gearing() const { return m_gearing; }
double GetGearing() const;
/**
* Returns the moment of inertia
*/
units::kilogram_square_meter_t J() const { return m_j; }
units::kilogram_square_meter_t GetJ() const;
private:
DCMotor m_gearbox;

10
wpilibj/src/main/java/edu/wpi/first/wpilibj/AddressableLED.java
View File

@@ -10,9 +10,10 @@ import edu.wpi.first.hal.HAL;
import edu.wpi.first.hal.PWMJNI;
/**
* A class for driving addressable LEDs, such as WS2812Bs and NeoPixels.
* A class for driving addressable LEDs, such as WS2812B, WS2815, and NeoPixels.
*
* <p>By default, the timing supports WS2812B LEDs, but is configurable using setBitTiming()
* <p>By default, the timing supports WS2812B and WS2815 LEDs, but is configurable using
* setBitTiming()
*
* <p>Only 1 LED driver is currently supported by the roboRIO. However, multiple LED strips can be
* connected in series and controlled from the single driver.
@@ -70,7 +71,8 @@ public class AddressableLED implements AutoCloseable {
/**
* Sets the bit timing.
*
* <p>By default, the driver is set up to drive WS2812Bs, so nothing needs to be set for those.
* <p>By default, the driver is set up to drive WS2812B and WS2815, so nothing needs to be set for
* those.
*
* @param highTime0NanoSeconds high time for 0 bit (default 400ns)
* @param lowTime0NanoSeconds low time for 0 bit (default 900ns)
@@ -93,7 +95,7 @@ public class AddressableLED implements AutoCloseable {
/**
* Sets the sync time.
*
* <p>The sync time is the time to hold output so LEDs enable. Default set for WS2812B.
* <p>The sync time is the time to hold output so LEDs enable. Default set for WS2812B and WS2815.
*
* @param syncTimeMicroSeconds the sync time (default 280us)
*/

11
wpimath/src/main/java/edu/wpi/first/math/geometry/Pose2d.java
View File

@@ -238,6 +238,17 @@ public class Pose2d implements Interpolatable<Pose2d>, ProtobufSerializable, Str
return new Pose2d(transform.getTranslation(), transform.getRotation());
}
/**
* Rotates the current pose around a point in 2D space.
*
* @param point The point in 2D space to rotate around.
* @param rot The rotation to rotate the pose by.
* @return The new rotated pose.
*/
public Pose2d rotateAround(Translation2d point, Rotation2d rot) {
return new Pose2d(m_translation.rotateAround(point, rot), m_rotation.rotateBy(rot));
}
/**
* Obtain a new Pose2d from a (constant curvature) velocity.
*

11
wpimath/src/main/java/edu/wpi/first/math/geometry/Pose3d.java
View File

@@ -271,6 +271,17 @@ public class Pose3d implements Interpolatable<Pose3d>, ProtobufSerializable, Str
return new Pose3d(transform.getTranslation(), transform.getRotation());
}
/**
* Rotates the current pose around a point in 3D space.
*
* @param point The point in 3D space to rotate around.
* @param rot The rotation to rotate the pose by.
* @return The new rotated pose.
*/
public Pose3d rotateAround(Translation3d point, Rotation3d rot) {
return new Pose3d(m_translation.rotateAround(point, rot), m_rotation.rotateBy(rot));
}
/**
* Obtain a new Pose3d from a (constant curvature) velocity.
*

4
wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java
View File

@@ -110,11 +110,11 @@ public class Rotation2d
public Rotation2d(double x, double y) {
double magnitude = Math.hypot(x, y);
if (magnitude > 1e-6) {
m_sin = y / magnitude;
m_cos = x / magnitude;
m_sin = y / magnitude;
} else {
m_sin = 0.0;
m_cos = 1.0;
m_sin = 0.0;
MathSharedStore.reportError(
"x and y components of Rotation2d are zero\n", Thread.currentThread().getStackTrace());
}

11
wpimath/src/main/java/edu/wpi/first/math/geometry/Translation3d.java
View File

@@ -216,6 +216,17 @@ public class Translation3d
return new Translation3d(qprime.getX(), qprime.getY(), qprime.getZ());
}
/**
* Rotates this translation around another translation in 3D space.
*
* @param other The other translation to rotate around.
* @param rot The rotation to rotate the translation by.
* @return The new rotated translation.
*/
public Translation3d rotateAround(Translation3d other, Rotation3d rot) {
return this.minus(other).rotateBy(rot).plus(other);
}
/**
* Returns a Translation2d representing this Translation3d projected into the X-Y plane.
*

6
wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveWheelPositions.java
View File

@@ -11,11 +11,15 @@ import edu.wpi.first.math.interpolation.Interpolatable;
import edu.wpi.first.math.kinematics.proto.DifferentialDriveWheelPositionsProto;
import edu.wpi.first.math.kinematics.struct.DifferentialDriveWheelPositionsStruct;
import edu.wpi.first.units.measure.Distance;
import edu.wpi.first.util.protobuf.ProtobufSerializable;
import edu.wpi.first.util.struct.StructSerializable;
import java.util.Objects;
/** Represents the wheel positions for a differential drive drivetrain. */
public class DifferentialDriveWheelPositions
implements Interpolatable<DifferentialDriveWheelPositions> {
implements StructSerializable,
ProtobufSerializable,
Interpolatable<DifferentialDriveWheelPositions> {
/** Distance measured by the left side. */
public double leftMeters;

12
wpimath/src/main/java/edu/wpi/first/math/system/plant/LinearSystemId.java
View File

@@ -20,7 +20,7 @@ public final class LinearSystemId {
/**
* Create a state-space model of an elevator system. The states of the system are [position,
* velocity]ᵀ, inputs are [voltage], and outputs are [position].
* velocity]ᵀ, inputs are [voltage], and outputs are [position, velocity]ᵀ.
*
* @param motor The motor (or gearbox) attached to the carriage.
* @param massKg The mass of the elevator carriage, in kilograms.
@@ -88,8 +88,8 @@ public final class LinearSystemId {
/**
* Create a state-space model of a DC motor system. The states of the system are [angular
* position, angular velocity], inputs are [voltage], and outputs are [angular position, angular
* velocity].
* position, angular velocity]ᵀ, inputs are [voltage], and outputs are [angular position, angular
* velocity]ᵀ.
*
* @param motor The motor (or gearbox) attached to system.
* @param JKgMetersSquared The moment of inertia J of the DC motor.
@@ -125,7 +125,7 @@ public final class LinearSystemId {
/**
* Create a state-space model of a DC motor system from its kV (volts/(unit/sec)) and kA
* (volts/(unit/sec²)). These constants can be found using SysId. the states of the system are
* [position, velocity], inputs are [voltage], and outputs are [position].
* [position, velocity]ᵀ, inputs are [voltage], and outputs are [position].
*
* <p>The distance unit you choose MUST be an SI unit (i.e. meters or radians). You can use the
* {@link edu.wpi.first.math.util.Units} class for converting between unit types.
@@ -211,7 +211,7 @@ public final class LinearSystemId {
/**
* Create a state-space model of a single jointed arm system. The states of the system are [angle,
* angular velocity], inputs are [voltage], and outputs are [angle].
* angular velocity]ᵀ, inputs are [voltage], and outputs are [angle, angular velocity]ᵀ.
*
* @param motor The motor (or gearbox) attached to the arm.
* @param JKgSquaredMeters The moment of inertia J of the arm.
@@ -279,7 +279,7 @@ public final class LinearSystemId {
/**
* Create a state-space model for a 1 DOF position system from its kV (volts/(unit/sec)) and kA
* (volts/(unit/sec²). These constants cam be found using SysId. The states of the system are
* [position, velocity]ᵀ, inputs are [voltage], and outputs are [position].
* [position, velocity]ᵀ, inputs are [voltage], and outputs are [position, velocity]ᵀ.
*
* <p>The distance unit you choose MUST be an SI unit (i.e. meters or radians). You can use the
* {@link edu.wpi.first.math.util.Units} class for converting between unit types.

12
wpimath/src/main/native/include/frc/ct_matrix.h
View File

@@ -163,8 +163,8 @@ class ct_matrix {
if (std::is_constant_evaluated()) {
ct_matrix<Scalar, Rows, Cols> result;
for (int row = 0; row < 3; ++row) {
for (int col = 0; col < 3; ++col) {
for (int row = 0; row < rhs.rows(); ++row) {
for (int col = 0; col < rhs.cols(); ++col) {
result(row, col) = lhs(row, col) + rhs(row, col);
}
}
@@ -188,8 +188,8 @@ class ct_matrix {
if (std::is_constant_evaluated()) {
ct_matrix<Scalar, Rows, Cols> result;
for (int row = 0; row < 3; ++row) {
for (int col = 0; col < 3; ++col) {
for (int row = 0; row < rhs.rows(); ++row) {
for (int col = 0; col < rhs.cols(); ++col) {
result(row, col) = lhs(row, col) - rhs(row, col);
}
}
@@ -282,8 +282,8 @@ class ct_matrix {
if (std::is_constant_evaluated()) {
Scalar sum = 0.0;
for (int row = 0; row < Rows; ++row) {
for (int col = 0; col < Cols; ++col) {
for (int row = 0; row < rows(); ++row) {
for (int col = 0; col < cols(); ++col) {
sum += (*this)(row, col) * (*this)(row, col);
}
}

13
wpimath/src/main/native/include/frc/geometry/Pose2d.h
View File

@@ -184,6 +184,19 @@ class WPILIB_DLLEXPORT Pose2d {
*/
constexpr Pose2d RelativeTo(const Pose2d& other) const;
/**
* Rotates the current pose around a point in 2D space.
*
* @param point The point in 2D space to rotate around.
* @param rot The rotation to rotate the pose by.
*
* @return The new rotated pose.
*/
constexpr Pose2d RotateAround(const Translation2d& point,
const Rotation2d& rot) const {
return {m_translation.RotateAround(point, rot), m_rotation.RotateBy(rot)};
}
/**
* Obtain a new Pose2d from a (constant curvature) velocity.
*

13
wpimath/src/main/native/include/frc/geometry/Pose3d.h
View File

@@ -207,6 +207,19 @@ class WPILIB_DLLEXPORT Pose3d {
*/
constexpr Pose3d RelativeTo(const Pose3d& other) const;
/**
* Rotates the current pose around a point in 3D space.
*
* @param point The point in 3D space to rotate around.
* @param rot The rotation to rotate the pose by.
*
* @return The new rotated pose.
*/
constexpr Pose3d RotateAround(const Translation3d& point,
const Rotation3d& rot) const {
return {m_translation.RotateAround(point, rot), m_rotation.RotateBy(rot)};
}
/**
* Obtain a new Pose3d from a (constant curvature) velocity.
*

4
wpimath/src/main/native/include/frc/geometry/Rotation2d.h
View File

@@ -49,11 +49,11 @@ class WPILIB_DLLEXPORT Rotation2d {
constexpr Rotation2d(double x, double y) {
double magnitude = gcem::hypot(x, y);
if (magnitude > 1e-6) {
m_sin = y / magnitude;
m_cos = x / magnitude;
m_sin = y / magnitude;
} else {
m_sin = 0.0;
m_cos = 1.0;
m_sin = 0.0;
if (!std::is_constant_evaluated()) {
wpi::math::MathSharedStore::ReportError(
"x and y components of Rotation2d are zero\n{}",

12
wpimath/src/main/native/include/frc/geometry/Translation3d.h
View File

@@ -148,6 +148,18 @@ class WPILIB_DLLEXPORT Translation3d {
units::meter_t{qprime.Z()}};
}
/**
* Rotates this translation around another translation in 3D space.
*
* @param other The other translation to rotate around.
* @param rot The rotation to rotate the translation by.
* @return The new rotated translation.
*/
constexpr Translation3d RotateAround(const Translation3d& other,
const Rotation3d& rot) const {
return (*this - other).RotateBy(rot) + other;
}
/**
* Returns a Translation2d representing this Translation3d projected into the
* X-Y plane.

34
wpimath/src/main/native/include/frc/system/plant/LinearSystemId.h
View File

@@ -37,7 +37,8 @@ class WPILIB_DLLEXPORT LinearSystemId {
/**
* Create a state-space model of the elevator system. The states of the system
* are [position, velocity], inputs are [voltage], and outputs are [position].
* are [position, velocity]ᵀ, inputs are [voltage], and outputs are [position,
* velocity]ᵀ.
*
* @param motor The motor (or gearbox) attached to the carriage.
* @param mass The mass of the elevator carriage, in kilograms.
@@ -74,8 +75,8 @@ class WPILIB_DLLEXPORT LinearSystemId {
/**
* Create a state-space model of a single-jointed arm system.The states of the
* system are [angle, angular velocity], inputs are [voltage], and outputs are
* [angle].
* system are [angle, angular velocity]ᵀ, inputs are [voltage], and outputs
* are [angle, angular velocity]ᵀ.
*
* @param motor The motor (or gearbox) attached to the arm.
* @param J The moment of inertia J of the arm.
@@ -147,8 +148,8 @@ class WPILIB_DLLEXPORT LinearSystemId {
/**
* Create a state-space model for a 1 DOF position system from its kV
* (volts/(unit/sec)) and kA (volts/(unit/sec²)). These constants can be
* found using SysId. the states of the system are [position, velocity],
* inputs are [voltage], and outputs are [position].
* found using SysId. the states of the system are [position, velocity]ᵀ,
* inputs are [voltage], and outputs are [position, velocity]ᵀ.
*
* You MUST use an SI unit (i.e. meters or radians) for the Distance template
* argument. You may still use non-SI units (such as feet or inches) for the
@@ -169,7 +170,7 @@ class WPILIB_DLLEXPORT LinearSystemId {
template <typename Distance>
requires std::same_as<units::meter, Distance> ||
std::same_as<units::radian, Distance>
static constexpr LinearSystem<2, 1, 1> IdentifyPositionSystem(
static constexpr LinearSystem<2, 1, 2> IdentifyPositionSystem(
decltype(1_V / Velocity_t<Distance>(1)) kV,
decltype(1_V / Acceleration_t<Distance>(1)) kA) {
if (kV < decltype(kV){0}) {
@@ -180,11 +181,11 @@ class WPILIB_DLLEXPORT LinearSystemId {
}
Matrixd<2, 2> A{{0.0, 1.0}, {0.0, -kV.value() / kA.value()}};
Matrixd<2, 1> B{0.0, 1.0 / kA.value()};
Matrixd<1, 2> C{1.0, 0.0};
Matrixd<1, 1> D{0.0};
Matrixd<2, 1> B{{0.0}, {1.0 / kA.value()}};
Matrixd<2, 2> C{{1.0, 0.0}, {0.0, 1.0}};
Matrixd<2, 1> D{{0.0}, {0.0}};
return LinearSystem<2, 1, 1>(A, B, C, D);
return LinearSystem<2, 1, 2>(A, B, C, D);
}
/**
@@ -337,8 +338,8 @@ class WPILIB_DLLEXPORT LinearSystemId {
/**
* Create a state-space model of a DC motor system. The states of the system
* are [angular position, angular velocity], inputs are [voltage], and outputs
* are [angular position, angular velocity].
* are [angular position, angular velocity]ᵀ, inputs are [voltage], and
* outputs are [angular position, angular velocity]ᵀ.
*
* @param motor The motor (or gearbox) attached to the system.
* @param J the moment of inertia J of the DC motor.
@@ -370,8 +371,9 @@ class WPILIB_DLLEXPORT LinearSystemId {
/**
* Create a state-space model of a DC motor system from its kV
* (volts/(unit/sec)) and kA (volts/(unit/sec²)). These constants can be
* found using SysId. the states of the system are [position, velocity],
* inputs are [voltage], and outputs are [position].
* found using SysId. the states of the system are [angular position, angular
* velocity]ᵀ, inputs are [voltage], and outputs are [angular position,
* angular velocity]ᵀ.
*
* You MUST use an SI unit (i.e. meters or radians) for the Distance template
* argument. You may still use non-SI units (such as feet or inches) for the
@@ -410,9 +412,9 @@ class WPILIB_DLLEXPORT LinearSystemId {
/**
* Create a state-space model of differential drive drivetrain. In this model,
* the states are [left velocity, right velocity], the inputs are [left
* the states are [left velocity, right velocity]ᵀ, the inputs are [left
* voltage, right voltage], and the outputs are [left velocity, right
* velocity]
* velocity]ᵀ.
*
* @param motor The motor (or gearbox) driving the drivetrain.
* @param mass The mass of the robot in kilograms.

153
wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/AssignEvaluator.h vendored
View File

@@ -263,7 +263,7 @@ struct copy_using_evaluator_innervec_CompleteUnrolling {
DstAlignment = Kernel::AssignmentTraits::DstAlignment
};
EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE void run(Kernel& kernel) {
EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE constexpr void run(Kernel& kernel) {
kernel.template assignPacketByOuterInner<DstAlignment, SrcAlignment, PacketType>(outer, inner);
enum { NextIndex = Index + unpacket_traits<PacketType>::size };
copy_using_evaluator_innervec_CompleteUnrolling<Kernel, NextIndex, Stop>::run(kernel);
@@ -431,17 +431,25 @@ struct dense_assignment_loop<Kernel, LinearVectorizedTraversal, NoUnrolling> {
template <typename Kernel>
struct dense_assignment_loop<Kernel, LinearVectorizedTraversal, CompleteUnrolling> {
EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE EIGEN_CONSTEXPR void run(Kernel& kernel) {
typedef typename Kernel::DstEvaluatorType::XprType DstXprType;
typedef typename Kernel::PacketType PacketType;
if (internal::is_constant_evaluated()) {
for (Index outer = 0; outer < kernel.outerSize(); ++outer) {
for (Index inner = 0; inner < kernel.innerSize(); ++inner) {
kernel.assignCoeffByOuterInner(outer, inner);
}
}
} else {
typedef typename Kernel::DstEvaluatorType::XprType DstXprType;
typedef typename Kernel::PacketType PacketType;
enum {
size = DstXprType::SizeAtCompileTime,
packetSize = unpacket_traits<PacketType>::size,
alignedSize = (int(size) / packetSize) * packetSize
};
enum {
size = DstXprType::SizeAtCompileTime,
packetSize = unpacket_traits<PacketType>::size,
alignedSize = (int(size) / packetSize) * packetSize
};
copy_using_evaluator_linearvec_CompleteUnrolling<Kernel, 0, alignedSize>::run(kernel);
copy_using_evaluator_LinearTraversal_CompleteUnrolling<Kernel, alignedSize, size>::run(kernel);
copy_using_evaluator_linearvec_CompleteUnrolling<Kernel, 0, alignedSize>::run(kernel);
copy_using_evaluator_LinearTraversal_CompleteUnrolling<Kernel, alignedSize, size>::run(kernel);
}
}
};
@@ -465,9 +473,17 @@ struct dense_assignment_loop<Kernel, InnerVectorizedTraversal, NoUnrolling> {
template <typename Kernel>
struct dense_assignment_loop<Kernel, InnerVectorizedTraversal, CompleteUnrolling> {
EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE void run(Kernel& kernel) {
typedef typename Kernel::DstEvaluatorType::XprType DstXprType;
copy_using_evaluator_innervec_CompleteUnrolling<Kernel, 0, DstXprType::SizeAtCompileTime>::run(kernel);
EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE constexpr void run(Kernel& kernel) {
if (internal::is_constant_evaluated()) {
for (Index outer = 0; outer < kernel.outerSize(); ++outer) {
for (Index inner = 0; inner < kernel.innerSize(); ++inner) {
kernel.assignCoeffByOuterInner(outer, inner);
}
}
} else {
typedef typename Kernel::DstEvaluatorType::XprType DstXprType;
copy_using_evaluator_innervec_CompleteUnrolling<Kernel, 0, DstXprType::SizeAtCompileTime>::run(kernel);
}
}
};
@@ -498,8 +514,16 @@ struct dense_assignment_loop<Kernel, LinearTraversal, NoUnrolling> {
template <typename Kernel>
struct dense_assignment_loop<Kernel, LinearTraversal, CompleteUnrolling> {
EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE EIGEN_CONSTEXPR void run(Kernel& kernel) {
typedef typename Kernel::DstEvaluatorType::XprType DstXprType;
copy_using_evaluator_LinearTraversal_CompleteUnrolling<Kernel, 0, DstXprType::SizeAtCompileTime>::run(kernel);
if (internal::is_constant_evaluated()) {
for (Index outer = 0; outer < kernel.outerSize(); ++outer) {
for (Index inner = 0; inner < kernel.innerSize(); ++inner) {
kernel.assignCoeffByOuterInner(outer, inner);
}
}
} else {
typedef typename Kernel::DstEvaluatorType::XprType DstXprType;
copy_using_evaluator_LinearTraversal_CompleteUnrolling<Kernel, 0, DstXprType::SizeAtCompileTime>::run(kernel);
}
}
};
@@ -510,41 +534,49 @@ struct dense_assignment_loop<Kernel, LinearTraversal, CompleteUnrolling> {
template <typename Kernel>
struct dense_assignment_loop<Kernel, SliceVectorizedTraversal, NoUnrolling> {
EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE EIGEN_CONSTEXPR void run(Kernel& kernel) {
typedef typename Kernel::Scalar Scalar;
typedef typename Kernel::PacketType PacketType;
enum {
packetSize = unpacket_traits<PacketType>::size,
requestedAlignment = int(Kernel::AssignmentTraits::InnerRequiredAlignment),
alignable =
packet_traits<Scalar>::AlignedOnScalar || int(Kernel::AssignmentTraits::DstAlignment) >= sizeof(Scalar),
dstIsAligned = int(Kernel::AssignmentTraits::DstAlignment) >= int(requestedAlignment),
dstAlignment = alignable ? int(requestedAlignment) : int(Kernel::AssignmentTraits::DstAlignment)
};
const Scalar* dst_ptr = kernel.dstDataPtr();
if ((!bool(dstIsAligned)) && (std::uintptr_t(dst_ptr) % sizeof(Scalar)) > 0) {
// the pointer is not aligned-on scalar, so alignment is not possible
return dense_assignment_loop<Kernel, DefaultTraversal, NoUnrolling>::run(kernel);
}
const Index packetAlignedMask = packetSize - 1;
const Index innerSize = kernel.innerSize();
const Index outerSize = kernel.outerSize();
const Index alignedStep = alignable ? (packetSize - kernel.outerStride() % packetSize) & packetAlignedMask : 0;
Index alignedStart =
((!alignable) || bool(dstIsAligned)) ? 0 : internal::first_aligned<requestedAlignment>(dst_ptr, innerSize);
if (internal::is_constant_evaluated()) {
for (Index outer = 0; outer < kernel.outerSize(); ++outer) {
for (Index inner = 0; inner < kernel.innerSize(); ++inner) {
kernel.assignCoeffByOuterInner(outer, inner);
}
}
} else {
typedef typename Kernel::Scalar Scalar;
typedef typename Kernel::PacketType PacketType;
enum {
packetSize = unpacket_traits<PacketType>::size,
requestedAlignment = int(Kernel::AssignmentTraits::InnerRequiredAlignment),
alignable =
packet_traits<Scalar>::AlignedOnScalar || int(Kernel::AssignmentTraits::DstAlignment) >= sizeof(Scalar),
dstIsAligned = int(Kernel::AssignmentTraits::DstAlignment) >= int(requestedAlignment),
dstAlignment = alignable ? int(requestedAlignment) : int(Kernel::AssignmentTraits::DstAlignment)
};
const Scalar* dst_ptr = kernel.dstDataPtr();
if ((!bool(dstIsAligned)) && (std::uintptr_t(dst_ptr) % sizeof(Scalar)) > 0) {
// the pointer is not aligned-on scalar, so alignment is not possible
return dense_assignment_loop<Kernel, DefaultTraversal, NoUnrolling>::run(kernel);
}
const Index packetAlignedMask = packetSize - 1;
const Index innerSize = kernel.innerSize();
const Index outerSize = kernel.outerSize();
const Index alignedStep = alignable ? (packetSize - kernel.outerStride() % packetSize) & packetAlignedMask : 0;
Index alignedStart =
((!alignable) || bool(dstIsAligned)) ? 0 : internal::first_aligned<requestedAlignment>(dst_ptr, innerSize);
for (Index outer = 0; outer < outerSize; ++outer) {
const Index alignedEnd = alignedStart + ((innerSize - alignedStart) & ~packetAlignedMask);
// do the non-vectorizable part of the assignment
for (Index inner = 0; inner < alignedStart; ++inner) kernel.assignCoeffByOuterInner(outer, inner);
for (Index outer = 0; outer < outerSize; ++outer) {
const Index alignedEnd = alignedStart + ((innerSize - alignedStart) & ~packetAlignedMask);
// do the non-vectorizable part of the assignment
for (Index inner = 0; inner < alignedStart; ++inner) kernel.assignCoeffByOuterInner(outer, inner);
// do the vectorizable part of the assignment
for (Index inner = alignedStart; inner < alignedEnd; inner += packetSize)
kernel.template assignPacketByOuterInner<dstAlignment, Unaligned, PacketType>(outer, inner);
// do the vectorizable part of the assignment
for (Index inner = alignedStart; inner < alignedEnd; inner += packetSize)
kernel.template assignPacketByOuterInner<dstAlignment, Unaligned, PacketType>(outer, inner);
// do the non-vectorizable part of the assignment
for (Index inner = alignedEnd; inner < innerSize; ++inner) kernel.assignCoeffByOuterInner(outer, inner);
// do the non-vectorizable part of the assignment
for (Index inner = alignedEnd; inner < innerSize; ++inner) kernel.assignCoeffByOuterInner(outer, inner);
alignedStart = numext::mini((alignedStart + alignedStep) % packetSize, innerSize);
alignedStart = numext::mini((alignedStart + alignedStep) % packetSize, innerSize);
}
}
}
};
@@ -594,9 +626,9 @@ class generic_dense_assignment_kernel {
typedef copy_using_evaluator_traits<DstEvaluatorTypeT, SrcEvaluatorTypeT, Functor> AssignmentTraits;
typedef typename AssignmentTraits::PacketType PacketType;
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE generic_dense_assignment_kernel(DstEvaluatorType& dst,
const SrcEvaluatorType& src,
const Functor& func, DstXprType& dstExpr)
EIGEN_DEVICE_FUNC
EIGEN_STRONG_INLINE constexpr generic_dense_assignment_kernel(DstEvaluatorType& dst, const SrcEvaluatorType& src,
const Functor& func, DstXprType& dstExpr)
: m_dst(dst), m_src(src), m_functor(func), m_dstExpr(dstExpr) {
#ifdef EIGEN_DEBUG_ASSIGN
AssignmentTraits::debug();
@@ -614,7 +646,7 @@ class generic_dense_assignment_kernel {
EIGEN_DEVICE_FUNC const SrcEvaluatorType& srcEvaluator() const EIGEN_NOEXCEPT { return m_src; }
/// Assign src(row,col) to dst(row,col) through the assignment functor.
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void assignCoeff(Index row, Index col) {
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE constexpr void assignCoeff(Index row, Index col) {
m_functor.assignCoeff(m_dst.coeffRef(row, col), m_src.coeff(row, col));
}
@@ -624,7 +656,7 @@ class generic_dense_assignment_kernel {
}
/// \sa assignCoeff(Index,Index)
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void assignCoeffByOuterInner(Index outer, Index inner) {
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE constexpr void assignCoeffByOuterInner(Index outer, Index inner) {
Index row = rowIndexByOuterInner(outer, inner);
Index col = colIndexByOuterInner(outer, inner);
assignCoeff(row, col);
@@ -648,7 +680,7 @@ class generic_dense_assignment_kernel {
assignPacket<StoreMode, LoadMode, Packet>(row, col);
}
EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE Index rowIndexByOuterInner(Index outer, Index inner) {
EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE constexpr Index rowIndexByOuterInner(Index outer, Index inner) {
typedef typename DstEvaluatorType::ExpressionTraits Traits;
return int(Traits::RowsAtCompileTime) == 1 ? 0
: int(Traits::ColsAtCompileTime) == 1 ? inner
@@ -656,7 +688,7 @@ class generic_dense_assignment_kernel {
: inner;
}
EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE Index colIndexByOuterInner(Index outer, Index inner) {
EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE constexpr Index colIndexByOuterInner(Index outer, Index inner) {
typedef typename DstEvaluatorType::ExpressionTraits Traits;
return int(Traits::ColsAtCompileTime) == 1 ? 0
: int(Traits::RowsAtCompileTime) == 1 ? inner
@@ -708,8 +740,8 @@ EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void resize_if_allowed(DstXprType& dst, co
}
template <typename DstXprType, typename SrcXprType, typename T1, typename T2>
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void resize_if_allowed(DstXprType& dst, const SrcXprType& src,
const internal::assign_op<T1, T2>& /*func*/) {
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE constexpr void resize_if_allowed(DstXprType& dst, const SrcXprType& src,
const internal::assign_op<T1, T2>& /*func*/) {
Index dstRows = src.rows();
Index dstCols = src.cols();
if (((dst.rows() != dstRows) || (dst.cols() != dstCols))) dst.resize(dstRows, dstCols);
@@ -790,7 +822,7 @@ struct Assignment;
// not has to bother about these annoying details.
template <typename Dst, typename Src>
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void call_assignment(Dst& dst, const Src& src) {
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE constexpr void call_assignment(Dst& dst, const Src& src) {
call_assignment(dst, src, internal::assign_op<typename Dst::Scalar, typename Src::Scalar>());
}
template <typename Dst, typename Src>
@@ -807,7 +839,7 @@ EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR void call_assignment(
}
template <typename Dst, typename Src, typename Func>
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void call_assignment(
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE constexpr void call_assignment(
Dst& dst, const Src& src, const Func& func, std::enable_if_t<!evaluator_assume_aliasing<Src>::value, void*> = 0) {
call_assignment_no_alias(dst, src, func);
}
@@ -891,9 +923,12 @@ EIGEN_DEVICE_FUNC void check_for_aliasing(const Dst& dst, const Src& src);
// both partial specialization+SFINAE without ambiguous specialization
template <typename DstXprType, typename SrcXprType, typename Functor, typename Weak>
struct Assignment<DstXprType, SrcXprType, Functor, Dense2Dense, Weak> {
EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE void run(DstXprType& dst, const SrcXprType& src, const Functor& func) {
EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE constexpr void run(DstXprType& dst, const SrcXprType& src,
const Functor& func) {
#ifndef EIGEN_NO_DEBUG
internal::check_for_aliasing(dst, src);
if (!internal::is_constant_evaluated()) {
internal::check_for_aliasing(dst, src);
}
#endif
call_dense_assignment_loop(dst, src, func);

2
wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/EigenBase.h vendored
View File

@@ -50,7 +50,7 @@ struct EigenBase {
/** \returns a const reference to the derived object */
EIGEN_DEVICE_FUNC constexpr const Derived& derived() const { return *static_cast<const Derived*>(this); }
EIGEN_DEVICE_FUNC inline Derived& const_cast_derived() const {
EIGEN_DEVICE_FUNC inline constexpr Derived& const_cast_derived() const {
return *static_cast<Derived*>(const_cast<EigenBase*>(this));
}
EIGEN_DEVICE_FUNC inline const Derived& const_derived() const { return *static_cast<const Derived*>(this); }

2
wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/functors/AssignmentFunctors.h vendored
View File

@@ -23,7 +23,7 @@ namespace internal {
*/
template <typename DstScalar, typename SrcScalar>
struct assign_op {
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void assignCoeff(DstScalar& a, const SrcScalar& b) const { a = b; }
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE constexpr void assignCoeff(DstScalar& a, const SrcScalar& b) const { a = b; }
template <int Alignment, typename Packet>
EIGEN_STRONG_INLINE void assignPacket(DstScalar* a, const Packet& b) const {

13
wpimath/src/test/java/edu/wpi/first/math/geometry/Pose2dTest.java
View File

@@ -72,6 +72,19 @@ class Pose2dTest {
() -> assertEquals(0.0, finalRelativeToInitial.getRotation().getDegrees(), kEpsilon));
}
@Test
void testRotateAround() {
var initial = new Pose2d(5, 0, Rotation2d.kZero);
var point = Translation2d.kZero;
var rotated = initial.rotateAround(point, Rotation2d.kPi);
assertAll(
() -> assertEquals(-5.0, rotated.getX(), kEpsilon),
() -> assertEquals(0.0, rotated.getY(), kEpsilon),
() -> assertEquals(180.0, rotated.getRotation().getDegrees(), kEpsilon));
}
@Test
void testEquality() {
var one = new Pose2d(0.0, 5.0, Rotation2d.fromDegrees(43.0));

13
wpimath/src/test/java/edu/wpi/first/math/geometry/Pose3dTest.java
View File

@@ -136,6 +136,19 @@ class Pose3dTest {
() -> assertEquals(0.0, finalRelativeToInitial.getRotation().getZ(), kEpsilon));
}
@Test
void testRotateAround() {
var initial = new Pose3d(new Translation3d(5, 0, 0), Rotation3d.kZero);
var point = Translation3d.kZero;
var rotated = initial.rotateAround(point, new Rotation3d(0, 0, Math.PI));
assertAll(
() -> assertEquals(-5.0, rotated.getX(), kEpsilon),
() -> assertEquals(0.0, rotated.getY(), kEpsilon),
() -> assertEquals(Math.PI, rotated.getRotation().getZ(), kEpsilon));
}
@Test
void testEquality() {
var zAxis = VecBuilder.fill(0.0, 0.0, 1.0);

34
wpimath/src/test/java/edu/wpi/first/math/geometry/Translation3dTest.java
View File

@@ -78,6 +78,40 @@ class Translation3dTest {
() -> assertEquals(3.0, rotated3.getZ(), kEpsilon));
}
@Test
void testRotateAround() {
var xAxis = VecBuilder.fill(1.0, 0.0, 0.0);
var yAxis = VecBuilder.fill(0.0, 1.0, 0.0);
var zAxis = VecBuilder.fill(0.0, 0.0, 1.0);
var translation = new Translation3d(1.0, 2.0, 3.0);
var around = new Translation3d(3.0, 2.0, 1.0);
var rotated1 =
translation.rotateAround(around, new Rotation3d(xAxis, Units.degreesToRadians(90.0)));
assertAll(
() -> assertEquals(1.0, rotated1.getX(), kEpsilon),
() -> assertEquals(0.0, rotated1.getY(), kEpsilon),
() -> assertEquals(1.0, rotated1.getZ(), kEpsilon));
var rotated2 =
translation.rotateAround(around, new Rotation3d(yAxis, Units.degreesToRadians(90.0)));
assertAll(
() -> assertEquals(5.0, rotated2.getX(), kEpsilon),
() -> assertEquals(2.0, rotated2.getY(), kEpsilon),
() -> assertEquals(3.0, rotated2.getZ(), kEpsilon));
var rotated3 =
translation.rotateAround(around, new Rotation3d(zAxis, Units.degreesToRadians(90.0)));
assertAll(
() -> assertEquals(3.0, rotated3.getX(), kEpsilon),
() -> assertEquals(0.0, rotated3.getY(), kEpsilon),
() -> assertEquals(3.0, rotated3.getZ(), kEpsilon));
}
@Test
void testToTranslation2d() {
var translation = new Translation3d(1.0, 2.0, 3.0);

11
wpimath/src/test/native/cpp/geometry/Pose2dTest.cpp
View File

@@ -51,6 +51,17 @@ TEST(Pose2dTest, RelativeTo) {
EXPECT_NEAR(0.0, finalRelativeToInitial.Rotation().Degrees().value(), 1e-9);
}
TEST(Pose2dTest, RotateAround) {
const Pose2d initial{5_m, 0_m, 0_deg};
const Translation2d point{0_m, 0_m};
const auto rotated = initial.RotateAround(point, Rotation2d{180_deg});
EXPECT_NEAR(-5.0, rotated.X().value(), 1e-9);
EXPECT_NEAR(0.0, rotated.Y().value(), 1e-9);
EXPECT_NEAR(180.0, rotated.Rotation().Degrees().value(), 1e-9);
}
TEST(Pose2dTest, Equality) {
const Pose2d a{0_m, 5_m, 43_deg};
const Pose2d b{0_m, 5_m, 43_deg};

13
wpimath/src/test/native/cpp/geometry/Pose3dTest.cpp
View File

@@ -83,6 +83,19 @@ TEST(Pose3dTest, RelativeTo) {
EXPECT_NEAR(0.0, finalRelativeToInitial.Rotation().Z().value(), 1e-9);
}
TEST(Pose3dTest, RotateAround) {
const Pose3d initial{5_m, 0_m, 0_m, Rotation3d{}};
const Translation3d point{0_m, 0_m, 0_m};
const auto rotated =
initial.RotateAround(point, Rotation3d{0_deg, 0_deg, 180_deg});
EXPECT_NEAR(-5.0, rotated.X().value(), 1e-9);
EXPECT_NEAR(0.0, rotated.Y().value(), 1e-9);
EXPECT_NEAR(units::radian_t{180_deg}.value(), rotated.Rotation().Z().value(),
1e-9);
}
TEST(Pose3dTest, Equality) {
Eigen::Vector3d zAxis{0.0, 0.0, 1.0};

5
wpimath/src/test/native/cpp/geometry/Rotation2dTest.cpp
View File

@@ -79,8 +79,13 @@ TEST(Rotation2dTest, Inequality) {
}
TEST(Rotation2dTest, ToMatrix) {
#if __GNUC__ <= 11
Rotation2d before{20_deg};
Rotation2d after{before.ToMatrix()};
#else
constexpr Rotation2d before{20_deg};
constexpr Rotation2d after{before.ToMatrix()};
#endif
EXPECT_EQ(before, after);
}

5
wpimath/src/test/native/cpp/geometry/Rotation3dTest.cpp
View File

@@ -308,8 +308,13 @@ TEST(Rotation3dTest, Inequality) {
}
TEST(Rotation3dTest, ToMatrix) {
#if __GNUC__ <= 11
Rotation3d before{10_deg, 20_deg, 30_deg};
Rotation3d after{before.ToMatrix()};
#else
constexpr Rotation3d before{10_deg, 20_deg, 30_deg};
constexpr Rotation3d after{before.ToMatrix()};
#endif
EXPECT_EQ(before, after);
}

11
wpimath/src/test/native/cpp/geometry/Translation2dTest.cpp
View File

@@ -35,7 +35,16 @@ TEST(Translation2dTest, RotateBy) {
const auto rotated = another.RotateBy(90_deg);
EXPECT_NEAR(0.0, rotated.X().value(), 1e-9);
EXPECT_DOUBLE_EQ(3.0, rotated.Y().value());
EXPECT_NEAR(3.0, rotated.Y().value(), 1e-9);
}
TEST(Translation2dTest, RotateAround) {
const Translation2d translation{2_m, 1_m};
const Translation2d other{3_m, 2_m};
const auto rotated = translation.RotateAround(other, 180_deg);
EXPECT_NEAR(4.0, rotated.X().value(), 1e-9);
EXPECT_NEAR(3.0, rotated.Y().value(), 1e-9);
}
TEST(Translation2dTest, Multiplication) {

27
wpimath/src/test/native/cpp/geometry/Translation3dTest.cpp
View File

@@ -57,6 +57,33 @@ TEST(Translation3dTest, RotateBy) {
EXPECT_NEAR(rotated3.Z().value(), 3.0, kEpsilon);
}
TEST(Translation3dTest, RotateAround) {
Eigen::Vector3d xAxis{1.0, 0.0, 0.0};
Eigen::Vector3d yAxis{0.0, 1.0, 0.0};
Eigen::Vector3d zAxis{0.0, 0.0, 1.0};
const Translation3d translation{1_m, 2_m, 3_m};
const Translation3d around{3_m, 2_m, 1_m};
const auto rotated1 =
translation.RotateAround(around, Rotation3d{xAxis, 90_deg});
EXPECT_NEAR(rotated1.X().value(), 1.0, kEpsilon);
EXPECT_NEAR(rotated1.Y().value(), 0.0, kEpsilon);
EXPECT_NEAR(rotated1.Z().value(), 1.0, kEpsilon);
const auto rotated2 =
translation.RotateAround(around, Rotation3d{yAxis, 90_deg});
EXPECT_NEAR(rotated2.X().value(), 5.0, kEpsilon);
EXPECT_NEAR(rotated2.Y().value(), 2.0, kEpsilon);
EXPECT_NEAR(rotated2.Z().value(), 3.0, kEpsilon);
const auto rotated3 =
translation.RotateAround(around, Rotation3d{zAxis, 90_deg});
EXPECT_NEAR(rotated3.X().value(), 3.0, kEpsilon);
EXPECT_NEAR(rotated3.Y().value(), 0.0, kEpsilon);
EXPECT_NEAR(rotated3.Z().value(), 3.0, kEpsilon);
}
TEST(Translation3dTest, ToTranslation2d) {
Translation3d translation{1_m, 2_m, 3_m};
Translation2d expected{1_m, 2_m};

39
wpimath/src/test/native/cpp/system/LinearSystemIDTest.cpp
View File

@@ -12,8 +12,13 @@
#include "units/mass.h"
TEST(LinearSystemIDTest, IdentifyDrivetrainVelocitySystem) {
#if __GNUC__ <= 11
auto model = frc::LinearSystemId::DrivetrainVelocitySystem(
frc::DCMotor::NEO(4), 70_kg, 0.05_m, 0.4_m, 6.0_kg_sq_m, 6.0);
#else
constexpr auto model = frc::LinearSystemId::DrivetrainVelocitySystem(
frc::DCMotor::NEO(4), 70_kg, 0.05_m, 0.4_m, 6.0_kg_sq_m, 6.0);
#endif
ASSERT_TRUE(model.A().isApprox(
frc::Matrixd<2, 2>{{-10.14132, 3.06598}, {3.06598, -10.14132}}, 0.001));
@@ -37,8 +42,14 @@ TEST(LinearSystemIDTest, ElevatorSystem) {
}
TEST(LinearSystemIDTest, FlywheelSystem) {
#if __GNUC__ <= 11
auto model = frc::LinearSystemId::FlywheelSystem(frc::DCMotor::NEO(2),
0.00032_kg_sq_m, 1.0);
#else
constexpr auto model = frc::LinearSystemId::FlywheelSystem(
frc::DCMotor::NEO(2), 0.00032_kg_sq_m, 1.0);
#endif
ASSERT_TRUE(model.A().isApprox(frc::Matrixd<1, 1>{-26.87032}, 0.001));
ASSERT_TRUE(model.B().isApprox(frc::Matrixd<1, 1>{1354.166667}, 0.001));
ASSERT_TRUE(model.C().isApprox(frc::Matrixd<1, 1>{1.0}, 0.001));
@@ -46,8 +57,14 @@ TEST(LinearSystemIDTest, FlywheelSystem) {
}
TEST(LinearSystemIDTest, DCMotorSystem) {
#if __GNUC__ <= 11
auto model = frc::LinearSystemId::DCMotorSystem(frc::DCMotor::NEO(2),
0.00032_kg_sq_m, 1.0);
#else
constexpr auto model = frc::LinearSystemId::DCMotorSystem(
frc::DCMotor::NEO(2), 0.00032_kg_sq_m, 1.0);
#endif
ASSERT_TRUE(
model.A().isApprox(frc::Matrixd<2, 2>{{0, 1}, {0, -26.87032}}, 0.001));
ASSERT_TRUE(model.B().isApprox(frc::Matrixd<2, 1>{0, 1354.166667}, 0.001));
@@ -59,10 +76,17 @@ TEST(LinearSystemIDTest, DCMotorSystem) {
TEST(LinearSystemIDTest, IdentifyPositionSystem) {
// By controls engineering in frc,
// x-dot = [0 1 | 0 -kv/ka] x = [0 | 1/ka] u
double kv = 1.0;
double ka = 0.5;
constexpr double kv = 1.0;
constexpr double ka = 0.5;
#if __GNUC__ <= 11
auto model = frc::LinearSystemId::IdentifyPositionSystem<units::meter>(
kv * 1_V / 1_mps, ka * 1_V / 1_mps_sq);
#else
constexpr auto model =
frc::LinearSystemId::IdentifyPositionSystem<units::meter>(
kv * 1_V / 1_mps, ka * 1_V / 1_mps_sq);
#endif
ASSERT_TRUE(model.A().isApprox(
frc::Matrixd<2, 2>{{0.0, 1.0}, {0.0, -kv / ka}}, 0.001));
@@ -73,10 +97,17 @@ TEST(LinearSystemIDTest, IdentifyVelocitySystem) {
// By controls engineering in frc,
// V = kv * velocity + ka * acceleration
// x-dot = -kv/ka * v + 1/ka \cdot V
double kv = 1.0;
double ka = 0.5;
constexpr double kv = 1.0;
constexpr double ka = 0.5;
#if __GNUC__ <= 11
auto model = frc::LinearSystemId::IdentifyVelocitySystem<units::meter>(
kv * 1_V / 1_mps, ka * 1_V / 1_mps_sq);
#else
constexpr auto model =
frc::LinearSystemId::IdentifyVelocitySystem<units::meter>(
kv * 1_V / 1_mps, ka * 1_V / 1_mps_sq);
#endif
ASSERT_TRUE(model.A().isApprox(frc::Matrixd<1, 1>{-kv / ka}, 0.001));
ASSERT_TRUE(model.B().isApprox(frc::Matrixd<1, 1>{1.0 / ka}, 0.001));

42
wpiutil/src/main/java/edu/wpi/first/util/RawFrame.java
View File

@@ -18,6 +18,8 @@ public class RawFrame implements AutoCloseable {
private int m_height;
private int m_stride;
private PixelFormat m_pixelFormat = PixelFormat.kUnknown;
private long m_time;
private TimestampSource m_timeSource = TimestampSource.kUnknown;
/** Construct a new empty RawFrame. */
public RawFrame() {
@@ -43,12 +45,15 @@ public class RawFrame implements AutoCloseable {
* @param stride The number of bytes in each row of image data
* @param pixelFormat The PixelFormat of the frame
*/
void setDataJNI(ByteBuffer data, int width, int height, int stride, int pixelFormat) {
void setDataJNI(
ByteBuffer data, int width, int height, int stride, int pixelFormat, long time, int timeSrc) {
m_data = data;
m_width = width;
m_height = height;
m_stride = stride;
m_pixelFormat = PixelFormat.getFromInt(pixelFormat);
m_time = time;
m_timeSource = TimestampSource.getFromInt(timeSrc);
}
/**
@@ -59,11 +64,13 @@ public class RawFrame implements AutoCloseable {
* @param stride The number of bytes in each row of image data
* @param pixelFormat The PixelFormat of the frame
*/
void setInfoJNI(int width, int height, int stride, int pixelFormat) {
void setInfoJNI(int width, int height, int stride, int pixelFormat, long time, int timeSrc) {
m_width = width;
m_height = height;
m_stride = stride;
m_pixelFormat = PixelFormat.getFromInt(pixelFormat);
m_time = time;
m_timeSource = TimestampSource.getFromInt(timeSrc);
}
/**
@@ -110,6 +117,19 @@ public class RawFrame implements AutoCloseable {
pixelFormat.getValue());
}
/**
* Update this frame's timestamp info.
*
* @param frameTime the time this frame was grabbed at. This uses the same time base as
* wpi::Now(), in us.
* @param frameTimeSource the time source for the timestamp this frame was grabbed at.
*/
public void setTimeInfo(long frameTime, TimestampSource frameTimeSource) {
m_time = frameTime;
m_timeSource = frameTimeSource;
WPIUtilJNI.setRawFrameTime(m_nativeObj, frameTime, frameTimeSource.getValue());
}
/**
* Get the pointer to native representation of this frame.
*
@@ -185,4 +205,22 @@ public class RawFrame implements AutoCloseable {
public PixelFormat getPixelFormat() {
return m_pixelFormat;
}
/**
* Get the time this frame was grabbed at. This uses the same time base as wpi::Now(), in us.
*
* @return Time in 1 us increments.
*/
public long getTimestamp() {
return m_time;
}
/**
* Get the time source for the timestamp this frame was grabbed at.
*
* @return Time source
*/
public TimestampSource getTimestampSource() {
return m_timeSource;
}
}

51
wpiutil/src/main/java/edu/wpi/first/util/TimestampSource.java Normal file
View File

@@ -0,0 +1,51 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.util;
/**
* Options for where the timestamp an {@link RawFrame} was captured at can be measured relative to.
*/
public enum TimestampSource {
/** unknown. */
kUnknown(0),
/**
* wpi::Now when the new frame was dequeued by CSCore. Does not account for camera exposure time
* or V4L latency.
*/
kFrameDequeue(1),
/** End of Frame. Same as V4L2_BUF_FLAG_TSTAMP_SRC_EOF, translated into wpi::Now's timebase. */
kV4LEOF(2),
/**
* Start of Exposure. Same as V4L2_BUF_FLAG_TSTAMP_SRC_SOE, translated into wpi::Now's timebase.
*/
kV4LSOE(3);
private final int value;
TimestampSource(int value) {
this.value = value;
}
/**
* Gets the integer value of the pixel format.
*
* @return Integer value
*/
public int getValue() {
return value;
}
private static final TimestampSource[] s_values = values();
/**
* Gets a TimestampSource enum value from its integer value.
*
* @param timestampSource integer value
* @return Enum value
*/
public static TimestampSource getFromInt(int timestampSource) {
return s_values[timestampSource];
}
}

2
wpiutil/src/main/java/edu/wpi/first/util/WPIUtilJNI.java
View File

@@ -175,6 +175,8 @@ public class WPIUtilJNI {
static native void setRawFrameInfo(
long frame, int size, int width, int height, int stride, int pixelFormat);
static native void setRawFrameTime(long frame, long timestamp, int timeSource);
/**
* Waits for a handle to be signaled.
*

18
wpiutil/src/main/native/cpp/jni/WPIUtilJNI.cpp
View File

@@ -426,6 +426,24 @@ Java_edu_wpi_first_util_WPIUtilJNI_setRawFrameData
f->pixelFormat = pixelFormat;
}
/*
* Class: edu_wpi_first_util_WPIUtilJNI
* Method: setRawFrameTime
* Signature: (JJI)V
*/
JNIEXPORT void JNICALL
Java_edu_wpi_first_util_WPIUtilJNI_setRawFrameTime
(JNIEnv* env, jclass, jlong frame, jlong time, jint timeSource)
{
auto* f = reinterpret_cast<wpi::RawFrame*>(frame);
if (!f) {
wpi::ThrowNullPointerException(env, "frame is null");
return;
}
f->timestamp = time;
f->timestampSrc = timeSource;
}
/*
* Class: edu_wpi_first_util_WPIUtilJNI
* Method: setRawFrameInfo

47
wpiutil/src/main/native/include/wpi/RawFrame.h
View File

@@ -34,13 +34,15 @@ typedef struct WPI_RawFrame { // NOLINT
uint8_t* data;
// function to free image data (may be NULL)
void (*freeFunc)(void* cbdata, void* data, size_t capacity);
void* freeCbData; // data passed to freeFunc
size_t capacity; // data buffer capacity, in bytes
size_t size; // actual size of data, in bytes
int pixelFormat; // WPI_PixelFormat
int width; // width of image, in pixels
int height; // height of image, in pixels
int stride; // size of each row of data, in bytes (may be 0)
void* freeCbData; // data passed to freeFunc
size_t capacity; // data buffer capacity, in bytes
size_t size; // actual size of data, in bytes
int pixelFormat; // WPI_PixelFormat
int width; // width of image, in pixels
int height; // height of image, in pixels
int stride; // size of each row of data, in bytes (may be 0)
uint64_t timestamp; // image capture timestamp
int timestampSrc; // WPI_TimestampSource
} WPI_RawFrame;
/**
@@ -58,6 +60,21 @@ enum WPI_PixelFormat {
WPI_PIXFMT_BGRA, // BGRA 8-8-8-8-, 32 bpp
};
/**
* Timestamp metadata. Timebase is the same as wpi::Now
*/
enum WPI_TimestampSource {
WPI_TIMESRC_UNKNOWN = 0, // unknown
WPI_TIMESRC_FRAME_DEQUEUE, // wpi::Now when the new frame was dequeued by
// CSCore. Does not account for camera exposure
// time or V4L latency.
WPI_TIMESRC_V4L_EOF, // End of Frame. Same as V4L2_BUF_FLAG_TSTAMP_SRC_EOF,
// translated into wpi::Now's timebase.
WPI_TIMESRC_V4L_SOE, // Start of Exposure. Same as
// V4L2_BUF_FLAG_TSTAMP_SRC_SOE, translated into
// wpi::Now's timebase.
};
// Returns nonzero if the frame data was allocated/reallocated
int WPI_AllocateRawFrameData(WPI_RawFrame* frame, size_t requestedSize);
void WPI_FreeRawFrameData(WPI_RawFrame* frame);
@@ -82,6 +99,8 @@ struct RawFrame : public WPI_RawFrame {
pixelFormat = WPI_PIXFMT_UNKNOWN;
width = 0;
height = 0;
timestamp = 0;
timestampSrc = WPI_TIMESRC_UNKNOWN;
}
RawFrame(const RawFrame&) = delete;
RawFrame& operator=(const RawFrame&) = delete;
@@ -120,19 +139,23 @@ template <std::same_as<wpi::RawFrame> T>
void SetFrameData(JNIEnv* env, jclass rawFrameCls, jobject jframe,
const T& frame, bool newData) {
if (newData) {
static jmethodID setData = env->GetMethodID(rawFrameCls, "setDataJNI",
"(Ljava/nio/ByteBuffer;IIII)V");
static jmethodID setData = env->GetMethodID(
rawFrameCls, "setDataJNI", "(Ljava/nio/ByteBuffer;IIIIJI)V");
env->CallVoidMethod(
jframe, setData, env->NewDirectByteBuffer(frame.data, frame.size),
static_cast<jint>(frame.width), static_cast<jint>(frame.height),
static_cast<jint>(frame.stride), static_cast<jint>(frame.pixelFormat));
static_cast<jint>(frame.stride), static_cast<jint>(frame.pixelFormat),
static_cast<jlong>(frame.timestamp),
static_cast<jint>(frame.timestampSrc));
} else {
static jmethodID setInfo =
env->GetMethodID(rawFrameCls, "setInfoJNI", "(IIII)V");
env->GetMethodID(rawFrameCls, "setInfoJNI", "(IIIIJI)V");
env->CallVoidMethod(jframe, setInfo, static_cast<jint>(frame.width),
static_cast<jint>(frame.height),
static_cast<jint>(frame.stride),
static_cast<jint>(frame.pixelFormat));
static_cast<jint>(frame.pixelFormat),
static_cast<jlong>(frame.timestamp),
static_cast<jint>(frame.timestampSrc));
}
}
#endif

2
wpiutil/src/main/native/include/wpi/Synchronization.h
View File

@@ -407,7 +407,7 @@ class SignalObject final {
}
SignalObject& operator=(SignalObject&& rhs) {
if (m_handle != 0) {
DestroySemaphore(m_handle);
DestroySignalObject(m_handle);
}
m_handle = rhs.m_handle;
rhs.m_handle = 0;