diff --git a/.bazelrc b/.bazelrc index bb73151474..aad909f686 100644 --- a/.bazelrc +++ b/.bazelrc @@ -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 diff --git a/.github/workflows/fix_compile_commands.py b/.github/workflows/fix_compile_commands.py index 8a52830dea..82df7a8472 100755 --- a/.github/workflows/fix_compile_commands.py +++ b/.github/workflows/fix_compile_commands.py @@ -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) diff --git a/LICENSE.md b/LICENSE.md index 645e54253a..d744196fe9 100644 --- a/LICENSE.md +++ b/LICENSE.md @@ -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 diff --git a/ThirdPartyNotices.txt b/ThirdPartyNotices.txt index 5be015e7c7..01e52c7fd3 100644 --- a/ThirdPartyNotices.txt +++ b/ThirdPartyNotices.txt @@ -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 - -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 + +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 + +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 + +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 diff --git a/WORKSPACE b/WORKSPACE index 0d7786c2a2..eb731acfd2 100644 --- a/WORKSPACE +++ b/WORKSPACE @@ -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") diff --git a/apriltag/convert_apriltag_layouts.py b/apriltag/convert_apriltag_layouts.py index 75d1f143cb..73b8aa7ed1 100755 --- a/apriltag/convert_apriltag_layouts.py +++ b/apriltag/convert_apriltag_layouts.py @@ -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( diff --git a/apriltag/src/main/java/edu/wpi/first/apriltag/AprilTagFields.java b/apriltag/src/main/java/edu/wpi/first/apriltag/AprilTagFields.java index 016c982fe7..63a516aa38 100644 --- a/apriltag/src/main/java/edu/wpi/first/apriltag/AprilTagFields.java +++ b/apriltag/src/main/java/edu/wpi/first/apriltag/AprilTagFields.java @@ -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; diff --git a/apriltag/src/main/native/cpp/AprilTagFieldLayout.cpp b/apriltag/src/main/native/cpp/AprilTagFieldLayout.cpp index c90fad3626..04d7a78c60 100644 --- a/apriltag/src/main/native/cpp/AprilTagFieldLayout.cpp +++ b/apriltag/src/main/native/cpp/AprilTagFieldLayout.cpp @@ -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"); } diff --git a/apriltag/src/main/native/include/frc/apriltag/AprilTagFields.h b/apriltag/src/main/native/include/frc/apriltag/AprilTagFields.h index 6ef0930e9b..94ac799c85 100644 --- a/apriltag/src/main/native/include/frc/apriltag/AprilTagFields.h +++ b/apriltag/src/main/native/include/frc/apriltag/AprilTagFields.h @@ -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 diff --git a/apriltag/src/main/native/resources/edu/wpi/first/apriltag/2024-crescendo.csv b/apriltag/src/main/native/resources/edu/wpi/first/apriltag/2024-crescendo.csv deleted file mode 100644 index cf570ce1b5..0000000000 --- a/apriltag/src/main/native/resources/edu/wpi/first/apriltag/2024-crescendo.csv +++ /dev/null @@ -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 diff --git a/apriltag/src/main/native/resources/edu/wpi/first/apriltag/2025-reefscape.csv b/apriltag/src/main/native/resources/edu/wpi/first/apriltag/2025-reefscape.csv new file mode 100644 index 0000000000..b8fd985408 --- /dev/null +++ b/apriltag/src/main/native/resources/edu/wpi/first/apriltag/2025-reefscape.csv @@ -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 diff --git a/apriltag/src/main/native/resources/edu/wpi/first/apriltag/2025-reefscape.json b/apriltag/src/main/native/resources/edu/wpi/first/apriltag/2025-reefscape.json new file mode 100644 index 0000000000..eb395c0b67 --- /dev/null +++ b/apriltag/src/main/native/resources/edu/wpi/first/apriltag/2025-reefscape.json @@ -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 + } +} diff --git a/cscore/src/main/java/edu/wpi/first/cscore/CvSink.java b/cscore/src/main/java/edu/wpi/first/cscore/CvSink.java index 0b5bd3239d..e75062878a 100644 --- a/cscore/src/main/java/edu/wpi/first/cscore/CvSink.java +++ b/cscore/src/main/java/edu/wpi/first/cscore/CvSink.java @@ -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(); + } } diff --git a/cscore/src/main/native/cpp/Frame.cpp b/cscore/src/main/native/cpp/Frame.cpp index 759086f4e0..6599e029b9 100644 --- a/cscore/src/main/native/cpp/Frame.cpp +++ b/cscore/src/main/native/cpp/Frame.cpp @@ -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, Time time) +Frame::Frame(SourceImpl& source, std::unique_ptr 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()); } diff --git a/cscore/src/main/native/cpp/Frame.h b/cscore/src/main/native/cpp/Frame.h index d835de9efc..f44a6e11f5 100644 --- a/cscore/src/main/native/cpp/Frame.h +++ b/cscore/src/main/native/cpp/Frame.h @@ -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 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, Time time); + Frame(SourceImpl& source, std::unique_ptr 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) { diff --git a/cscore/src/main/native/cpp/RawSinkImpl.cpp b/cscore/src/main/native/cpp/RawSinkImpl.cpp index 355b0913e4..d73155bcef 100644 --- a/cscore/src/main/native/cpp/RawSinkImpl.cpp +++ b/cscore/src/main/native/cpp/RawSinkImpl.cpp @@ -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(); } diff --git a/cscore/src/main/native/cpp/SourceImpl.cpp b/cscore/src/main/native/cpp/SourceImpl.cpp index 067ba15014..fc9eb24cb4 100644 --- a/cscore/src/main/native/cpp/SourceImpl.cpp +++ b/cscore/src/main/native/cpp/SourceImpl.cpp @@ -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(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 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, Frame::Time time) { +void SourceImpl::PutFrame(std::unique_ptr image, Frame::Time time, + WPI_TimestampSource timeSrc) { // Update telemetry m_telemetry.RecordSourceFrames(*this, 1); m_telemetry.RecordSourceBytes(*this, static_cast(image->size())); @@ -491,7 +494,7 @@ void SourceImpl::PutFrame(std::unique_ptr 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 diff --git a/cscore/src/main/native/cpp/SourceImpl.h b/cscore/src/main/native/cpp/SourceImpl.h index f023e81f2d..dbaa62762a 100644 --- a/cscore/src/main/native/cpp/SourceImpl.h +++ b/cscore/src/main/native/cpp/SourceImpl.h @@ -13,6 +13,7 @@ #include #include +#include #include #include #include @@ -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, Frame::Time time); + std::string_view data, Frame::Time time, + WPI_TimestampSource timeSrc = WPI_TIMESRC_FRAME_DEQUEUE); + void PutFrame(std::unique_ptr 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 diff --git a/cscore/src/main/native/include/cscore_cv.h b/cscore/src/main/native/include/cscore_cv.h index d703582c06..e44e7ecb4f 100644 --- a/cscore/src/main/native/include/cscore_cv.h +++ b/cscore/src/main/native/include/cscore_cv.h @@ -8,6 +8,7 @@ #include #include +#include #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(rawFrame.timestampSrc); +} + } // namespace cs #endif // CSCORE_CSCORE_CV_H_ diff --git a/cscore/src/main/native/linux/UsbCameraImpl.cpp b/cscore/src/main/native/linux/UsbCameraImpl.cpp index e46bb1402f..9c6548435b 100644 --- a/cscore/src/main/native/linux/UsbCameraImpl.cpp +++ b/cscore/src/main/native/linux/UsbCameraImpl.cpp @@ -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(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(m_mode.pixelFormat), - width, height, image, wpi::Now()); // TODO: time + width, height, image, frameTime, timeSource); } } diff --git a/epilogue-runtime/src/main/java/edu/wpi/first/epilogue/logging/EpilogueBackend.java b/epilogue-runtime/src/main/java/edu/wpi/first/epilogue/logging/EpilogueBackend.java index 6759460fad..f006a319dc 100644 --- a/epilogue-runtime/src/main/java/edu/wpi/first/epilogue/logging/EpilogueBackend.java +++ b/epilogue-runtime/src/main/java/edu/wpi/first/epilogue/logging/EpilogueBackend.java @@ -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 the dimension of the unit */ - default void log(String identifier, Measure 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 the dimension of the unit */ default void log(String identifier, Measure value, U unit) { - log(identifier + " (" + unit.symbol() + ")", value.in(unit)); + log(identifier, value.in(unit)); } /** diff --git a/epilogue-runtime/src/main/java/edu/wpi/first/epilogue/logging/LazyBackend.java b/epilogue-runtime/src/main/java/edu/wpi/first/epilogue/logging/LazyBackend.java index 7a04cffbde..adad963e07 100644 --- a/epilogue-runtime/src/main/java/edu/wpi/first/epilogue/logging/LazyBackend.java +++ b/epilogue-runtime/src/main/java/edu/wpi/first/epilogue/logging/LazyBackend.java @@ -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); } } diff --git a/epilogue-runtime/src/test/java/edu/wpi/first/epilogue/logging/CustomStruct.java b/epilogue-runtime/src/test/java/edu/wpi/first/epilogue/logging/CustomStruct.java new file mode 100644 index 0000000000..d79ae2bc16 --- /dev/null +++ b/epilogue-runtime/src/test/java/edu/wpi/first/epilogue/logging/CustomStruct.java @@ -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 { + @Override + public Class 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); + } + } +} diff --git a/epilogue-runtime/src/test/java/edu/wpi/first/epilogue/logging/LazyBackendTest.java b/epilogue-runtime/src/test/java/edu/wpi/first/epilogue/logging/LazyBackendTest.java index 104cb0f380..d0b394330c 100644 --- a/epilogue-runtime/src/test/java/edu/wpi/first/epilogue/logging/LazyBackendTest.java +++ b/epilogue-runtime/src/test/java/edu/wpi/first/epilogue/logging/LazyBackendTest.java @@ -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()); + } } diff --git a/epilogue-runtime/src/test/java/edu/wpi/first/epilogue/logging/TestBackend.java b/epilogue-runtime/src/test/java/edu/wpi/first/epilogue/logging/TestBackend.java index e2b88a1066..1372921002 100644 --- a/epilogue-runtime/src/test/java/edu/wpi/first/epilogue/logging/TestBackend.java +++ b/epilogue-runtime/src/test/java/edu/wpi/first/epilogue/logging/TestBackend.java @@ -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 void log(String identifier, S value, Struct 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 void log(String identifier, S[] value, Struct 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)); } diff --git a/fieldImages/src/main/java/edu/wpi/first/fields/Fields.java b/fieldImages/src/main/java/edu/wpi/first/fields/Fields.java index b282bffd54..942af5f2fb 100644 --- a/fieldImages/src/main/java/edu/wpi/first/fields/Fields.java +++ b/fieldImages/src/main/java/edu/wpi/first/fields/Fields.java @@ -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; diff --git a/fieldImages/src/main/native/cpp/fields.cpp b/fieldImages/src/main/native/cpp/fields.cpp index 6fd6af0936..06e5f8ed40 100644 --- a/fieldImages/src/main/native/cpp/fields.cpp +++ b/fieldImages/src/main/native/cpp/fields.cpp @@ -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, diff --git a/fieldImages/src/main/native/include/fields/2025-reefscape.h b/fieldImages/src/main/native/include/fields/2025-reefscape.h new file mode 100644 index 0000000000..ada49b88a6 --- /dev/null +++ b/fieldImages/src/main/native/include/fields/2025-reefscape.h @@ -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 + +namespace fields { +std::string_view GetResource_2025_reefscape_json(); +std::string_view GetResource_2025_field_png(); +} // namespace fields diff --git a/fieldImages/src/main/native/resources/edu/wpi/first/fields/2025-field.png b/fieldImages/src/main/native/resources/edu/wpi/first/fields/2025-field.png new file mode 100644 index 0000000000..43b79febe7 Binary files /dev/null and b/fieldImages/src/main/native/resources/edu/wpi/first/fields/2025-field.png differ diff --git a/fieldImages/src/main/native/resources/edu/wpi/first/fields/2025-reefscape.json b/fieldImages/src/main/native/resources/edu/wpi/first/fields/2025-reefscape.json new file mode 100644 index 0000000000..c84464788c --- /dev/null +++ b/fieldImages/src/main/native/resources/edu/wpi/first/fields/2025-reefscape.json @@ -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" +} diff --git a/glass/src/lib/native/cpp/Window.cpp b/glass/src/lib/native/cpp/Window.cpp index 64b043ffb1..8a69ed8b6e 100644 --- a/glass/src/lib/native/cpp/Window.cpp +++ b/glass/src/lib/native/cpp/Window.cpp @@ -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()) { diff --git a/glass/src/lib/native/cpp/hardware/LEDDisplay.cpp b/glass/src/lib/native/cpp/hardware/LEDDisplay.cpp index 6ad7fc54a7..8e02be30a4 100644 --- a/glass/src/lib/native/cpp/hardware/LEDDisplay.cpp +++ b/glass/src/lib/native/cpp/hardware/LEDDisplay.cpp @@ -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); diff --git a/glass/src/lib/native/cpp/other/Field2D.cpp b/glass/src/lib/native/cpp/other/Field2D.cpp index f8ce7155b6..609a75b182 100644 --- a/glass/src/lib/native/cpp/other/Field2D.cpp +++ b/glass/src/lib/native/cpp/other/Field2D.cpp @@ -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( - "Choose field image", "", - std::vector{"Image File", + "Choose field JSON/image", "", + std::vector{"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; diff --git a/hal/src/generate/ResourceType.txt b/hal/src/generate/ResourceType.txt index aafdeb0bf0..a275ff7aff 100644 --- a/hal/src/generate/ResourceType.txt +++ b/hal/src/generate/ResourceType.txt @@ -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 diff --git a/hal/src/generated/main/java/edu/wpi/first/hal/FRCNetComm.java b/hal/src/generated/main/java/edu/wpi/first/hal/FRCNetComm.java index 9c73848199..30ba993033 100644 --- a/hal/src/generated/main/java/edu/wpi/first/hal/FRCNetComm.java +++ b/hal/src/generated/main/java/edu/wpi/first/hal/FRCNetComm.java @@ -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; } /** diff --git a/hal/src/generated/main/native/include/hal/FRCUsageReporting.h b/hal/src/generated/main/native/include/hal/FRCUsageReporting.h index 60267b6cac..65299cf91e 100644 --- a/hal/src/generated/main/native/include/hal/FRCUsageReporting.h +++ b/hal/src/generated/main/native/include/hal/FRCUsageReporting.h @@ -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, diff --git a/hal/src/generated/main/native/include/hal/UsageReporting.h b/hal/src/generated/main/native/include/hal/UsageReporting.h index 0f1271b7bf..e2fc6900d0 100644 --- a/hal/src/generated/main/native/include/hal/UsageReporting.h +++ b/hal/src/generated/main/native/include/hal/UsageReporting.h @@ -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; diff --git a/hal/src/main/java/edu/wpi/first/hal/AddressableLEDJNI.java b/hal/src/main/java/edu/wpi/first/hal/AddressableLEDJNI.java index c9a2185bf0..4d4c1975ad 100644 --- a/hal/src/main/java/edu/wpi/first/hal/AddressableLEDJNI.java +++ b/hal/src/main/java/edu/wpi/first/hal/AddressableLEDJNI.java @@ -53,7 +53,8 @@ public class AddressableLEDJNI extends JNIWrapper { /** * Sets the bit timing. * - *

By default, the driver is set up to drive WS2812Bs, so nothing needs to be set for those. + *

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. * - *

The sync time is the time to hold output so LEDs enable. Default set for WS2812B. + *

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) diff --git a/hal/src/main/native/include/hal/AddressableLED.h b/hal/src/main/native/include/hal/AddressableLED.h index 1fba59ce81..3181e7e2bb 100644 --- a/hal/src/main/native/include/hal/AddressableLED.h +++ b/hal/src/main/native/include/hal/AddressableLED.h @@ -77,8 +77,8 @@ void HAL_WriteAddressableLEDData(HAL_AddressableLEDHandle handle, /** * Sets the bit timing. * - *

By default, the driver is set up to drive WS2812Bs, so nothing needs to - * be set for those. + *

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. * *

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) diff --git a/shared/bazel/compiler_flags/systemcore_flags.rc b/shared/bazel/compiler_flags/systemcore_flags.rc new file mode 100644 index 0000000000..f50d4c203c --- /dev/null +++ b/shared/bazel/compiler_flags/systemcore_flags.rc @@ -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 diff --git a/simulation/halsim_gui/src/main/native/cpp/DriverStationGui.cpp b/simulation/halsim_gui/src/main/native/cpp/DriverStationGui.cpp index cdc46f1a8a..aca5ce2feb 100644 --- a/simulation/halsim_gui/src/main/native/cpp/DriverStationGui.cpp +++ b/simulation/halsim_gui/src/main/native/cpp/DriverStationGui.cpp @@ -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); diff --git a/upstream_utils/eigen_patches/0001-Disable-warnings.patch b/upstream_utils/eigen_patches/0001-Disable-warnings.patch index 0ff38396e5..eceda4a347 100644 --- a/upstream_utils/eigen_patches/0001-Disable-warnings.patch +++ b/upstream_utils/eigen_patches/0001-Disable-warnings.patch @@ -1,7 +1,7 @@ From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001 From: Tyler Veness 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 ++++++ diff --git a/upstream_utils/eigen_patches/0002-Intellisense-fix.patch b/upstream_utils/eigen_patches/0002-Intellisense-fix.patch index 6c369f26fd..73a956d5ca 100644 --- a/upstream_utils/eigen_patches/0002-Intellisense-fix.patch +++ b/upstream_utils/eigen_patches/0002-Intellisense-fix.patch @@ -1,7 +1,7 @@ From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001 From: Peter Johnson 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 +++++++ diff --git a/upstream_utils/eigen_patches/0003-Make-assignment-constexpr.patch b/upstream_utils/eigen_patches/0003-Make-assignment-constexpr.patch new file mode 100644 index 0000000000..a5c7dc83ff --- /dev/null +++ b/upstream_utils/eigen_patches/0003-Make-assignment-constexpr.patch @@ -0,0 +1,305 @@ +From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001 +From: Tyler Veness +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(outer, inner); + enum { NextIndex = Index + unpacket_traits::size }; + copy_using_evaluator_innervec_CompleteUnrolling::run(kernel); +@@ -431,17 +431,25 @@ struct dense_assignment_loop { + template + struct dense_assignment_loop { + 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::size, +- alignedSize = (int(size) / packetSize) * packetSize +- }; +- +- copy_using_evaluator_linearvec_CompleteUnrolling::run(kernel); +- copy_using_evaluator_LinearTraversal_CompleteUnrolling::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::size, ++ alignedSize = (int(size) / packetSize) * packetSize ++ }; ++ ++ copy_using_evaluator_linearvec_CompleteUnrolling::run(kernel); ++ copy_using_evaluator_LinearTraversal_CompleteUnrolling::run(kernel); ++ } + } + }; + +@@ -465,9 +473,17 @@ struct dense_assignment_loop { + + template + struct dense_assignment_loop { +- EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE void run(Kernel& kernel) { +- typedef typename Kernel::DstEvaluatorType::XprType DstXprType; +- copy_using_evaluator_innervec_CompleteUnrolling::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::run(kernel); ++ } + } + }; + +@@ -498,8 +514,16 @@ struct dense_assignment_loop { + template + struct dense_assignment_loop { + EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE EIGEN_CONSTEXPR void run(Kernel& kernel) { +- typedef typename Kernel::DstEvaluatorType::XprType DstXprType; +- copy_using_evaluator_LinearTraversal_CompleteUnrolling::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::run(kernel); ++ } + } + }; + +@@ -510,41 +534,49 @@ struct dense_assignment_loop { + template + struct dense_assignment_loop { + 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::size, +- requestedAlignment = int(Kernel::AssignmentTraits::InnerRequiredAlignment), +- alignable = +- packet_traits::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::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(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(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::size, ++ requestedAlignment = int(Kernel::AssignmentTraits::InnerRequiredAlignment), ++ alignable = ++ packet_traits::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::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(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(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 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(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 +-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void resize_if_allowed(DstXprType& dst, const SrcXprType& src, +- const internal::assign_op& /*func*/) { ++EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE constexpr void resize_if_allowed(DstXprType& dst, const SrcXprType& src, ++ const internal::assign_op& /*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 +-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()); + } + template +@@ -807,7 +839,7 @@ EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR void call_assignment( + } + + template +-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::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 + struct Assignment { +- 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(this); } + +- EIGEN_DEVICE_FUNC inline Derived& const_cast_derived() const { ++ EIGEN_DEVICE_FUNC inline constexpr Derived& const_cast_derived() const { + return *static_cast(const_cast(this)); + } + EIGEN_DEVICE_FUNC inline const Derived& const_derived() const { return *static_cast(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 + 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 + EIGEN_STRONG_INLINE void assignPacket(DstScalar* a, const Packet& b) const { diff --git a/wpical/src/main/native/cpp/WPIcal.cpp b/wpical/src/main/native/cpp/WPIcal.cpp index 75066b5124..f73105a50d 100644 --- a/wpical/src/main/native/cpp/WPIcal.cpp +++ b/wpical/src/main/native/cpp/WPIcal.cpp @@ -9,12 +9,14 @@ #include #include +#include #include #include #include #include #include +#include #include #include #include @@ -58,6 +60,112 @@ void drawCheck() { ImGui::NewLine(); } +void processFileSelector(std::unique_ptr& 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& selector, + std::vector& selected_files) { + if (selector && selector->ready(0)) { + auto selectedFiles = selector->result(); + if (!selectedFiles.empty()) { + selected_files = selectedFiles; + } + selector.reset(); + } +} + +void processDirectorySelector(std::unique_ptr& 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& selector, + const std::string& file_type, + const std::string& file_extensions) { + if (ImGui::Button(text)) { + selector = std::make_unique( + "Select File", "", std::vector{file_type, file_extensions}, + pfd::opt::none); + } +} + +void openFilesButton(const char* text, std::vector& selected_files, + std::unique_ptr& selector, + const std::string& file_type, + const std::string& file_extensions) { + if (ImGui::Button(text)) { + selector = std::make_unique( + "Select File", "", std::vector{file_type, file_extensions}, + pfd::opt::multiselect); + } +} + +void openDirectoryButton(const char* text, + std::unique_ptr& selector, + std::string& selected_directory) { + if (ImGui::Button(text)) { + selector = std::make_unique("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: ", 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 camera_intrinsics_selector; - static std::string selected_camera_intrinsics; - static std::unique_ptr field_map_selector; - static std::string selected_field_map; + static std::unique_ptr output_calibration_json_selector; + static std::unique_ptr combination_calibrations_selector; static std::unique_ptr field_calibration_directory_selector; - static std::string selected_field_calibration_directory; - static std::unique_ptr 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 selected_combination_calibrations; + + static std::map combiner_map; + static int current_combiner_tag_id = 0; + + static bool isCalibrating = false; cameracalibration::CameraModel cameraModel = { .intrinsic_matrix = Eigen::Matrix::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( - "Select Camera Intrinsics JSON", "", - std::vector{"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( - "Select Json File", "", - std::vector{"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( - "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("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( - "Select Camera Calibration Video", "", - std::vector{"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( - "Select Camera Calibration Video", "", - std::vector{"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( - "Select Json File", "", - std::vector{"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( - "Select Json File", "", - std::vector{"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", ¤t_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("Select Download Folder", ""); + } + + processDirectorySelector(download_directory_selector, + selected_download_directory); + saveCalibration(field_combination_json, selected_download_directory, + "combined_calibration", isCalibrating); + + ImGui::EndPopup(); + } + ImGui::End(); } diff --git a/wpical/src/main/native/cpp/fieldcalibration.cpp b/wpical/src/main/native/cpp/fieldcalibration.cpp index 1f3ff6970a..dec885ecf3 100644 --- a/wpical/src/main/native/cpp/fieldcalibration.cpp +++ b/wpical/src/main/native/cpp/fieldcalibration.cpp @@ -23,7 +23,6 @@ #include #include #include -#include #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(json.at("field").at("length"))}, {"width", static_cast(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; } diff --git a/wpical/src/main/native/cpp/tagpose.cpp b/wpical/src/main/native/cpp/tagpose.cpp index 761bf89573..38977f0961 100644 --- a/wpical/src/main/native/cpp/tagpose.cpp +++ b/wpical/src/main/native/cpp/tagpose.cpp @@ -5,8 +5,10 @@ #include 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 diff --git a/wpical/src/main/native/include/fieldcalibration.h b/wpical/src/main/native/include/fieldcalibration.h index d9de45990b..ac8475666d 100644 --- a/wpical/src/main/native/include/fieldcalibration.h +++ b/wpical/src/main/native/include/fieldcalibration.h @@ -6,10 +6,12 @@ #include +#include + #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 diff --git a/wpical/src/main/native/include/fieldmap.h b/wpical/src/main/native/include/fieldmap.h index 22fefc28a4..5e581c7322 100644 --- a/wpical/src/main/native/include/fieldmap.h +++ b/wpical/src/main/native/include/fieldmap.h @@ -5,7 +5,7 @@ #pragma once #include -#include +#include #include #include @@ -19,6 +19,7 @@ class Fieldmap { double field_width_meters = static_cast(json.at("field").at("width")); for (const auto& tag : json.at("tags").items()) { + double tag_id = static_cast(tag.value().at("ID")); double tagXPos = static_cast(tag.value().at("pose").at("translation").at("x")); double tagYPos = @@ -34,15 +35,30 @@ class Fieldmap { double tagZQuat = static_cast( 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 tagVec; + double fieldLength; + double fieldWidth; + std::map tagMap; }; diff --git a/wpical/src/main/native/include/tagpose.h b/wpical/src/main/native/include/tagpose.h index 422fc76a93..aeb06a426c 100644 --- a/wpical/src/main/native/include/tagpose.h +++ b/wpical/src/main/native/include/tagpose.h @@ -6,15 +6,19 @@ #include #include +#include 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 diff --git a/wpical/src/test/native/cpp/test_calibrate.cpp b/wpical/src/test/native/cpp/test_calibrate.cpp index c185face6f..0e48bcf018 100644 --- a/wpical/src/test/native/cpp/test_calibrate.cpp +++ b/wpical/src/test/native/cpp/test_calibrate.cpp @@ -19,6 +19,9 @@ cameracalibration::CameraModel cameraModel = { .intrinsic_matrix = Eigen::Matrix::Identity(), .distortion_coefficients = Eigen::Matrix::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); +} diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandScheduler.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandScheduler.java index 0e80a74c61..15babf85bb 100644 --- a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandScheduler.java +++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandScheduler.java @@ -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. * + *

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. * + *

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) { diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/Commands.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/Commands.java index c53409a033..8ba81cfb16 100644 --- a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/Commands.java +++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/Commands.java @@ -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 supplier) { - return new ProxyCommand(supplier); + return defer(() -> supplier.get().asProxy(), Set.of()); } // Command Groups diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/Commands.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/Commands.cpp index bc4bd39da7..84c056f1f7 100644 --- a/wpilibNewCommands/src/main/native/cpp/frc2/command/Commands.cpp +++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/Commands.cpp @@ -7,6 +7,7 @@ #include #include +#include #include #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 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 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(); diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/CommandScheduler.h b/wpilibNewCommands/src/main/native/include/frc2/command/CommandScheduler.h index 2f43ff7bc2..c45d381155 100644 --- a/wpilibNewCommands/src/main/native/include/frc2/command/CommandScheduler.h +++ b/wpilibNewCommands/src/main/native/include/frc2/command/CommandScheduler.h @@ -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 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 commands); diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/Commands.h b/wpilibNewCommands/src/main/native/include/frc2/command/Commands.h index 6cfad01b1f..b0dcda022f 100644 --- a/wpilibNewCommands/src/main/native/include/frc2/command/Commands.h +++ b/wpilibNewCommands/src/main/native/include/frc2/command/Commands.h @@ -169,15 +169,11 @@ CommandPtr Defer(wpi::unique_function 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 supplier); /** @@ -187,11 +183,8 @@ CommandPtr DeferredProxy(wpi::unique_function 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 supplier); -WPI_UNIGNORE_DEPRECATED // Command Groups namespace impl { diff --git a/wpilibc/src/main/native/cpp/Joystick.cpp b/wpilibc/src/main/native/cpp/Joystick.cpp index 851e25c127..b58a514cbd 100644 --- a/wpilibc/src/main/native/cpp/Joystick.cpp +++ b/wpilibc/src/main/native/cpp/Joystick.cpp @@ -5,12 +5,8 @@ #include "frc/Joystick.h" #include -#include #include -#include -#include -#include #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())}; } diff --git a/wpilibc/src/main/native/cpp/simulation/DCMotorSim.cpp b/wpilibc/src/main/native/cpp/simulation/DCMotorSim.cpp index 98a02126f9..ff8fb1fa76 100644 --- a/wpilibc/src/main/native/cpp/simulation/DCMotorSim.cpp +++ b/wpilibc/src/main/native/cpp/simulation/DCMotorSim.cpp @@ -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; +} diff --git a/wpilibc/src/main/native/include/frc/AddressableLED.h b/wpilibc/src/main/native/include/frc/AddressableLED.h index d395ab0d92..c7af852860 100644 --- a/wpilibc/src/main/native/include/frc/AddressableLED.h +++ b/wpilibc/src/main/native/include/frc/AddressableLED.h @@ -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() * *

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. * - *

By default, the driver is set up to drive WS2812Bs, so nothing needs to - * be set for those. + *

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. * *

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) */ diff --git a/wpilibc/src/main/native/include/frc/simulation/DCMotorSim.h b/wpilibc/src/main/native/include/frc/simulation/DCMotorSim.h index c5c761845a..3473938f62 100644 --- a/wpilibc/src/main/native/include/frc/simulation/DCMotorSim.h +++ b/wpilibc/src/main/native/include/frc/simulation/DCMotorSim.h @@ -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; diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/AddressableLED.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AddressableLED.java index bb097ab94f..a8c160fa4b 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/AddressableLED.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AddressableLED.java @@ -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. * - *

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() * *

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. * - *

By default, the driver is set up to drive WS2812Bs, so nothing needs to be set for those. + *

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. * - *

The sync time is the time to hold output so LEDs enable. Default set for WS2812B. + *

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) */ diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose2d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose2d.java index 0705d61ea7..a8f250fba2 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose2d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose2d.java @@ -238,6 +238,17 @@ public class Pose2d implements Interpolatable, 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. * diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose3d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose3d.java index 02c2b9ba4e..16de1dc33b 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose3d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose3d.java @@ -271,6 +271,17 @@ public class Pose3d implements Interpolatable, 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. * diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java index 54a4fbfcf0..f8c2b965a8 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java @@ -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()); } diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation3d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation3d.java index 20cede3d3d..efaf92a670 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation3d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation3d.java @@ -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. * diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveWheelPositions.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveWheelPositions.java index b4503e6480..b0dcd11d5b 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveWheelPositions.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveWheelPositions.java @@ -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 { + implements StructSerializable, + ProtobufSerializable, + Interpolatable { /** Distance measured by the left side. */ public double leftMeters; diff --git a/wpimath/src/main/java/edu/wpi/first/math/system/plant/LinearSystemId.java b/wpimath/src/main/java/edu/wpi/first/math/system/plant/LinearSystemId.java index ac48939ef0..ce1129b059 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/system/plant/LinearSystemId.java +++ b/wpimath/src/main/java/edu/wpi/first/math/system/plant/LinearSystemId.java @@ -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]. * *

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]ᵀ. * *

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. diff --git a/wpimath/src/main/native/include/frc/ct_matrix.h b/wpimath/src/main/native/include/frc/ct_matrix.h index b520cb46fd..ffa684738a 100644 --- a/wpimath/src/main/native/include/frc/ct_matrix.h +++ b/wpimath/src/main/native/include/frc/ct_matrix.h @@ -163,8 +163,8 @@ class ct_matrix { if (std::is_constant_evaluated()) { ct_matrix 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 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); } } diff --git a/wpimath/src/main/native/include/frc/geometry/Pose2d.h b/wpimath/src/main/native/include/frc/geometry/Pose2d.h index 0664161d6d..2d8825caa5 100644 --- a/wpimath/src/main/native/include/frc/geometry/Pose2d.h +++ b/wpimath/src/main/native/include/frc/geometry/Pose2d.h @@ -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. * diff --git a/wpimath/src/main/native/include/frc/geometry/Pose3d.h b/wpimath/src/main/native/include/frc/geometry/Pose3d.h index 176f2c03ab..110faf7355 100644 --- a/wpimath/src/main/native/include/frc/geometry/Pose3d.h +++ b/wpimath/src/main/native/include/frc/geometry/Pose3d.h @@ -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. * diff --git a/wpimath/src/main/native/include/frc/geometry/Rotation2d.h b/wpimath/src/main/native/include/frc/geometry/Rotation2d.h index 8498128c21..2b33d6d26a 100644 --- a/wpimath/src/main/native/include/frc/geometry/Rotation2d.h +++ b/wpimath/src/main/native/include/frc/geometry/Rotation2d.h @@ -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{}", diff --git a/wpimath/src/main/native/include/frc/geometry/Translation3d.h b/wpimath/src/main/native/include/frc/geometry/Translation3d.h index c16fc56a5b..72e220f08c 100644 --- a/wpimath/src/main/native/include/frc/geometry/Translation3d.h +++ b/wpimath/src/main/native/include/frc/geometry/Translation3d.h @@ -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. diff --git a/wpimath/src/main/native/include/frc/system/plant/LinearSystemId.h b/wpimath/src/main/native/include/frc/system/plant/LinearSystemId.h index 0af472b934..f26c8a0376 100644 --- a/wpimath/src/main/native/include/frc/system/plant/LinearSystemId.h +++ b/wpimath/src/main/native/include/frc/system/plant/LinearSystemId.h @@ -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 requires std::same_as || std::same_as - static constexpr LinearSystem<2, 1, 1> IdentifyPositionSystem( + static constexpr LinearSystem<2, 1, 2> IdentifyPositionSystem( decltype(1_V / Velocity_t(1)) kV, decltype(1_V / Acceleration_t(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. diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/AssignEvaluator.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/AssignEvaluator.h index f7f0b238b8..9c2436afa7 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/AssignEvaluator.h +++ b/wpimath/src/main/native/thirdparty/eigen/include/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(outer, inner); enum { NextIndex = Index + unpacket_traits::size }; copy_using_evaluator_innervec_CompleteUnrolling::run(kernel); @@ -431,17 +431,25 @@ struct dense_assignment_loop { template struct dense_assignment_loop { 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::size, - alignedSize = (int(size) / packetSize) * packetSize - }; + enum { + size = DstXprType::SizeAtCompileTime, + packetSize = unpacket_traits::size, + alignedSize = (int(size) / packetSize) * packetSize + }; - copy_using_evaluator_linearvec_CompleteUnrolling::run(kernel); - copy_using_evaluator_LinearTraversal_CompleteUnrolling::run(kernel); + copy_using_evaluator_linearvec_CompleteUnrolling::run(kernel); + copy_using_evaluator_LinearTraversal_CompleteUnrolling::run(kernel); + } } }; @@ -465,9 +473,17 @@ struct dense_assignment_loop { template struct dense_assignment_loop { - EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE void run(Kernel& kernel) { - typedef typename Kernel::DstEvaluatorType::XprType DstXprType; - copy_using_evaluator_innervec_CompleteUnrolling::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::run(kernel); + } } }; @@ -498,8 +514,16 @@ struct dense_assignment_loop { template struct dense_assignment_loop { EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE EIGEN_CONSTEXPR void run(Kernel& kernel) { - typedef typename Kernel::DstEvaluatorType::XprType DstXprType; - copy_using_evaluator_LinearTraversal_CompleteUnrolling::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::run(kernel); + } } }; @@ -510,41 +534,49 @@ struct dense_assignment_loop { template struct dense_assignment_loop { 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::size, - requestedAlignment = int(Kernel::AssignmentTraits::InnerRequiredAlignment), - alignable = - packet_traits::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::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(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::size, + requestedAlignment = int(Kernel::AssignmentTraits::InnerRequiredAlignment), + alignable = + packet_traits::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::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(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(outer, inner); + // do the vectorizable part of the assignment + for (Index inner = alignedStart; inner < alignedEnd; inner += packetSize) + kernel.template assignPacketByOuterInner(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 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(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 -EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void resize_if_allowed(DstXprType& dst, const SrcXprType& src, - const internal::assign_op& /*func*/) { +EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE constexpr void resize_if_allowed(DstXprType& dst, const SrcXprType& src, + const internal::assign_op& /*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 -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()); } template @@ -807,7 +839,7 @@ EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR void call_assignment( } template -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::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 struct Assignment { - 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/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/EigenBase.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/EigenBase.h index 6d167006a0..894bfc13b1 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/EigenBase.h +++ b/wpimath/src/main/native/thirdparty/eigen/include/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(this); } - EIGEN_DEVICE_FUNC inline Derived& const_cast_derived() const { + EIGEN_DEVICE_FUNC inline constexpr Derived& const_cast_derived() const { return *static_cast(const_cast(this)); } EIGEN_DEVICE_FUNC inline const Derived& const_derived() const { return *static_cast(this); } diff --git a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/functors/AssignmentFunctors.h b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/functors/AssignmentFunctors.h index 09d1da8ca2..3687bb20db 100644 --- a/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/functors/AssignmentFunctors.h +++ b/wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/functors/AssignmentFunctors.h @@ -23,7 +23,7 @@ namespace internal { */ template 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 EIGEN_STRONG_INLINE void assignPacket(DstScalar* a, const Packet& b) const { diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/Pose2dTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/Pose2dTest.java index 81f8150992..895c20de92 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/geometry/Pose2dTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/Pose2dTest.java @@ -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)); diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/Pose3dTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/Pose3dTest.java index 00c63eae08..f8fadea6a5 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/geometry/Pose3dTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/Pose3dTest.java @@ -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); diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/Translation3dTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/Translation3dTest.java index 3dd54227ea..49b146fdd0 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/geometry/Translation3dTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/Translation3dTest.java @@ -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); diff --git a/wpimath/src/test/native/cpp/geometry/Pose2dTest.cpp b/wpimath/src/test/native/cpp/geometry/Pose2dTest.cpp index 9a038590b1..eb5c3bef99 100644 --- a/wpimath/src/test/native/cpp/geometry/Pose2dTest.cpp +++ b/wpimath/src/test/native/cpp/geometry/Pose2dTest.cpp @@ -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}; diff --git a/wpimath/src/test/native/cpp/geometry/Pose3dTest.cpp b/wpimath/src/test/native/cpp/geometry/Pose3dTest.cpp index f0168e9543..ee3f8fa53a 100644 --- a/wpimath/src/test/native/cpp/geometry/Pose3dTest.cpp +++ b/wpimath/src/test/native/cpp/geometry/Pose3dTest.cpp @@ -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}; diff --git a/wpimath/src/test/native/cpp/geometry/Rotation2dTest.cpp b/wpimath/src/test/native/cpp/geometry/Rotation2dTest.cpp index 3150feeb93..d9d1502471 100644 --- a/wpimath/src/test/native/cpp/geometry/Rotation2dTest.cpp +++ b/wpimath/src/test/native/cpp/geometry/Rotation2dTest.cpp @@ -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); } diff --git a/wpimath/src/test/native/cpp/geometry/Rotation3dTest.cpp b/wpimath/src/test/native/cpp/geometry/Rotation3dTest.cpp index 6088bdd759..903d4144bc 100644 --- a/wpimath/src/test/native/cpp/geometry/Rotation3dTest.cpp +++ b/wpimath/src/test/native/cpp/geometry/Rotation3dTest.cpp @@ -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); } diff --git a/wpimath/src/test/native/cpp/geometry/Translation2dTest.cpp b/wpimath/src/test/native/cpp/geometry/Translation2dTest.cpp index 46d383ea58..294c213a3d 100644 --- a/wpimath/src/test/native/cpp/geometry/Translation2dTest.cpp +++ b/wpimath/src/test/native/cpp/geometry/Translation2dTest.cpp @@ -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) { diff --git a/wpimath/src/test/native/cpp/geometry/Translation3dTest.cpp b/wpimath/src/test/native/cpp/geometry/Translation3dTest.cpp index 56eaf6da75..f463c0bfa4 100644 --- a/wpimath/src/test/native/cpp/geometry/Translation3dTest.cpp +++ b/wpimath/src/test/native/cpp/geometry/Translation3dTest.cpp @@ -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}; diff --git a/wpimath/src/test/native/cpp/system/LinearSystemIDTest.cpp b/wpimath/src/test/native/cpp/system/LinearSystemIDTest.cpp index 39e8a8c256..6aec204fc7 100644 --- a/wpimath/src/test/native/cpp/system/LinearSystemIDTest.cpp +++ b/wpimath/src/test/native/cpp/system/LinearSystemIDTest.cpp @@ -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( kv * 1_V / 1_mps, ka * 1_V / 1_mps_sq); +#else + constexpr auto model = + frc::LinearSystemId::IdentifyPositionSystem( + 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( kv * 1_V / 1_mps, ka * 1_V / 1_mps_sq); +#else + constexpr auto model = + frc::LinearSystemId::IdentifyVelocitySystem( + 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)); diff --git a/wpiutil/src/main/java/edu/wpi/first/util/RawFrame.java b/wpiutil/src/main/java/edu/wpi/first/util/RawFrame.java index dd074bf340..dd12d84a98 100644 --- a/wpiutil/src/main/java/edu/wpi/first/util/RawFrame.java +++ b/wpiutil/src/main/java/edu/wpi/first/util/RawFrame.java @@ -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; + } } diff --git a/wpiutil/src/main/java/edu/wpi/first/util/TimestampSource.java b/wpiutil/src/main/java/edu/wpi/first/util/TimestampSource.java new file mode 100644 index 0000000000..f1da693228 --- /dev/null +++ b/wpiutil/src/main/java/edu/wpi/first/util/TimestampSource.java @@ -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]; + } +} diff --git a/wpiutil/src/main/java/edu/wpi/first/util/WPIUtilJNI.java b/wpiutil/src/main/java/edu/wpi/first/util/WPIUtilJNI.java index c69895231e..2818489c59 100644 --- a/wpiutil/src/main/java/edu/wpi/first/util/WPIUtilJNI.java +++ b/wpiutil/src/main/java/edu/wpi/first/util/WPIUtilJNI.java @@ -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. * diff --git a/wpiutil/src/main/native/cpp/jni/WPIUtilJNI.cpp b/wpiutil/src/main/native/cpp/jni/WPIUtilJNI.cpp index 6eb87259ce..b55100de0e 100644 --- a/wpiutil/src/main/native/cpp/jni/WPIUtilJNI.cpp +++ b/wpiutil/src/main/native/cpp/jni/WPIUtilJNI.cpp @@ -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(frame); + if (!f) { + wpi::ThrowNullPointerException(env, "frame is null"); + return; + } + f->timestamp = time; + f->timestampSrc = timeSource; +} + /* * Class: edu_wpi_first_util_WPIUtilJNI * Method: setRawFrameInfo diff --git a/wpiutil/src/main/native/include/wpi/RawFrame.h b/wpiutil/src/main/native/include/wpi/RawFrame.h index 420f819979..1fbfd4da84 100644 --- a/wpiutil/src/main/native/include/wpi/RawFrame.h +++ b/wpiutil/src/main/native/include/wpi/RawFrame.h @@ -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 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(frame.width), static_cast(frame.height), - static_cast(frame.stride), static_cast(frame.pixelFormat)); + static_cast(frame.stride), static_cast(frame.pixelFormat), + static_cast(frame.timestamp), + static_cast(frame.timestampSrc)); } else { static jmethodID setInfo = - env->GetMethodID(rawFrameCls, "setInfoJNI", "(IIII)V"); + env->GetMethodID(rawFrameCls, "setInfoJNI", "(IIIIJI)V"); env->CallVoidMethod(jframe, setInfo, static_cast(frame.width), static_cast(frame.height), static_cast(frame.stride), - static_cast(frame.pixelFormat)); + static_cast(frame.pixelFormat), + static_cast(frame.timestamp), + static_cast(frame.timestampSrc)); } } #endif diff --git a/wpiutil/src/main/native/include/wpi/Synchronization.h b/wpiutil/src/main/native/include/wpi/Synchronization.h index 7b98322139..aa067662d3 100644 --- a/wpiutil/src/main/native/include/wpi/Synchronization.h +++ b/wpiutil/src/main/native/include/wpi/Synchronization.h @@ -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;