diff --git a/.github/workflows/gradle.yml b/.github/workflows/gradle.yml index 3af54fb6101..2334b5f028c 100644 --- a/.github/workflows/gradle.yml +++ b/.github/workflows/gradle.yml @@ -61,7 +61,7 @@ jobs: build-host: env: - MACOSX_DEPLOYMENT_TARGET: 11 + MACOSX_DEPLOYMENT_TARGET: 12 strategy: fail-fast: false matrix: diff --git a/CMakeLists.txt b/CMakeLists.txt index f994bf13e54..e10ad6135a0 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -60,6 +60,7 @@ option(WITH_JAVA_SOURCE "Build Java source jars" ON) option(WITH_CSCORE "Build cscore (needs OpenCV)" ON) option(WITH_NTCORE "Build ntcore" ON) option(WITH_WPIMATH "Build wpimath" ON) +option(WITH_WPIUNITS "Build wpiunits" ON) option(WITH_WPILIB "Build hal, wpilibc/j, and myRobot (needs OpenCV)" ON) option(WITH_EXAMPLES "Build examples" OFF) option(WITH_TESTS "Build unit tests (requires internet connection)" ON) @@ -75,9 +76,6 @@ option(USE_SYSTEM_FMTLIB "Use system fmtlib" OFF) option(USE_SYSTEM_LIBUV "Use system libuv" OFF) option(USE_SYSTEM_EIGEN "Use system eigen" OFF) -# Options for installation. -option(WITH_FLAT_INSTALL "Use a flat install directory" OFF) - # Options for location of OpenCV Java. set(OPENCV_JAVA_INSTALL_DIR "" CACHE PATH "Location to search for the OpenCV jar file") @@ -154,17 +152,17 @@ FATAL: Cannot build wpilib without wpimath. ") endif() -set( wpilib_dest "") +if (NOT WITH_WPIUNITS AND WITH_WPIMATH AND WITH_JAVA) + message(FATAL_ERROR " +FATAL: Cannot build Java wpimath without wpiunits. + Enable wpiunits by setting WITH_WPIUNITS=ON or disable the Java build by setting WITH_JAVA=OFF +") +endif() + set( include_dest include ) set( java_lib_dest java ) set( jni_lib_dest jni ) -if (WITH_FLAT_INSTALL) - set (wpilib_config_dir ${wpilib_dest}) -else() - set (wpilib_config_dir share/wpilib) -endif() - if (USE_SYSTEM_LIBUV) set (LIBUV_SYSTEM_REPLACE " find_dependency(libuv CONFIG) @@ -183,17 +181,6 @@ find_program(Quickbuf_EXECUTABLE DOC "The Quickbuf protoc plugin" ) -if (WITH_FLAT_INSTALL) -set(WPIUTIL_DEP_REPLACE "include($\{SELF_DIR\}/wpiutil-config.cmake)") -set(WPINET_DEP_REPLACE "include($\{SELF_DIR\}/wpinet-config.cmake)") -set(NTCORE_DEP_REPLACE "include($\{SELF_DIR\}/ntcore-config.cmake)") -set(CSCORE_DEP_REPLACE_IMPL "include(\${SELF_DIR}/cscore-config.cmake)") -set(CAMERASERVER_DEP_REPLACE_IMPL "include(\${SELF_DIR}/cameraserver-config.cmake)") -set(HAL_DEP_REPLACE_IMPL "include(\${SELF_DIR}/hal-config.cmake)") -set(WPIMATH_DEP_REPLACE "include($\{SELF_DIR\}/wpimath-config.cmake)") -set(WPILIBC_DEP_REPLACE_IMPL "include(\${SELF_DIR}/wpilibc-config.cmake)") -set(WPILIBNEWCOMMANDS_DEP_REPLACE "include(\${SELF_DIR}/wpilibNewcommands-config.cmake)") -else() set(WPIUTIL_DEP_REPLACE "find_dependency(wpiutil)") set(WPINET_DEP_REPLACE "find_dependency(wpinet)") set(NTCORE_DEP_REPLACE "find_dependency(ntcore)") @@ -201,9 +188,9 @@ set(CSCORE_DEP_REPLACE_IMPL "find_dependency(cscore)") set(CAMERASERVER_DEP_REPLACE_IMPL "find_dependency(cameraserver)") set(HAL_DEP_REPLACE_IMPL "find_dependency(hal)") set(WPIMATH_DEP_REPLACE "find_dependency(wpimath)") +set(WPIUNITS_DEP_REPLACE "find_dependency(wpiunits)") set(WPILIBC_DEP_REPLACE_IMPL "find_dependency(wpilibc)") set(WPILIBNEWCOMMANDS_DEP_REPLACE "find_dependency(wpilibNewCommands)") -endif() set(FILENAME_DEP_REPLACE "get_filename_component(SELF_DIR \"$\{CMAKE_CURRENT_LIST_FILE\}\" PATH)") set(SELF_DIR "$\{SELF_DIR\}") @@ -291,9 +278,17 @@ if (WITH_NTCORE) endif() if (WITH_WPIMATH) + if (WITH_JAVA) + add_subdirectory(wpiunits) + endif() add_subdirectory(wpimath) endif() +if (WITH_WPIUNITS AND NOT WITH_WPIMATH) + # In case of building wpiunits standalone + add_subdirectory(wpiunits) +endif() + if (WITH_GUI) add_subdirectory(fieldImages) add_subdirectory(imgui) @@ -338,4 +333,4 @@ if (WITH_SIMULATION_MODULES AND NOT WITH_EXTERNAL_HAL) endif() configure_file(wpilib-config.cmake.in ${WPILIB_BINARY_DIR}/wpilib-config.cmake ) -install(FILES ${WPILIB_BINARY_DIR}/wpilib-config.cmake DESTINATION ${wpilib_config_dir}) +install(FILES ${WPILIB_BINARY_DIR}/wpilib-config.cmake DESTINATION share/wpilib) diff --git a/README-CMAKE.md b/README-CMAKE.md index 776f808d911..a2750bd70d6 100644 --- a/README-CMAKE.md +++ b/README-CMAKE.md @@ -12,6 +12,7 @@ WPILib is normally built with Gradle, however for some systems, such as Linux ba * halsim * wpigui * wpimath +* wpiunits * wpilibNewCommands By default, all libraries except for the HAL and WPILib get built with a default CMake setup. The libraries are built as shared libraries, and include the JNI libraries as well as building the Java JARs. @@ -44,6 +45,8 @@ The following build options are available: * This option will cause ntcore to be built. Turning this off will implicitly disable wpinet and wpilib as well, irrespective of their specific options. * `WITH_WPIMATH` (ON Default) * This option will build the wpimath library. This option must be on to build wpilib. +* `WITH_WPIUNITS` (ON Default) + * This option will build the wpiunits library. This option must be on to build the Java wpimath library and requires `WITH_JAVA` to also be on. * `WITH_WPILIB` (ON Default) * This option will build the hal and wpilibc/j during the build. The HAL is the simulator hal, unless the external hal options are used. The cmake build has no capability to build for the RoboRIO. * `WITH_EXAMPLES` (ON Default) diff --git a/apriltag/CMakeLists.txt b/apriltag/CMakeLists.txt index 9d97363760e..984c5026871 100644 --- a/apriltag/CMakeLists.txt +++ b/apriltag/CMakeLists.txt @@ -44,7 +44,7 @@ if (WITH_JAVA) add_jar(apriltag_jar SOURCES ${JAVA_SOURCES} RESOURCES NAMESPACE "edu/wpi/first/apriltag" ${JAVA_RESOURCES} - INCLUDE_JARS wpimath_jar ${EJML_JARS} wpiutil_jar ${OPENCV_JAR_FILE} + INCLUDE_JARS wpimath_jar wpiunits_jar ${EJML_JARS} wpiutil_jar ${OPENCV_JAR_FILE} OUTPUT_NAME apriltag GENERATE_NATIVE_HEADERS apriltag_jni_headers) @@ -108,15 +108,10 @@ target_include_directories(apriltag PUBLIC install(TARGETS apriltag EXPORT apriltag) install(DIRECTORY src/main/native/include/ DESTINATION "${include_dest}/apriltag") -if (WITH_FLAT_INSTALL) - set (apriltag_config_dir ${wpilib_dest}) -else() - set (apriltag_config_dir share/apriltag) -endif() configure_file(apriltag-config.cmake.in ${WPILIB_BINARY_DIR}/apriltag-config.cmake ) -install(FILES ${WPILIB_BINARY_DIR}/apriltag-config.cmake DESTINATION ${apriltag_config_dir}) -install(EXPORT apriltag DESTINATION ${apriltag_config_dir}) +install(FILES ${WPILIB_BINARY_DIR}/apriltag-config.cmake DESTINATION share/apriltag) +install(EXPORT apriltag DESTINATION share/apriltag) if (WITH_TESTS) wpilib_add_test(apriltag src/test/native/cpp) diff --git a/apriltag/src/main/java/edu/wpi/first/apriltag/AprilTagDetection.java b/apriltag/src/main/java/edu/wpi/first/apriltag/AprilTagDetection.java index a4bdd1fe123..954cb8635da 100644 --- a/apriltag/src/main/java/edu/wpi/first/apriltag/AprilTagDetection.java +++ b/apriltag/src/main/java/edu/wpi/first/apriltag/AprilTagDetection.java @@ -72,7 +72,7 @@ public double[] getHomography() { * @return Homography matrix */ public Matrix getHomographyMatrix() { - return new MatBuilder<>(Nat.N3(), Nat.N3()).fill(m_homography); + return MatBuilder.fill(Nat.N3(), Nat.N3(), m_homography); } /** diff --git a/buildSrc/build.gradle b/buildSrc/build.gradle index 2c52479ef8a..1e5d02cdee3 100644 --- a/buildSrc/build.gradle +++ b/buildSrc/build.gradle @@ -9,5 +9,5 @@ repositories { } } dependencies { - implementation "edu.wpi.first:native-utils:2024.3.1" + implementation "edu.wpi.first:native-utils:2024.3.2" } diff --git a/cameraserver/CMakeLists.txt b/cameraserver/CMakeLists.txt index 177e009eef9..c2e2bd5f76d 100644 --- a/cameraserver/CMakeLists.txt +++ b/cameraserver/CMakeLists.txt @@ -59,15 +59,9 @@ set_property(TARGET cameraserver PROPERTY FOLDER "libraries") install(TARGETS cameraserver EXPORT cameraserver) install(DIRECTORY src/main/native/include/ DESTINATION "${include_dest}/cameraserver") -if (WITH_FLAT_INSTALL) - set (cameraserver_config_dir ${wpilib_dest}) -else() - set (cameraserver_config_dir share/cameraserver) -endif() - configure_file(cameraserver-config.cmake.in ${WPILIB_BINARY_DIR}/cameraserver-config.cmake ) -install(FILES ${WPILIB_BINARY_DIR}/cameraserver-config.cmake DESTINATION ${cameraserver_config_dir}) -install(EXPORT cameraserver DESTINATION ${cameraserver_config_dir}) +install(FILES ${WPILIB_BINARY_DIR}/cameraserver-config.cmake DESTINATION share/cameraserver) +install(EXPORT cameraserver DESTINATION share/cameraserver) file(GLOB multiCameraServer_src multiCameraServer/src/main/native/cpp/*.cpp) add_executable(multiCameraServer ${multiCameraServer_src}) diff --git a/cscore/CMakeLists.txt b/cscore/CMakeLists.txt index a6a89b36352..e19579caa0f 100644 --- a/cscore/CMakeLists.txt +++ b/cscore/CMakeLists.txt @@ -43,15 +43,9 @@ set_property(TARGET cscore PROPERTY FOLDER "libraries") install(TARGETS cscore EXPORT cscore) install(DIRECTORY src/main/native/include/ DESTINATION "${include_dest}/cscore") -if (WITH_FLAT_INSTALL) - set (cscore_config_dir ${wpilib_dest}) -else() - set (cscore_config_dir share/cscore) -endif() - configure_file(cscore-config.cmake.in ${WPILIB_BINARY_DIR}/cscore-config.cmake ) -install(FILES ${WPILIB_BINARY_DIR}/cscore-config.cmake DESTINATION ${cscore_config_dir}) -install(EXPORT cscore DESTINATION ${cscore_config_dir}) +install(FILES ${WPILIB_BINARY_DIR}/cscore-config.cmake DESTINATION share/cscore) +install(EXPORT cscore DESTINATION share/cscore) subdir_list(cscore_examples "${CMAKE_CURRENT_SOURCE_DIR}/examples") foreach(example ${cscore_examples}) diff --git a/cscore/src/main/java/edu/wpi/first/cscore/CameraServerCvJNI.java b/cscore/src/main/java/edu/wpi/first/cscore/CameraServerCvJNI.java index 66195221f89..2259582f847 100644 --- a/cscore/src/main/java/edu/wpi/first/cscore/CameraServerCvJNI.java +++ b/cscore/src/main/java/edu/wpi/first/cscore/CameraServerCvJNI.java @@ -64,7 +64,7 @@ public static native int createCvSource( public static native void putSourceFrame(int source, long imageNativeObj); - public static native int createCvSink(String name); + public static native int createCvSink(String name, int pixelFormat); // public static native int createCvSinkCallback(String name, // void (*processFrame)(long time)); 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 88ca8b1b548..75d9e1cbc1f 100644 --- a/cscore/src/main/java/edu/wpi/first/cscore/CvSink.java +++ b/cscore/src/main/java/edu/wpi/first/cscore/CvSink.java @@ -4,6 +4,7 @@ package edu.wpi.first.cscore; +import edu.wpi.first.cscore.VideoMode.PixelFormat; import org.opencv.core.Mat; /** @@ -16,9 +17,20 @@ public class CvSink extends ImageSink { * get each new image. * * @param name Source name (arbitrary unique identifier) + * @param pixelFormat Source pixel format + */ + public CvSink(String name, PixelFormat pixelFormat) { + super(CameraServerCvJNI.createCvSink(name, pixelFormat.getValue())); + } + + /** + * Create a sink for accepting OpenCV images. WaitForFrame() must be called on the created sink to + * get each new image. Defaults to kBGR for pixelFormat + * + * @param name Source name (arbitrary unique identifier) */ public CvSink(String name) { - super(CameraServerCvJNI.createCvSink(name)); + this(name, PixelFormat.kBGR); } /// Create a sink for accepting OpenCV images in a separate thread. diff --git a/cscore/src/main/native/cpp/CvSinkImpl.cpp b/cscore/src/main/native/cpp/CvSinkImpl.cpp index 942fdaf3266..bfe96adac1b 100644 --- a/cscore/src/main/native/cpp/CvSinkImpl.cpp +++ b/cscore/src/main/native/cpp/CvSinkImpl.cpp @@ -19,16 +19,18 @@ using namespace cs; CvSinkImpl::CvSinkImpl(std::string_view name, wpi::Logger& logger, - Notifier& notifier, Telemetry& telemetry) - : SinkImpl{name, logger, notifier, telemetry} { + Notifier& notifier, Telemetry& telemetry, + VideoMode::PixelFormat pixelFormat) + : SinkImpl{name, logger, notifier, telemetry}, m_pixelFormat{pixelFormat} { m_active = true; // m_thread = std::thread(&CvSinkImpl::ThreadMain, this); } CvSinkImpl::CvSinkImpl(std::string_view name, wpi::Logger& logger, Notifier& notifier, Telemetry& telemetry, + VideoMode::PixelFormat pixelFormat, std::function processFrame) - : SinkImpl{name, logger, notifier, telemetry} {} + : SinkImpl{name, logger, notifier, telemetry}, m_pixelFormat{pixelFormat} {} CvSinkImpl::~CvSinkImpl() { Stop(); @@ -65,7 +67,7 @@ uint64_t CvSinkImpl::GrabFrame(cv::Mat& image) { return 0; // signal error } - if (!frame.GetCv(image)) { + if (!frame.GetCv(image, m_pixelFormat)) { // Shouldn't happen, but just in case... std::this_thread::sleep_for(std::chrono::milliseconds(20)); return 0; @@ -91,7 +93,7 @@ uint64_t CvSinkImpl::GrabFrame(cv::Mat& image, double timeout) { return 0; // signal error } - if (!frame.GetCv(image)) { + if (!frame.GetCv(image, m_pixelFormat)) { // Shouldn't happen, but just in case... std::this_thread::sleep_for(std::chrono::milliseconds(20)); return 0; @@ -127,20 +129,23 @@ void CvSinkImpl::ThreadMain() { namespace cs { -CS_Sink CreateCvSink(std::string_view name, CS_Status* status) { +CS_Sink CreateCvSink(std::string_view name, VideoMode::PixelFormat pixelFormat, + CS_Status* status) { auto& inst = Instance::GetInstance(); return inst.CreateSink( CS_SINK_CV, std::make_shared(name, inst.logger, inst.notifier, - inst.telemetry)); + inst.telemetry, pixelFormat)); } CS_Sink CreateCvSinkCallback(std::string_view name, + VideoMode::PixelFormat pixelFormat, std::function processFrame, CS_Status* status) { auto& inst = Instance::GetInstance(); return inst.CreateSink( - CS_SINK_CV, std::make_shared(name, inst.logger, inst.notifier, - inst.telemetry, processFrame)); + CS_SINK_CV, + std::make_shared(name, inst.logger, inst.notifier, + inst.telemetry, pixelFormat, processFrame)); } static constexpr unsigned SinkMask = CS_SINK_CV | CS_SINK_RAW; @@ -206,15 +211,19 @@ void SetSinkEnabled(CS_Sink sink, bool enabled, CS_Status* status) { extern "C" { -CS_Sink CS_CreateCvSink(const char* name, CS_Status* status) { - return cs::CreateCvSink(name, status); +CS_Sink CS_CreateCvSink(const char* name, enum CS_PixelFormat pixelFormat, + CS_Status* status) { + return cs::CreateCvSink( + name, static_cast(pixelFormat), status); } -CS_Sink CS_CreateCvSinkCallback(const char* name, void* data, +CS_Sink CS_CreateCvSinkCallback(const char* name, + enum CS_PixelFormat pixelFormat, void* data, void (*processFrame)(void* data, uint64_t time), CS_Status* status) { return cs::CreateCvSinkCallback( - name, [=](uint64_t time) { processFrame(data, time); }, status); + name, static_cast(pixelFormat), + [=](uint64_t time) { processFrame(data, time); }, status); } void CS_SetSinkDescription(CS_Sink sink, const char* description, diff --git a/cscore/src/main/native/cpp/CvSinkImpl.h b/cscore/src/main/native/cpp/CvSinkImpl.h index ad63a20f1ae..da9392f39c6 100644 --- a/cscore/src/main/native/cpp/CvSinkImpl.h +++ b/cscore/src/main/native/cpp/CvSinkImpl.h @@ -25,9 +25,9 @@ class SourceImpl; class CvSinkImpl : public SinkImpl { public: CvSinkImpl(std::string_view name, wpi::Logger& logger, Notifier& notifier, - Telemetry& telemetry); + Telemetry& telemetry, VideoMode::PixelFormat pixelFormat); CvSinkImpl(std::string_view name, wpi::Logger& logger, Notifier& notifier, - Telemetry& telemetry, + Telemetry& telemetry, VideoMode::PixelFormat pixelFormat, std::function processFrame); ~CvSinkImpl() override; @@ -42,6 +42,7 @@ class CvSinkImpl : public SinkImpl { std::atomic_bool m_active; // set to false to terminate threads std::thread m_thread; std::function m_processFrame; + VideoMode::PixelFormat m_pixelFormat; }; } // namespace cs diff --git a/cscore/src/main/native/cpp/Frame.cpp b/cscore/src/main/native/cpp/Frame.cpp index e6f8e7c683d..1a30393e595 100644 --- a/cscore/src/main/native/cpp/Frame.cpp +++ b/cscore/src/main/native/cpp/Frame.cpp @@ -709,8 +709,9 @@ Image* Frame::GetImageImpl(int width, int height, return ConvertImpl(cur, pixelFormat, requiredJpegQuality, defaultJpegQuality); } -bool Frame::GetCv(cv::Mat& image, int width, int height) { - Image* rawImage = GetImage(width, height, VideoMode::kBGR); +bool Frame::GetCv(cv::Mat& image, int width, int height, + VideoMode::PixelFormat pixelFormat) { + Image* rawImage = GetImage(width, height, pixelFormat); if (!rawImage) { return false; } diff --git a/cscore/src/main/native/cpp/Frame.h b/cscore/src/main/native/cpp/Frame.h index d5f537331e3..61324636805 100644 --- a/cscore/src/main/native/cpp/Frame.h +++ b/cscore/src/main/native/cpp/Frame.h @@ -219,10 +219,11 @@ class Frame { defaultQuality); } - bool GetCv(cv::Mat& image) { - return GetCv(image, GetOriginalWidth(), GetOriginalHeight()); + bool GetCv(cv::Mat& image, VideoMode::PixelFormat pixelFormat) { + return GetCv(image, GetOriginalWidth(), GetOriginalHeight(), pixelFormat); } - bool GetCv(cv::Mat& image, int width, int height); + bool GetCv(cv::Mat& image, int width, int height, + VideoMode::PixelFormat pixelFormat); private: Image* ConvertImpl(Image* image, VideoMode::PixelFormat pixelFormat, diff --git a/cscore/src/main/native/cpp/jni/CameraServerJNI.cpp b/cscore/src/main/native/cpp/jni/CameraServerJNI.cpp index ddd0ff542dd..f3bde4e9613 100644 --- a/cscore/src/main/native/cpp/jni/CameraServerJNI.cpp +++ b/cscore/src/main/native/cpp/jni/CameraServerJNI.cpp @@ -1392,18 +1392,20 @@ Java_edu_wpi_first_cscore_CameraServerJNI_createMjpegServer /* * Class: edu_wpi_first_cscore_CameraServerCvJNI * Method: createCvSink - * Signature: (Ljava/lang/String;)I + * Signature: (Ljava/lang/String;I)I */ JNIEXPORT jint JNICALL Java_edu_wpi_first_cscore_CameraServerCvJNI_createCvSink - (JNIEnv* env, jclass, jstring name) + (JNIEnv* env, jclass, jstring name, jint pixelFormat) { if (!name) { nullPointerEx.Throw(env, "name cannot be null"); return 0; } CS_Status status = 0; - auto val = cs::CreateCvSink(JStringRef{env, name}.str(), &status); + auto val = cs::CreateCvSink( + JStringRef{env, name}.str(), + static_cast(pixelFormat), &status); CheckStatus(env, status); return val; } diff --git a/cscore/src/main/native/include/cscore_c.h b/cscore/src/main/native/include/cscore_c.h index 3d30a0e7ec7..c19d7bdea08 100644 --- a/cscore/src/main/native/include/cscore_c.h +++ b/cscore/src/main/native/include/cscore_c.h @@ -382,8 +382,10 @@ void CS_SetSourceEnumPropertyChoices(CS_Source source, CS_Property property, */ CS_Sink CS_CreateMjpegServer(const char* name, const char* listenAddress, int port, CS_Status* status); -CS_Sink CS_CreateCvSink(const char* name, CS_Status* status); -CS_Sink CS_CreateCvSinkCallback(const char* name, void* data, +CS_Sink CS_CreateCvSink(const char* name, enum CS_PixelFormat pixelFormat, + CS_Status* status); +CS_Sink CS_CreateCvSinkCallback(const char* name, + enum CS_PixelFormat pixelFormat, void* data, void (*processFrame)(void* data, uint64_t time), CS_Status* status); /** @} */ diff --git a/cscore/src/main/native/include/cscore_cpp.h b/cscore/src/main/native/include/cscore_cpp.h index 72c37f8b070..28d2d9d2649 100644 --- a/cscore/src/main/native/include/cscore_cpp.h +++ b/cscore/src/main/native/include/cscore_cpp.h @@ -316,8 +316,10 @@ void SetSourceEnumPropertyChoices(CS_Source source, CS_Property property, */ CS_Sink CreateMjpegServer(std::string_view name, std::string_view listenAddress, int port, CS_Status* status); -CS_Sink CreateCvSink(std::string_view name, CS_Status* status); +CS_Sink CreateCvSink(std::string_view name, VideoMode::PixelFormat pixelFormat, + CS_Status* status); CS_Sink CreateCvSinkCallback(std::string_view name, + VideoMode::PixelFormat pixelFormat, std::function processFrame, CS_Status* status); diff --git a/cscore/src/main/native/include/cscore_cv.h b/cscore/src/main/native/include/cscore_cv.h index 66cb23e3858..6fdfdfb95d0 100644 --- a/cscore/src/main/native/include/cscore_cv.h +++ b/cscore/src/main/native/include/cscore_cv.h @@ -127,8 +127,10 @@ class CvSink : public ImageSink { * image. * * @param name Source name (arbitrary unique identifier) + * @param pixelFormat Source pixel format */ - explicit CvSink(std::string_view name); + explicit CvSink(std::string_view name, VideoMode::PixelFormat pixelFormat = + VideoMode::PixelFormat::kBGR); /** * Create a sink for accepting OpenCV images in a separate thread. @@ -141,9 +143,10 @@ class CvSink : public ImageSink { * time=0 if an error occurred. processFrame should call GetImage() * or GetError() as needed, but should not call (except in very * unusual circumstances) WaitForImage(). + * @param pixelFormat Source pixel format */ - CvSink(std::string_view name, - std::function processFrame); + CvSink(std::string_view name, std::function processFrame, + VideoMode::PixelFormat pixelFormat = VideoMode::PixelFormat::kBGR); /** * Wait for the next frame and get the image. @@ -184,13 +187,15 @@ inline void CvSource::PutFrame(cv::Mat& image) { PutSourceFrame(m_handle, image, &m_status); } -inline CvSink::CvSink(std::string_view name) { - m_handle = CreateCvSink(name, &m_status); +inline CvSink::CvSink(std::string_view name, + VideoMode::PixelFormat pixelFormat) { + m_handle = CreateCvSink(name, pixelFormat, &m_status); } inline CvSink::CvSink(std::string_view name, - std::function processFrame) { - m_handle = CreateCvSinkCallback(name, processFrame, &m_status); + std::function processFrame, + VideoMode::PixelFormat pixelFormat) { + m_handle = CreateCvSinkCallback(name, pixelFormat, processFrame, &m_status); } inline uint64_t CvSink::GrabFrame(cv::Mat& image, double timeout) const { diff --git a/fieldImages/publish.gradle b/fieldImages/publish.gradle index 33eb5ae3a5e..688376e8b33 100644 --- a/fieldImages/publish.gradle +++ b/fieldImages/publish.gradle @@ -2,7 +2,7 @@ apply plugin: 'maven-publish' def baseArtifactId = project.nativeName def artifactGroupId = project.groupId -def cppZipBaseName = "_GROUP_edu_wpi_first_fieldIimages_ID_${baseArtifactId}-cpp_CLS" +def cppZipBaseName = "_GROUP_edu_wpi_first_fieldImages_ID_${baseArtifactId}-cpp_CLS" def outputsFolder = file("$project.buildDir/outputs") diff --git a/glass/CMakeLists.txt b/glass/CMakeLists.txt index b949fc00fe5..e71b407e6ff 100644 --- a/glass/CMakeLists.txt +++ b/glass/CMakeLists.txt @@ -73,13 +73,3 @@ if (WIN32) elseif(APPLE) set_target_properties(glass PROPERTIES MACOSX_BUNDLE YES OUTPUT_NAME "Glass") endif() - -#if (MSVC OR FLAT_INSTALL_WPILIB) -# set (wpigui_config_dir ${wpilib_dest}) -#else() -# set (wpigui_config_dir share/wpigui) -#endif() - -#configure_file(wpigui-config.cmake.in ${CMAKE_BINARY_DIR}/wpigui-config.cmake ) -#install(FILES ${CMAKE_BINARY_DIR}/wpigui-config.cmake DESTINATION ${wpigui_config_dir}) -#install(EXPORT wpigui DESTINATION ${wpigui_config_dir}) diff --git a/hal/CMakeLists.txt b/hal/CMakeLists.txt index 603190b2421..39c018a6c15 100644 --- a/hal/CMakeLists.txt +++ b/hal/CMakeLists.txt @@ -59,15 +59,9 @@ install(TARGETS hal EXPORT hal) install(DIRECTORY src/main/native/include/ DESTINATION "${include_dest}/hal") install(DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/gen/ DESTINATION "${include_dest}/hal") -if (WITH_FLAT_INSTALL) - set (hal_config_dir ${wpilib_dest}) -else() - set (hal_config_dir share/hal) -endif() - configure_file(hal-config.cmake.in ${WPILIB_BINARY_DIR}/hal-config.cmake ) -install(FILES ${WPILIB_BINARY_DIR}/hal-config.cmake DESTINATION ${hal_config_dir}) -install(EXPORT hal DESTINATION ${hal_config_dir}) +install(FILES ${WPILIB_BINARY_DIR}/hal-config.cmake DESTINATION share/hal) +install(EXPORT hal DESTINATION share/hal) # Java bindings if (WITH_JAVA) diff --git a/ntcore/CMakeLists.txt b/ntcore/CMakeLists.txt index 5216eda5227..141613bcd69 100644 --- a/ntcore/CMakeLists.txt +++ b/ntcore/CMakeLists.txt @@ -37,15 +37,9 @@ install(TARGETS ntcore EXPORT ntcore) install(DIRECTORY src/main/native/include/ DESTINATION "${include_dest}/ntcore") install(DIRECTORY ${WPILIB_BINARY_DIR}/ntcore/generated/main/native/include/ DESTINATION "${include_dest}/ntcore") -if (WITH_FLAT_INSTALL) - set (ntcore_config_dir ${wpilib_dest}) -else() - set (ntcore_config_dir share/ntcore) -endif() - configure_file(ntcore-config.cmake.in ${WPILIB_BINARY_DIR}/ntcore-config.cmake ) -install(FILES ${WPILIB_BINARY_DIR}/ntcore-config.cmake DESTINATION ${ntcore_config_dir}) -install(EXPORT ntcore DESTINATION ${ntcore_config_dir}) +install(FILES ${WPILIB_BINARY_DIR}/ntcore-config.cmake DESTINATION share/ntcore) +install(EXPORT ntcore DESTINATION share/ntcore) # Java bindings if (WITH_JAVA) diff --git a/ntcore/src/main/native/cpp/ntcore_meta.cpp b/ntcore/src/main/native/cpp/ntcore_meta.cpp index 05cb4b7b3df..3b947a01fde 100644 --- a/ntcore/src/main/native/cpp/ntcore_meta.cpp +++ b/ntcore/src/main/native/cpp/ntcore_meta.cpp @@ -39,7 +39,7 @@ std::optional> nt::meta::DecodeClientPublishers( std::span data) { mpack_reader_t r; mpack_reader_init_data(&r, data); - uint32_t numPub = mpack_expect_array_max(&r, 1000); + uint32_t numPub = mpack_expect_array_max(&r, 10000); std::vector publishers; publishers.reserve(numPub); for (uint32_t i = 0; i < numPub; ++i) { @@ -71,7 +71,7 @@ std::optional> nt::meta::DecodeClientSubscribers( std::span data) { mpack_reader_t r; mpack_reader_init_data(&r, data); - uint32_t numSub = mpack_expect_array_max(&r, 1000); + uint32_t numSub = mpack_expect_array_max(&r, 10000); std::vector subscribers; subscribers.reserve(numSub); for (uint32_t i = 0; i < numSub; ++i) { diff --git a/ntcore/src/main/native/include/networktables/NetworkTableInstance.inc b/ntcore/src/main/native/include/networktables/NetworkTableInstance.inc index fdd517e2010..b4bbee16c00 100644 --- a/ntcore/src/main/native/include/networktables/NetworkTableInstance.inc +++ b/ntcore/src/main/native/include/networktables/NetworkTableInstance.inc @@ -257,6 +257,12 @@ inline void NetworkTableInstance::AddSchema(std::string_view name, ::nt::AddSchema(m_handle, name, type, schema); } +// Suppress unused-lambda-capture warning on AddSchema() call +#ifdef __clang__ +#pragma clang diagnostic push +#pragma clang diagnostic ignored "-Wunused-lambda-capture" +#endif + template void NetworkTableInstance::AddProtobufSchema(wpi::ProtobufMessage& msg) { msg.ForEachProtobufDescriptor( @@ -273,4 +279,8 @@ void NetworkTableInstance::AddStructSchema() { }); } +#ifdef __clang__ +#pragma clang diagnostic pop +#endif + } // namespace nt diff --git a/ntcore/src/main/native/include/networktables/ProtobufTopic.h b/ntcore/src/main/native/include/networktables/ProtobufTopic.h index 362d2cb5b5b..7a56759f673 100644 --- a/ntcore/src/main/native/include/networktables/ProtobufTopic.h +++ b/ntcore/src/main/native/include/networktables/ProtobufTopic.h @@ -60,12 +60,12 @@ class ProtobufSubscriber : public Subscriber { ProtobufSubscriber(ProtobufSubscriber&& rhs) : Subscriber{std::move(rhs)}, m_msg{std::move(rhs.m_msg)}, - m_defaultValue{std::move(rhs.defaultValue)} {} + m_defaultValue{std::move(rhs.m_defaultValue)} {} ProtobufSubscriber& operator=(ProtobufSubscriber&& rhs) { Subscriber::operator=(std::move(rhs)); m_msg = std::move(rhs.m_msg); - m_defaultValue = std::move(rhs.defaultValue); + m_defaultValue = std::move(rhs.m_defaultValue); return *this; } @@ -172,8 +172,8 @@ class ProtobufSubscriber : public Subscriber { } private: - wpi::mutex m_mutex; - wpi::ProtobufMessage m_msg; + mutable wpi::mutex m_mutex; + mutable wpi::ProtobufMessage m_msg; ValueType m_defaultValue; }; @@ -212,10 +212,9 @@ class ProtobufPublisher : public Publisher { ProtobufPublisher& operator=(ProtobufPublisher&& rhs) { Publisher::operator=(std::move(rhs)); m_msg = std::move(rhs.m_msg); - m_schemaPublished.clear(); - if (rhs.m_schemaPublished.test()) { - m_schemaPublished.test_and_set(); - } + m_schemaPublished.store( + rhs.m_schemaPublished.load(std::memory_order_relaxed), + std::memory_order_relaxed); return *this; } @@ -229,7 +228,7 @@ class ProtobufPublisher : public Publisher { wpi::SmallVector buf; { std::scoped_lock lock{m_mutex}; - if (!m_schemaPublished.test_and_set()) { + if (!m_schemaPublished.exchange(true, std::memory_order_relaxed)) { GetTopic().GetInstance().template AddProtobufSchema(m_msg); } m_msg.Pack(buf, value); @@ -248,7 +247,7 @@ class ProtobufPublisher : public Publisher { wpi::SmallVector buf; { std::scoped_lock lock{m_mutex}; - if (!m_schemaPublished.test_and_set()) { + if (!m_schemaPublished.exchange(true, std::memory_order_relaxed)) { GetTopic().GetInstance().template AddProtobufSchema(m_msg); } m_msg.Pack(buf, value); @@ -268,7 +267,7 @@ class ProtobufPublisher : public Publisher { private: wpi::mutex m_mutex; wpi::ProtobufMessage m_msg; - std::atomic_flag m_schemaPublished = ATOMIC_FLAG_INIT; + std::atomic_bool m_schemaPublished{false}; }; /** diff --git a/ntcore/src/main/native/include/networktables/StructArrayTopic.h b/ntcore/src/main/native/include/networktables/StructArrayTopic.h index e27b8e462dd..bb5bed2ef39 100644 --- a/ntcore/src/main/native/include/networktables/StructArrayTopic.h +++ b/ntcore/src/main/native/include/networktables/StructArrayTopic.h @@ -238,10 +238,9 @@ class StructArrayPublisher : public Publisher { StructArrayPublisher& operator=(StructArrayPublisher&& rhs) { Publisher::operator=(std::move(rhs)); m_buf = std::move(rhs.m_buf); - m_schemaPublished.clear(); - if (rhs.m_schemaPublished.test()) { - m_schemaPublished.test_and_set(); - } + m_schemaPublished.store( + rhs.m_schemaPublished.load(std::memory_order_relaxed), + std::memory_order_relaxed); return *this; } @@ -257,7 +256,7 @@ class StructArrayPublisher : public Publisher { std::convertible_to, T> #endif void Set(U&& value, int64_t time = 0) { - if (!m_schemaPublished.test_and_set()) { + if (!m_schemaPublished.exchange(true, std::memory_order_relaxed)) { GetTopic().GetInstance().template AddStructSchema(); } m_buf.Write(std::forward(value), @@ -288,7 +287,7 @@ class StructArrayPublisher : public Publisher { std::convertible_to, T> #endif void SetDefault(U&& value) { - if (!m_schemaPublished.test_and_set()) { + if (!m_schemaPublished.exchange(true, std::memory_order_relaxed)) { GetTopic().GetInstance().template AddStructSchema(); } m_buf.Write(std::forward(value), @@ -318,7 +317,7 @@ class StructArrayPublisher : public Publisher { private: wpi::StructArrayBuffer m_buf; - std::atomic_flag m_schemaPublished = ATOMIC_FLAG_INIT; + std::atomic_bool m_schemaPublished{false}; }; /** diff --git a/ntcore/src/main/native/include/networktables/StructTopic.h b/ntcore/src/main/native/include/networktables/StructTopic.h index 619f94fa5ab..d035d3b681a 100644 --- a/ntcore/src/main/native/include/networktables/StructTopic.h +++ b/ntcore/src/main/native/include/networktables/StructTopic.h @@ -184,10 +184,9 @@ class StructPublisher : public Publisher { StructPublisher& operator=(StructPublisher&& rhs) { Publisher::operator=(std::move(rhs)); - m_schemaPublished.clear(); - if (rhs.m_schemaPublished.test()) { - m_schemaPublished.test_and_set(); - } + m_schemaPublished.store( + rhs.m_schemaPublished.load(std::memory_order_relaxed), + std::memory_order_relaxed); return *this; } @@ -206,7 +205,7 @@ class StructPublisher : public Publisher { * @param time timestamp; 0 indicates current NT time should be used */ void Set(const T& value, int64_t time = 0) { - if (!m_schemaPublished.test_and_set()) { + if (!m_schemaPublished.exchange(true, std::memory_order_relaxed)) { GetTopic().GetInstance().template AddStructSchema(); } uint8_t buf[S::kSize]; @@ -222,7 +221,7 @@ class StructPublisher : public Publisher { * @param value value */ void SetDefault(const T& value) { - if (!m_schemaPublished.test_and_set()) { + if (!m_schemaPublished.exchange(true, std::memory_order_relaxed)) { GetTopic().GetInstance().template AddStructSchema(); } uint8_t buf[S::kSize]; @@ -240,7 +239,7 @@ class StructPublisher : public Publisher { } private: - std::atomic_flag m_schemaPublished = ATOMIC_FLAG_INIT; + std::atomic_bool m_schemaPublished{false}; }; /** diff --git a/ntcoreffi/src/main/native/cpp/DataLogManager.cpp b/ntcoreffi/src/main/native/cpp/DataLogManager.cpp index f81cd6e9650..d007f4f064c 100644 --- a/ntcoreffi/src/main/native/cpp/DataLogManager.cpp +++ b/ntcoreffi/src/main/native/cpp/DataLogManager.cpp @@ -229,8 +229,11 @@ static std::string MakeLogDir(std::string_view dir) { "DataLogManager: Logging to RoboRIO 1 internal storage is " "not recommended! Plug in a FAT32 formatted flash drive!"); } -#endif + fs::create_directory("/home/lvuser/logs", ec); + return "/home/lvuser/logs"; +#else return filesystem::GetOperatingDirectory(); +#endif } static std::string MakeLogFilename(std::string_view filenameOverride) { diff --git a/romiVendordep/CMakeLists.txt b/romiVendordep/CMakeLists.txt index bd5f413fc5b..0a5323b843c 100644 --- a/romiVendordep/CMakeLists.txt +++ b/romiVendordep/CMakeLists.txt @@ -10,18 +10,12 @@ if (WITH_JAVA) set(CMAKE_JAVA_COMPILE_FLAGS "-encoding" "UTF8" "-Xlint:unchecked") file(GLOB_RECURSE JAVA_SOURCES src/main/java/*.java) - add_jar(romiVendordep_jar ${JAVA_SOURCES} INCLUDE_JARS hal_jar ntcore_jar cscore_jar cameraserver_jar wpimath_jar wpiutil_jar wpilibj_jar OUTPUT_NAME romiVendordep) + add_jar(romiVendordep_jar ${JAVA_SOURCES} INCLUDE_JARS hal_jar ntcore_jar cscore_jar cameraserver_jar wpimath_jar wpiunits_jar wpiutil_jar wpilibj_jar OUTPUT_NAME romiVendordep) get_property(ROMIVENDORDEP_JAR_FILE TARGET romiVendordep_jar PROPERTY JAR_FILE) install(FILES ${ROMIVENDORDEP_JAR_FILE} DESTINATION "${java_lib_dest}") set_property(TARGET romiVendordep_jar PROPERTY FOLDER "java") - - if (WITH_FLAT_INSTALL) - set (romiVendordep_config_dir ${wpilib_dest}) - else() - set (romiVendordep_config_dir share/romiVendordep) - endif() endif() if (WITH_JAVA_SOURCE) @@ -54,15 +48,9 @@ target_include_directories(romiVendordep PUBLIC install(TARGETS romiVendordep EXPORT romiVendordep DESTINATION "${main_lib_dest}") install(DIRECTORY src/main/native/include/ DESTINATION "${include_dest}/romiVendordep") -if (FLAT_INSTALL_WPILIB) - set(romiVendordep_config_dir ${wpilib_dest}) - else() - set(romiVendordep_config_dir share/romiVendordep) - endif() - - configure_file(romiVendordep-config.cmake.in ${WPILIB_BINARY_DIR}/romiVendordep-config.cmake) - install(FILES ${WPILIB_BINARY_DIR}/romiVendordep-config.cmake DESTINATION ${romiVendordep_config_dir}) - install(EXPORT romiVendordep DESTINATION ${romiVendordep_config_dir}) +configure_file(romiVendordep-config.cmake.in ${WPILIB_BINARY_DIR}/romiVendordep-config.cmake) +install(FILES ${WPILIB_BINARY_DIR}/romiVendordep-config.cmake DESTINATION share/romiVendordep) +install(EXPORT romiVendordep DESTINATION share/romiVendordep) if (WITH_TESTS) wpilib_add_test(romiVendordep src/test/native/cpp) diff --git a/shared/java/javacommon.gradle b/shared/java/javacommon.gradle index 32078f8f108..f5813328cd7 100644 --- a/shared/java/javacommon.gradle +++ b/shared/java/javacommon.gradle @@ -117,9 +117,8 @@ tasks.withType(JavaCompile).configureEach { } dependencies { - testImplementation 'org.junit.jupiter:junit-jupiter-api:5.10.0' - testImplementation 'org.junit.jupiter:junit-jupiter-params:5.10.0' - testRuntimeOnly 'org.junit.jupiter:junit-jupiter-engine:5.10.0' + testImplementation 'org.junit.jupiter:junit-jupiter:5.10.1' + testRuntimeOnly 'org.junit.platform:junit-platform-launcher' devImplementation sourceSets.main.output } diff --git a/shared/jni/setupBuild.gradle b/shared/jni/setupBuild.gradle index 9f9334ffc02..de7c50f49ba 100644 --- a/shared/jni/setupBuild.gradle +++ b/shared/jni/setupBuild.gradle @@ -223,6 +223,9 @@ model { nativeUtils.useRequiredLibrary(it, 'ni_link_libraries', 'ni_runtime_libraries') } } + it.tasks.withType(CppCompile) { + it.dependsOn generateProto + } if (project.hasProperty('exeSplitSetup')) { exeSplitSetup(it) } @@ -257,7 +260,7 @@ model { include '**/*.cpp' } exportedHeaders { - srcDirs 'src/test/native/include', 'src/main/native/cpp' + srcDirs 'src/test/native/include', 'src/main/native/cpp', "$buildDir/generated/source/proto/main/cpp" if (project.hasProperty('generatedHeaders')) { srcDir generatedHeaders } @@ -276,6 +279,9 @@ model { nativeUtils.useRequiredLibrary(it, 'ni_link_libraries', 'ni_runtime_libraries') } } + it.tasks.withType(CppCompile) { + it.dependsOn generateProto + } if (project.hasProperty('exeSplitSetup')) { exeSplitSetup(it) } diff --git a/upstream_utils/fmt_patches/0001-Don-t-throw-on-write-failure.patch b/upstream_utils/fmt_patches/0001-Don-t-throw-on-write-failure.patch index 5b05dbf66fc..5670adbd237 100644 --- a/upstream_utils/fmt_patches/0001-Don-t-throw-on-write-failure.patch +++ b/upstream_utils/fmt_patches/0001-Don-t-throw-on-write-failure.patch @@ -1,7 +1,7 @@ From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Wed, 18 May 2022 10:21:49 -0700 -Subject: [PATCH 1/2] Don't throw on write failure +Subject: [PATCH 1/3] Don't throw on write failure --- include/fmt/format-inl.h | 4 +--- diff --git a/upstream_utils/fmt_patches/0002-Suppress-warnings-we-can-t-fix.patch b/upstream_utils/fmt_patches/0002-Suppress-warnings-we-can-t-fix.patch index a3866e22990..abc08e6dfe8 100644 --- a/upstream_utils/fmt_patches/0002-Suppress-warnings-we-can-t-fix.patch +++ b/upstream_utils/fmt_patches/0002-Suppress-warnings-we-can-t-fix.patch @@ -1,14 +1,14 @@ From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Tue, 16 May 2023 13:49:18 -0700 -Subject: [PATCH 2/2] Suppress warnings we can't fix +Subject: [PATCH 2/3] Suppress warnings we can't fix --- include/fmt/format.h | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/include/fmt/format.h b/include/fmt/format.h -index e5bd8b110efe49e12a12b004ea246a4dba671a6f..f11be0d6d58f3d992d7d06adb3d9576f81ecfe11 100644 +index 87a34b972ce6af4e2209e4d6cf78e8401e8f0037..ac0f52def2f3e2bc88d11903f5532efd89af454d 100644 --- a/include/fmt/format.h +++ b/include/fmt/format.h @@ -1324,7 +1324,14 @@ inline auto equal2(const char* lhs, const char* rhs) -> bool { diff --git a/upstream_utils/fmt_patches/0003-Remove-this-from-decltype.patch b/upstream_utils/fmt_patches/0003-Remove-this-from-decltype.patch new file mode 100644 index 00000000000..b1ed6bebd7e --- /dev/null +++ b/upstream_utils/fmt_patches/0003-Remove-this-from-decltype.patch @@ -0,0 +1,39 @@ +From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001 +From: Peter Johnson +Date: Mon, 20 Nov 2023 23:29:02 -0800 +Subject: [PATCH 3/3] Remove this from decltype + +This doesn't work on the most recent version of MSVC. +--- + include/fmt/core.h | 6 +++--- + 1 file changed, 3 insertions(+), 3 deletions(-) + +diff --git a/include/fmt/core.h b/include/fmt/core.h +index 1fe13888a00263a085032272482475e5dafbef26..915d8952d5f6d5255f5a1aab956ef56245b80dc4 100644 +--- a/include/fmt/core.h ++++ b/include/fmt/core.h +@@ -1435,7 +1435,7 @@ template struct arg_mapper { + // Only map owning types because mapping views can be unsafe. + template , + FMT_ENABLE_IF(std::is_arithmetic::value)> +- FMT_CONSTEXPR FMT_INLINE auto map(const T& val) -> decltype(this->map(U())) { ++ FMT_CONSTEXPR FMT_INLINE auto map(const T& val) -> decltype(map(U())) { + return map(format_as(val)); + } + +@@ -1459,13 +1459,13 @@ template struct arg_mapper { + !is_string::value && !is_char::value && + !is_named_arg::value && + !std::is_arithmetic>::value)> +- FMT_CONSTEXPR FMT_INLINE auto map(T& val) -> decltype(this->do_map(val)) { ++ FMT_CONSTEXPR FMT_INLINE auto map(T& val) -> decltype(do_map(val)) { + return do_map(val); + } + + template ::value)> + FMT_CONSTEXPR FMT_INLINE auto map(const T& named_arg) +- -> decltype(this->map(named_arg.value)) { ++ -> decltype(map(named_arg.value)) { + return map(named_arg.value); + } + diff --git a/upstream_utils/libuv_patches/0001-Revert-win-process-write-minidumps-when-sending-SIGQ.patch b/upstream_utils/libuv_patches/0001-Revert-win-process-write-minidumps-when-sending-SIGQ.patch index 890b8c37d9b..e09186acd10 100644 --- a/upstream_utils/libuv_patches/0001-Revert-win-process-write-minidumps-when-sending-SIGQ.patch +++ b/upstream_utils/libuv_patches/0001-Revert-win-process-write-minidumps-when-sending-SIGQ.patch @@ -6,37 +6,38 @@ Subject: [PATCH 01/10] Revert "win,process: write minidumps when sending This reverts commit 748d894e82abcdfff7429cf745003e182c47f163. --- - CMakeLists.txt | 5 +- + CMakeLists.txt | 6 +-- configure.ac | 2 +- include/uv/win.h | 1 - src/win/process.c | 116 ---------------------------------------------- - 4 files changed, 2 insertions(+), 122 deletions(-) + 4 files changed, 2 insertions(+), 123 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt -index 93733dd04783436cc1f1a801133e67e315f4af8d..0958dfb1bd93311cd0e20506311e1e41774c5fa4 100644 +index 72377851b69f74c5285fd34ae206ad6bed3745c1..3ec6bd00542f5aacfc6245b1f82e365eb1cff02c 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt -@@ -183,10 +183,7 @@ if(WIN32) +@@ -183,11 +183,7 @@ if(WIN32) advapi32 iphlpapi userenv - ws2_32 - dbghelp - ole32 -- uuid) +- uuid +- shell32) + ws2_32) list(APPEND uv_sources src/win/async.c src/win/core.c diff --git a/configure.ac b/configure.ac -index deb083605de639e896df83882715ddca25340fa3..76177a4bc8e5f17bc1e062af3a9028d2dfc76dc9 100644 +index 0a1042ce3d384f6a4392a100275c14cb31ba2816..6c87c36039446e04f6c30c23b8fdb9b957dd610d 100644 --- a/configure.ac +++ b/configure.ac @@ -74,7 +74,7 @@ AM_CONDITIONAL([OS400], [AS_CASE([$host_os],[os400], [true], [false]) AM_CONDITIONAL([SUNOS], [AS_CASE([$host_os],[solaris*], [true], [false])]) AM_CONDITIONAL([WINNT], [AS_CASE([$host_os],[mingw*], [true], [false])]) AS_CASE([$host_os],[mingw*], [ -- LIBS="$LIBS -lws2_32 -lpsapi -liphlpapi -lshell32 -luserenv -luser32 -ldbghelp -lole32 -luuid" +- LIBS="$LIBS -lws2_32 -lpsapi -liphlpapi -lshell32 -luserenv -luser32 -ldbghelp -lole32 -luuid -lshell32" + LIBS="$LIBS -lws2_32 -lpsapi -liphlpapi -lshell32 -luserenv -luser32" ]) AS_CASE([$host_os], [solaris2.10], [ @@ -54,7 +55,7 @@ index 6f8c47298e407bcb0151cf383a8370b71074f03e..eb74776978340a4910194bae35a9da64 #define SIGWINCH 28 diff --git a/src/win/process.c b/src/win/process.c -index 3e451e2291d6ed200ec258e874becbbea22bbc27..ed44adc67c6d52785a199206d9ba0357e2d0b045 100644 +index 43059858f3112e7e7185796525697629b72988df..119b46cb3f37122395c172c6e9700d472a2173ed 100644 --- a/src/win/process.c +++ b/src/win/process.c @@ -32,9 +32,6 @@ @@ -67,7 +68,7 @@ index 3e451e2291d6ed200ec258e874becbbea22bbc27..ed44adc67c6d52785a199206d9ba0357 #define SIGKILL 9 -@@ -1197,120 +1194,7 @@ static int uv__kill(HANDLE process_handle, int signum) { +@@ -1173,120 +1170,7 @@ static int uv__kill(HANDLE process_handle, int signum) { return UV_EINVAL; } diff --git a/upstream_utils/libuv_patches/0002-Fix-missing-casts.patch b/upstream_utils/libuv_patches/0002-Fix-missing-casts.patch index 4d9603f8629..de3bebd093b 100644 --- a/upstream_utils/libuv_patches/0002-Fix-missing-casts.patch +++ b/upstream_utils/libuv_patches/0002-Fix-missing-casts.patch @@ -5,6 +5,7 @@ Subject: [PATCH 02/10] Fix missing casts --- src/fs-poll.c | 10 ++++---- + src/idna.c | 2 +- src/inet.c | 11 ++++---- src/strscpy.c | 2 +- src/thread-common.c | 2 +- @@ -30,19 +31,23 @@ Subject: [PATCH 02/10] Fix missing casts src/unix/proctitle.c | 2 +- src/unix/random-sysctl-linux.c | 2 +- src/unix/stream.c | 31 ++++++++++++----------- + src/unix/tcp.c | 2 +- src/unix/thread.c | 5 ++-- src/unix/udp.c | 8 +++--- src/uv-common.c | 16 ++++++------ src/win/core.c | 8 +++--- + src/win/dl.c | 2 +- src/win/fs-event.c | 4 +-- src/win/fs-fd-hash-inl.h | 2 +- - src/win/fs.c | 28 ++++++++++----------- - src/win/pipe.c | 12 ++++----- - src/win/process.c | 12 ++++----- + src/win/fs.c | 38 ++++++++++++++-------------- + src/win/getaddrinfo.c | 12 ++++----- + src/win/pipe.c | 8 +++--- + src/win/process.c | 22 ++++++++-------- src/win/tcp.c | 2 +- src/win/thread.c | 4 +-- - src/win/util.c | 27 ++++++++++---------- - 38 files changed, 183 insertions(+), 178 deletions(-) + src/win/tty.c | 6 ++--- + src/win/util.c | 35 +++++++++++++------------- + 43 files changed, 207 insertions(+), 202 deletions(-) diff --git a/src/fs-poll.c b/src/fs-poll.c index 1bac1c568e36cadd0b68451926c6f045f88342d2..5a39daed095502b2db34f23fcaf0ab04f31f96ff 100644 @@ -93,6 +98,19 @@ index 1bac1c568e36cadd0b68451926c6f045f88342d2..5a39daed095502b2db34f23fcaf0ab04 it != ctx; last = it, it = it->previous) { assert(last->previous != NULL); +diff --git a/src/idna.c b/src/idna.c +index 1c0a60cf3e3becc1badafa394e7c422af7f56833..0c952cf605a88136ed9035f9385f9b1080c30c28 100644 +--- a/src/idna.c ++++ b/src/idna.c +@@ -477,7 +477,7 @@ int uv_utf16_to_wtf8(const uint16_t* w_source_ptr, + return 0; + + if (*target_ptr == NULL) { +- target = uv__malloc(target_len + 1); ++ target = (char*)uv__malloc(target_len + 1); + if (target == NULL) { + return UV_ENOMEM; + } diff --git a/src/inet.c b/src/inet.c index cd77496846e90e8b8e61c63c10f498f153344fe5..dd94bea3886ca37945fcad7909d765e3700e3c21 100644 --- a/src/inet.c @@ -279,7 +297,7 @@ index 5288083ef04fd78d90c34071cc76281adbc310d8..9bd55dd764b845cf8ea441d525b4e136 if (display_name_key == NULL || *display_name_key == NULL) goto out; diff --git a/src/unix/darwin.c b/src/unix/darwin.c -index 90790d701c4327518d17230c5aa69b9a74112e73..9ee5cd8eb9d02fb8b71986c47fe8a686f0983847 100644 +index 5e764a65ee4c71efd61876c70b0e999420f24f61..dc93d236b6b7c6da62dc7aa66cb42ddc125575a2 100644 --- a/src/unix/darwin.c +++ b/src/unix/darwin.c @@ -217,7 +217,7 @@ int uv_cpu_info(uv_cpu_info_t** cpu_infos, int* count) { @@ -314,10 +332,10 @@ index 191bc8bc213ffddb15c5e04baa66e2a0a8d69a3d..1bd63886b823be6451ac013d94e29885 uv__free(*cpu_infos); return UV_ENOMEM; diff --git a/src/unix/fs.c b/src/unix/fs.c -index 6b051c124f2fd9b0f72b41d7d7ba9c715e9686e1..e25d02e54dbe93e4b9c22b0140108c99ae2cb4f7 100644 +index 891306daedcc6a9b493bdfefcdb7d43c7efbb622..bc00c90b07a32a823412b216df6f2d758dbc423b 100644 --- a/src/unix/fs.c +++ b/src/unix/fs.c -@@ -140,7 +140,7 @@ extern char *mkdtemp(char *template); /* See issue #740 on AIX < 7 */ +@@ -134,7 +134,7 @@ extern char *mkdtemp(char *template); /* See issue #740 on AIX < 7 */ size_t new_path_len; \ path_len = strlen(path) + 1; \ new_path_len = strlen(new_path) + 1; \ @@ -326,7 +344,7 @@ index 6b051c124f2fd9b0f72b41d7d7ba9c715e9686e1..e25d02e54dbe93e4b9c22b0140108c99 if (req->path == NULL) \ return UV_ENOMEM; \ req->new_path = req->path + path_len; \ -@@ -568,7 +568,7 @@ static ssize_t uv__fs_scandir(uv_fs_t* req) { +@@ -496,7 +496,7 @@ static ssize_t uv__fs_scandir(uv_fs_t* req) { static int uv__fs_opendir(uv_fs_t* req) { uv_dir_t* dir; @@ -335,7 +353,7 @@ index 6b051c124f2fd9b0f72b41d7d7ba9c715e9686e1..e25d02e54dbe93e4b9c22b0140108c99 if (dir == NULL) goto error; -@@ -592,7 +592,7 @@ static int uv__fs_readdir(uv_fs_t* req) { +@@ -520,7 +520,7 @@ static int uv__fs_readdir(uv_fs_t* req) { unsigned int dirent_idx; unsigned int i; @@ -344,7 +362,7 @@ index 6b051c124f2fd9b0f72b41d7d7ba9c715e9686e1..e25d02e54dbe93e4b9c22b0140108c99 dirent_idx = 0; while (dirent_idx < dir->nentries) { -@@ -634,7 +634,7 @@ error: +@@ -562,7 +562,7 @@ error: static int uv__fs_closedir(uv_fs_t* req) { uv_dir_t* dir; @@ -353,7 +371,7 @@ index 6b051c124f2fd9b0f72b41d7d7ba9c715e9686e1..e25d02e54dbe93e4b9c22b0140108c99 if (dir->dir != NULL) { closedir(dir->dir); -@@ -663,7 +663,7 @@ static int uv__fs_statfs(uv_fs_t* req) { +@@ -591,7 +591,7 @@ static int uv__fs_statfs(uv_fs_t* req) { #endif /* defined(__sun) */ return -1; @@ -362,7 +380,7 @@ index 6b051c124f2fd9b0f72b41d7d7ba9c715e9686e1..e25d02e54dbe93e4b9c22b0140108c99 if (stat_fs == NULL) { errno = ENOMEM; return -1; -@@ -727,7 +727,7 @@ static ssize_t uv__fs_readlink(uv_fs_t* req) { +@@ -655,7 +655,7 @@ static ssize_t uv__fs_readlink(uv_fs_t* req) { maxlen = uv__fs_pathmax_size(req->path); #endif @@ -371,7 +389,7 @@ index 6b051c124f2fd9b0f72b41d7d7ba9c715e9686e1..e25d02e54dbe93e4b9c22b0140108c99 if (buf == NULL) { errno = ENOMEM; -@@ -747,7 +747,7 @@ static ssize_t uv__fs_readlink(uv_fs_t* req) { +@@ -675,7 +675,7 @@ static ssize_t uv__fs_readlink(uv_fs_t* req) { /* Uncommon case: resize to make room for the trailing nul byte. */ if (len == maxlen) { @@ -380,7 +398,7 @@ index 6b051c124f2fd9b0f72b41d7d7ba9c715e9686e1..e25d02e54dbe93e4b9c22b0140108c99 if (buf == NULL) return -1; -@@ -770,7 +770,7 @@ static ssize_t uv__fs_realpath(uv_fs_t* req) { +@@ -698,7 +698,7 @@ static ssize_t uv__fs_realpath(uv_fs_t* req) { ssize_t len; len = uv__fs_pathmax_size(req->path); @@ -389,8 +407,8 @@ index 6b051c124f2fd9b0f72b41d7d7ba9c715e9686e1..e25d02e54dbe93e4b9c22b0140108c99 if (buf == NULL) { errno = ENOMEM; -@@ -1984,7 +1984,7 @@ int uv_fs_read(uv_loop_t* loop, uv_fs_t* req, - req->nbufs = nbufs; +@@ -1886,7 +1886,7 @@ int uv_fs_read(uv_loop_t* loop, uv_fs_t* req, + req->bufs = req->bufsml; if (nbufs > ARRAY_SIZE(req->bufsml)) - req->bufs = uv__malloc(nbufs * sizeof(*bufs)); @@ -398,7 +416,7 @@ index 6b051c124f2fd9b0f72b41d7d7ba9c715e9686e1..e25d02e54dbe93e4b9c22b0140108c99 if (req->bufs == NULL) return UV_ENOMEM; -@@ -2171,7 +2171,7 @@ int uv_fs_write(uv_loop_t* loop, +@@ -2071,7 +2071,7 @@ int uv_fs_write(uv_loop_t* loop, req->nbufs = nbufs; req->bufs = req->bufsml; if (nbufs > ARRAY_SIZE(req->bufsml)) @@ -585,10 +603,10 @@ index 837bba6e2fef7b834a8d104d263bef47eaed0950..5e0fa98d104428534e5264a1c6358e3f return UV_ENOMEM; } diff --git a/src/unix/kqueue.c b/src/unix/kqueue.c -index b78242d3be4e3cf6b7b998f56dc65213982d4bc7..28e55aae6c613576ede7024a5c73d746e134d865 100644 +index 94ace58680cf391707f68357d7927173cb1db08e..06fbdb24b4adc4adb781d32150d40836fa745531 100644 --- a/src/unix/kqueue.c +++ b/src/unix/kqueue.c -@@ -299,8 +299,8 @@ void uv__io_poll(uv_loop_t* loop, int timeout) { +@@ -303,8 +303,8 @@ void uv__io_poll(uv_loop_t* loop, int timeout) { nevents = 0; assert(loop->watchers != NULL); @@ -599,7 +617,7 @@ index b78242d3be4e3cf6b7b998f56dc65213982d4bc7..28e55aae6c613576ede7024a5c73d746 for (i = 0; i < nfds; i++) { ev = events + i; fd = ev->ident; -@@ -322,7 +322,7 @@ void uv__io_poll(uv_loop_t* loop, int timeout) { +@@ -326,7 +326,7 @@ void uv__io_poll(uv_loop_t* loop, int timeout) { /* Skip invalidated events, see uv__platform_invalidate_fd */ if (fd == -1) continue; @@ -609,10 +627,10 @@ index b78242d3be4e3cf6b7b998f56dc65213982d4bc7..28e55aae6c613576ede7024a5c73d746 if (w == NULL) { /* File descriptor that we've stopped watching, disarm it. */ diff --git a/src/unix/linux.c b/src/unix/linux.c -index 48b9c2c43e104079d3ccb5d830d1d79f891fb1a3..9173850bd158eaf9c41deca38f9ba84762a027a1 100644 +index 8eeb352e6238a9e9557ec4dfc71f192105135cd7..2b8e1d8fe593a181d049aa50ff9edaf6da258a24 100644 --- a/src/unix/linux.c +++ b/src/unix/linux.c -@@ -456,8 +456,8 @@ static void uv__iou_init(int epollfd, +@@ -518,8 +518,8 @@ static void uv__iou_init(int epollfd, char* sqe; int ringfd; @@ -623,7 +641,7 @@ index 48b9c2c43e104079d3ccb5d830d1d79f891fb1a3..9173850bd158eaf9c41deca38f9ba847 if (!uv__use_io_uring()) return; -@@ -496,14 +496,14 @@ static void uv__iou_init(int epollfd, +@@ -558,14 +558,14 @@ static void uv__iou_init(int epollfd, maxlen = sqlen < cqlen ? cqlen : sqlen; sqelen = params.sq_entries * sizeof(struct uv__io_uring_sqe); @@ -640,7 +658,7 @@ index 48b9c2c43e104079d3ccb5d830d1d79f891fb1a3..9173850bd158eaf9c41deca38f9ba847 sqelen, PROT_READ | PROT_WRITE, MAP_SHARED | MAP_POPULATE, -@@ -643,7 +643,7 @@ void uv__platform_invalidate_fd(uv_loop_t* loop, int fd) { +@@ -705,7 +705,7 @@ void uv__platform_invalidate_fd(uv_loop_t* loop, int fd) { int i; lfields = uv__get_internal_fields(loop); @@ -649,7 +667,7 @@ index 48b9c2c43e104079d3ccb5d830d1d79f891fb1a3..9173850bd158eaf9c41deca38f9ba847 /* Invalidate events with same file descriptor */ if (inv != NULL) -@@ -718,7 +718,7 @@ static struct uv__io_uring_sqe* uv__iou_get_sqe(struct uv__iou* iou, +@@ -780,7 +780,7 @@ static struct uv__io_uring_sqe* uv__iou_get_sqe(struct uv__iou* iou, return NULL; /* No room in ring buffer. TODO(bnoordhuis) maybe flush it? */ slot = tail & mask; @@ -658,7 +676,7 @@ index 48b9c2c43e104079d3ccb5d830d1d79f891fb1a3..9173850bd158eaf9c41deca38f9ba847 sqe = &sqe[slot]; memset(sqe, 0, sizeof(*sqe)); sqe->user_data = (uintptr_t) req; -@@ -986,7 +986,7 @@ int uv__iou_fs_statx(uv_loop_t* loop, +@@ -1057,7 +1057,7 @@ int uv__iou_fs_statx(uv_loop_t* loop, struct uv__statx* statxbuf; struct uv__iou* iou; @@ -667,7 +685,7 @@ index 48b9c2c43e104079d3ccb5d830d1d79f891fb1a3..9173850bd158eaf9c41deca38f9ba847 if (statxbuf == NULL) return 0; -@@ -1050,7 +1050,7 @@ static void uv__iou_fs_statx_post(uv_fs_t* req) { +@@ -1121,7 +1121,7 @@ static void uv__iou_fs_statx_post(uv_fs_t* req) { uv_stat_t* buf; buf = &req->statbuf; @@ -676,7 +694,7 @@ index 48b9c2c43e104079d3ccb5d830d1d79f891fb1a3..9173850bd158eaf9c41deca38f9ba847 req->ptr = NULL; if (req->result == 0) { -@@ -1079,7 +1079,7 @@ static void uv__poll_io_uring(uv_loop_t* loop, struct uv__iou* iou) { +@@ -1150,7 +1150,7 @@ static void uv__poll_io_uring(uv_loop_t* loop, struct uv__iou* iou) { tail = atomic_load_explicit((_Atomic uint32_t*) iou->cqtail, memory_order_acquire); mask = iou->cqmask; @@ -685,7 +703,7 @@ index 48b9c2c43e104079d3ccb5d830d1d79f891fb1a3..9173850bd158eaf9c41deca38f9ba847 nevents = 0; for (i = head; i != tail; i++) { -@@ -1170,7 +1170,7 @@ static void uv__epoll_ctl_prep(int epollfd, +@@ -1241,7 +1241,7 @@ static void uv__epoll_ctl_prep(int epollfd, pe = &(*events)[slot]; *pe = *e; @@ -694,7 +712,7 @@ index 48b9c2c43e104079d3ccb5d830d1d79f891fb1a3..9173850bd158eaf9c41deca38f9ba847 sqe = &sqe[slot]; memset(sqe, 0, sizeof(*sqe)); -@@ -1226,7 +1226,7 @@ static void uv__epoll_ctl_flush(int epollfd, +@@ -1297,7 +1297,7 @@ static void uv__epoll_ctl_flush(int epollfd, while (*ctl->cqhead != *ctl->cqtail) { slot = (*ctl->cqhead)++ & ctl->cqmask; @@ -703,7 +721,7 @@ index 48b9c2c43e104079d3ccb5d830d1d79f891fb1a3..9173850bd158eaf9c41deca38f9ba847 cqe = &cqe[slot]; if (cqe->res == 0) -@@ -1708,7 +1708,7 @@ int uv_cpu_info(uv_cpu_info_t** ci, int* count) { +@@ -1758,7 +1758,7 @@ int uv_cpu_info(uv_cpu_info_t** ci, int* count) { snprintf(*models, sizeof(*models), "unknown"); maxcpu = 0; @@ -712,7 +730,7 @@ index 48b9c2c43e104079d3ccb5d830d1d79f891fb1a3..9173850bd158eaf9c41deca38f9ba847 if (cpus == NULL) return UV_ENOMEM; -@@ -1764,9 +1764,9 @@ int uv_cpu_info(uv_cpu_info_t** ci, int* count) { +@@ -1816,9 +1816,9 @@ int uv_cpu_info(uv_cpu_info_t** ci, int* count) { /* arm64: translate CPU part code to model name. */ if (*parts) { @@ -724,7 +742,7 @@ index 48b9c2c43e104079d3ccb5d830d1d79f891fb1a3..9173850bd158eaf9c41deca38f9ba847 else p += n + 1; n = (int) strcspn(p, "\n"); -@@ -1815,7 +1815,7 @@ nocpuinfo: +@@ -1868,7 +1868,7 @@ nocpuinfo: } size = n * sizeof(**ci) + sizeof(models); @@ -733,7 +751,7 @@ index 48b9c2c43e104079d3ccb5d830d1d79f891fb1a3..9173850bd158eaf9c41deca38f9ba847 *count = 0; if (*ci == NULL) { -@@ -1824,7 +1824,7 @@ nocpuinfo: +@@ -1877,7 +1877,7 @@ nocpuinfo: } *count = n; @@ -742,7 +760,7 @@ index 48b9c2c43e104079d3ccb5d830d1d79f891fb1a3..9173850bd158eaf9c41deca38f9ba847 i = 0; for (cpu = 0; cpu < maxcpu; cpu++) { -@@ -1833,19 +1833,19 @@ nocpuinfo: +@@ -1886,19 +1886,19 @@ nocpuinfo: c = *cpus + cpu; @@ -766,7 +784,7 @@ index 48b9c2c43e104079d3ccb5d830d1d79f891fb1a3..9173850bd158eaf9c41deca38f9ba847 }; } -@@ -1902,7 +1902,7 @@ int uv_interface_addresses(uv_interface_address_t** addresses, int* count) { +@@ -1955,7 +1955,7 @@ int uv_interface_addresses(uv_interface_address_t** addresses, int* count) { } /* Make sure the memory is initiallized to zero using calloc() */ @@ -775,7 +793,7 @@ index 48b9c2c43e104079d3ccb5d830d1d79f891fb1a3..9173850bd158eaf9c41deca38f9ba847 if (!(*addresses)) { freeifaddrs(addrs); return UV_ENOMEM; -@@ -2470,12 +2470,12 @@ int uv_fs_event_start(uv_fs_event_t* handle, +@@ -2523,12 +2523,12 @@ int uv_fs_event_start(uv_fs_event_t* handle, goto no_insert; len = strlen(path) + 1; @@ -1058,8 +1076,21 @@ index 28c4d5463c4622725a433b8807e5e7bde580dadd..265ddade7aec129eb9dbf07cde2a16a0 for (i = 0; i < queued_fds->offset; i++) uv__close(queued_fds->fds[i]); uv__free(handle->queued_fds); +diff --git a/src/unix/tcp.c b/src/unix/tcp.c +index a6b53e5913271d0c83e1d7f7e4cb8140f5f3936d..4c4213a4241b51d245146f1a37a371448d57b3b8 100644 +--- a/src/unix/tcp.c ++++ b/src/unix/tcp.c +@@ -274,7 +274,7 @@ int uv__tcp_connect(uv_connect_t* req, + memcpy(&tmp6, addr, sizeof(tmp6)); + if (tmp6.sin6_scope_id == 0) { + tmp6.sin6_scope_id = uv__ipv6_link_local_scope_id(); +- addr = (void*) &tmp6; ++ addr = (const struct sockaddr*) &tmp6; + } + } + diff --git a/src/unix/thread.c b/src/unix/thread.c -index 4d6f4b8232ec6dc0bd27258a1315340e3e272ae9..531c6211bb4321e5f11031a0644b4e3ab9174396 100644 +index f05e6fe0f7dd5ac579f6a9d6f93bffb99e1bcbc2..20409541de3cb300504b823472a73bc95fa38f62 100644 --- a/src/unix/thread.c +++ b/src/unix/thread.c @@ -168,8 +168,7 @@ int uv_thread_create_ex(uv_thread_t* tid, @@ -1118,7 +1149,7 @@ index c2814512a5f507ceb9e764cdb7c6ff3d36e77974..cbee16b22a36b1c82e74f6a81c381105 if (req->bufs == NULL) { uv__req_unregister(handle->loop, req); diff --git a/src/uv-common.c b/src/uv-common.c -index 916f3f4e00664ab47f94d2a6ee7c6a9ef0461389..c04f93596ab1f730576256d86e216ccb7f258b72 100644 +index 2200fe3f0a41e295acb426f39ccc9f0133994675..69e95801a18104ea910abf86db236d85f62afb66 100644 --- a/src/uv-common.c +++ b/src/uv-common.c @@ -54,10 +54,10 @@ static uv__allocator_t uv__allocator = { @@ -1148,7 +1179,7 @@ index 916f3f4e00664ab47f94d2a6ee7c6a9ef0461389..c04f93596ab1f730576256d86e216ccb } void* uv__malloc(size_t size) { -@@ -688,7 +688,7 @@ void uv__fs_scandir_cleanup(uv_fs_t* req) { +@@ -691,7 +691,7 @@ void uv__fs_scandir_cleanup(uv_fs_t* req) { unsigned int n; if (req->result >= 0) { @@ -1157,7 +1188,7 @@ index 916f3f4e00664ab47f94d2a6ee7c6a9ef0461389..c04f93596ab1f730576256d86e216ccb nbufs = uv__get_nbufs(req); i = 0; -@@ -721,7 +721,7 @@ int uv_fs_scandir_next(uv_fs_t* req, uv_dirent_t* ent) { +@@ -724,7 +724,7 @@ int uv_fs_scandir_next(uv_fs_t* req, uv_dirent_t* ent) { nbufs = uv__get_nbufs(req); assert(nbufs); @@ -1166,7 +1197,7 @@ index 916f3f4e00664ab47f94d2a6ee7c6a9ef0461389..c04f93596ab1f730576256d86e216ccb /* Free previous entity */ if (*nbufs > 0) -@@ -786,7 +786,7 @@ void uv__fs_readdir_cleanup(uv_fs_t* req) { +@@ -789,7 +789,7 @@ void uv__fs_readdir_cleanup(uv_fs_t* req) { if (req->ptr == NULL) return; @@ -1175,7 +1206,7 @@ index 916f3f4e00664ab47f94d2a6ee7c6a9ef0461389..c04f93596ab1f730576256d86e216ccb dirents = dir->dirents; req->ptr = NULL; -@@ -832,7 +832,7 @@ uv_loop_t* uv_default_loop(void) { +@@ -835,7 +835,7 @@ uv_loop_t* uv_default_loop(void) { uv_loop_t* uv_loop_new(void) { uv_loop_t* loop; @@ -1217,8 +1248,21 @@ index e9885a0f1ff3890a8d957c8793e22b01cedc0e97..87ade7ad65243ee3ff940320f84e1960 if (timer_heap == NULL) { err = UV_ENOMEM; goto fail_timers_alloc; +diff --git a/src/win/dl.c b/src/win/dl.c +index 7880c9595be1f66ea0dcbdbcc4a91ce40577587f..d88400f0e819d74998e13f60f13c67a606dec398 100644 +--- a/src/win/dl.c ++++ b/src/win/dl.c +@@ -37,7 +37,7 @@ int uv_dlopen(const char* filename, uv_lib_t* lib) { + return uv__dlerror(lib, filename, ERROR_NO_UNICODE_TRANSLATION); + if ((size_t) r > ARRAY_SIZE(filename_w)) + return uv__dlerror(lib, filename, ERROR_INSUFFICIENT_BUFFER); +- uv_wtf8_to_utf16(filename, filename_w, r); ++ uv_wtf8_to_utf16(filename, (uint16_t*)filename_w, r); + + lib->handle = LoadLibraryExW(filename_w, NULL, LOAD_WITH_ALTERED_SEARCH_PATH); + if (lib->handle == NULL) { diff --git a/src/win/fs-event.c b/src/win/fs-event.c -index 6758c7c78bc1d6a5316a8ae7dc2d1e23cd0f32bc..150467313d576bfe2966b55f3d8cffa23cbb8ea3 100644 +index 4a0ca1f70a22b6342e208124838d6ecf3173f1a8..5a07acfe54efe90cf2ab0bca5b5998a961e72ebd 100644 --- a/src/win/fs-event.c +++ b/src/win/fs-event.c @@ -73,7 +73,7 @@ static void uv__relative_path(const WCHAR* filename, @@ -1230,7 +1274,7 @@ index 6758c7c78bc1d6a5316a8ae7dc2d1e23cd0f32bc..150467313d576bfe2966b55f3d8cffa2 if (!*relpath) uv_fatal_error(ERROR_OUTOFMEMORY, "uv__malloc"); wcsncpy(*relpath, filename + dirlen + 1, relpathlen); -@@ -242,7 +242,7 @@ int uv_fs_event_start(uv_fs_event_t* handle, +@@ -229,7 +229,7 @@ int uv_fs_event_start(uv_fs_event_t* handle, if (short_path_buffer_len == 0) { goto short_path_done; } @@ -1253,28 +1297,53 @@ index 0b532af12d4371c2311bd50a66913287a0716f43..703a8d8f87de1089ac8b18bd817d416d uv_fatal_error(ERROR_OUTOFMEMORY, "uv__malloc"); } diff --git a/src/win/fs.c b/src/win/fs.c -index fc209c54f470edaa031009979061cff071cbf66d..328c8f2e0513562b53c948ffea59d0841e14b264 100644 +index 99c8a2bf8bf1244f0db7114c1eaa8bff22564996..d9c2a4f728c7fb491995c6153b2a63a835b206b7 100644 --- a/src/win/fs.c +++ b/src/win/fs.c -@@ -258,7 +258,7 @@ INLINE static int fs__capture_path(uv_fs_t* req, const char* path, +@@ -280,7 +280,7 @@ INLINE static int fs__readlink_handle(HANDLE handle, + } + + assert(target_ptr == NULL || *target_ptr == NULL); +- return uv_utf16_to_wtf8(w_target, w_target_len, target_ptr, target_len_ptr); ++ return uv_utf16_to_wtf8((const uint16_t*)w_target, w_target_len, target_ptr, target_len_ptr); + } + + +@@ -323,7 +323,7 @@ INLINE static int fs__capture_path(uv_fs_t* req, const char* path, return 0; } - buf = uv__malloc(buf_sz); -+ buf = (WCHAR *)uv__malloc(buf_sz); ++ buf = (WCHAR*)uv__malloc(buf_sz); if (buf == NULL) { return ERROR_OUTOFMEMORY; } -@@ -376,7 +376,7 @@ static int fs__wide_to_wtf8(WCHAR* w_source_ptr, - return 0; +@@ -331,7 +331,7 @@ INLINE static int fs__capture_path(uv_fs_t* req, const char* path, + pos = buf; + + if (path != NULL) { +- uv_wtf8_to_utf16(path, pos, pathw_len); ++ uv_wtf8_to_utf16(path, (uint16_t*)pos, pathw_len); + req->file.pathw = pos; + pos += pathw_len; + } else { +@@ -339,7 +339,7 @@ INLINE static int fs__capture_path(uv_fs_t* req, const char* path, + } - if (*target_ptr == NULL) { -- target = uv__malloc(target_len + 1); -+ target = (char *)uv__malloc(target_len + 1); - if (target == NULL) { - SetLastError(ERROR_OUTOFMEMORY); - return -1; -@@ -1575,7 +1575,7 @@ void fs__scandir(uv_fs_t* req) { + if (new_path != NULL) { +- uv_wtf8_to_utf16(new_path, pos, new_pathw_len); ++ uv_wtf8_to_utf16(new_path, (uint16_t*)pos, new_pathw_len); + req->fs.info.new_pathw = pos; + pos += new_pathw_len; + } else { +@@ -1390,13 +1390,13 @@ void fs__scandir(uv_fs_t* req) { + continue; + + /* Compute the space required to store the filename as WTF-8. */ +- wtf8_len = uv_utf16_length_as_wtf8(&info->FileName[0], wchar_len); ++ wtf8_len = uv_utf16_length_as_wtf8((const uint16_t*)&info->FileName[0], wchar_len); + + /* Resize the dirent array if needed. */ if (dirents_used >= dirents_size) { size_t new_dirents_size = dirents_size == 0 ? dirents_initial_size : dirents_size << 1; @@ -1283,7 +1352,7 @@ index fc209c54f470edaa031009979061cff071cbf66d..328c8f2e0513562b53c948ffea59d084 uv__realloc(dirents, new_dirents_size * sizeof *dirents); if (new_dirents == NULL) -@@ -1589,7 +1589,7 @@ void fs__scandir(uv_fs_t* req) { +@@ -1410,7 +1410,7 @@ void fs__scandir(uv_fs_t* req) { * includes room for the first character of the filename, but `utf8_len` * doesn't count the NULL terminator at this point. */ @@ -1292,7 +1361,16 @@ index fc209c54f470edaa031009979061cff071cbf66d..328c8f2e0513562b53c948ffea59d084 if (dirent == NULL) goto out_of_memory_error; -@@ -1691,7 +1691,7 @@ void fs__opendir(uv_fs_t* req) { +@@ -1418,7 +1418,7 @@ void fs__scandir(uv_fs_t* req) { + + /* Convert file name to UTF-8. */ + wtf8 = &dirent->d_name[0]; +- if (uv_utf16_to_wtf8(&info->FileName[0], wchar_len, &wtf8, &wtf8_len) != 0) ++ if (uv_utf16_to_wtf8((const uint16_t*)&info->FileName[0], wchar_len, &wtf8, &wtf8_len) != 0) + goto out_of_memory_error; + + /* Fill out the type field. */ +@@ -1512,7 +1512,7 @@ void fs__opendir(uv_fs_t* req) { goto error; } @@ -1301,7 +1379,7 @@ index fc209c54f470edaa031009979061cff071cbf66d..328c8f2e0513562b53c948ffea59d084 if (dir == NULL) { SET_REQ_UV_ERROR(req, UV_ENOMEM, ERROR_OUTOFMEMORY); goto error; -@@ -1706,7 +1706,7 @@ void fs__opendir(uv_fs_t* req) { +@@ -1527,7 +1527,7 @@ void fs__opendir(uv_fs_t* req) { else fmt = L"%s\\*"; @@ -1310,7 +1388,7 @@ index fc209c54f470edaa031009979061cff071cbf66d..328c8f2e0513562b53c948ffea59d084 if (find_path == NULL) { SET_REQ_UV_ERROR(req, UV_ENOMEM, ERROR_OUTOFMEMORY); goto error; -@@ -1743,7 +1743,7 @@ void fs__readdir(uv_fs_t* req) { +@@ -1564,7 +1564,7 @@ void fs__readdir(uv_fs_t* req) { int r; req->flags |= UV_FS_FREE_PTR; @@ -1319,7 +1397,7 @@ index fc209c54f470edaa031009979061cff071cbf66d..328c8f2e0513562b53c948ffea59d084 dirents = dir->dirents; memset(dirents, 0, dir->nentries * sizeof(*dir->dirents)); find_data = &dir->find_data; -@@ -1800,7 +1800,7 @@ error: +@@ -1621,7 +1621,7 @@ error: void fs__closedir(uv_fs_t* req) { uv_dir_t* dir; @@ -1328,7 +1406,7 @@ index fc209c54f470edaa031009979061cff071cbf66d..328c8f2e0513562b53c948ffea59d084 FindClose(dir->dir_handle); uv__free(req->ptr); SET_REQ_RESULT(req, 0); -@@ -2791,7 +2791,7 @@ static ssize_t fs__realpath_handle(HANDLE handle, char** realpath_ptr) { +@@ -2612,7 +2612,7 @@ static ssize_t fs__realpath_handle(HANDLE handle, char** realpath_ptr) { return -1; } @@ -1337,7 +1415,16 @@ index fc209c54f470edaa031009979061cff071cbf66d..328c8f2e0513562b53c948ffea59d084 if (w_realpath_buf == NULL) { SetLastError(ERROR_OUTOFMEMORY); return -1; -@@ -2904,7 +2904,7 @@ retry_get_disk_free_space: +@@ -2645,7 +2645,7 @@ static ssize_t fs__realpath_handle(HANDLE handle, char** realpath_ptr) { + } + + assert(*realpath_ptr == NULL); +- r = uv_utf16_to_wtf8(w_realpath_ptr, w_realpath_len, realpath_ptr, NULL); ++ r = uv_utf16_to_wtf8((const uint16_t*)w_realpath_ptr, w_realpath_len, realpath_ptr, NULL); + uv__free(w_realpath_buf); + return r; + } +@@ -2725,7 +2725,7 @@ retry_get_disk_free_space: } len = MAX_PATH + 1; @@ -1346,7 +1433,7 @@ index fc209c54f470edaa031009979061cff071cbf66d..328c8f2e0513562b53c948ffea59d084 if (pathw == NULL) { SET_REQ_UV_ERROR(req, UV_ENOMEM, ERROR_OUTOFMEMORY); return; -@@ -2920,7 +2920,7 @@ retry_get_full_path_name: +@@ -2741,7 +2741,7 @@ retry_get_full_path_name: return; } else if (ret > len) { len = ret; @@ -1355,7 +1442,7 @@ index fc209c54f470edaa031009979061cff071cbf66d..328c8f2e0513562b53c948ffea59d084 if (pathw == NULL) { SET_REQ_UV_ERROR(req, UV_ENOMEM, ERROR_OUTOFMEMORY); return; -@@ -2936,7 +2936,7 @@ retry_get_full_path_name: +@@ -2757,7 +2757,7 @@ retry_get_full_path_name: uv__free(pathw); } @@ -1364,7 +1451,7 @@ index fc209c54f470edaa031009979061cff071cbf66d..328c8f2e0513562b53c948ffea59d084 if (stat_fs == NULL) { SET_REQ_UV_ERROR(req, UV_ENOMEM, ERROR_OUTOFMEMORY); return; -@@ -3095,7 +3095,7 @@ int uv_fs_read(uv_loop_t* loop, +@@ -2916,7 +2916,7 @@ int uv_fs_read(uv_loop_t* loop, req->fs.info.nbufs = nbufs; req->fs.info.bufs = req->fs.info.bufsml; if (nbufs > ARRAY_SIZE(req->fs.info.bufsml)) @@ -1373,7 +1460,7 @@ index fc209c54f470edaa031009979061cff071cbf66d..328c8f2e0513562b53c948ffea59d084 if (req->fs.info.bufs == NULL) { SET_REQ_UV_ERROR(req, UV_ENOMEM, ERROR_OUTOFMEMORY); -@@ -3128,7 +3128,7 @@ int uv_fs_write(uv_loop_t* loop, +@@ -2949,7 +2949,7 @@ int uv_fs_write(uv_loop_t* loop, req->fs.info.nbufs = nbufs; req->fs.info.bufs = req->fs.info.bufsml; if (nbufs > ARRAY_SIZE(req->fs.info.bufsml)) @@ -1382,38 +1469,64 @@ index fc209c54f470edaa031009979061cff071cbf66d..328c8f2e0513562b53c948ffea59d084 if (req->fs.info.bufs == NULL) { SET_REQ_UV_ERROR(req, UV_ENOMEM, ERROR_OUTOFMEMORY); +diff --git a/src/win/getaddrinfo.c b/src/win/getaddrinfo.c +index 8b8406ada8e7434e291b2e50caf4ed7f36613fa0..5bc63d8e19411b967a2acf5c24b34b9b17323ccc 100644 +--- a/src/win/getaddrinfo.c ++++ b/src/win/getaddrinfo.c +@@ -132,7 +132,7 @@ static void uv__getaddrinfo_done(struct uv__work* w, int status) { + addrinfo_len += addrinfo_struct_len + + ALIGNED_SIZE(addrinfow_ptr->ai_addrlen); + if (addrinfow_ptr->ai_canonname != NULL) { +- name_len = uv_utf16_length_as_wtf8(addrinfow_ptr->ai_canonname, -1); ++ name_len = uv_utf16_length_as_wtf8((const uint16_t*)addrinfow_ptr->ai_canonname, -1); + if (name_len < 0) { + req->retcode = name_len; + goto complete; +@@ -281,9 +281,9 @@ int uv_getaddrinfo(uv_loop_t* loop, + } + + /* allocate memory for inputs, and partition it as needed */ +- alloc_ptr = uv__malloc(ALIGNED_SIZE(nodesize * sizeof(WCHAR)) + +- ALIGNED_SIZE(servicesize * sizeof(WCHAR)) + +- hintssize); ++ alloc_ptr = (char*)uv__malloc(ALIGNED_SIZE(nodesize * sizeof(WCHAR)) + ++ ALIGNED_SIZE(servicesize * sizeof(WCHAR)) + ++ hintssize); + if (!alloc_ptr) + return UV_ENOMEM; + +@@ -294,7 +294,7 @@ int uv_getaddrinfo(uv_loop_t* loop, + * request. The node here has been converted to ascii. */ + if (node != NULL) { + req->node = (WCHAR*) alloc_ptr; +- uv_wtf8_to_utf16(node, (WCHAR*) alloc_ptr, nodesize); ++ uv_wtf8_to_utf16(node, (uint16_t*) alloc_ptr, nodesize); + alloc_ptr += ALIGNED_SIZE(nodesize * sizeof(WCHAR)); + } else { + req->node = NULL; +@@ -304,7 +304,7 @@ int uv_getaddrinfo(uv_loop_t* loop, + * the req. */ + if (service != NULL) { + req->service = (WCHAR*) alloc_ptr; +- uv_wtf8_to_utf16(service, (WCHAR*) alloc_ptr, servicesize); ++ uv_wtf8_to_utf16(service, (uint16_t*) alloc_ptr, servicesize); + alloc_ptr += ALIGNED_SIZE(servicesize * sizeof(WCHAR)); + } else { + req->service = NULL; diff --git a/src/win/pipe.c b/src/win/pipe.c -index f0cac3822564e14052feb5e1934f54c57c78160d..c1739efe82b8755999145860b4da6b50c73518a2 100644 +index cec72ff750b5a17e139fd85080ccbdfc3c71d8c0..0f045a873073cf9b07feb457ea199990df521e5f 100644 --- a/src/win/pipe.c +++ b/src/win/pipe.c -@@ -756,7 +756,7 @@ int uv_pipe_bind2(uv_pipe_t* handle, - - /* Convert name to UTF16. */ - nameSize = MultiByteToWideChar(CP_UTF8, 0, name, -1, NULL, 0) * sizeof(WCHAR); -- handle->name = uv__malloc(nameSize); -+ handle->name = (WCHAR*)uv__malloc(nameSize); - if (!handle->name) { - uv_fatal_error(ERROR_OUTOFMEMORY, "uv__malloc"); - } -@@ -906,7 +906,7 @@ int uv_pipe_connect2(uv_connect_t* req, - - /* Convert name to UTF16. */ - nameSize = MultiByteToWideChar(CP_UTF8, 0, name, -1, NULL, 0) * sizeof(WCHAR); -- handle->name = uv__malloc(nameSize); -+ handle->name = (WCHAR*)uv__malloc(nameSize); - if (!handle->name) { - uv_fatal_error(ERROR_OUTOFMEMORY, "uv__malloc"); - } -@@ -924,7 +924,7 @@ int uv_pipe_connect2(uv_connect_t* req, - pipeHandle = open_named_pipe(handle->name, &duplex_flags); +@@ -899,7 +899,7 @@ int uv_pipe_connect2(uv_connect_t* req, if (pipeHandle == INVALID_HANDLE_VALUE) { if (GetLastError() == ERROR_PIPE_BUSY) { + nameSize = (wcslen(handle->name) + 1) * sizeof(WCHAR); - req->u.connect.name = uv__malloc(nameSize); + req->u.connect.name = (WCHAR *)uv__malloc(nameSize); if (!req->u.connect.name) { uv_fatal_error(ERROR_OUTOFMEMORY, "uv__malloc"); } -@@ -1528,7 +1528,7 @@ static int uv__build_coalesced_write_req(uv_write_t* user_req, +@@ -1503,7 +1503,7 @@ static int uv__build_coalesced_write_req(uv_write_t* user_req, data_length; /* (c) */ /* Allocate buffer. */ @@ -1422,7 +1535,7 @@ index f0cac3822564e14052feb5e1934f54c57c78160d..c1739efe82b8755999145860b4da6b50 if (heap_buffer == NULL) return ERROR_NOT_ENOUGH_MEMORY; /* Maps to UV_ENOMEM. */ -@@ -1777,7 +1777,7 @@ int uv__pipe_write_ipc(uv_loop_t* loop, +@@ -1752,7 +1752,7 @@ int uv__pipe_write_ipc(uv_loop_t* loop, bufs = stack_bufs; } else { /* Use heap-allocated buffer array. */ @@ -1431,7 +1544,7 @@ index f0cac3822564e14052feb5e1934f54c57c78160d..c1739efe82b8755999145860b4da6b50 if (bufs == NULL) return ERROR_NOT_ENOUGH_MEMORY; /* Maps to UV_ENOMEM. */ } -@@ -2514,7 +2514,7 @@ static int uv__pipe_getname(const uv_pipe_t* handle, char* buffer, size_t* size) +@@ -2449,7 +2449,7 @@ static int uv__pipe_getname(const uv_pipe_t* handle, char* buffer, size_t* size) FileNameInformation); if (nt_status == STATUS_BUFFER_OVERFLOW) { name_size = sizeof(*name_info) + tmp_name_info.FileNameLength; @@ -1439,12 +1552,38 @@ index f0cac3822564e14052feb5e1934f54c57c78160d..c1739efe82b8755999145860b4da6b50 + name_info = (FILE_NAME_INFORMATION*)uv__malloc(name_size); if (!name_info) { *size = 0; - err = UV_ENOMEM; + return UV_ENOMEM; diff --git a/src/win/process.c b/src/win/process.c -index ed44adc67c6d52785a199206d9ba0357e2d0b045..b383e8b405db56d413985b38df216d09c58ec4a0 100644 +index 119b46cb3f37122395c172c6e9700d472a2173ed..0103004c565bb09b7b9c150e874f9c68486ab26c 100644 --- a/src/win/process.c +++ b/src/win/process.c -@@ -615,8 +615,8 @@ error: +@@ -543,14 +543,14 @@ int make_program_args(char** args, int verbatim_arguments, WCHAR** dst_ptr) { + dst_len = dst_len * 2 + arg_count * 2; + + /* Allocate buffer for the final command line. */ +- dst = uv__malloc(dst_len * sizeof(WCHAR)); ++ dst = (WCHAR*)uv__malloc(dst_len * sizeof(WCHAR)); + if (dst == NULL) { + err = UV_ENOMEM; + goto error; + } + + /* Allocate temporary working buffer. */ +- temp_buffer = uv__malloc(temp_buffer_len * sizeof(WCHAR)); ++ temp_buffer = (WCHAR*)uv__malloc(temp_buffer_len * sizeof(WCHAR)); + if (temp_buffer == NULL) { + err = UV_ENOMEM; + goto error; +@@ -564,7 +564,7 @@ int make_program_args(char** args, int verbatim_arguments, WCHAR** dst_ptr) { + arg_len = uv_wtf8_length_as_utf16(*arg); + assert(arg_len > 0); + assert(temp_buffer_len >= (size_t) arg_len); +- uv_wtf8_to_utf16(*arg, temp_buffer, arg_len); ++ uv_wtf8_to_utf16(*arg, (uint16_t*)temp_buffer, arg_len); + + if (verbatim_arguments) { + /* Copy verbatim. */ +@@ -592,8 +592,8 @@ error: int env_strncmp(const wchar_t* a, int na, const wchar_t* b) { @@ -1455,7 +1594,7 @@ index ed44adc67c6d52785a199206d9ba0357e2d0b045..b383e8b405db56d413985b38df216d09 wchar_t* A; wchar_t* B; int nb; -@@ -633,8 +633,8 @@ int env_strncmp(const wchar_t* a, int na, const wchar_t* b) { +@@ -610,8 +610,8 @@ int env_strncmp(const wchar_t* a, int na, const wchar_t* b) { assert(b_eq); nb = b_eq - b; @@ -1466,16 +1605,30 @@ index ed44adc67c6d52785a199206d9ba0357e2d0b045..b383e8b405db56d413985b38df216d09 r = LCMapStringW(LOCALE_INVARIANT, LCMAP_UPPERCASE, a, na, A, na); assert(r==na); -@@ -717,7 +717,7 @@ int make_program_env(char* env_block[], WCHAR** dst_ptr) { +@@ -684,11 +684,11 @@ int make_program_env(char* env_block[], WCHAR** dst_ptr) { + } + + /* second pass: copy to UTF-16 environment block */ +- dst_copy = uv__malloc(env_len * sizeof(WCHAR)); ++ dst_copy = (WCHAR*)uv__malloc(env_len * sizeof(WCHAR)); if (dst_copy == NULL && env_len > 0) { - return ERROR_OUTOFMEMORY; + return UV_ENOMEM; } - env_copy = alloca(env_block_count * sizeof(WCHAR*)); + env_copy = (WCHAR**)alloca(env_block_count * sizeof(WCHAR*)); ptr = dst_copy; ptr_copy = env_copy; -@@ -771,7 +771,7 @@ int make_program_env(char* env_block[], WCHAR** dst_ptr) { +@@ -698,7 +698,7 @@ int make_program_env(char* env_block[], WCHAR** dst_ptr) { + len = uv_wtf8_length_as_utf16(*env); + assert(len > 0); + assert((size_t) len <= env_len - (ptr - dst_copy)); +- uv_wtf8_to_utf16(*env, ptr, len); ++ uv_wtf8_to_utf16(*env, (uint16_t*)ptr, len); + *ptr_copy++ = ptr; + ptr += len; + } +@@ -736,7 +736,7 @@ int make_program_env(char* env_block[], WCHAR** dst_ptr) { } /* final pass: copy, in sort order, and inserting required variables */ @@ -1483,7 +1636,7 @@ index ed44adc67c6d52785a199206d9ba0357e2d0b045..b383e8b405db56d413985b38df216d09 + dst = (WCHAR*)uv__malloc((1+env_len) * sizeof(WCHAR)); if (!dst) { uv__free(dst_copy); - return ERROR_OUTOFMEMORY; + return UV_ENOMEM; diff --git a/src/win/tcp.c b/src/win/tcp.c index 187f36e2a61c870b0d16e17e9d4a9e1161ba8851..d8da4d941a51b0625fc0c072342ec4edf74c0ea3 100644 --- a/src/win/tcp.c @@ -1519,11 +1672,51 @@ index 57c25e8f5a861c9d8a4c402c260d3ac235200423..57f1698f595e2410a51044f7f228b5a2 if (ctx == NULL) return UV_ENOMEM; +diff --git a/src/win/tty.c b/src/win/tty.c +index ac836930d6f3a185d57ba59fb42c817496aa0fdf..3aacaaf0b514ba71755dc51f1bd7263de5cbc308 100644 +--- a/src/win/tty.c ++++ b/src/win/tty.c +@@ -528,7 +528,7 @@ static DWORD CALLBACK uv_tty_line_read_thread(void* data) { + + if (read_console_success) { + read_bytes = bytes; +- uv_utf16_to_wtf8(utf16, ++ uv_utf16_to_wtf8((const uint16_t*)utf16, + read_chars, + &handle->tty.rd.read_line_buffer.base, + &read_bytes); +@@ -827,7 +827,7 @@ void uv_process_tty_read_raw_req(uv_loop_t* loop, uv_tty_t* handle, + WCHAR utf16_buffer[2]; + utf16_buffer[0] = handle->tty.rd.last_utf16_high_surrogate; + utf16_buffer[1] = KEV.uChar.UnicodeChar; +- if (uv_utf16_to_wtf8(utf16_buffer, ++ if (uv_utf16_to_wtf8((const uint16_t*)utf16_buffer, + 2, + &last_key_buf, + &char_len)) +@@ -835,7 +835,7 @@ void uv_process_tty_read_raw_req(uv_loop_t* loop, uv_tty_t* handle, + handle->tty.rd.last_utf16_high_surrogate = 0; + } else { + /* Single UTF-16 character */ +- if (uv_utf16_to_wtf8(&KEV.uChar.UnicodeChar, ++ if (uv_utf16_to_wtf8((const uint16_t*)&KEV.uChar.UnicodeChar, + 1, + &last_key_buf, + &char_len)) diff --git a/src/win/util.c b/src/win/util.c -index f6ec79cd57b5010ed5fd6829d952bcdacc8b7671..1cfd7b2caf0d4ad1a6a66df9406c21f4e2b69b04 100644 +index 91d88a54fb24a613df45d7915c345b6764dd6ffd..5fbc70f52952f90b36fb22ee03ec7f70e6ed5954 100644 --- a/src/win/util.c +++ b/src/win/util.c -@@ -160,7 +160,7 @@ static int uv__cwd(WCHAR** buf, DWORD *len) { +@@ -124,7 +124,7 @@ int uv_exepath(char* buffer, size_t* size_ptr) { + + /* Convert to UTF-8 */ + utf8_len = *size_ptr - 1; /* Reserve space for NUL */ +- err = uv_utf16_to_wtf8(utf16_buffer, utf16_len, &buffer, &utf8_len); ++ err = uv_utf16_to_wtf8((const uint16_t*)utf16_buffer, utf16_len, &buffer, &utf8_len); + if (err == UV_ENOBUFS) { + utf8_len = *size_ptr - 1; + err = 0; +@@ -152,7 +152,7 @@ static int uv__cwd(WCHAR** buf, DWORD *len) { return uv_translate_sys_error(GetLastError()); /* |t| is the size of the buffer _including_ nul. */ @@ -1532,16 +1725,7 @@ index f6ec79cd57b5010ed5fd6829d952bcdacc8b7671..1cfd7b2caf0d4ad1a6a66df9406c21f4 if (p == NULL) return UV_ENOMEM; -@@ -265,7 +265,7 @@ int uv_chdir(const char* dir) { - if (utf16_len == 0) { - return uv_translate_sys_error(GetLastError()); - } -- utf16_buffer = uv__malloc(utf16_len * sizeof(WCHAR)); -+ utf16_buffer = (WCHAR*)uv__malloc(utf16_len * sizeof(WCHAR)); - if (utf16_buffer == NULL) { - return UV_ENOMEM; - } -@@ -623,14 +623,14 @@ int uv_cpu_info(uv_cpu_info_t** cpu_infos_ptr, int* cpu_count_ptr) { +@@ -548,14 +548,14 @@ int uv_cpu_info(uv_cpu_info_t** cpu_infos_ptr, int* cpu_count_ptr) { GetSystemInfo(&system_info); cpu_count = system_info.dwNumberOfProcessors; @@ -1558,7 +1742,7 @@ index f6ec79cd57b5010ed5fd6829d952bcdacc8b7671..1cfd7b2caf0d4ad1a6a66df9406c21f4 if (sppi == NULL) { err = ERROR_OUTOFMEMORY; goto error; -@@ -774,7 +774,8 @@ int uv_interface_addresses(uv_interface_address_t** addresses_ptr, +@@ -699,7 +699,8 @@ int uv_interface_addresses(uv_interface_address_t** addresses_ptr, case ERROR_BUFFER_OVERFLOW: /* This happens when win_address_buf is NULL or too small to hold all * adapters. */ @@ -1568,7 +1752,7 @@ index f6ec79cd57b5010ed5fd6829d952bcdacc8b7671..1cfd7b2caf0d4ad1a6a66df9406c21f4 if (win_address_buf == NULL) return UV_ENOMEM; -@@ -782,7 +783,7 @@ int uv_interface_addresses(uv_interface_address_t** addresses_ptr, +@@ -707,7 +708,7 @@ int uv_interface_addresses(uv_interface_address_t** addresses_ptr, case ERROR_NO_DATA: { /* No adapters were found. */ @@ -1577,7 +1761,16 @@ index f6ec79cd57b5010ed5fd6829d952bcdacc8b7671..1cfd7b2caf0d4ad1a6a66df9406c21f4 if (uv_address_buf == NULL) return UV_ENOMEM; -@@ -859,7 +860,7 @@ int uv_interface_addresses(uv_interface_address_t** addresses_ptr, +@@ -758,7 +759,7 @@ int uv_interface_addresses(uv_interface_address_t** addresses_ptr, + continue; + + /* Compute the size of the interface name. */ +- name_size = uv_utf16_length_as_wtf8(adapter->FriendlyName, -1); ++ name_size = uv_utf16_length_as_wtf8((const uint16_t*)adapter->FriendlyName, -1); + uv_address_buf_size += name_size + 1; + + /* Count the number of addresses associated with this interface, and +@@ -773,7 +774,7 @@ int uv_interface_addresses(uv_interface_address_t** addresses_ptr, } /* Allocate space to store interface data plus adapter names. */ @@ -1586,7 +1779,7 @@ index f6ec79cd57b5010ed5fd6829d952bcdacc8b7671..1cfd7b2caf0d4ad1a6a66df9406c21f4 if (uv_address_buf == NULL) { uv__free(win_address_buf); return UV_ENOMEM; -@@ -1074,7 +1075,7 @@ int uv_os_tmpdir(char* buffer, size_t* size) { +@@ -982,7 +983,7 @@ int uv_os_tmpdir(char* buffer, size_t* size) { } /* Include space for terminating null char. */ len += 1; @@ -1595,25 +1788,46 @@ index f6ec79cd57b5010ed5fd6829d952bcdacc8b7671..1cfd7b2caf0d4ad1a6a66df9406c21f4 if (path == NULL) { return UV_ENOMEM; } -@@ -1153,7 +1154,7 @@ int uv__convert_utf16_to_utf8(const WCHAR* utf16, int utf16len, char** utf8) { - /* Allocate the destination buffer adding an extra byte for the terminating - * NULL. If utf16len is not -1 WideCharToMultiByte will not add it, so - * we do it ourselves always, just in case. */ -- *utf8 = uv__malloc(bufsize + 1); -+ *utf8 = (char*)uv__malloc(bufsize + 1); - - if (*utf8 == NULL) - return UV_ENOMEM; -@@ -1201,7 +1202,7 @@ int uv__convert_utf8_to_utf16(const char* utf8, int utf8len, WCHAR** utf16) { - /* Allocate the destination buffer adding an extra byte for the terminating - * NULL. If utf8len is not -1 MultiByteToWideChar will not add it, so - * we do it ourselves always, just in case. */ -- *utf16 = uv__malloc(sizeof(WCHAR) * (bufsize + 1)); -+ *utf16 = (WCHAR*)uv__malloc(sizeof(WCHAR) * (bufsize + 1)); +@@ -1019,7 +1020,7 @@ int uv__convert_utf16_to_utf8(const WCHAR* utf16, size_t utf16len, char** utf8) + return UV_EINVAL; + + *utf8 = NULL; +- return uv_utf16_to_wtf8(utf16, utf16len, utf8, &utf8_len); ++ return uv_utf16_to_wtf8((const uint16_t*)utf16, utf16len, utf8, &utf8_len); + } + + +@@ -1039,13 +1040,13 @@ int uv__convert_utf8_to_utf16(const char* utf8, WCHAR** utf16) { + return UV__EINVAL; + + /* Allocate the destination buffer. */ +- *utf16 = uv__malloc(sizeof(WCHAR) * bufsize); ++ *utf16 = (WCHAR*)uv__malloc(sizeof(WCHAR) * bufsize); if (*utf16 == NULL) return UV_ENOMEM; -@@ -1242,7 +1243,7 @@ static int uv__getpwuid_r(uv_passwd_t* pwd) { + + /* Convert to UTF-16 */ +- uv_wtf8_to_utf16(utf8, *utf16, bufsize); ++ uv_wtf8_to_utf16(utf8, (uint16_t*)*utf16, bufsize); + + return 0; + } +@@ -1065,11 +1066,11 @@ int uv__copy_utf16_to_utf8(const WCHAR* utf16buffer, size_t utf16len, char* utf8 + return UV_EINVAL; + + if (*size == 0) { +- *size = uv_utf16_length_as_wtf8(utf16buffer, utf16len); ++ *size = uv_utf16_length_as_wtf8((const uint16_t*)utf16buffer, utf16len); + r = UV_ENOBUFS; + } else { + *size -= 1; /* Reserve space for NUL. */ +- r = uv_utf16_to_wtf8(utf16buffer, utf16len, &utf8, size); ++ r = uv_utf16_to_wtf8((const uint16_t*)utf16buffer, utf16len, &utf8, size); + } + if (r == UV_ENOBUFS) + *size += 1; /* Add space for NUL. */ +@@ -1099,7 +1100,7 @@ static int uv__getpwuid_r(uv_passwd_t* pwd) { return uv_translate_sys_error(r); } @@ -1622,7 +1836,7 @@ index f6ec79cd57b5010ed5fd6829d952bcdacc8b7671..1cfd7b2caf0d4ad1a6a66df9406c21f4 if (path == NULL) { CloseHandle(token); return UV_ENOMEM; -@@ -1323,7 +1324,7 @@ int uv_os_environ(uv_env_item_t** envitems, int* count) { +@@ -1180,7 +1181,7 @@ int uv_os_environ(uv_env_item_t** envitems, int* count) { for (penv = env, i = 0; *penv != L'\0'; penv += wcslen(penv) + 1, i++); @@ -1631,7 +1845,7 @@ index f6ec79cd57b5010ed5fd6829d952bcdacc8b7671..1cfd7b2caf0d4ad1a6a66df9406c21f4 if (*envitems == NULL) { FreeEnvironmentStringsW(env); return UV_ENOMEM; -@@ -1413,7 +1414,7 @@ int uv_os_getenv(const char* name, char* buffer, size_t* size) { +@@ -1269,7 +1270,7 @@ int uv_os_getenv(const char* name, char* buffer, size_t* size) { uv__free(var); varlen = 1 + len; diff --git a/upstream_utils/libuv_patches/0003-Fix-warnings.patch b/upstream_utils/libuv_patches/0003-Fix-warnings.patch index cbf5c701bf1..55cbb9f9ed7 100644 --- a/upstream_utils/libuv_patches/0003-Fix-warnings.patch +++ b/upstream_utils/libuv_patches/0003-Fix-warnings.patch @@ -5,12 +5,11 @@ Subject: [PATCH 03/10] Fix warnings --- include/uv/win.h | 5 +++++ - src/idna.c | 2 +- + src/idna.c | 4 ++-- src/inet.c | 4 ++++ src/threadpool.c | 4 ++++ src/unix/core.c | 12 ++++++++++-- src/unix/internal.h | 4 ++-- - src/unix/linux.c | 15 ++++++++++++--- src/unix/thread.c | 6 ------ src/uv-common.c | 8 ++++++++ src/win/fs-event.c | 2 ++ @@ -19,7 +18,7 @@ Subject: [PATCH 03/10] Fix warnings src/win/process.c | 2 ++ src/win/thread.c | 4 ++-- src/win/tty.c | 2 ++ - 15 files changed, 58 insertions(+), 16 deletions(-) + 14 files changed, 47 insertions(+), 14 deletions(-) diff --git a/include/uv/win.h b/include/uv/win.h index eb74776978340a4910194bae35a9da6493e8c0a6..6d0afe69e7dd4caf4c9459e548fe75cf0c51b501 100644 @@ -43,10 +42,10 @@ index eb74776978340a4910194bae35a9da6493e8c0a6..6d0afe69e7dd4caf4c9459e548fe75cf typedef PVOID CONDITION_VARIABLE, *PCONDITION_VARIABLE; #endif diff --git a/src/idna.c b/src/idna.c -index 93d982ca018f2d39d9c0ffab07998c2c637029eb..36a39a089019fb4c2a35ec84516658c392eec0a3 100644 +index 0c952cf605a88136ed9035f9385f9b1080c30c28..fe60fb59bea59986a612cfa1747802b0636482d9 100644 --- a/src/idna.c +++ b/src/idna.c -@@ -106,7 +106,7 @@ static int uv__idna_toascii_label(const char* s, const char* se, +@@ -153,7 +153,7 @@ static int uv__idna_toascii_label(const char* s, const char* se, char** d, char* de) { static const char alphabet[] = "abcdefghijklmnopqrstuvwxyz0123456789"; const char* ss; @@ -55,6 +54,15 @@ index 93d982ca018f2d39d9c0ffab07998c2c637029eb..36a39a089019fb4c2a35ec84516658c3 unsigned h; unsigned k; unsigned n; +@@ -382,7 +382,7 @@ ssize_t uv_wtf8_length_as_utf16(const char* source_ptr) { + + void uv_wtf8_to_utf16(const char* source_ptr, + uint16_t* w_target, +- size_t w_target_len) { ++ [[maybe_unused]] size_t w_target_len) { + int32_t code_point; + + do { diff --git a/src/inet.c b/src/inet.c index dd94bea3886ca37945fcad7909d765e3700e3c21..71c9e5b774d64d505e6c6d6ed2637178b8532d4d 100644 --- a/src/inet.c @@ -132,48 +140,8 @@ index fe5885136039d5332623467b86bf52cd4b32ca0f..98c437dcadec5b5106d697e82d5394d4 s = strrchr(path, '/'); if (s == NULL) -diff --git a/src/unix/linux.c b/src/unix/linux.c -index 9173850bd158eaf9c41deca38f9ba84762a027a1..157443792f1216c83b4221c3810d17c81c5913c4 100644 ---- a/src/unix/linux.c -+++ b/src/unix/linux.c -@@ -1718,7 +1718,11 @@ int uv_cpu_info(uv_cpu_info_t** ci, int* count) { - return UV__ERR(errno); - } - -- fgets(buf, sizeof(buf), fp); /* Skip first line. */ -+ /* Skip first line. */ -+ if (!fgets(buf, sizeof(buf), fp)) { -+ uv__free(cpus); -+ return UV__ERR(errno); -+ } - - for (;;) { - memset(&t, 0, sizeof(t)); -@@ -1729,7 +1733,10 @@ int uv_cpu_info(uv_cpu_info_t** ci, int* count) { - if (n != 7) - break; - -- fgets(buf, sizeof(buf), fp); /* Skip rest of line. */ -+ /* Skip rest of line. */ -+ if (!fgets(buf, sizeof(buf), fp)) { -+ break; -+ } - - if (cpu >= ARRAY_SIZE(*cpus)) - continue; -@@ -1809,7 +1816,9 @@ nocpuinfo: - if (fp == NULL) - continue; - -- fscanf(fp, "%llu", &(*cpus)[cpu].freq); -+ if (0 > fscanf(fp, "%llu", &(*cpus)[cpu].freq)) { -+ (*cpus)[cpu].freq = 0llu; -+ } - fclose(fp); - fp = NULL; - } diff --git a/src/unix/thread.c b/src/unix/thread.c -index 531c6211bb4321e5f11031a0644b4e3ab9174396..f8600947e3e7df015c4302af4feee740707b2c46 100644 +index 20409541de3cb300504b823472a73bc95fa38f62..688c042e1aedf379264900c29758c8b01a4a90ed 100644 --- a/src/unix/thread.c +++ b/src/unix/thread.c @@ -137,12 +137,6 @@ int uv_thread_create_ex(uv_thread_t* tid, @@ -190,10 +158,10 @@ index 531c6211bb4321e5f11031a0644b4e3ab9174396..f8600947e3e7df015c4302af4feee740 params->flags & UV_THREAD_HAS_STACK_SIZE ? params->stack_size : 0; diff --git a/src/uv-common.c b/src/uv-common.c -index c04f93596ab1f730576256d86e216ccb7f258b72..cd10b36b4a393e325ea03b93eb9897193ca9800b 100644 +index 69e95801a18104ea910abf86db236d85f62afb66..49126e50f07bac16d198775454b731f40630d1d1 100644 --- a/src/uv-common.c +++ b/src/uv-common.c -@@ -799,6 +799,10 @@ void uv__fs_readdir_cleanup(uv_fs_t* req) { +@@ -802,6 +802,10 @@ void uv__fs_readdir_cleanup(uv_fs_t* req) { } } @@ -204,7 +172,7 @@ index c04f93596ab1f730576256d86e216ccb7f258b72..cd10b36b4a393e325ea03b93eb989719 int uv_loop_configure(uv_loop_t* loop, uv_loop_option option, ...) { va_list ap; -@@ -812,6 +816,10 @@ int uv_loop_configure(uv_loop_t* loop, uv_loop_option option, ...) { +@@ -815,6 +819,10 @@ int uv_loop_configure(uv_loop_t* loop, uv_loop_option option, ...) { return err; } @@ -216,7 +184,7 @@ index c04f93596ab1f730576256d86e216ccb7f258b72..cd10b36b4a393e325ea03b93eb989719 static uv_loop_t default_loop_struct; static uv_loop_t* default_loop_ptr; diff --git a/src/win/fs-event.c b/src/win/fs-event.c -index 150467313d576bfe2966b55f3d8cffa23cbb8ea3..3244a4e4320d7ce98f226b49b2634c65de89c213 100644 +index 5a07acfe54efe90cf2ab0bca5b5998a961e72ebd..381220bf241d9572ff64576cf25700d9746d2806 100644 --- a/src/win/fs-event.c +++ b/src/win/fs-event.c @@ -19,6 +19,8 @@ @@ -229,7 +197,7 @@ index 150467313d576bfe2966b55f3d8cffa23cbb8ea3..3244a4e4320d7ce98f226b49b2634c65 #include #include diff --git a/src/win/fs.c b/src/win/fs.c -index 328c8f2e0513562b53c948ffea59d0841e14b264..565c05fff13c2e6e74091c1da7b31636d7fd370d 100644 +index d9c2a4f728c7fb491995c6153b2a63a835b206b7..7df1df2b9ba23c0ecb96e82a6cfcaae824826b69 100644 --- a/src/win/fs.c +++ b/src/win/fs.c @@ -19,6 +19,8 @@ @@ -242,7 +210,7 @@ index 328c8f2e0513562b53c948ffea59d0841e14b264..565c05fff13c2e6e74091c1da7b31636 #include #include diff --git a/src/win/pipe.c b/src/win/pipe.c -index c1739efe82b8755999145860b4da6b50c73518a2..258d6a684c67f154096a25e7226f1a7d08b93d5b 100644 +index 0f045a873073cf9b07feb457ea199990df521e5f..b2348bd0430aada98ff10313f6f926ea2f1c4491 100644 --- a/src/win/pipe.c +++ b/src/win/pipe.c @@ -19,6 +19,8 @@ @@ -255,7 +223,7 @@ index c1739efe82b8755999145860b4da6b50c73518a2..258d6a684c67f154096a25e7226f1a7d #include #include diff --git a/src/win/process.c b/src/win/process.c -index b383e8b405db56d413985b38df216d09c58ec4a0..2b1b46259959867482079962d0ea44246a42e7cb 100644 +index 0103004c565bb09b7b9c150e874f9c68486ab26c..eeb0f6a984bb0f61b956cb4b907fbfdb3b4ac7e2 100644 --- a/src/win/process.c +++ b/src/win/process.c @@ -19,6 +19,8 @@ @@ -283,7 +251,7 @@ index 57f1698f595e2410a51044f7f228b5a235206819..03b33e9b4de6fe2532095d717a8639e8 return UV_EINVAL; } diff --git a/src/win/tty.c b/src/win/tty.c -index 7e1f15544b177382a774300f832bc982d85bd62b..abbe1315883257d6825b794344dcd4cba9514097 100644 +index 3aacaaf0b514ba71755dc51f1bd7263de5cbc308..c90995afb6166a7cab0a267f5eb513705ce05148 100644 --- a/src/win/tty.c +++ b/src/win/tty.c @@ -19,6 +19,8 @@ diff --git a/upstream_utils/libuv_patches/0004-Preprocessor-cleanup.patch b/upstream_utils/libuv_patches/0004-Preprocessor-cleanup.patch index c95b28a839c..d5aa3e2f1bc 100644 --- a/upstream_utils/libuv_patches/0004-Preprocessor-cleanup.patch +++ b/upstream_utils/libuv_patches/0004-Preprocessor-cleanup.patch @@ -14,7 +14,7 @@ Subject: [PATCH 04/10] Preprocessor cleanup 7 files changed, 14 insertions(+), 20 deletions(-) diff --git a/include/uv.h b/include/uv.h -index 02397dd0fdd43d51f86c0dde9a62046702f12bdb..d5342b0d52232bbf83825948cc6bc09e5d74a4c7 100644 +index 5642101c10c351a602a195d3b74027b53bf641d2..aff06cb7f4352eb4fcf93c938f09a789318bddc3 100644 --- a/include/uv.h +++ b/include/uv.h @@ -23,9 +23,6 @@ @@ -44,7 +44,7 @@ index 02397dd0fdd43d51f86c0dde9a62046702f12bdb..d5342b0d52232bbf83825948cc6bc09e UV_EXTERN uv_handle_type uv_guess_handle(uv_file file); enum { -@@ -1906,7 +1897,4 @@ UV_EXTERN void uv_loop_set_data(uv_loop_t*, void* data); +@@ -1918,7 +1909,4 @@ void uv_wtf8_to_utf16(const char* wtf8, #undef UV_LOOP_PRIVATE_PLATFORM_FIELDS #undef UV__ERR @@ -85,19 +85,19 @@ index 98c437dcadec5b5106d697e82d5394d459f55e47..854d98a16a74c45e0b6cb92b17782de6 /* core */ diff --git a/src/win/fs.c b/src/win/fs.c -index 565c05fff13c2e6e74091c1da7b31636d7fd370d..f415ddc2c39d09eea317fc857777acce1ce7d13e 100644 +index 7df1df2b9ba23c0ecb96e82a6cfcaae824826b69..26ba539936786902af5830c2811594292815d2d7 100644 --- a/src/win/fs.c +++ b/src/win/fs.c -@@ -40,6 +40,7 @@ - - #include +@@ -43,6 +43,7 @@ + #include "handle-inl.h" + #include "fs-fd-hash-inl.h" +#pragma comment(lib, "Advapi32.lib") #define UV_FS_FREE_PATHS 0x0002 #define UV_FS_FREE_PTR 0x0008 diff --git a/src/win/tty.c b/src/win/tty.c -index abbe1315883257d6825b794344dcd4cba9514097..9bb3d9e830c901122da5e521e0c6b032dfd5044c 100644 +index c90995afb6166a7cab0a267f5eb513705ce05148..7adf3cd3ba81a9c41064f4ef2af01667092f27d1 100644 --- a/src/win/tty.c +++ b/src/win/tty.c @@ -37,6 +37,8 @@ @@ -110,7 +110,7 @@ index abbe1315883257d6825b794344dcd4cba9514097..9bb3d9e830c901122da5e521e0c6b032 # define InterlockedOr _InterlockedOr #endif diff --git a/src/win/util.c b/src/win/util.c -index 1cfd7b2caf0d4ad1a6a66df9406c21f4e2b69b04..af18cfa6c106c5de0996e4fff9b4127aaa3b576b 100644 +index 5fbc70f52952f90b36fb22ee03ec7f70e6ed5954..f505cfad4987768fa6470ca27612312c2ba0729d 100644 --- a/src/win/util.c +++ b/src/win/util.c @@ -64,12 +64,20 @@ diff --git a/upstream_utils/libuv_patches/0006-Style-comments-cleanup.patch b/upstream_utils/libuv_patches/0006-Style-comments-cleanup.patch index d4aa9a27ac1..494a4099229 100644 --- a/upstream_utils/libuv_patches/0006-Style-comments-cleanup.patch +++ b/upstream_utils/libuv_patches/0006-Style-comments-cleanup.patch @@ -36,10 +36,10 @@ index 268fc9652f437eb0d0cda2a9e0b06b9e91eb9742..f53adc156a7c454c492abaeac29d90be #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wdollar-in-identifier-extension" diff --git a/src/uv-common.c b/src/uv-common.c -index cd10b36b4a393e325ea03b93eb9897193ca9800b..bfcc3ef10f4fd7763221638947da6e02e7a17c33 100644 +index 49126e50f07bac16d198775454b731f40630d1d1..3d802cf1d629272f281d07b9ea424ec259b4377e 100644 --- a/src/uv-common.c +++ b/src/uv-common.c -@@ -799,6 +799,7 @@ void uv__fs_readdir_cleanup(uv_fs_t* req) { +@@ -802,6 +802,7 @@ void uv__fs_readdir_cleanup(uv_fs_t* req) { } } @@ -48,7 +48,7 @@ index cd10b36b4a393e325ea03b93eb9897193ca9800b..bfcc3ef10f4fd7763221638947da6e02 # pragma clang diagnostic push # pragma clang diagnostic ignored "-Wvarargs" diff --git a/src/win/process.c b/src/win/process.c -index 2b1b46259959867482079962d0ea44246a42e7cb..18816d3b1e8c863f8ca74fe0104de1aecd0ae3fa 100644 +index eeb0f6a984bb0f61b956cb4b907fbfdb3b4ac7e2..69f94b913f0a4051a36690725c7f79eb3535e594 100644 --- a/src/win/process.c +++ b/src/win/process.c @@ -35,7 +35,6 @@ diff --git a/upstream_utils/libuv_patches/0007-Fix-Win32-warning-suppression-pragma.patch b/upstream_utils/libuv_patches/0007-Fix-Win32-warning-suppression-pragma.patch index 5ef597ca650..a656ba55fa9 100644 --- a/upstream_utils/libuv_patches/0007-Fix-Win32-warning-suppression-pragma.patch +++ b/upstream_utils/libuv_patches/0007-Fix-Win32-warning-suppression-pragma.patch @@ -8,10 +8,10 @@ Subject: [PATCH 07/10] Fix Win32 warning suppression pragma 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/win/util.c b/src/win/util.c -index af18cfa6c106c5de0996e4fff9b4127aaa3b576b..9324992ec521cc3496e3e9304e600963a3f20897 100644 +index f505cfad4987768fa6470ca27612312c2ba0729d..8828857db76a54d6e184c7f5714a091e92eba8ed 100644 --- a/src/win/util.c +++ b/src/win/util.c -@@ -1692,7 +1692,7 @@ int uv_os_uname(uv_utsname_t* buffer) { +@@ -1502,7 +1502,7 @@ int uv_os_uname(uv_utsname_t* buffer) { } else { /* Silence GetVersionEx() deprecation warning. */ #ifdef _MSC_VER diff --git a/upstream_utils/libuv_patches/0008-Use-C-atomics.patch b/upstream_utils/libuv_patches/0008-Use-C-atomics.patch index ffdc5887f51..7304fe66d21 100644 --- a/upstream_utils/libuv_patches/0008-Use-C-atomics.patch +++ b/upstream_utils/libuv_patches/0008-Use-C-atomics.patch @@ -6,13 +6,13 @@ Subject: [PATCH 08/10] Use C++ atomics --- src/unix/async.c | 25 +++++++++++++------------ src/unix/core.c | 3 ++- - src/unix/fs.c | 32 +++++++++++++++++--------------- + src/unix/fs.c | 20 +++++++++++--------- src/unix/kqueue.c | 10 ++++++---- src/unix/linux.c | 45 +++++++++++++++++++++++---------------------- src/unix/tty.c | 5 +++-- src/uv-common.c | 2 +- src/uv-common.h | 8 +++----- - 8 files changed, 68 insertions(+), 62 deletions(-) + 8 files changed, 62 insertions(+), 56 deletions(-) diff --git a/src/unix/async.c b/src/unix/async.c index 0ff2669e30a628dbb2df9e28ba14b38cf14114e5..fef4ae93343edc0341179a1c4739dcd831ef6e26 100644 @@ -112,19 +112,19 @@ index f53adc156a7c454c492abaeac29d90be436785fc..ce7fd2cdfdd53410dc694450bd56dffc iovmax = atomic_load_explicit(&iovmax_cached, memory_order_relaxed); diff --git a/src/unix/fs.c b/src/unix/fs.c -index e25d02e54dbe93e4b9c22b0140108c99ae2cb4f7..aba190a9c0240fba0128fb7fbc5d92d7fa86214b 100644 +index bc00c90b07a32a823412b216df6f2d758dbc423b..afdf0c6e592c69961b1286a87b61e62601331763 100644 --- a/src/unix/fs.c +++ b/src/unix/fs.c -@@ -46,6 +46,8 @@ +@@ -45,6 +45,8 @@ #include #include +#include + - #if defined(__DragonFly__) || \ - defined(__FreeBSD__) || \ - defined(__OpenBSD__) || \ -@@ -313,7 +315,7 @@ static int uv__fs_mkstemp(uv_fs_t* req) { + #if defined(__linux__) + # include + #endif +@@ -307,7 +309,7 @@ static int uv__fs_mkstemp(uv_fs_t* req) { static uv_once_t once = UV_ONCE_INIT; int r; #ifdef O_CLOEXEC @@ -133,7 +133,7 @@ index e25d02e54dbe93e4b9c22b0140108c99ae2cb4f7..aba190a9c0240fba0128fb7fbc5d92d7 #endif static const char pattern[] = "XXXXXX"; static const size_t pattern_size = sizeof(pattern) - 1; -@@ -338,7 +340,7 @@ static int uv__fs_mkstemp(uv_fs_t* req) { +@@ -332,7 +334,7 @@ static int uv__fs_mkstemp(uv_fs_t* req) { uv_once(&once, uv__mkostemp_initonce); #ifdef O_CLOEXEC @@ -142,7 +142,7 @@ index e25d02e54dbe93e4b9c22b0140108c99ae2cb4f7..aba190a9c0240fba0128fb7fbc5d92d7 uv__mkostemp != NULL) { r = uv__mkostemp(path, O_CLOEXEC); -@@ -352,7 +354,7 @@ static int uv__fs_mkstemp(uv_fs_t* req) { +@@ -346,7 +348,7 @@ static int uv__fs_mkstemp(uv_fs_t* req) { /* We set the static variable so that next calls don't even try to use mkostemp. */ @@ -151,34 +151,7 @@ index e25d02e54dbe93e4b9c22b0140108c99ae2cb4f7..aba190a9c0240fba0128fb7fbc5d92d7 } #endif /* O_CLOEXEC */ -@@ -462,7 +464,7 @@ static ssize_t uv__fs_preadv(uv_file fd, - - static ssize_t uv__fs_read(uv_fs_t* req) { - #if TRY_PREADV -- static _Atomic int no_preadv; -+ static std::atomic no_preadv; - #endif - unsigned int iovmax; - ssize_t result; -@@ -486,7 +488,7 @@ static ssize_t uv__fs_read(uv_fs_t* req) { - result = preadv(req->file, (struct iovec*) req->bufs, req->nbufs, req->off); - #else - # if TRY_PREADV -- if (atomic_load_explicit(&no_preadv, memory_order_relaxed)) retry: -+ if (atomic_load_explicit(&no_preadv, std::memory_order_relaxed)) retry: - # endif - { - result = uv__fs_preadv(req->file, req->bufs, req->nbufs, req->off); -@@ -498,7 +500,7 @@ static ssize_t uv__fs_read(uv_fs_t* req) { - req->nbufs, - req->off); - if (result == -1 && errno == ENOSYS) { -- atomic_store_explicit(&no_preadv, 1, memory_order_relaxed); -+ atomic_store_explicit(&no_preadv, 1, std::memory_order_relaxed); - goto retry; - } - } -@@ -939,10 +941,10 @@ static int uv__is_cifs_or_smb(int fd) { +@@ -867,10 +869,10 @@ static int uv__is_cifs_or_smb(int fd) { static ssize_t uv__fs_try_copy_file_range(int in_fd, off_t* off, int out_fd, size_t len) { @@ -191,7 +164,7 @@ index e25d02e54dbe93e4b9c22b0140108c99ae2cb4f7..aba190a9c0240fba0128fb7fbc5d92d7 errno = ENOSYS; return -1; } -@@ -961,7 +963,7 @@ static ssize_t uv__fs_try_copy_file_range(int in_fd, off_t* off, +@@ -889,7 +891,7 @@ static ssize_t uv__fs_try_copy_file_range(int in_fd, off_t* off, errno = ENOSYS; /* Use fallback. */ break; case ENOSYS: @@ -200,34 +173,7 @@ index e25d02e54dbe93e4b9c22b0140108c99ae2cb4f7..aba190a9c0240fba0128fb7fbc5d92d7 break; case EPERM: /* It's been reported that CIFS spuriously fails. -@@ -1162,7 +1164,7 @@ static ssize_t uv__fs_lutime(uv_fs_t* req) { - - static ssize_t uv__fs_write(uv_fs_t* req) { - #if TRY_PREADV -- static _Atomic int no_pwritev; -+ static std::atomic no_pwritev; - #endif - ssize_t r; - -@@ -1191,7 +1193,7 @@ static ssize_t uv__fs_write(uv_fs_t* req) { - r = pwritev(req->file, (struct iovec*) req->bufs, req->nbufs, req->off); - #else - # if TRY_PREADV -- if (atomic_load_explicit(&no_pwritev, memory_order_relaxed)) retry: -+ if (atomic_load_explicit(&no_pwritev, std::memory_order_relaxed)) retry: - # endif - { - r = pwrite(req->file, req->bufs[0].base, req->bufs[0].len, req->off); -@@ -1203,7 +1205,7 @@ static ssize_t uv__fs_write(uv_fs_t* req) { - req->nbufs, - req->off); - if (r == -1 && errno == ENOSYS) { -- atomic_store_explicit(&no_pwritev, 1, memory_order_relaxed); -+ atomic_store_explicit(&no_pwritev, 1, std::memory_order_relaxed); - goto retry; - } - } -@@ -1483,14 +1485,14 @@ static int uv__fs_statx(int fd, +@@ -1380,14 +1382,14 @@ static int uv__fs_statx(int fd, uv_stat_t* buf) { STATIC_ASSERT(UV_ENOSYS != -1); #ifdef __linux__ @@ -244,7 +190,7 @@ index e25d02e54dbe93e4b9c22b0140108c99ae2cb4f7..aba190a9c0240fba0128fb7fbc5d92d7 return UV_ENOSYS; dirfd = AT_FDCWD; -@@ -1524,7 +1526,7 @@ static int uv__fs_statx(int fd, +@@ -1421,7 +1423,7 @@ static int uv__fs_statx(int fd, * implemented, rc might return 1 with 0 set as the error code in which * case we return ENOSYS. */ @@ -254,10 +200,10 @@ index e25d02e54dbe93e4b9c22b0140108c99ae2cb4f7..aba190a9c0240fba0128fb7fbc5d92d7 } diff --git a/src/unix/kqueue.c b/src/unix/kqueue.c -index 28e55aae6c613576ede7024a5c73d746e134d865..ffe0f9191cc7b0c233447db358077d8814e0217e 100644 +index 06fbdb24b4adc4adb781d32150d40836fa745531..939a42696778741f9184be29dcea7c858736d181 100644 --- a/src/unix/kqueue.c +++ b/src/unix/kqueue.c -@@ -34,6 +34,8 @@ +@@ -37,6 +37,8 @@ #include #include @@ -266,7 +212,7 @@ index 28e55aae6c613576ede7024a5c73d746e134d865..ffe0f9191cc7b0c233447db358077d88 /* * Required on * - Until at least FreeBSD 11.0 -@@ -60,7 +62,7 @@ int uv__kqueue_init(uv_loop_t* loop) { +@@ -63,7 +65,7 @@ int uv__kqueue_init(uv_loop_t* loop) { #if defined(__APPLE__) && MAC_OS_X_VERSION_MAX_ALLOWED >= 1070 @@ -275,7 +221,7 @@ index 28e55aae6c613576ede7024a5c73d746e134d865..ffe0f9191cc7b0c233447db358077d88 #endif int uv__io_fork(uv_loop_t* loop) { -@@ -84,7 +86,7 @@ int uv__io_fork(uv_loop_t* loop) { +@@ -87,7 +89,7 @@ int uv__io_fork(uv_loop_t* loop) { */ atomic_store_explicit(&uv__has_forked_with_cfrunloop, 1, @@ -284,7 +230,7 @@ index 28e55aae6c613576ede7024a5c73d746e134d865..ffe0f9191cc7b0c233447db358077d88 uv__free(loop->cf_state); loop->cf_state = NULL; } -@@ -541,7 +543,7 @@ int uv_fs_event_start(uv_fs_event_t* handle, +@@ -555,7 +557,7 @@ int uv_fs_event_start(uv_fs_event_t* handle, goto fallback; if (0 == atomic_load_explicit(&uv__has_forked_with_cfrunloop, @@ -293,7 +239,7 @@ index 28e55aae6c613576ede7024a5c73d746e134d865..ffe0f9191cc7b0c233447db358077d88 int r; /* The fallback fd is no longer needed */ uv__close_nocheckstdio(fd); -@@ -577,7 +579,7 @@ int uv_fs_event_stop(uv_fs_event_t* handle) { +@@ -591,7 +593,7 @@ int uv_fs_event_stop(uv_fs_event_t* handle) { #if defined(__APPLE__) && MAC_OS_X_VERSION_MAX_ALLOWED >= 1070 if (0 == atomic_load_explicit(&uv__has_forked_with_cfrunloop, @@ -303,7 +249,7 @@ index 28e55aae6c613576ede7024a5c73d746e134d865..ffe0f9191cc7b0c233447db358077d88 r = uv__fsevents_close(handle); #endif diff --git a/src/unix/linux.c b/src/unix/linux.c -index 157443792f1216c83b4221c3810d17c81c5913c4..e3dfb186dc531e5c8197a81681c00d693e0913c6 100644 +index 2b8e1d8fe593a181d049aa50ff9edaf6da258a24..b23d88bd824843eebc3b439e5a18e6f796a747be 100644 --- a/src/unix/linux.c +++ b/src/unix/linux.c @@ -27,7 +27,6 @@ @@ -314,7 +260,7 @@ index 157443792f1216c83b4221c3810d17c81c5913c4..e3dfb186dc531e5c8197a81681c00d69 #include /* offsetof */ #include #include -@@ -133,6 +132,8 @@ +@@ -139,6 +138,8 @@ # include #endif /* HAVE_IFADDRS_H */ @@ -323,7 +269,7 @@ index 157443792f1216c83b4221c3810d17c81c5913c4..e3dfb186dc531e5c8197a81681c00d69 enum { UV__IORING_SETUP_SQPOLL = 2u, }; -@@ -311,14 +312,14 @@ static struct watcher_root* uv__inotify_watchers(uv_loop_t* loop) { +@@ -317,7 +318,7 @@ static struct watcher_root* uv__inotify_watchers(uv_loop_t* loop) { unsigned uv__kernel_version(void) { @@ -332,25 +278,26 @@ index 157443792f1216c83b4221c3810d17c81c5913c4..e3dfb186dc531e5c8197a81681c00d69 struct utsname u; unsigned version; unsigned major; - unsigned minor; - unsigned patch; +@@ -326,7 +327,7 @@ unsigned uv__kernel_version(void) { + char v_sig[256]; + char* needle; - version = atomic_load_explicit(&cached_version, memory_order_relaxed); + version = std::atomic_load_explicit(&cached_version, std::memory_order_relaxed); if (version != 0) return version; -@@ -329,7 +330,7 @@ unsigned uv__kernel_version(void) { - return 0; +@@ -382,7 +383,7 @@ unsigned uv__kernel_version(void) { + calculate_version: version = major * 65536 + minor * 256 + patch; - atomic_store_explicit(&cached_version, version, memory_order_relaxed); + std::atomic_store_explicit(&cached_version, version, std::memory_order_relaxed); return version; } -@@ -424,16 +425,16 @@ static int uv__use_io_uring(void) { - return 0; /* Possibly available but blocked by seccomp. */ +@@ -480,11 +481,11 @@ static int uv__use_io_uring(void) { + return 0; /* All 32 bits kernels appear buggy. */ #else /* Ternary: unknown=0, yes=1, no=-1 */ - static _Atomic int use_io_uring; @@ -362,14 +309,17 @@ index 157443792f1216c83b4221c3810d17c81c5913c4..e3dfb186dc531e5c8197a81681c00d69 + use = std::atomic_load_explicit(&use_io_uring, std::memory_order_relaxed); if (use == 0) { - val = getenv("UV_USE_IO_URING"); - use = val == NULL || atoi(val) ? 1 : -1; + /* Older kernels have a bug where the sqpoll thread uses 100% CPU. */ +@@ -495,7 +496,7 @@ static int uv__use_io_uring(void) { + if (val != NULL) + use = atoi(val) ? 1 : -1; + - atomic_store_explicit(&use_io_uring, use, memory_order_relaxed); + std::atomic_store_explicit(&use_io_uring, use, std::memory_order_relaxed); } return use > 0; -@@ -709,8 +710,8 @@ static struct uv__io_uring_sqe* uv__iou_get_sqe(struct uv__iou* iou, +@@ -771,8 +772,8 @@ static struct uv__io_uring_sqe* uv__iou_get_sqe(struct uv__iou* iou, if (iou->ringfd == -1) return NULL; @@ -380,7 +330,7 @@ index 157443792f1216c83b4221c3810d17c81c5913c4..e3dfb186dc531e5c8197a81681c00d69 tail = *iou->sqtail; mask = iou->sqmask; -@@ -739,12 +740,12 @@ static struct uv__io_uring_sqe* uv__iou_get_sqe(struct uv__iou* iou, +@@ -801,12 +802,12 @@ static struct uv__io_uring_sqe* uv__iou_get_sqe(struct uv__iou* iou, static void uv__iou_submit(struct uv__iou* iou) { uint32_t flags; @@ -397,7 +347,7 @@ index 157443792f1216c83b4221c3810d17c81c5913c4..e3dfb186dc531e5c8197a81681c00d69 if (flags & UV__IORING_SQ_NEED_WAKEUP) if (uv__io_uring_enter(iou->ringfd, 0, 0, UV__IORING_ENTER_SQ_WAKEUP)) -@@ -1076,8 +1077,8 @@ static void uv__poll_io_uring(uv_loop_t* loop, struct uv__iou* iou) { +@@ -1147,8 +1148,8 @@ static void uv__poll_io_uring(uv_loop_t* loop, struct uv__iou* iou) { int rc; head = *iou->cqhead; @@ -408,7 +358,7 @@ index 157443792f1216c83b4221c3810d17c81c5913c4..e3dfb186dc531e5c8197a81681c00d69 mask = iou->cqmask; cqe = (uv__io_uring_cqe*)iou->cqe; nevents = 0; -@@ -1109,15 +1110,15 @@ static void uv__poll_io_uring(uv_loop_t* loop, struct uv__iou* iou) { +@@ -1180,15 +1181,15 @@ static void uv__poll_io_uring(uv_loop_t* loop, struct uv__iou* iou) { nevents++; } @@ -428,7 +378,7 @@ index 157443792f1216c83b4221c3810d17c81c5913c4..e3dfb186dc531e5c8197a81681c00d69 if (flags & UV__IORING_SQ_CQ_OVERFLOW) { do -@@ -1531,7 +1532,7 @@ update_timeout: +@@ -1581,7 +1582,7 @@ update_timeout: } uint64_t uv__hrtime(uv_clocktype_t type) { @@ -437,7 +387,7 @@ index 157443792f1216c83b4221c3810d17c81c5913c4..e3dfb186dc531e5c8197a81681c00d69 struct timespec t; clock_t clock_id; -@@ -1547,7 +1548,7 @@ uint64_t uv__hrtime(uv_clocktype_t type) { +@@ -1597,7 +1598,7 @@ uint64_t uv__hrtime(uv_clocktype_t type) { if (type != UV_CLOCK_FAST) goto done; @@ -446,7 +396,7 @@ index 157443792f1216c83b4221c3810d17c81c5913c4..e3dfb186dc531e5c8197a81681c00d69 if (clock_id != -1) goto done; -@@ -1556,7 +1557,7 @@ uint64_t uv__hrtime(uv_clocktype_t type) { +@@ -1606,7 +1607,7 @@ uint64_t uv__hrtime(uv_clocktype_t type) { if (t.tv_nsec <= 1 * 1000 * 1000) clock_id = CLOCK_MONOTONIC_COARSE; @@ -486,10 +436,10 @@ index 1bd217b5a15eed13a8349c479b53471dd36ca216..1304c6d8685cfd122cffea066dc668d1 int uv__tcsetattr(int fd, int how, const struct termios *term) { int rc; diff --git a/src/uv-common.c b/src/uv-common.c -index bfcc3ef10f4fd7763221638947da6e02e7a17c33..5c6d84155408ae4f7c3c6ff9b48bd09ccd16a92e 100644 +index 3d802cf1d629272f281d07b9ea424ec259b4377e..569c6906b7e54c4af1c17564a18066b847ab7ca1 100644 --- a/src/uv-common.c +++ b/src/uv-common.c -@@ -951,7 +951,7 @@ void uv_free_cpu_info(uv_cpu_info_t* cpu_infos, int count) { +@@ -954,7 +954,7 @@ void uv_free_cpu_info(uv_cpu_info_t* cpu_infos, int count) { __attribute__((destructor)) #endif void uv_library_shutdown(void) { diff --git a/upstream_utils/libuv_patches/0009-Remove-static-from-array-indices.patch b/upstream_utils/libuv_patches/0009-Remove-static-from-array-indices.patch index f95fc5fb34d..43d6eddc1e9 100644 --- a/upstream_utils/libuv_patches/0009-Remove-static-from-array-indices.patch +++ b/upstream_utils/libuv_patches/0009-Remove-static-from-array-indices.patch @@ -8,10 +8,10 @@ Subject: [PATCH 09/10] Remove static from array indices 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/unix/linux.c b/src/unix/linux.c -index e3dfb186dc531e5c8197a81681c00d693e0913c6..d365b623a0a25228f0c6acf1fa14a5c7a9f1efbf 100644 +index b23d88bd824843eebc3b439e5a18e6f796a747be..95a2fe2b42895626f207f740bdccfe938915d836 100644 --- a/src/unix/linux.c +++ b/src/unix/linux.c -@@ -2060,7 +2060,7 @@ static uint64_t uv__read_uint64(const char* filename) { +@@ -2104,7 +2104,7 @@ static uint64_t uv__read_uint64(const char* filename) { * finds the location and length of the memory controller mount path. * This disregards the leading / for easy concatenation of paths. * Returns NULL if the memory controller wasn't found. */ @@ -20,7 +20,7 @@ index e3dfb186dc531e5c8197a81681c00d693e0913c6..d365b623a0a25228f0c6acf1fa14a5c7 int* n) { char* p; -@@ -2081,7 +2081,7 @@ static char* uv__cgroup1_find_memory_controller(char buf[static 1024], +@@ -2125,7 +2125,7 @@ static char* uv__cgroup1_find_memory_controller(char buf[static 1024], return p; } @@ -29,7 +29,7 @@ index e3dfb186dc531e5c8197a81681c00d693e0913c6..d365b623a0a25228f0c6acf1fa14a5c7 uint64_t* max) { char filename[4097]; char* p; -@@ -2121,7 +2121,7 @@ update_limits: +@@ -2165,7 +2165,7 @@ update_limits: *max = UINT64_MAX; } @@ -38,7 +38,7 @@ index e3dfb186dc531e5c8197a81681c00d693e0913c6..d365b623a0a25228f0c6acf1fa14a5c7 uint64_t* max) { char filename[4097]; char* p; -@@ -2138,7 +2138,7 @@ static void uv__get_cgroup2_memory_limits(char buf[static 1024], uint64_t* high, +@@ -2182,7 +2182,7 @@ static void uv__get_cgroup2_memory_limits(char buf[static 1024], uint64_t* high, *high = uv__read_uint64(filename); } @@ -47,7 +47,7 @@ index e3dfb186dc531e5c8197a81681c00d693e0913c6..d365b623a0a25228f0c6acf1fa14a5c7 uint64_t high; uint64_t max; -@@ -2164,7 +2164,7 @@ uint64_t uv_get_constrained_memory(void) { +@@ -2208,7 +2208,7 @@ uint64_t uv_get_constrained_memory(void) { } @@ -56,7 +56,7 @@ index e3dfb186dc531e5c8197a81681c00d693e0913c6..d365b623a0a25228f0c6acf1fa14a5c7 char filename[4097]; uint64_t current; char* p; -@@ -2188,7 +2188,7 @@ static uint64_t uv__get_cgroup1_current_memory(char buf[static 1024]) { +@@ -2232,7 +2232,7 @@ static uint64_t uv__get_cgroup1_current_memory(char buf[static 1024]) { return uv__read_uint64("/sys/fs/cgroup/memory/memory.usage_in_bytes"); } diff --git a/upstream_utils/libuv_patches/0010-Add-pragmas-for-missing-libraries-and-set-_WIN32_WIN.patch b/upstream_utils/libuv_patches/0010-Add-pragmas-for-missing-libraries-and-set-_WIN32_WIN.patch index 07d02f5b95e..17b2bfd00bd 100644 --- a/upstream_utils/libuv_patches/0010-Add-pragmas-for-missing-libraries-and-set-_WIN32_WIN.patch +++ b/upstream_utils/libuv_patches/0010-Add-pragmas-for-missing-libraries-and-set-_WIN32_WIN.patch @@ -27,7 +27,7 @@ index 6d0afe69e7dd4caf4c9459e548fe75cf0c51b501..613065df435d813cd517efbc138b13ee #if !defined(_SSIZE_T_) && !defined(_SSIZE_T_DEFINED) diff --git a/src/win/util.c b/src/win/util.c -index 9324992ec521cc3496e3e9304e600963a3f20897..4b76417fcbac2480725471740c037deb859e17ca 100644 +index 8828857db76a54d6e184c7f5714a091e92eba8ed..14295c41342b6dda16f39c5c548a6cc8269bfa0b 100644 --- a/src/win/util.c +++ b/src/win/util.c @@ -73,7 +73,9 @@ static char *process_title; diff --git a/upstream_utils/update_fmt.py b/upstream_utils/update_fmt.py index 746e879ad24..3af48a27f20 100755 --- a/upstream_utils/update_fmt.py +++ b/upstream_utils/update_fmt.py @@ -22,6 +22,7 @@ def main(): for f in [ "0001-Don-t-throw-on-write-failure.patch", "0002-Suppress-warnings-we-can-t-fix.patch", + "0003-Remove-this-from-decltype.patch", ]: git_am(os.path.join(wpilib_root, "upstream_utils/fmt_patches", f)) diff --git a/upstream_utils/update_libuv.py b/upstream_utils/update_libuv.py index 179632f0aed..e4c6b2b8bb2 100755 --- a/upstream_utils/update_libuv.py +++ b/upstream_utils/update_libuv.py @@ -13,7 +13,7 @@ def main(): - upstream_root = clone_repo("https://github.com/libuv/libuv", "v1.46.0") + upstream_root = clone_repo("https://github.com/libuv/libuv", "v1.47.0") wpilib_root = get_repo_root() wpinet = os.path.join(wpilib_root, "wpinet") diff --git a/wpigui/CMakeLists.txt b/wpigui/CMakeLists.txt index 68acbea643c..4e8b6c3609c 100644 --- a/wpigui/CMakeLists.txt +++ b/wpigui/CMakeLists.txt @@ -44,13 +44,3 @@ target_link_libraries(wpiguidev wpigui) install(TARGETS wpigui EXPORT wpigui) install(DIRECTORY src/main/native/include/ DESTINATION "${include_dest}/wpigui") - -#if (WITH_FLAT_INSTALL) -# set (wpigui_config_dir ${wpilib_dest}) -#else() -# set (wpigui_config_dir share/wpigui) -#endif() - -#configure_file(wpigui-config.cmake.in ${WPILIB_BINARY_DIR}/wpigui-config.cmake ) -#install(FILES ${WPILIB_BINARY_DIR}/wpigui-config.cmake DESTINATION ${wpigui_config_dir}) -#install(EXPORT wpigui DESTINATION ${wpigui_config_dir}) diff --git a/wpilibNewCommands/CMakeLists.txt b/wpilibNewCommands/CMakeLists.txt index 34a6bb1f5f0..5f525bb5bea 100644 --- a/wpilibNewCommands/CMakeLists.txt +++ b/wpilibNewCommands/CMakeLists.txt @@ -10,18 +10,12 @@ if (WITH_JAVA) set(CMAKE_JAVA_COMPILE_FLAGS "-encoding" "UTF8" "-Xlint:unchecked") file(GLOB_RECURSE JAVA_SOURCES src/main/java/*.java) - add_jar(wpilibNewCommands_jar ${JAVA_SOURCES} INCLUDE_JARS hal_jar ntcore_jar cscore_jar cameraserver_jar wpimath_jar wpiutil_jar wpilibj_jar OUTPUT_NAME wpilibNewCommands) + add_jar(wpilibNewCommands_jar ${JAVA_SOURCES} INCLUDE_JARS hal_jar ntcore_jar cscore_jar cameraserver_jar wpimath_jar wpiunits_jar wpiutil_jar wpilibj_jar OUTPUT_NAME wpilibNewCommands) get_property(WPILIBNEWCOMMANDS_JAR_FILE TARGET wpilibNewCommands_jar PROPERTY JAR_FILE) install(FILES ${WPILIBNEWCOMMANDS_JAR_FILE} DESTINATION "${java_lib_dest}") set_property(TARGET wpilibNewCommands_jar PROPERTY FOLDER "java") - - if (WITH_FLAT_INSTALL) - set (wpilibNewCommands_config_dir ${wpilib_dest}) - else() - set (wpilibNewCommands_config_dir share/wpilibNewCommands) - endif() endif() if (WITH_JAVA_SOURCE) @@ -56,15 +50,9 @@ target_include_directories(wpilibNewCommands PUBLIC install(TARGETS wpilibNewCommands EXPORT wpilibNewCommands) install(DIRECTORY src/main/native/include/ DESTINATION "${include_dest}/wpilibNewCommands") -if (FLAT_INSTALL_WPILIB) - set(wpilibNewCommands_config_dir ${wpilib_dest}) - else() - set(wpilibNewCommands_config_dir share/wpilibNewCommands) - endif() - - configure_file(wpilibNewCommands-config.cmake.in ${WPILIB_BINARY_DIR}/wpilibNewCommands-config.cmake) - install(FILES ${WPILIB_BINARY_DIR}/wpilibNewCommands-config.cmake DESTINATION ${wpilibNewCommands_config_dir}) - install(EXPORT wpilibNewCommands DESTINATION ${wpilibNewCommands_config_dir}) +configure_file(wpilibNewCommands-config.cmake.in ${WPILIB_BINARY_DIR}/wpilibNewCommands-config.cmake) +install(FILES ${WPILIB_BINARY_DIR}/wpilibNewCommands-config.cmake DESTINATION share/wpilibNewCommands) +install(EXPORT wpilibNewCommands DESTINATION share/wpilibNewCommands) if (WITH_TESTS) wpilib_add_test(wpilibNewCommands src/test/native/cpp) diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/RobotModeTriggers.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/RobotModeTriggers.java new file mode 100644 index 00000000000..9bef66abfa3 --- /dev/null +++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/RobotModeTriggers.java @@ -0,0 +1,52 @@ +// 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.wpilibj2.command.button; + +import edu.wpi.first.wpilibj.DriverStation; + +/** + * A class containing static {@link Trigger} factories for running callbacks when the robot mode + * changes. + */ +public final class RobotModeTriggers { + // Utility class + private RobotModeTriggers() {} + + /** + * Returns a trigger that is true when the robot is enabled in autonomous mode. + * + * @return A trigger that is true when the robot is enabled in autonomous mode. + */ + public static Trigger autonomous() { + return new Trigger(DriverStation::isAutonomousEnabled); + } + + /** + * Returns a trigger that is true when the robot is enabled in teleop mode. + * + * @return A trigger that is true when the robot is enabled in teleop mode. + */ + public static Trigger teleop() { + return new Trigger(DriverStation::isTeleopEnabled); + } + + /** + * Returns a trigger that is true when the robot is disabled. + * + * @return A trigger that is true when the robot is disabled. + */ + public static Trigger disabled() { + return new Trigger(DriverStation::isDisabled); + } + + /** + * Returns a trigger that is true when the robot is enabled in test mode. + * + * @return A trigger that is true when the robot is enabled in test mode. + */ + public static Trigger test() { + return new Trigger(DriverStation::isTestEnabled); + } +} diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/button/RobotModeTriggers.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/button/RobotModeTriggers.cpp new file mode 100644 index 00000000000..ab593d8ff29 --- /dev/null +++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/button/RobotModeTriggers.cpp @@ -0,0 +1,25 @@ +// 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. + +#include "frc2/command/button/RobotModeTriggers.h" + +#include + +using namespace frc2; + +Trigger RobotModeTriggers::Autonomous() { + return Trigger{&frc::DriverStation::IsAutonomousEnabled}; +} + +Trigger RobotModeTriggers::Teleop() { + return Trigger{&frc::DriverStation::IsTeleopEnabled}; +} + +Trigger RobotModeTriggers::Disabled() { + return Trigger{&frc::DriverStation::IsDisabled}; +} + +Trigger RobotModeTriggers::Test() { + return Trigger{&frc::DriverStation::IsTestEnabled}; +} diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/button/Trigger.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/button/Trigger.cpp index 38ec741e473..190a5448177 100644 --- a/wpilibNewCommands/src/main/native/cpp/frc2/command/button/Trigger.cpp +++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/button/Trigger.cpp @@ -212,3 +212,7 @@ Trigger Trigger::Debounce(units::second_t debounceTime, return debouncer.Calculate(condition()); }); } + +bool Trigger::Get() const { + return m_condition(); +} diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/button/RobotModeTriggers.h b/wpilibNewCommands/src/main/native/include/frc2/command/button/RobotModeTriggers.h new file mode 100644 index 00000000000..2fbcc643851 --- /dev/null +++ b/wpilibNewCommands/src/main/native/include/frc2/command/button/RobotModeTriggers.h @@ -0,0 +1,50 @@ +// 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 "frc2/command/button/Trigger.h" + +namespace frc2 { + +/** + * A class containing static Trigger factories for running callbacks when robot + * mode changes. + */ +class RobotModeTriggers { + public: + RobotModeTriggers() = delete; + + /** + * Returns a trigger that is true when the robot is enabled in autonomous + * mode. + * + * @return A trigger that is true when the robot is enabled in autonomous + * mode. + */ + static Trigger Autonomous(); + + /** + * Returns a trigger that is true when the robot is enabled in teleop mode. + * + * @return A trigger that is true when the robot is enabled in teleop mode. + */ + static Trigger Teleop(); + + /** + * Returns a trigger that is true when the robot is disabled. + * + * @return A trigger that is true when the robot is disabled. + */ + static Trigger Disabled(); + + /** + * Returns a trigger that is true when the robot is enabled in test mode. + * + * @return A trigger that is true when the robot is enabled in test mode. + */ + static Trigger Test(); +}; + +} // namespace frc2 diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/button/Trigger.h b/wpilibNewCommands/src/main/native/include/frc2/command/button/Trigger.h index 533e0a6787b..4438aacdf90 100644 --- a/wpilibNewCommands/src/main/native/include/frc2/command/button/Trigger.h +++ b/wpilibNewCommands/src/main/native/include/frc2/command/button/Trigger.h @@ -268,6 +268,12 @@ class Trigger { frc::Debouncer::DebounceType type = frc::Debouncer::DebounceType::kRising); + /** + * Returns the current state of this trigger. + * @return A bool representing the current state of the trigger. + */ + bool Get() const; + private: frc::EventLoop* m_loop; std::function m_condition; diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/button/RobotModeTriggersTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/button/RobotModeTriggersTest.java new file mode 100644 index 00000000000..b2b41c4dfb6 --- /dev/null +++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/button/RobotModeTriggersTest.java @@ -0,0 +1,58 @@ +// 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.wpilibj2.command.button; + +import static org.junit.jupiter.api.Assertions.assertTrue; + +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.simulation.DriverStationSim; +import edu.wpi.first.wpilibj2.command.CommandTestBase; +import org.junit.jupiter.api.Test; + +class RobotModeTriggersTest extends CommandTestBase { + @Test + void autonomousTest() { + DriverStationSim.resetData(); + DriverStationSim.setAutonomous(true); + DriverStationSim.setTest(false); + DriverStationSim.setEnabled(true); + DriverStation.refreshData(); + Trigger auto = RobotModeTriggers.autonomous(); + assertTrue(auto.getAsBoolean()); + } + + @Test + void teleopTest() { + DriverStationSim.resetData(); + DriverStationSim.setAutonomous(false); + DriverStationSim.setTest(false); + DriverStationSim.setEnabled(true); + DriverStation.refreshData(); + Trigger teleop = RobotModeTriggers.teleop(); + assertTrue(teleop.getAsBoolean()); + } + + @Test + void testModeTest() { + DriverStationSim.resetData(); + DriverStationSim.setAutonomous(false); + DriverStationSim.setTest(true); + DriverStationSim.setEnabled(true); + DriverStation.refreshData(); + Trigger test = RobotModeTriggers.test(); + assertTrue(test.getAsBoolean()); + } + + @Test + void disabledTest() { + DriverStationSim.resetData(); + DriverStationSim.setAutonomous(false); + DriverStationSim.setTest(false); + DriverStationSim.setEnabled(false); + DriverStation.refreshData(); + Trigger disabled = RobotModeTriggers.disabled(); + assertTrue(disabled.getAsBoolean()); + } +} diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/ParallelCommandGroupTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/ParallelCommandGroupTest.cpp index 28d498c8089..ff5384c8f98 100644 --- a/wpilibNewCommands/src/test/native/cpp/frc2/command/ParallelCommandGroupTest.cpp +++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/ParallelCommandGroupTest.cpp @@ -20,7 +20,7 @@ TEST_F(ParallelCommandGroupTest, ParallelGroupSchedule) { MockCommand* command1 = command1Holder.get(); MockCommand* command2 = command2Holder.get(); - ParallelCommandGroup group(tcb::make_vector>( + ParallelCommandGroup group(make_vector>( std::move(command1Holder), std::move(command2Holder))); EXPECT_CALL(*command1, Initialize()); @@ -50,7 +50,7 @@ TEST_F(ParallelCommandGroupTest, ParallelGroupInterrupt) { MockCommand* command1 = command1Holder.get(); MockCommand* command2 = command2Holder.get(); - ParallelCommandGroup group(tcb::make_vector>( + ParallelCommandGroup group(make_vector>( std::move(command1Holder), std::move(command2Holder))); EXPECT_CALL(*command1, Initialize()); diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/ParallelDeadlineGroupTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/ParallelDeadlineGroupTest.cpp index 4abab336f25..f35bd000c28 100644 --- a/wpilibNewCommands/src/test/native/cpp/frc2/command/ParallelDeadlineGroupTest.cpp +++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/ParallelDeadlineGroupTest.cpp @@ -24,8 +24,8 @@ TEST_F(ParallelDeadlineGroupTest, DeadlineGroupSchedule) { ParallelDeadlineGroup group( std::move(command1Holder), - tcb::make_vector>(std::move(command2Holder), - std::move(command3Holder))); + make_vector>(std::move(command2Holder), + std::move(command3Holder))); EXPECT_CALL(*command1, Initialize()); EXPECT_CALL(*command1, Execute()).Times(2); @@ -64,8 +64,8 @@ TEST_F(ParallelDeadlineGroupTest, SequentialGroupInterrupt) { ParallelDeadlineGroup group( std::move(command1Holder), - tcb::make_vector>(std::move(command2Holder), - std::move(command3Holder))); + make_vector>(std::move(command2Holder), + std::move(command3Holder))); EXPECT_CALL(*command1, Initialize()); EXPECT_CALL(*command1, Execute()).Times(1); diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/ParallelRaceGroupTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/ParallelRaceGroupTest.cpp index 3df2147da8f..be18fc30490 100644 --- a/wpilibNewCommands/src/test/native/cpp/frc2/command/ParallelRaceGroupTest.cpp +++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/ParallelRaceGroupTest.cpp @@ -23,7 +23,7 @@ TEST_F(ParallelRaceGroupTest, ParallelRaceSchedule) { MockCommand* command2 = command2Holder.get(); MockCommand* command3 = command3Holder.get(); - ParallelRaceGroup group{tcb::make_vector>( + ParallelRaceGroup group{make_vector>( std::move(command1Holder), std::move(command2Holder), std::move(command3Holder))}; @@ -59,7 +59,7 @@ TEST_F(ParallelRaceGroupTest, ParallelRaceInterrupt) { MockCommand* command2 = command2Holder.get(); MockCommand* command3 = command3Holder.get(); - ParallelRaceGroup group{tcb::make_vector>( + ParallelRaceGroup group{make_vector>( std::move(command1Holder), std::move(command2Holder), std::move(command3Holder))}; @@ -164,7 +164,7 @@ TEST_F(ParallelRaceGroupTest, ParallelRaceScheduleTwice) { MockCommand* command2 = command2Holder.get(); MockCommand* command3 = command3Holder.get(); - ParallelRaceGroup group{tcb::make_vector>( + ParallelRaceGroup group{make_vector>( std::move(command1Holder), std::move(command2Holder), std::move(command3Holder))}; diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/SequentialCommandGroupTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/SequentialCommandGroupTest.cpp index 0048b2416c1..e1283136e8f 100644 --- a/wpilibNewCommands/src/test/native/cpp/frc2/command/SequentialCommandGroupTest.cpp +++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/SequentialCommandGroupTest.cpp @@ -22,7 +22,7 @@ TEST_F(SequentialCommandGroupTest, SequentialGroupSchedule) { MockCommand* command2 = command2Holder.get(); MockCommand* command3 = command3Holder.get(); - SequentialCommandGroup group{tcb::make_vector>( + SequentialCommandGroup group{make_vector>( std::move(command1Holder), std::move(command2Holder), std::move(command3Holder))}; @@ -61,7 +61,7 @@ TEST_F(SequentialCommandGroupTest, SequentialGroupInterrupt) { MockCommand* command2 = command2Holder.get(); MockCommand* command3 = command3Holder.get(); - SequentialCommandGroup group{tcb::make_vector>( + SequentialCommandGroup group{make_vector>( std::move(command1Holder), std::move(command2Holder), std::move(command3Holder))}; diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/button/RobotModeTriggersTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/button/RobotModeTriggersTest.cpp new file mode 100644 index 00000000000..cb9e9664ba0 --- /dev/null +++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/button/RobotModeTriggersTest.cpp @@ -0,0 +1,54 @@ +// 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. + +#include +#include + +#include "../CommandTestBase.h" +#include "frc2/command/button/RobotModeTriggers.h" +#include "frc2/command/button/Trigger.h" + +using namespace frc2; +using namespace frc::sim; +class RobotModeTriggersTest : public CommandTestBase {}; + +TEST(RobotModeTriggersTest, Autonomous) { + DriverStationSim::ResetData(); + DriverStationSim::SetAutonomous(true); + DriverStationSim::SetTest(false); + DriverStationSim::SetEnabled(true); + frc::DriverStation::RefreshData(); + Trigger autonomous = RobotModeTriggers::Autonomous(); + EXPECT_TRUE(autonomous.Get()); +} + +TEST(RobotModeTriggersTest, Teleop) { + DriverStationSim::ResetData(); + DriverStationSim::SetAutonomous(false); + DriverStationSim::SetTest(false); + DriverStationSim::SetEnabled(true); + frc::DriverStation::RefreshData(); + Trigger teleop = RobotModeTriggers::Teleop(); + EXPECT_TRUE(teleop.Get()); +} + +TEST(RobotModeTriggersTest, Disabled) { + DriverStationSim::ResetData(); + DriverStationSim::SetAutonomous(false); + DriverStationSim::SetTest(false); + DriverStationSim::SetEnabled(false); + frc::DriverStation::RefreshData(); + Trigger disabled = RobotModeTriggers::Disabled(); + EXPECT_TRUE(disabled.Get()); +} + +TEST(RobotModeTriggersTest, TestMode) { + DriverStationSim::ResetData(); + DriverStationSim::SetAutonomous(false); + DriverStationSim::SetTest(true); + DriverStationSim::SetEnabled(true); + frc::DriverStation::RefreshData(); + Trigger test = RobotModeTriggers::Test(); + EXPECT_TRUE(test.Get()); +} diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/make_vector.h b/wpilibNewCommands/src/test/native/cpp/frc2/command/make_vector.h index 996ddba2aa5..295eba76458 100644 --- a/wpilibNewCommands/src/test/native/cpp/frc2/command/make_vector.h +++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/make_vector.h @@ -8,59 +8,27 @@ #include #include -namespace tcb { - -namespace detail { - -template -struct vec_type_helper { - using type = T; -}; - -template -struct vec_type_helper { - using type = typename std::common_type_t; -}; - -template -using vec_type_helper_t = typename vec_type_helper::type; - -template -struct all_constructible_and_convertible : std::true_type {}; - -template -struct all_constructible_and_convertible - : std::conditional_t< - std::is_constructible_v && std::is_convertible_v, - all_constructible_and_convertible, std::false_type> {}; - -template -inline constexpr bool all_constructible_and_convertible_v = - all_constructible_and_convertible::value; - -template - requires(!std::is_trivially_copyable_v) -std::vector make_vector_impl(Args&&... args) { - std::vector vec; - vec.reserve(sizeof...(Args)); - using arr_t = int[]; - (void)arr_t{0, (vec.emplace_back(std::forward(args)), 0)...}; - return vec; -} - -template - requires std::is_trivially_copyable_v -std::vector make_vector_impl(Args&&... args) { - return std::vector{std::forward(args)...}; -} - -} // namespace detail +namespace frc2 { template > - requires detail::all_constructible_and_convertible_v -std::vector make_vector(Args&&... args) { - return detail::make_vector_impl(std::forward(args)...); + typename CommonType = std::conditional_t< + std::same_as, std::common_type_t, T>> + requires((std::is_constructible_v && + std::is_convertible_v) && + ...) +std::vector make_vector(Args&&... args) { + if constexpr (std::is_trivially_copyable_v) { + return std::vector{std::forward(args)...}; + } else { + std::vector vec; + vec.reserve(sizeof...(Args)); + + using arr_t = int[]; + [[maybe_unused]] arr_t arr{ + 0, (vec.emplace_back(std::forward(args)), 0)...}; + + return vec; + } } -} // namespace tcb +} // namespace frc2 diff --git a/wpilibc/CMakeLists.txt b/wpilibc/CMakeLists.txt index ed8583497c4..fc7b54ea3a9 100644 --- a/wpilibc/CMakeLists.txt +++ b/wpilibc/CMakeLists.txt @@ -33,15 +33,9 @@ set_property(TARGET wpilibc PROPERTY FOLDER "libraries") install(TARGETS wpilibc EXPORT wpilibc) install(DIRECTORY src/main/native/include/ DESTINATION "${include_dest}/wpilibc") -if (WITH_FLAT_INSTALL) - set (wpilibc_config_dir ${wpilib_dest}) -else() - set (wpilibc_config_dir share/wpilibc) -endif() - configure_file(wpilibc-config.cmake.in ${WPILIB_BINARY_DIR}/wpilibc-config.cmake ) -install(FILES ${WPILIB_BINARY_DIR}/wpilibc-config.cmake DESTINATION ${wpilibc_config_dir}) -install(EXPORT wpilibc DESTINATION ${wpilibc_config_dir}) +install(FILES ${WPILIB_BINARY_DIR}/wpilibc-config.cmake DESTINATION share/wpilibc) +install(EXPORT wpilibc DESTINATION share/wpilibc) if (WITH_TESTS) wpilib_add_test(wpilibc src/test/native/cpp) diff --git a/wpilibc/src/main/native/cpp/DataLogManager.cpp b/wpilibc/src/main/native/cpp/DataLogManager.cpp index f148a62259f..156b4054cc4 100644 --- a/wpilibc/src/main/native/cpp/DataLogManager.cpp +++ b/wpilibc/src/main/native/cpp/DataLogManager.cpp @@ -76,8 +76,11 @@ static std::string MakeLogDir(std::string_view dir) { "DataLogManager: Logging to RoboRIO 1 internal storage is " "not recommended! Plug in a FAT32 formatted flash drive!"); } -#endif + fs::create_directory("/home/lvuser/logs", ec); + return "/home/lvuser/logs"; +#else return filesystem::GetOperatingDirectory(); +#endif } static std::string MakeLogFilename(std::string_view filenameOverride) { diff --git a/wpilibc/src/main/native/cpp/simulation/DifferentialDrivetrainSim.cpp b/wpilibc/src/main/native/cpp/simulation/DifferentialDrivetrainSim.cpp index 8c27cdf15bb..4d2720f5a6f 100644 --- a/wpilibc/src/main/native/cpp/simulation/DifferentialDrivetrainSim.cpp +++ b/wpilibc/src/main/native/cpp/simulation/DifferentialDrivetrainSim.cpp @@ -58,7 +58,7 @@ void DifferentialDrivetrainSim::SetGearing(double newGearing) { } void DifferentialDrivetrainSim::Update(units::second_t dt) { - m_x = RK4([this](auto& x, auto& u) { return Dynamics(x, u); }, m_x, m_u, dt); + m_x = RKDP([this](auto& x, auto& u) { return Dynamics(x, u); }, m_x, m_u, dt); m_y = m_x + frc::MakeWhiteNoiseVector<7>(m_measurementStdDevs); } diff --git a/wpilibc/src/test/native/cpp/simulation/DifferentialDrivetrainSimTest.cpp b/wpilibc/src/test/native/cpp/simulation/DifferentialDrivetrainSimTest.cpp index 7e564260ff2..a942d16e3d5 100644 --- a/wpilibc/src/test/native/cpp/simulation/DifferentialDrivetrainSimTest.cpp +++ b/wpilibc/src/test/native/cpp/simulation/DifferentialDrivetrainSimTest.cpp @@ -55,7 +55,7 @@ TEST(DifferentialDrivetrainSimTest, Convergence) { sim.Update(20_ms); // Update ground truth. - groundTruthX = frc::RK4( + groundTruthX = frc::RKDP( [&sim](const auto& x, const auto& u) -> frc::Vectord<7> { return sim.Dynamics(x, u); }, @@ -63,7 +63,7 @@ TEST(DifferentialDrivetrainSimTest, Convergence) { } // 2 inch tolerance is OK since our ground truth is an approximation of the - // ODE solution using RK4 anyway + // ODE solution using RKDP anyway EXPECT_NEAR(groundTruthX(0, 0), sim.GetPose().X().value(), 0.05); EXPECT_NEAR(groundTruthX(1, 0), sim.GetPose().Y().value(), 0.05); EXPECT_NEAR(groundTruthX(2, 0), sim.GetHeading().Radians().value(), 0.01); diff --git a/wpilibj/CMakeLists.txt b/wpilibj/CMakeLists.txt index 5924f4ddee8..a5cd637473e 100644 --- a/wpilibj/CMakeLists.txt +++ b/wpilibj/CMakeLists.txt @@ -17,20 +17,14 @@ if (WITH_JAVA) file(GLOB EJML_JARS "${WPILIB_BINARY_DIR}/wpimath/thirdparty/ejml/*.jar") file(GLOB JACKSON_JARS "${WPILIB_BINARY_DIR}/wpiutil/thirdparty/jackson/*.jar") - add_jar(wpilibj_jar ${JAVA_SOURCES} ${CMAKE_CURRENT_BINARY_DIR}/WPILibVersion.java INCLUDE_JARS hal_jar ntcore_jar ${EJML_JARS} ${JACKSON_JARS} ${OPENCV_JAR_FILE} cscore_jar cameraserver_jar wpimath_jar wpiutil_jar OUTPUT_NAME wpilibj) + add_jar(wpilibj_jar ${JAVA_SOURCES} ${CMAKE_CURRENT_BINARY_DIR}/WPILibVersion.java INCLUDE_JARS hal_jar ntcore_jar ${EJML_JARS} ${JACKSON_JARS} ${OPENCV_JAR_FILE} cscore_jar cameraserver_jar wpimath_jar wpiunits_jar wpiutil_jar OUTPUT_NAME wpilibj) get_property(WPILIBJ_JAR_FILE TARGET wpilibj_jar PROPERTY JAR_FILE) install(FILES ${WPILIBJ_JAR_FILE} DESTINATION "${java_lib_dest}") set_property(TARGET wpilibj_jar PROPERTY FOLDER "java") - if (WITH_FLAT_INSTALL) - set (wpilibj_config_dir ${wpilib_dest}) - else() - set (wpilibj_config_dir share/wpilibj) - endif() - - install(FILES wpilibj-config.cmake DESTINATION ${wpilibj_config_dir}) + install(FILES wpilibj-config.cmake DESTINATION share/wpilibj) endif() if (WITH_JAVA_SOURCE) diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/DataLogManager.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DataLogManager.java index 8098ae9af95..d588d4049a8 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/DataLogManager.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DataLogManager.java @@ -225,6 +225,10 @@ private static String makeLogDir(String dir) { + " Plug in a FAT32 formatted flash drive!", false); } + if (!new File("/home/lvuser/logs").mkdir()) { + // ignored + } + return "/home/lvuser/logs"; } return Filesystem.getOperatingDirectory().getAbsolutePath(); } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DifferentialDrivetrainSim.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DifferentialDrivetrainSim.java index 94db6f667de..67133f6e9ca 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DifferentialDrivetrainSim.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DifferentialDrivetrainSim.java @@ -148,8 +148,7 @@ public void setInputs(double leftVoltageVolts, double rightVoltageVolts) { * @param dtSeconds the time difference */ public void update(double dtSeconds) { - // Update state estimate with RK4 - m_x = NumericalIntegration.rk4(this::getDynamics, m_x, m_u, dtSeconds); + m_x = NumericalIntegration.rkdp(this::getDynamics, m_x, m_u, dtSeconds); m_y = m_x; if (m_measurementStdDevs != null) { m_y = m_y.plus(StateSpaceUtil.makeWhiteNoiseVector(m_measurementStdDevs)); diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/DifferentialDrivetrainSimTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/DifferentialDrivetrainSimTest.java index 5baed3ab154..cbb8d18c86f 100644 --- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/DifferentialDrivetrainSimTest.java +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/DifferentialDrivetrainSimTest.java @@ -76,11 +76,11 @@ void testConvergence() { sim.update(0.020); // Update our ground truth - groundTruthX = NumericalIntegration.rk4(sim::getDynamics, groundTruthX, voltages, 0.020); + groundTruthX = NumericalIntegration.rkdp(sim::getDynamics, groundTruthX, voltages, 0.020); } // 2 inch tolerance is OK since our ground truth is an approximation of the - // ODE solution using RK4 anyway + // ODE solution using RKDP anyway assertEquals( groundTruthX.get(DifferentialDrivetrainSim.State.kX.value, 0), sim.getState(DifferentialDrivetrainSim.State.kX), diff --git a/wpilibjExamples/build.gradle b/wpilibjExamples/build.gradle index d5f71e98403..0d1aabde3d4 100644 --- a/wpilibjExamples/build.gradle +++ b/wpilibjExamples/build.gradle @@ -25,9 +25,8 @@ dependencies { implementation project(':romiVendordep') implementation project(':xrpVendordep') - testImplementation 'org.junit.jupiter:junit-jupiter-api:5.10.0' - testImplementation 'org.junit.jupiter:junit-jupiter-params:5.10.0' - testRuntimeOnly 'org.junit.jupiter:junit-jupiter-engine:5.10.0' + testImplementation 'org.junit.jupiter:junit-jupiter:5.10.1' + testRuntimeOnly 'org.junit.platform:junit-platform-launcher' } jacoco { diff --git a/wpimath/CMakeLists.txt b/wpimath/CMakeLists.txt index 997ec46e9f4..029a9d6571f 100644 --- a/wpimath/CMakeLists.txt +++ b/wpimath/CMakeLists.txt @@ -110,7 +110,7 @@ if (WITH_JAVA) file(GLOB_RECURSE JAVA_SOURCES src/main/java/*.java ${WPILIB_BINARY_DIR}/wpimath/generated/*.java) - add_jar(wpimath_jar ${JAVA_SOURCES} ${WPIMATH_QUICKBUF_SRCS} INCLUDE_JARS ${EJML_JARS} wpiutil_jar OUTPUT_NAME wpimath GENERATE_NATIVE_HEADERS wpimath_jni_headers) + add_jar(wpimath_jar ${JAVA_SOURCES} ${WPIMATH_QUICKBUF_SRCS} INCLUDE_JARS ${EJML_JARS} wpiutil_jar wpiunits_jar OUTPUT_NAME wpimath GENERATE_NATIVE_HEADERS wpimath_jni_headers) get_property(WPIMATH_JAR_FILE TARGET wpimath_jar PROPERTY JAR_FILE) install(FILES ${WPIMATH_JAR_FILE} DESTINATION "${java_lib_dest}") @@ -208,15 +208,9 @@ target_include_directories(wpimath PUBLIC install(TARGETS wpimath EXPORT wpimath) -if (WITH_FLAT_INSTALL) - set (wpimath_config_dir ${wpilib_dest}) -else() - set (wpimath_config_dir share/wpimath) -endif() - configure_file(wpimath-config.cmake.in ${WPILIB_BINARY_DIR}/wpimath-config.cmake ) -install(FILES ${WPILIB_BINARY_DIR}/wpimath-config.cmake DESTINATION ${wpimath_config_dir}) -install(EXPORT wpimath DESTINATION ${wpimath_config_dir}) +install(FILES ${WPILIB_BINARY_DIR}/wpimath-config.cmake DESTINATION share/wpimath) +install(EXPORT wpimath DESTINATION share/wpimath) if (WITH_TESTS) wpilib_add_test(wpimath src/test/native/cpp) diff --git a/wpimath/build.gradle b/wpimath/build.gradle index 3496ba7f982..32ea0a1b569 100644 --- a/wpimath/build.gradle +++ b/wpimath/build.gradle @@ -43,6 +43,7 @@ model { } dependencies { + api project(":wpiunits") api "org.ejml:ejml-simple:0.43.1" api "com.fasterxml.jackson.core:jackson-annotations:2.15.2" api "com.fasterxml.jackson.core:jackson-core:2.15.2" diff --git a/wpimath/src/main/java/edu/wpi/first/math/MatBuilder.java b/wpimath/src/main/java/edu/wpi/first/math/MatBuilder.java index 2ee010f194e..80c9d0dc9d6 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/MatBuilder.java +++ b/wpimath/src/main/java/edu/wpi/first/math/MatBuilder.java @@ -14,39 +14,56 @@ * @param The number of columns of the desired matrix. */ public class MatBuilder { - final Nat m_rows; - final Nat m_cols; - /** * Fills the matrix with the given data, encoded in row major form. (The matrix is filled row by * row, left to right with the given data). * + * @param rows The number of rows of the matrix. + * @param cols The number of columns of the matrix. * @param data The data to fill the matrix with. + * @param The number of rows of the matrix. + * @param The number of columns of the matrix. * @return The constructed matrix. */ - public final Matrix fill(double... data) { - if (Objects.requireNonNull(data).length != this.m_rows.getNum() * this.m_cols.getNum()) { + public static final Matrix fill( + Nat rows, Nat cols, double... data) { + if (Objects.requireNonNull(data).length != rows.getNum() * cols.getNum()) { throw new IllegalArgumentException( "Invalid matrix data provided. Wanted " - + this.m_rows.getNum() + + rows.getNum() + " x " - + this.m_cols.getNum() + + cols.getNum() + " matrix, but got " + data.length + " elements"); - } else { - return new Matrix<>(new SimpleMatrix(this.m_rows.getNum(), this.m_cols.getNum(), true, data)); } + return new Matrix<>(new SimpleMatrix(rows.getNum(), cols.getNum(), true, data)); } + protected final Nat m_rows; + protected final Nat m_cols; + /** * Creates a new {@link MatBuilder} with the given dimensions. * * @param rows The number of rows of the matrix. * @param cols The number of columns of the matrix. + * @deprecated Use {@link MatBuilder#fill} instead. */ + @Deprecated(since = "2024", forRemoval = true) public MatBuilder(Nat rows, Nat cols) { this.m_rows = Objects.requireNonNull(rows); this.m_cols = Objects.requireNonNull(cols); } + + /** + * Fills the matrix with the given data, encoded in row major form. (The matrix is filled row by + * row, left to right with the given data). + * + * @param data The data to fill the matrix with. + * @return The constructed matrix. + */ + public final Matrix fill(double... data) { + return fill(m_rows, m_cols, data); + } } diff --git a/wpimath/src/main/java/edu/wpi/first/math/Matrix.java b/wpimath/src/main/java/edu/wpi/first/math/Matrix.java index b7a86fa7d0a..8ac8d137e04 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/Matrix.java +++ b/wpimath/src/main/java/edu/wpi/first/math/Matrix.java @@ -654,7 +654,10 @@ public static Matrix eye(D dim) { * @param The number of rows of the desired matrix as a generic. * @param The number of columns of the desired matrix as a generic. * @return A builder to construct the matrix. + * @deprecated Use {@link MatBuilder#fill} instead. */ + @Deprecated(since = "2024", forRemoval = true) + @SuppressWarnings("removal") public static MatBuilder mat(Nat rows, Nat cols) { return new MatBuilder<>(Objects.requireNonNull(rows), Objects.requireNonNull(cols)); } diff --git a/wpimath/src/main/java/edu/wpi/first/math/VecBuilder.java b/wpimath/src/main/java/edu/wpi/first/math/VecBuilder.java index 9ba91b0ea5c..0e47ae35338 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/VecBuilder.java +++ b/wpimath/src/main/java/edu/wpi/first/math/VecBuilder.java @@ -17,26 +17,22 @@ import java.util.Objects; import org.ejml.simple.SimpleMatrix; -/** - * A specialization of {@link MatBuilder} for constructing vectors (Nx1 matrices). - * - * @param The dimension of the vector to be constructed. - */ -public class VecBuilder extends MatBuilder { - public VecBuilder(Nat rows) { - super(rows, Nat.N1()); +/** A class for constructing vectors (Nx1 matrices). */ +public final class VecBuilder { + private VecBuilder() { + throw new UnsupportedOperationException("this is a utility class!"); } - private Vector fillVec(double... data) { - if (Objects.requireNonNull(data).length != this.m_rows.getNum()) { + private static Vector fillVec(Nat rows, double... data) { + if (Objects.requireNonNull(data).length != rows.getNum()) { throw new IllegalArgumentException( "Invalid vector data provided. Wanted " - + this.m_rows.getNum() + + rows.getNum() + " vector, but got " + data.length + " elements"); } - return new Vector<>(new SimpleMatrix(this.m_rows.getNum(), 1, true, data)); + return new Vector<>(new SimpleMatrix(data)); } /** @@ -46,7 +42,7 @@ private Vector fillVec(double... data) { * @return 1x1 vector */ public static Vector fill(double n1) { - return new VecBuilder<>(Nat.N1()).fillVec(n1); + return fillVec(Nat.N1(), n1); } /** @@ -57,7 +53,7 @@ public static Vector fill(double n1) { * @return 2x1 vector */ public static Vector fill(double n1, double n2) { - return new VecBuilder<>(Nat.N2()).fillVec(n1, n2); + return fillVec(Nat.N2(), n1, n2); } /** @@ -69,7 +65,7 @@ public static Vector fill(double n1, double n2) { * @return 3x1 vector */ public static Vector fill(double n1, double n2, double n3) { - return new VecBuilder<>(Nat.N3()).fillVec(n1, n2, n3); + return fillVec(Nat.N3(), n1, n2, n3); } /** @@ -82,7 +78,7 @@ public static Vector fill(double n1, double n2, double n3) { * @return 4x1 vector */ public static Vector fill(double n1, double n2, double n3, double n4) { - return new VecBuilder<>(Nat.N4()).fillVec(n1, n2, n3, n4); + return fillVec(Nat.N4(), n1, n2, n3, n4); } /** @@ -96,7 +92,7 @@ public static Vector fill(double n1, double n2, double n3, double n4) { * @return 5x1 vector */ public static Vector fill(double n1, double n2, double n3, double n4, double n5) { - return new VecBuilder<>(Nat.N5()).fillVec(n1, n2, n3, n4, n5); + return fillVec(Nat.N5(), n1, n2, n3, n4, n5); } /** @@ -111,7 +107,7 @@ public static Vector fill(double n1, double n2, double n3, double n4, double * @return 6x1 vector */ public static Vector fill(double n1, double n2, double n3, double n4, double n5, double n6) { - return new VecBuilder<>(Nat.N6()).fillVec(n1, n2, n3, n4, n5, n6); + return fillVec(Nat.N6(), n1, n2, n3, n4, n5, n6); } /** @@ -128,7 +124,7 @@ public static Vector fill(double n1, double n2, double n3, double n4, double */ public static Vector fill( double n1, double n2, double n3, double n4, double n5, double n6, double n7) { - return new VecBuilder<>(Nat.N7()).fillVec(n1, n2, n3, n4, n5, n6, n7); + return fillVec(Nat.N7(), n1, n2, n3, n4, n5, n6, n7); } /** @@ -146,7 +142,7 @@ public static Vector fill( */ public static Vector fill( double n1, double n2, double n3, double n4, double n5, double n6, double n7, double n8) { - return new VecBuilder<>(Nat.N8()).fillVec(n1, n2, n3, n4, n5, n6, n7, n8); + return fillVec(Nat.N8(), n1, n2, n3, n4, n5, n6, n7, n8); } /** @@ -173,7 +169,7 @@ public static Vector fill( double n7, double n8, double n9) { - return new VecBuilder<>(Nat.N9()).fillVec(n1, n2, n3, n4, n5, n6, n7, n8, n9); + return fillVec(Nat.N9(), n1, n2, n3, n4, n5, n6, n7, n8, n9); } /** @@ -202,6 +198,6 @@ public static Vector fill( double n8, double n9, double n10) { - return new VecBuilder<>(Nat.N10()).fillVec(n1, n2, n3, n4, n5, n6, n7, n8, n9, n10); + return fillVec(Nat.N10(), n1, n2, n3, n4, n5, n6, n7, n8, n9, n10); } } diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/ArmFeedforward.java b/wpimath/src/main/java/edu/wpi/first/math/controller/ArmFeedforward.java index 2e9a8d84348..8c53f61958e 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/controller/ArmFeedforward.java +++ b/wpimath/src/main/java/edu/wpi/first/math/controller/ArmFeedforward.java @@ -4,6 +4,9 @@ package edu.wpi.first.math.controller; +import edu.wpi.first.math.controller.proto.ArmFeedforwardProto; +import edu.wpi.first.math.controller.struct.ArmFeedforwardStruct; + /** * A helper class that computes feedforward outputs for a simple arm (modeled as a motor acting * against the force of gravity on a beam suspended at an angle). @@ -14,6 +17,9 @@ public class ArmFeedforward { public final double kv; public final double ka; + public static final ArmFeedforwardProto proto = new ArmFeedforwardProto(); + public static final ArmFeedforwardStruct struct = new ArmFeedforwardStruct(); + /** * Creates a new ArmFeedforward with the specified gains. Units of the gain values will dictate * units of the computed feedforward. diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/DifferentialDriveAccelerationLimiter.java b/wpimath/src/main/java/edu/wpi/first/math/controller/DifferentialDriveAccelerationLimiter.java index 8c47765fd97..d2ef273bbe5 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/controller/DifferentialDriveAccelerationLimiter.java +++ b/wpimath/src/main/java/edu/wpi/first/math/controller/DifferentialDriveAccelerationLimiter.java @@ -5,7 +5,10 @@ package edu.wpi.first.math.controller; import edu.wpi.first.math.MatBuilder; +import edu.wpi.first.math.Matrix; import edu.wpi.first.math.Nat; +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N2; import edu.wpi.first.math.system.LinearSystem; @@ -80,10 +83,10 @@ public DifferentialDriveAccelerationLimiter( */ public DifferentialDriveWheelVoltages calculate( double leftVelocity, double rightVelocity, double leftVoltage, double rightVoltage) { - var u = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(leftVoltage, rightVoltage); + Matrix u = VecBuilder.fill(leftVoltage, rightVoltage); // Find unconstrained wheel accelerations - var x = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(leftVelocity, rightVelocity); + var x = VecBuilder.fill(leftVelocity, rightVelocity); var dxdt = m_system.getA().times(x).plus(m_system.getB().times(u)); // Convert from wheel accelerations to linear and angular accelerations @@ -98,9 +101,7 @@ public DifferentialDriveWheelVoltages calculate( // [α] [-1/trackwidth 1/trackwidth][dxdt(1)] // // accels = M dxdt where M = [0.5, 0.5; -1/trackwidth, 1/trackwidth] - var M = - new MatBuilder<>(Nat.N2(), Nat.N2()) - .fill(0.5, 0.5, -1.0 / m_trackwidth, 1.0 / m_trackwidth); + var M = MatBuilder.fill(Nat.N2(), Nat.N2(), 0.5, 0.5, -1.0 / m_trackwidth, 1.0 / m_trackwidth); var accels = M.times(dxdt); // Constrain the linear and angular accelerations diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/DifferentialDriveWheelVoltages.java b/wpimath/src/main/java/edu/wpi/first/math/controller/DifferentialDriveWheelVoltages.java index 2fbd0aa381f..ff5e81475b0 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/controller/DifferentialDriveWheelVoltages.java +++ b/wpimath/src/main/java/edu/wpi/first/math/controller/DifferentialDriveWheelVoltages.java @@ -4,11 +4,19 @@ package edu.wpi.first.math.controller; +import edu.wpi.first.math.controller.proto.DifferentialDriveWheelVoltagesProto; +import edu.wpi.first.math.controller.struct.DifferentialDriveWheelVoltagesStruct; + /** Motor voltages for a differential drive. */ public class DifferentialDriveWheelVoltages { public double left; public double right; + public static final DifferentialDriveWheelVoltagesProto proto = + new DifferentialDriveWheelVoltagesProto(); + public static final DifferentialDriveWheelVoltagesStruct struct = + new DifferentialDriveWheelVoltagesStruct(); + public DifferentialDriveWheelVoltages() {} public DifferentialDriveWheelVoltages(double left, double right) { diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/ElevatorFeedforward.java b/wpimath/src/main/java/edu/wpi/first/math/controller/ElevatorFeedforward.java index cfe7c2b8926..b7986caedf0 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/controller/ElevatorFeedforward.java +++ b/wpimath/src/main/java/edu/wpi/first/math/controller/ElevatorFeedforward.java @@ -4,8 +4,10 @@ package edu.wpi.first.math.controller; -import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.MatBuilder; import edu.wpi.first.math.Nat; +import edu.wpi.first.math.controller.proto.ElevatorFeedforwardProto; +import edu.wpi.first.math.controller.struct.ElevatorFeedforwardStruct; import edu.wpi.first.math.system.plant.LinearSystemId; /** @@ -18,6 +20,9 @@ public class ElevatorFeedforward { public final double kv; public final double ka; + public static final ElevatorFeedforwardProto proto = new ElevatorFeedforwardProto(); + public static final ElevatorFeedforwardStruct struct = new ElevatorFeedforwardStruct(); + /** * Creates a new ElevatorFeedforward with the specified gains. Units of the gain values will * dictate units of the computed feedforward. @@ -93,8 +98,8 @@ public double calculate(double currentVelocity, double nextVelocity, double dtSe var plant = LinearSystemId.identifyVelocitySystem(this.kv, this.ka); var feedforward = new LinearPlantInversionFeedforward<>(plant, dtSeconds); - var r = Matrix.mat(Nat.N1(), Nat.N1()).fill(currentVelocity); - var nextR = Matrix.mat(Nat.N1(), Nat.N1()).fill(nextVelocity); + var r = MatBuilder.fill(Nat.N1(), Nat.N1(), currentVelocity); + var nextR = MatBuilder.fill(Nat.N1(), Nat.N1(), nextVelocity); return kg + ks * Math.signum(currentVelocity) + feedforward.calculate(r, nextR).get(0, 0); } diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/LTVDifferentialDriveController.java b/wpimath/src/main/java/edu/wpi/first/math/controller/LTVDifferentialDriveController.java index 2be2a478ec0..fa287831bc0 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/controller/LTVDifferentialDriveController.java +++ b/wpimath/src/main/java/edu/wpi/first/math/controller/LTVDifferentialDriveController.java @@ -11,6 +11,7 @@ import edu.wpi.first.math.Matrix; import edu.wpi.first.math.Nat; import edu.wpi.first.math.StateSpaceUtil; +import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.Vector; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.numbers.N1; @@ -88,46 +89,48 @@ public LTVDifferentialDriveController( // Control law derivation is in section 8.7 of // https://file.tavsys.net/control/controls-engineering-in-frc.pdf var A = - new MatBuilder<>(Nat.N5(), Nat.N5()) - .fill( - 0.0, - 0.0, - 0.0, - 0.5, - 0.5, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - -1.0 / m_trackwidth, - 1.0 / m_trackwidth, - 0.0, - 0.0, - 0.0, - plant.getA(0, 0), - plant.getA(0, 1), - 0.0, - 0.0, - 0.0, - plant.getA(1, 0), - plant.getA(1, 1)); + MatBuilder.fill( + Nat.N5(), + Nat.N5(), + 0.0, + 0.0, + 0.0, + 0.5, + 0.5, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + -1.0 / m_trackwidth, + 1.0 / m_trackwidth, + 0.0, + 0.0, + 0.0, + plant.getA(0, 0), + plant.getA(0, 1), + 0.0, + 0.0, + 0.0, + plant.getA(1, 0), + plant.getA(1, 1)); var B = - new MatBuilder<>(Nat.N5(), Nat.N2()) - .fill( - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - plant.getB(0, 0), - plant.getB(0, 1), - plant.getB(1, 0), - plant.getB(1, 1)); + MatBuilder.fill( + Nat.N5(), + Nat.N2(), + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + plant.getB(0, 0), + plant.getB(0, 1), + plant.getB(1, 0), + plant.getB(1, 1)); var Q = StateSpaceUtil.makeCostMatrix(qelems); var R = StateSpaceUtil.makeCostMatrix(relems); @@ -136,11 +139,7 @@ public LTVDifferentialDriveController( // Ax = -Bu // x = -A⁻¹Bu double maxV = - plant - .getA() - .solve(plant.getB().times(new MatBuilder<>(Nat.N2(), Nat.N1()).fill(12.0, 12.0))) - .times(-1.0) - .get(0, 0); + plant.getA().solve(plant.getB().times(VecBuilder.fill(12.0, 12.0))).times(-1.0).get(0, 0); if (maxV <= 0.0) { throw new IllegalArgumentException( @@ -201,13 +200,12 @@ public boolean atReference() { public void setTolerance( Pose2d poseTolerance, double leftVelocityTolerance, double rightVelocityTolerance) { m_tolerance = - new MatBuilder<>(Nat.N5(), Nat.N1()) - .fill( - poseTolerance.getX(), - poseTolerance.getY(), - poseTolerance.getRotation().getRadians(), - leftVelocityTolerance, - rightVelocityTolerance); + VecBuilder.fill( + poseTolerance.getX(), + poseTolerance.getY(), + poseTolerance.getRotation().getRadians(), + leftVelocityTolerance, + rightVelocityTolerance); } /** @@ -234,13 +232,12 @@ public DifferentialDriveWheelVoltages calculate( // This implements the linear time-varying differential drive controller in // theorem 9.6.3 of https://tavsys.net/controls-in-frc. var x = - new MatBuilder<>(Nat.N5(), Nat.N1()) - .fill( - currentPose.getX(), - currentPose.getY(), - currentPose.getRotation().getRadians(), - leftVelocity, - rightVelocity); + VecBuilder.fill( + currentPose.getX(), + currentPose.getY(), + currentPose.getRotation().getRadians(), + leftVelocity, + rightVelocity); var inRobotFrame = Matrix.eye(Nat.N5()); inRobotFrame.set(0, 0, Math.cos(x.get(State.kHeading.value, 0))); @@ -249,13 +246,12 @@ public DifferentialDriveWheelVoltages calculate( inRobotFrame.set(1, 1, Math.cos(x.get(State.kHeading.value, 0))); var r = - new MatBuilder<>(Nat.N5(), Nat.N1()) - .fill( - poseRef.getX(), - poseRef.getY(), - poseRef.getRotation().getRadians(), - leftVelocityRef, - rightVelocityRef); + VecBuilder.fill( + poseRef.getX(), + poseRef.getY(), + poseRef.getRotation().getRadians(), + leftVelocityRef, + rightVelocityRef); m_error = r.minus(x); m_error.set( State.kHeading.value, 0, MathUtil.angleModulus(m_error.get(State.kHeading.value, 0))); diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/LTVUnicycleController.java b/wpimath/src/main/java/edu/wpi/first/math/controller/LTVUnicycleController.java index 7e3391f7336..c4e7358c9f7 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/controller/LTVUnicycleController.java +++ b/wpimath/src/main/java/edu/wpi/first/math/controller/LTVUnicycleController.java @@ -149,7 +149,7 @@ public LTVUnicycleController( // A = [0 0 v] B = [0 0] // [0 0 0] [0 1] var A = new Matrix<>(Nat.N3(), Nat.N3()); - var B = new MatBuilder<>(Nat.N3(), Nat.N2()).fill(1.0, 0.0, 0.0, 0.0, 0.0, 1.0); + var B = MatBuilder.fill(Nat.N3(), Nat.N2(), 1.0, 0.0, 0.0, 0.0, 0.0, 1.0); var Q = StateSpaceUtil.makeCostMatrix(qelems); var R = StateSpaceUtil.makeCostMatrix(relems); @@ -226,8 +226,12 @@ public ChassisSpeeds calculate( var K = m_table.get(linearVelocityRef); var e = - new MatBuilder<>(Nat.N3(), Nat.N1()) - .fill(m_poseError.getX(), m_poseError.getY(), m_poseError.getRotation().getRadians()); + MatBuilder.fill( + Nat.N3(), + Nat.N1(), + m_poseError.getX(), + m_poseError.getY(), + m_poseError.getRotation().getRadians()); var u = K.times(e); return new ChassisSpeeds( diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/SimpleMotorFeedforward.java b/wpimath/src/main/java/edu/wpi/first/math/controller/SimpleMotorFeedforward.java index 714d70527a5..63ceb8fa8f9 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/controller/SimpleMotorFeedforward.java +++ b/wpimath/src/main/java/edu/wpi/first/math/controller/SimpleMotorFeedforward.java @@ -4,7 +4,7 @@ package edu.wpi.first.math.controller; -import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.MatBuilder; import edu.wpi.first.math.Nat; import edu.wpi.first.math.system.plant.LinearSystemId; @@ -62,8 +62,8 @@ public double calculate(double currentVelocity, double nextVelocity, double dtSe var plant = LinearSystemId.identifyVelocitySystem(this.kv, this.ka); var feedforward = new LinearPlantInversionFeedforward<>(plant, dtSeconds); - var r = Matrix.mat(Nat.N1(), Nat.N1()).fill(currentVelocity); - var nextR = Matrix.mat(Nat.N1(), Nat.N1()).fill(nextVelocity); + var r = MatBuilder.fill(Nat.N1(), Nat.N1(), currentVelocity); + var nextR = MatBuilder.fill(Nat.N1(), Nat.N1(), nextVelocity); return ks * Math.signum(currentVelocity) + feedforward.calculate(r, nextR).get(0, 0); } diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/proto/ArmFeedforwardProto.java b/wpimath/src/main/java/edu/wpi/first/math/controller/proto/ArmFeedforwardProto.java new file mode 100644 index 00000000000..ef3953a54d4 --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/controller/proto/ArmFeedforwardProto.java @@ -0,0 +1,40 @@ +// 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.math.controller.proto; + +import edu.wpi.first.math.controller.ArmFeedforward; +import edu.wpi.first.math.proto.Controller.ProtobufArmFeedforward; +import edu.wpi.first.util.protobuf.Protobuf; +import us.hebi.quickbuf.Descriptors.Descriptor; + +public class ArmFeedforwardProto implements Protobuf { + @Override + public Class getTypeClass() { + return ArmFeedforward.class; + } + + @Override + public Descriptor getDescriptor() { + return ProtobufArmFeedforward.getDescriptor(); + } + + @Override + public ProtobufArmFeedforward createMessage() { + return ProtobufArmFeedforward.newInstance(); + } + + @Override + public ArmFeedforward unpack(ProtobufArmFeedforward msg) { + return new ArmFeedforward(msg.getKs(), msg.getKg(), msg.getKv(), msg.getKa()); + } + + @Override + public void pack(ProtobufArmFeedforward msg, ArmFeedforward value) { + msg.setKs(value.ks); + msg.setKg(value.kg); + msg.setKv(value.kv); + msg.setKa(value.ka); + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/proto/DifferentialDriveWheelVoltagesProto.java b/wpimath/src/main/java/edu/wpi/first/math/controller/proto/DifferentialDriveWheelVoltagesProto.java new file mode 100644 index 00000000000..c22db5d107e --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/controller/proto/DifferentialDriveWheelVoltagesProto.java @@ -0,0 +1,40 @@ +// 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.math.controller.proto; + +import edu.wpi.first.math.controller.DifferentialDriveWheelVoltages; +import edu.wpi.first.math.proto.Controller.ProtobufDifferentialDriveWheelVoltages; +import edu.wpi.first.util.protobuf.Protobuf; +import us.hebi.quickbuf.Descriptors.Descriptor; + +public class DifferentialDriveWheelVoltagesProto + implements Protobuf { + @Override + public Class getTypeClass() { + return DifferentialDriveWheelVoltages.class; + } + + @Override + public Descriptor getDescriptor() { + return ProtobufDifferentialDriveWheelVoltages.getDescriptor(); + } + + @Override + public ProtobufDifferentialDriveWheelVoltages createMessage() { + return ProtobufDifferentialDriveWheelVoltages.newInstance(); + } + + @Override + public DifferentialDriveWheelVoltages unpack(ProtobufDifferentialDriveWheelVoltages msg) { + return new DifferentialDriveWheelVoltages(msg.getLeft(), msg.getRight()); + } + + @Override + public void pack( + ProtobufDifferentialDriveWheelVoltages msg, DifferentialDriveWheelVoltages value) { + msg.setLeft(value.left); + msg.setRight(value.right); + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/proto/ElevatorFeedforwardProto.java b/wpimath/src/main/java/edu/wpi/first/math/controller/proto/ElevatorFeedforwardProto.java new file mode 100644 index 00000000000..66399e1a0c2 --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/controller/proto/ElevatorFeedforwardProto.java @@ -0,0 +1,41 @@ +// 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.math.controller.proto; + +import edu.wpi.first.math.controller.ElevatorFeedforward; +import edu.wpi.first.math.proto.Controller.ProtobufElevatorFeedforward; +import edu.wpi.first.util.protobuf.Protobuf; +import us.hebi.quickbuf.Descriptors.Descriptor; + +public class ElevatorFeedforwardProto + implements Protobuf { + @Override + public Class getTypeClass() { + return ElevatorFeedforward.class; + } + + @Override + public Descriptor getDescriptor() { + return ProtobufElevatorFeedforward.getDescriptor(); + } + + @Override + public ProtobufElevatorFeedforward createMessage() { + return ProtobufElevatorFeedforward.newInstance(); + } + + @Override + public ElevatorFeedforward unpack(ProtobufElevatorFeedforward msg) { + return new ElevatorFeedforward(msg.getKs(), msg.getKg(), msg.getKv(), msg.getKa()); + } + + @Override + public void pack(ProtobufElevatorFeedforward msg, ElevatorFeedforward value) { + msg.setKs(value.ks); + msg.setKg(value.kg); + msg.setKv(value.kv); + msg.setKa(value.ka); + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/struct/ArmFeedforwardStruct.java b/wpimath/src/main/java/edu/wpi/first/math/controller/struct/ArmFeedforwardStruct.java new file mode 100644 index 00000000000..de72fea1bd8 --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/controller/struct/ArmFeedforwardStruct.java @@ -0,0 +1,48 @@ +// 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.math.controller.struct; + +import edu.wpi.first.math.controller.ArmFeedforward; +import edu.wpi.first.util.struct.Struct; +import java.nio.ByteBuffer; + +public class ArmFeedforwardStruct implements Struct { + @Override + public Class getTypeClass() { + return ArmFeedforward.class; + } + + @Override + public String getTypeString() { + return "struct:ArmFeedforward"; + } + + @Override + public int getSize() { + return kSizeDouble * 4; + } + + @Override + public String getSchema() { + return "double ks;double kg;double kv;double ka"; + } + + @Override + public ArmFeedforward unpack(ByteBuffer bb) { + double ks = bb.getDouble(); + double kg = bb.getDouble(); + double kv = bb.getDouble(); + double ka = bb.getDouble(); + return new ArmFeedforward(ks, kg, kv, ka); + } + + @Override + public void pack(ByteBuffer bb, ArmFeedforward value) { + bb.putDouble(value.ks); + bb.putDouble(value.kg); + bb.putDouble(value.kv); + bb.putDouble(value.ka); + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/struct/DifferentialDriveWheelVoltagesStruct.java b/wpimath/src/main/java/edu/wpi/first/math/controller/struct/DifferentialDriveWheelVoltagesStruct.java new file mode 100644 index 00000000000..1695d812e04 --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/controller/struct/DifferentialDriveWheelVoltagesStruct.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.math.controller.struct; + +import edu.wpi.first.math.controller.DifferentialDriveWheelVoltages; +import edu.wpi.first.util.struct.Struct; +import java.nio.ByteBuffer; + +public class DifferentialDriveWheelVoltagesStruct + implements Struct { + @Override + public Class getTypeClass() { + return DifferentialDriveWheelVoltages.class; + } + + @Override + public String getTypeString() { + return "struct:DifferentialDriveWheelVoltages"; + } + + @Override + public int getSize() { + return kSizeDouble * 2; + } + + @Override + public String getSchema() { + return "double left;double right"; + } + + @Override + public DifferentialDriveWheelVoltages unpack(ByteBuffer bb) { + double left = bb.getDouble(); + double right = bb.getDouble(); + return new DifferentialDriveWheelVoltages(left, right); + } + + @Override + public void pack(ByteBuffer bb, DifferentialDriveWheelVoltages value) { + bb.putDouble(value.left); + bb.putDouble(value.right); + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/struct/ElevatorFeedforwardStruct.java b/wpimath/src/main/java/edu/wpi/first/math/controller/struct/ElevatorFeedforwardStruct.java new file mode 100644 index 00000000000..f8b55595304 --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/controller/struct/ElevatorFeedforwardStruct.java @@ -0,0 +1,48 @@ +// 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.math.controller.struct; + +import edu.wpi.first.math.controller.ElevatorFeedforward; +import edu.wpi.first.util.struct.Struct; +import java.nio.ByteBuffer; + +public class ElevatorFeedforwardStruct implements Struct { + @Override + public Class getTypeClass() { + return ElevatorFeedforward.class; + } + + @Override + public String getTypeString() { + return "struct:ElevatorFeedforward"; + } + + @Override + public int getSize() { + return kSizeDouble * 4; + } + + @Override + public String getSchema() { + return "double ks;double kg;double kv;double ka"; + } + + @Override + public ElevatorFeedforward unpack(ByteBuffer bb) { + double ks = bb.getDouble(); + double kg = bb.getDouble(); + double kv = bb.getDouble(); + double ka = bb.getDouble(); + return new ElevatorFeedforward(ks, kg, kv, ka); + } + + @Override + public void pack(ByteBuffer bb, ElevatorFeedforward value) { + bb.putDouble(value.ks); + bb.putDouble(value.kg); + bb.putDouble(value.kv); + bb.putDouble(value.ka); + } +} 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 bc7f536c253..9844198462d 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 @@ -4,20 +4,21 @@ package edu.wpi.first.math.geometry; +import static edu.wpi.first.units.Units.Meters; + import com.fasterxml.jackson.annotation.JsonAutoDetect; import com.fasterxml.jackson.annotation.JsonCreator; import com.fasterxml.jackson.annotation.JsonIgnoreProperties; import com.fasterxml.jackson.annotation.JsonProperty; +import edu.wpi.first.math.geometry.proto.Pose2dProto; +import edu.wpi.first.math.geometry.struct.Pose2dStruct; import edu.wpi.first.math.interpolation.Interpolatable; -import edu.wpi.first.math.proto.Geometry2D.ProtobufPose2d; -import edu.wpi.first.util.protobuf.Protobuf; -import edu.wpi.first.util.struct.Struct; -import java.nio.ByteBuffer; +import edu.wpi.first.units.Distance; +import edu.wpi.first.units.Measure; import java.util.Collections; import java.util.Comparator; import java.util.List; import java.util.Objects; -import us.hebi.quickbuf.Descriptors.Descriptor; /** Represents a 2D pose containing translational and rotational elements. */ @JsonIgnoreProperties(ignoreUnknown = true) @@ -58,6 +59,18 @@ public Pose2d(double x, double y, Rotation2d rotation) { m_rotation = rotation; } + /** + * Constructs a pose with x and y translations instead of a separate Translation2d. The X and Y + * translations will be converted to and tracked as meters. + * + * @param x The x component of the translational component of the pose. + * @param y The y component of the translational component of the pose. + * @param rotation The rotational component of the pose. + */ + public Pose2d(Measure x, Measure y, Rotation2d rotation) { + this(x.in(Meters), y.in(Meters), rotation); + } + /** * Transforms the pose by the given transformation and returns the new transformed pose. * @@ -311,82 +324,6 @@ public Pose2d interpolate(Pose2d endValue, double t) { } } - public static final class AStruct implements Struct { - @Override - public Class getTypeClass() { - return Pose2d.class; - } - - @Override - public String getTypeString() { - return "struct:Pose2d"; - } - - @Override - public int getSize() { - return Translation2d.struct.getSize() + Rotation2d.struct.getSize(); - } - - @Override - public String getSchema() { - return "Translation2d translation;Rotation2d rotation"; - } - - @Override - public Struct[] getNested() { - return new Struct[] {Translation2d.struct, Rotation2d.struct}; - } - - @Override - public Pose2d unpack(ByteBuffer bb) { - Translation2d translation = Translation2d.struct.unpack(bb); - Rotation2d rotation = Rotation2d.struct.unpack(bb); - return new Pose2d(translation, rotation); - } - - @Override - public void pack(ByteBuffer bb, Pose2d value) { - Translation2d.struct.pack(bb, value.m_translation); - Rotation2d.struct.pack(bb, value.m_rotation); - } - } - - public static final AStruct struct = new AStruct(); - - public static final class AProto implements Protobuf { - @Override - public Class getTypeClass() { - return Pose2d.class; - } - - @Override - public Descriptor getDescriptor() { - return ProtobufPose2d.getDescriptor(); - } - - @Override - public Protobuf[] getNested() { - return new Protobuf[] {Translation2d.proto, Rotation2d.proto}; - } - - @Override - public ProtobufPose2d createMessage() { - return ProtobufPose2d.newInstance(); - } - - @Override - public Pose2d unpack(ProtobufPose2d msg) { - return new Pose2d( - Translation2d.proto.unpack(msg.getTranslation()), - Rotation2d.proto.unpack(msg.getRotation())); - } - - @Override - public void pack(ProtobufPose2d msg, Pose2d value) { - Translation2d.proto.pack(msg.getMutableTranslation(), value.m_translation); - Rotation2d.proto.pack(msg.getMutableRotation(), value.m_rotation); - } - } - - public static final AProto proto = new AProto(); + public static final Pose2dStruct struct = new Pose2dStruct(); + public static final Pose2dProto proto = new Pose2dProto(); } 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 2ea709439ac..3fd5b653b81 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 @@ -9,13 +9,10 @@ import com.fasterxml.jackson.annotation.JsonIgnoreProperties; import com.fasterxml.jackson.annotation.JsonProperty; import edu.wpi.first.math.WPIMathJNI; +import edu.wpi.first.math.geometry.proto.Pose3dProto; +import edu.wpi.first.math.geometry.struct.Pose3dStruct; import edu.wpi.first.math.interpolation.Interpolatable; -import edu.wpi.first.math.proto.Geometry3D.ProtobufPose3d; -import edu.wpi.first.util.protobuf.Protobuf; -import edu.wpi.first.util.struct.Struct; -import java.nio.ByteBuffer; import java.util.Objects; -import us.hebi.quickbuf.Descriptors.Descriptor; /** Represents a 3D pose containing translational and rotational elements. */ @JsonIgnoreProperties(ignoreUnknown = true) @@ -324,82 +321,6 @@ public Pose3d interpolate(Pose3d endValue, double t) { } } - public static final class AStruct implements Struct { - @Override - public Class getTypeClass() { - return Pose3d.class; - } - - @Override - public String getTypeString() { - return "struct:Pose3d"; - } - - @Override - public int getSize() { - return Translation3d.struct.getSize() + Rotation3d.struct.getSize(); - } - - @Override - public String getSchema() { - return "Translation3d translation;Rotation3d rotation"; - } - - @Override - public Struct[] getNested() { - return new Struct[] {Translation3d.struct, Rotation3d.struct}; - } - - @Override - public Pose3d unpack(ByteBuffer bb) { - Translation3d translation = Translation3d.struct.unpack(bb); - Rotation3d rotation = Rotation3d.struct.unpack(bb); - return new Pose3d(translation, rotation); - } - - @Override - public void pack(ByteBuffer bb, Pose3d value) { - Translation3d.struct.pack(bb, value.m_translation); - Rotation3d.struct.pack(bb, value.m_rotation); - } - } - - public static final AStruct struct = new AStruct(); - - public static final class AProto implements Protobuf { - @Override - public Class getTypeClass() { - return Pose3d.class; - } - - @Override - public Descriptor getDescriptor() { - return ProtobufPose3d.getDescriptor(); - } - - @Override - public Protobuf[] getNested() { - return new Protobuf[] {Translation3d.proto, Rotation3d.proto}; - } - - @Override - public ProtobufPose3d createMessage() { - return ProtobufPose3d.newInstance(); - } - - @Override - public Pose3d unpack(ProtobufPose3d msg) { - return new Pose3d( - Translation3d.proto.unpack(msg.getTranslation()), - Rotation3d.proto.unpack(msg.getRotation())); - } - - @Override - public void pack(ProtobufPose3d msg, Pose3d value) { - Translation3d.proto.pack(msg.getMutableTranslation(), value.m_translation); - Rotation3d.proto.pack(msg.getMutableRotation(), value.m_rotation); - } - } - - public static final AProto proto = new AProto(); + public static final Pose3dStruct struct = new Pose3dStruct(); + public static final Pose3dProto proto = new Pose3dProto(); } diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Quaternion.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Quaternion.java index 23fd26b6c05..0d9ef88145a 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Quaternion.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Quaternion.java @@ -10,13 +10,10 @@ import com.fasterxml.jackson.annotation.JsonProperty; import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.Vector; +import edu.wpi.first.math.geometry.proto.QuaternionProto; +import edu.wpi.first.math.geometry.struct.QuaternionStruct; import edu.wpi.first.math.numbers.N3; -import edu.wpi.first.math.proto.Geometry3D.ProtobufQuaternion; -import edu.wpi.first.util.protobuf.Protobuf; -import edu.wpi.first.util.struct.Struct; -import java.nio.ByteBuffer; import java.util.Objects; -import us.hebi.quickbuf.Descriptors.Descriptor; @JsonIgnoreProperties(ignoreUnknown = true) @JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE) @@ -406,73 +403,6 @@ public Vector toRotationVector() { return VecBuilder.fill(coeff * getX(), coeff * getY(), coeff * getZ()); } - public static final class AStruct implements Struct { - @Override - public Class getTypeClass() { - return Quaternion.class; - } - - @Override - public String getTypeString() { - return "struct:Quaternion"; - } - - @Override - public int getSize() { - return kSizeDouble * 4; - } - - @Override - public String getSchema() { - return "double w;double x;double y;double z"; - } - - @Override - public Quaternion unpack(ByteBuffer bb) { - double w = bb.getDouble(); - double x = bb.getDouble(); - double y = bb.getDouble(); - double z = bb.getDouble(); - return new Quaternion(w, x, y, z); - } - - @Override - public void pack(ByteBuffer bb, Quaternion value) { - bb.putDouble(value.getW()); - bb.putDouble(value.getX()); - bb.putDouble(value.getY()); - bb.putDouble(value.getZ()); - } - } - - public static final AStruct struct = new AStruct(); - - public static final class AProto implements Protobuf { - @Override - public Class getTypeClass() { - return Quaternion.class; - } - - @Override - public Descriptor getDescriptor() { - return ProtobufQuaternion.getDescriptor(); - } - - @Override - public ProtobufQuaternion createMessage() { - return ProtobufQuaternion.newInstance(); - } - - @Override - public Quaternion unpack(ProtobufQuaternion msg) { - return new Quaternion(msg.getW(), msg.getX(), msg.getY(), msg.getZ()); - } - - @Override - public void pack(ProtobufQuaternion msg, Quaternion value) { - msg.setW(value.getW()).setX(value.getX()).setY(value.getY()).setZ(value.getZ()); - } - } - - public static final AProto proto = new AProto(); + public static final QuaternionStruct struct = new QuaternionStruct(); + public static final QuaternionProto proto = new QuaternionProto(); } 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 0556669d7d2..9da3fa2ea28 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 @@ -4,19 +4,20 @@ package edu.wpi.first.math.geometry; +import static edu.wpi.first.units.Units.Radians; + import com.fasterxml.jackson.annotation.JsonAutoDetect; import com.fasterxml.jackson.annotation.JsonCreator; import com.fasterxml.jackson.annotation.JsonIgnoreProperties; import com.fasterxml.jackson.annotation.JsonProperty; import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.geometry.proto.Rotation2dProto; +import edu.wpi.first.math.geometry.struct.Rotation2dStruct; import edu.wpi.first.math.interpolation.Interpolatable; -import edu.wpi.first.math.proto.Geometry2D.ProtobufRotation2d; import edu.wpi.first.math.util.Units; -import edu.wpi.first.util.protobuf.Protobuf; -import edu.wpi.first.util.struct.Struct; -import java.nio.ByteBuffer; +import edu.wpi.first.units.Angle; +import edu.wpi.first.units.Measure; import java.util.Objects; -import us.hebi.quickbuf.Descriptors.Descriptor; /** * A rotation in a 2D coordinate frame represented by a point on the unit circle (cosine and sine). @@ -69,6 +70,15 @@ public Rotation2d(double x, double y) { m_value = Math.atan2(m_sin, m_cos); } + /** + * Constructs a Rotation2d with the given angle. + * + * @param angle The angle of the rotation. + */ + public Rotation2d(Measure angle) { + this(angle.in(Radians)); + } + /** * Constructs and returns a Rotation2d with the given radian value. * @@ -262,66 +272,6 @@ public Rotation2d interpolate(Rotation2d endValue, double t) { return plus(endValue.minus(this).times(MathUtil.clamp(t, 0, 1))); } - public static final class AStruct implements Struct { - @Override - public Class getTypeClass() { - return Rotation2d.class; - } - - @Override - public String getTypeString() { - return "struct:Rotation2d"; - } - - @Override - public int getSize() { - return kSizeDouble; - } - - @Override - public String getSchema() { - return "double value"; - } - - @Override - public Rotation2d unpack(ByteBuffer bb) { - return new Rotation2d(bb.getDouble()); - } - - @Override - public void pack(ByteBuffer bb, Rotation2d value) { - bb.putDouble(value.m_value); - } - } - - public static final AStruct struct = new AStruct(); - - public static final class AProto implements Protobuf { - @Override - public Class getTypeClass() { - return Rotation2d.class; - } - - @Override - public Descriptor getDescriptor() { - return ProtobufRotation2d.getDescriptor(); - } - - @Override - public ProtobufRotation2d createMessage() { - return ProtobufRotation2d.newInstance(); - } - - @Override - public Rotation2d unpack(ProtobufRotation2d msg) { - return new Rotation2d(msg.getValue()); - } - - @Override - public void pack(ProtobufRotation2d msg, Rotation2d value) { - msg.setValue(value.m_value); - } - } - - public static final AProto proto = new AProto(); + public static final Rotation2dStruct struct = new Rotation2dStruct(); + public static final Rotation2dProto proto = new Rotation2dProto(); } diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation3d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation3d.java index b434523687e..b4f93c038f8 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation3d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation3d.java @@ -8,22 +8,18 @@ import com.fasterxml.jackson.annotation.JsonCreator; import com.fasterxml.jackson.annotation.JsonIgnoreProperties; import com.fasterxml.jackson.annotation.JsonProperty; -import edu.wpi.first.math.MatBuilder; import edu.wpi.first.math.MathSharedStore; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.Nat; import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.Vector; +import edu.wpi.first.math.geometry.proto.Rotation3dProto; +import edu.wpi.first.math.geometry.struct.Rotation3dStruct; import edu.wpi.first.math.interpolation.Interpolatable; import edu.wpi.first.math.numbers.N3; -import edu.wpi.first.math.proto.Geometry3D.ProtobufRotation3d; -import edu.wpi.first.util.protobuf.Protobuf; -import edu.wpi.first.util.struct.Struct; -import java.nio.ByteBuffer; import java.util.Objects; import org.ejml.dense.row.factory.DecompositionFactory_DDRM; -import us.hebi.quickbuf.Descriptors.Descriptor; /** A rotation in a 3D coordinate frame represented by a quaternion. */ @JsonIgnoreProperties(ignoreUnknown = true) @@ -201,9 +197,7 @@ public Rotation3d(Vector initial, Vector last) { // so a 180 degree rotation is required. Any orthogonal vector can be used // for it. Q in the QR decomposition is an orthonormal basis, so it // contains orthogonal unit vectors. - var X = - new MatBuilder<>(Nat.N3(), Nat.N1()) - .fill(initial.get(0, 0), initial.get(1, 0), initial.get(2, 0)); + var X = VecBuilder.fill(initial.get(0, 0), initial.get(1, 0), initial.get(2, 0)); final var qr = DecompositionFactory_DDRM.qr(3, 1); qr.decompose(X.getStorage().getMatrix()); final var Q = qr.getQ(null, false); @@ -440,76 +434,6 @@ public Rotation3d interpolate(Rotation3d endValue, double t) { return plus(endValue.minus(this).times(MathUtil.clamp(t, 0, 1))); } - public static final class AStruct implements Struct { - @Override - public Class getTypeClass() { - return Rotation3d.class; - } - - @Override - public String getTypeString() { - return "struct:Rotation3d"; - } - - @Override - public int getSize() { - return Quaternion.struct.getSize(); - } - - @Override - public String getSchema() { - return "Quaternion q"; - } - - @Override - public Struct[] getNested() { - return new Struct[] {Quaternion.struct}; - } - - @Override - public Rotation3d unpack(ByteBuffer bb) { - return new Rotation3d(Quaternion.struct.unpack(bb)); - } - - @Override - public void pack(ByteBuffer bb, Rotation3d value) { - Quaternion.struct.pack(bb, value.m_q); - } - } - - public static final AStruct struct = new AStruct(); - - public static final class AProto implements Protobuf { - @Override - public Class getTypeClass() { - return Rotation3d.class; - } - - @Override - public Descriptor getDescriptor() { - return ProtobufRotation3d.getDescriptor(); - } - - @Override - public Protobuf[] getNested() { - return new Protobuf[] {Quaternion.proto}; - } - - @Override - public ProtobufRotation3d createMessage() { - return ProtobufRotation3d.newInstance(); - } - - @Override - public Rotation3d unpack(ProtobufRotation3d msg) { - return new Rotation3d(Quaternion.proto.unpack(msg.getQ())); - } - - @Override - public void pack(ProtobufRotation3d msg, Rotation3d value) { - Quaternion.proto.pack(msg.getMutableQ(), value.m_q); - } - } - - public static final AProto proto = new AProto(); + public static final Rotation3dStruct struct = new Rotation3dStruct(); + public static final Rotation3dProto proto = new Rotation3dProto(); } diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Transform2d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Transform2d.java index c7959ba953b..e5a8255f922 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Transform2d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Transform2d.java @@ -4,12 +4,13 @@ package edu.wpi.first.math.geometry; -import edu.wpi.first.math.proto.Geometry2D.ProtobufTransform2d; -import edu.wpi.first.util.protobuf.Protobuf; -import edu.wpi.first.util.struct.Struct; -import java.nio.ByteBuffer; +import static edu.wpi.first.units.Units.Meters; + +import edu.wpi.first.math.geometry.proto.Transform2dProto; +import edu.wpi.first.math.geometry.struct.Transform2dStruct; +import edu.wpi.first.units.Distance; +import edu.wpi.first.units.Measure; import java.util.Objects; -import us.hebi.quickbuf.Descriptors.Descriptor; /** Represents a transformation for a Pose2d in the pose's frame. */ public class Transform2d { @@ -57,6 +58,18 @@ public Transform2d(double x, double y, Rotation2d rotation) { m_rotation = rotation; } + /** + * Constructs a transform with x and y translations instead of a separate Translation2d. The X and + * Y translations will be converted to and tracked as meters. + * + * @param x The x component of the translational component of the transform. + * @param y The y component of the translational component of the transform. + * @param rotation The rotational component of the transform. + */ + public Transform2d(Measure x, Measure y, Rotation2d rotation) { + this(x.in(Meters), y.in(Meters), rotation); + } + /** Constructs the identity transform -- maps an initial pose to itself. */ public Transform2d() { m_translation = new Translation2d(); @@ -169,82 +182,6 @@ public int hashCode() { return Objects.hash(m_translation, m_rotation); } - public static final class AStruct implements Struct { - @Override - public Class getTypeClass() { - return Transform2d.class; - } - - @Override - public String getTypeString() { - return "struct:Transform2d"; - } - - @Override - public int getSize() { - return Translation2d.struct.getSize() + Rotation2d.struct.getSize(); - } - - @Override - public String getSchema() { - return "Translation2d translation;Rotation2d rotation"; - } - - @Override - public Struct[] getNested() { - return new Struct[] {Translation2d.struct, Rotation2d.struct}; - } - - @Override - public Transform2d unpack(ByteBuffer bb) { - Translation2d translation = Translation2d.struct.unpack(bb); - Rotation2d rotation = Rotation2d.struct.unpack(bb); - return new Transform2d(translation, rotation); - } - - @Override - public void pack(ByteBuffer bb, Transform2d value) { - Translation2d.struct.pack(bb, value.m_translation); - Rotation2d.struct.pack(bb, value.m_rotation); - } - } - - public static final AStruct struct = new AStruct(); - - public static final class AProto implements Protobuf { - @Override - public Class getTypeClass() { - return Transform2d.class; - } - - @Override - public Descriptor getDescriptor() { - return ProtobufTransform2d.getDescriptor(); - } - - @Override - public Protobuf[] getNested() { - return new Protobuf[] {Translation2d.proto, Rotation2d.proto}; - } - - @Override - public ProtobufTransform2d createMessage() { - return ProtobufTransform2d.newInstance(); - } - - @Override - public Transform2d unpack(ProtobufTransform2d msg) { - return new Transform2d( - Translation2d.proto.unpack(msg.getTranslation()), - Rotation2d.proto.unpack(msg.getRotation())); - } - - @Override - public void pack(ProtobufTransform2d msg, Transform2d value) { - Translation2d.proto.pack(msg.getMutableTranslation(), value.m_translation); - Rotation2d.proto.pack(msg.getMutableRotation(), value.m_rotation); - } - } - - public static final AProto proto = new AProto(); + public static final Transform2dStruct struct = new Transform2dStruct(); + public static final Transform2dProto proto = new Transform2dProto(); } diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Transform3d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Transform3d.java index 223a14b18f6..6b4fe1f63c6 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Transform3d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Transform3d.java @@ -4,12 +4,9 @@ package edu.wpi.first.math.geometry; -import edu.wpi.first.math.proto.Geometry3D.ProtobufTransform3d; -import edu.wpi.first.util.protobuf.Protobuf; -import edu.wpi.first.util.struct.Struct; -import java.nio.ByteBuffer; +import edu.wpi.first.math.geometry.proto.Transform3dProto; +import edu.wpi.first.math.geometry.struct.Transform3dStruct; import java.util.Objects; -import us.hebi.quickbuf.Descriptors.Descriptor; /** Represents a transformation for a Pose3d in the pose's frame. */ public class Transform3d { @@ -179,82 +176,6 @@ public int hashCode() { return Objects.hash(m_translation, m_rotation); } - public static final class AStruct implements Struct { - @Override - public Class getTypeClass() { - return Transform3d.class; - } - - @Override - public String getTypeString() { - return "struct:Transform3d"; - } - - @Override - public int getSize() { - return Translation3d.struct.getSize() + Rotation3d.struct.getSize(); - } - - @Override - public String getSchema() { - return "Translation3d translation;Rotation3d rotation"; - } - - @Override - public Struct[] getNested() { - return new Struct[] {Translation3d.struct, Rotation3d.struct}; - } - - @Override - public Transform3d unpack(ByteBuffer bb) { - Translation3d translation = Translation3d.struct.unpack(bb); - Rotation3d rotation = Rotation3d.struct.unpack(bb); - return new Transform3d(translation, rotation); - } - - @Override - public void pack(ByteBuffer bb, Transform3d value) { - Translation3d.struct.pack(bb, value.m_translation); - Rotation3d.struct.pack(bb, value.m_rotation); - } - } - - public static final AStruct struct = new AStruct(); - - public static final class AProto implements Protobuf { - @Override - public Class getTypeClass() { - return Transform3d.class; - } - - @Override - public Descriptor getDescriptor() { - return ProtobufTransform3d.getDescriptor(); - } - - @Override - public Protobuf[] getNested() { - return new Protobuf[] {Translation3d.proto, Rotation3d.proto}; - } - - @Override - public ProtobufTransform3d createMessage() { - return ProtobufTransform3d.newInstance(); - } - - @Override - public Transform3d unpack(ProtobufTransform3d msg) { - return new Transform3d( - Translation3d.proto.unpack(msg.getTranslation()), - Rotation3d.proto.unpack(msg.getRotation())); - } - - @Override - public void pack(ProtobufTransform3d msg, Transform3d value) { - Translation3d.proto.pack(msg.getMutableTranslation(), value.m_translation); - Rotation3d.proto.pack(msg.getMutableRotation(), value.m_rotation); - } - } - - public static final AProto proto = new AProto(); + public static final Transform3dStruct struct = new Transform3dStruct(); + public static final Transform3dProto proto = new Transform3dProto(); } diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation2d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation2d.java index 96e0001f501..4c8d17c8dcd 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation2d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation2d.java @@ -4,21 +4,22 @@ package edu.wpi.first.math.geometry; +import static edu.wpi.first.units.Units.Meters; + import com.fasterxml.jackson.annotation.JsonAutoDetect; import com.fasterxml.jackson.annotation.JsonCreator; import com.fasterxml.jackson.annotation.JsonIgnoreProperties; import com.fasterxml.jackson.annotation.JsonProperty; import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.geometry.proto.Translation2dProto; +import edu.wpi.first.math.geometry.struct.Translation2dStruct; import edu.wpi.first.math.interpolation.Interpolatable; -import edu.wpi.first.math.proto.Geometry2D.ProtobufTranslation2d; -import edu.wpi.first.util.protobuf.Protobuf; -import edu.wpi.first.util.struct.Struct; -import java.nio.ByteBuffer; +import edu.wpi.first.units.Distance; +import edu.wpi.first.units.Measure; import java.util.Collections; import java.util.Comparator; import java.util.List; import java.util.Objects; -import us.hebi.quickbuf.Descriptors.Descriptor; /** * Represents a translation in 2D space. This object can be used to represent a point or a vector. @@ -63,6 +64,17 @@ public Translation2d(double distance, Rotation2d angle) { m_y = distance * angle.getSin(); } + /** + * Constructs a Translation2d with the X and Y components equal to the provided values. The X and + * Y components will be converted to and tracked as meters. + * + * @param x The x component of the translation. + * @param y The y component of the translation. + */ + public Translation2d(Measure x, Measure y) { + this(x.in(Meters), y.in(Meters)); + } + /** * Calculates the distance between two translations in 2D space. * @@ -235,69 +247,6 @@ public Translation2d interpolate(Translation2d endValue, double t) { MathUtil.interpolate(this.getY(), endValue.getY(), t)); } - public static final class AStruct implements Struct { - @Override - public Class getTypeClass() { - return Translation2d.class; - } - - @Override - public String getTypeString() { - return "struct:Translation2d"; - } - - @Override - public int getSize() { - return kSizeDouble * 2; - } - - @Override - public String getSchema() { - return "double x;double y"; - } - - @Override - public Translation2d unpack(ByteBuffer bb) { - double x = bb.getDouble(); - double y = bb.getDouble(); - return new Translation2d(x, y); - } - - @Override - public void pack(ByteBuffer bb, Translation2d value) { - bb.putDouble(value.m_x); - bb.putDouble(value.m_y); - } - } - - public static final AStruct struct = new AStruct(); - - public static final class AProto implements Protobuf { - @Override - public Class getTypeClass() { - return Translation2d.class; - } - - @Override - public Descriptor getDescriptor() { - return ProtobufTranslation2d.getDescriptor(); - } - - @Override - public ProtobufTranslation2d createMessage() { - return ProtobufTranslation2d.newInstance(); - } - - @Override - public Translation2d unpack(ProtobufTranslation2d msg) { - return new Translation2d(msg.getX(), msg.getY()); - } - - @Override - public void pack(ProtobufTranslation2d msg, Translation2d value) { - msg.setX(value.m_x).setY(value.m_y); - } - } - - public static final AProto proto = new AProto(); + public static final Translation2dStruct struct = new Translation2dStruct(); + public static final Translation2dProto proto = new Translation2dProto(); } 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 bc55f650c8a..8d12d56512d 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 @@ -4,18 +4,19 @@ package edu.wpi.first.math.geometry; +import static edu.wpi.first.units.Units.Meters; + import com.fasterxml.jackson.annotation.JsonAutoDetect; import com.fasterxml.jackson.annotation.JsonCreator; import com.fasterxml.jackson.annotation.JsonIgnoreProperties; import com.fasterxml.jackson.annotation.JsonProperty; import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.geometry.proto.Translation3dProto; +import edu.wpi.first.math.geometry.struct.Translation3dStruct; import edu.wpi.first.math.interpolation.Interpolatable; -import edu.wpi.first.math.proto.Geometry3D.ProtobufTranslation3d; -import edu.wpi.first.util.protobuf.Protobuf; -import edu.wpi.first.util.struct.Struct; -import java.nio.ByteBuffer; +import edu.wpi.first.units.Distance; +import edu.wpi.first.units.Measure; import java.util.Objects; -import us.hebi.quickbuf.Descriptors.Descriptor; /** * Represents a translation in 3D space. This object can be used to represent a point or a vector. @@ -67,6 +68,18 @@ public Translation3d(double distance, Rotation3d angle) { m_z = rectangular.getZ(); } + /** + * Constructs a Translation3d with the X, Y, and Z components equal to the provided values. The + * components will be converted to and tracked as meters. + * + * @param x The x component of the translation. + * @param y The y component of the translation. + * @param z The z component of the translation. + */ + public Translation3d(Measure x, Measure y, Measure z) { + this(x.in(Meters), y.in(Meters), z.in(Meters)); + } + /** * Calculates the distance between two translations in 3D space. * @@ -237,71 +250,6 @@ public Translation3d interpolate(Translation3d endValue, double t) { MathUtil.interpolate(this.getZ(), endValue.getZ(), t)); } - public static final class AStruct implements Struct { - @Override - public Class getTypeClass() { - return Translation3d.class; - } - - @Override - public String getTypeString() { - return "struct:Translation3d"; - } - - @Override - public int getSize() { - return kSizeDouble * 3; - } - - @Override - public String getSchema() { - return "double x;double y;double z"; - } - - @Override - public Translation3d unpack(ByteBuffer bb) { - double x = bb.getDouble(); - double y = bb.getDouble(); - double z = bb.getDouble(); - return new Translation3d(x, y, z); - } - - @Override - public void pack(ByteBuffer bb, Translation3d value) { - bb.putDouble(value.m_x); - bb.putDouble(value.m_y); - bb.putDouble(value.m_z); - } - } - - public static final AStruct struct = new AStruct(); - - public static final class AProto implements Protobuf { - @Override - public Class getTypeClass() { - return Translation3d.class; - } - - @Override - public Descriptor getDescriptor() { - return ProtobufTranslation3d.getDescriptor(); - } - - @Override - public ProtobufTranslation3d createMessage() { - return ProtobufTranslation3d.newInstance(); - } - - @Override - public Translation3d unpack(ProtobufTranslation3d msg) { - return new Translation3d(msg.getX(), msg.getY(), msg.getZ()); - } - - @Override - public void pack(ProtobufTranslation3d msg, Translation3d value) { - msg.setX(value.m_x).setY(value.m_y).setZ(value.m_z); - } - } - - public static final AProto proto = new AProto(); + public static final Translation3dStruct struct = new Translation3dStruct(); + public static final Translation3dProto proto = new Translation3dProto(); } diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Twist2d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Twist2d.java index a4ae9f83ac9..1a0b6786646 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Twist2d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Twist2d.java @@ -4,12 +4,9 @@ package edu.wpi.first.math.geometry; -import edu.wpi.first.math.proto.Geometry2D.ProtobufTwist2d; -import edu.wpi.first.util.protobuf.Protobuf; -import edu.wpi.first.util.struct.Struct; -import java.nio.ByteBuffer; +import edu.wpi.first.math.geometry.proto.Twist2dProto; +import edu.wpi.first.math.geometry.struct.Twist2dStruct; import java.util.Objects; -import us.hebi.quickbuf.Descriptors.Descriptor; /** * A change in distance along a 2D arc since the last pose update. We can use ideas from @@ -68,71 +65,6 @@ public int hashCode() { return Objects.hash(dx, dy, dtheta); } - public static final class AStruct implements Struct { - @Override - public Class getTypeClass() { - return Twist2d.class; - } - - @Override - public String getTypeString() { - return "struct:Twist2d"; - } - - @Override - public int getSize() { - return kSizeDouble * 3; - } - - @Override - public String getSchema() { - return "double dx;double dy;double dtheta"; - } - - @Override - public Twist2d unpack(ByteBuffer bb) { - double dx = bb.getDouble(); - double dy = bb.getDouble(); - double dtheta = bb.getDouble(); - return new Twist2d(dx, dy, dtheta); - } - - @Override - public void pack(ByteBuffer bb, Twist2d value) { - bb.putDouble(value.dx); - bb.putDouble(value.dy); - bb.putDouble(value.dtheta); - } - } - - public static final AStruct struct = new AStruct(); - - public static final class AProto implements Protobuf { - @Override - public Class getTypeClass() { - return Twist2d.class; - } - - @Override - public Descriptor getDescriptor() { - return ProtobufTwist2d.getDescriptor(); - } - - @Override - public ProtobufTwist2d createMessage() { - return ProtobufTwist2d.newInstance(); - } - - @Override - public Twist2d unpack(ProtobufTwist2d msg) { - return new Twist2d(msg.getDx(), msg.getDy(), msg.getDtheta()); - } - - @Override - public void pack(ProtobufTwist2d msg, Twist2d value) { - msg.setDx(value.dx).setDy(value.dy).setDtheta(value.dtheta); - } - } - - public static final AProto proto = new AProto(); + public static final Twist2dStruct struct = new Twist2dStruct(); + public static final Twist2dProto proto = new Twist2dProto(); } diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Twist3d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Twist3d.java index d08d5cf158a..8b2470f7273 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Twist3d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Twist3d.java @@ -4,12 +4,9 @@ package edu.wpi.first.math.geometry; -import edu.wpi.first.math.proto.Geometry3D.ProtobufTwist3d; -import edu.wpi.first.util.protobuf.Protobuf; -import edu.wpi.first.util.struct.Struct; -import java.nio.ByteBuffer; +import edu.wpi.first.math.geometry.proto.Twist3dProto; +import edu.wpi.first.math.geometry.struct.Twist3dStruct; import java.util.Objects; -import us.hebi.quickbuf.Descriptors.Descriptor; /** * A change in distance along a 3D arc since the last pose update. We can use ideas from @@ -88,83 +85,6 @@ public int hashCode() { return Objects.hash(dx, dy, dz, rx, ry, rz); } - public static final class AStruct implements Struct { - @Override - public Class getTypeClass() { - return Twist3d.class; - } - - @Override - public String getTypeString() { - return "struct:Twist3d"; - } - - @Override - public int getSize() { - return kSizeDouble * 6; - } - - @Override - public String getSchema() { - return "double dx;double dy;double dz;double rx;double ry;double rz"; - } - - @Override - public Twist3d unpack(ByteBuffer bb) { - double dx = bb.getDouble(); - double dy = bb.getDouble(); - double dz = bb.getDouble(); - double rx = bb.getDouble(); - double ry = bb.getDouble(); - double rz = bb.getDouble(); - return new Twist3d(dx, dy, dz, rx, ry, rz); - } - - @Override - public void pack(ByteBuffer bb, Twist3d value) { - bb.putDouble(value.dx); - bb.putDouble(value.dy); - bb.putDouble(value.dz); - bb.putDouble(value.rx); - bb.putDouble(value.ry); - bb.putDouble(value.rz); - } - } - - public static final AStruct struct = new AStruct(); - - public static final class AProto implements Protobuf { - @Override - public Class getTypeClass() { - return Twist3d.class; - } - - @Override - public Descriptor getDescriptor() { - return ProtobufTwist3d.getDescriptor(); - } - - @Override - public ProtobufTwist3d createMessage() { - return ProtobufTwist3d.newInstance(); - } - - @Override - public Twist3d unpack(ProtobufTwist3d msg) { - return new Twist3d( - msg.getDx(), msg.getDy(), msg.getDz(), msg.getRx(), msg.getRy(), msg.getRz()); - } - - @Override - public void pack(ProtobufTwist3d msg, Twist3d value) { - msg.setDx(value.dx) - .setDy(value.dy) - .setDz(value.dz) - .setRx(value.rx) - .setRy(value.ry) - .setRz(value.rz); - } - } - - public static final AProto proto = new AProto(); + public static final Twist3dStruct struct = new Twist3dStruct(); + public static final Twist3dProto proto = new Twist3dProto(); } diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/proto/Pose2dProto.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/proto/Pose2dProto.java new file mode 100644 index 00000000000..7921100d6e7 --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/proto/Pose2dProto.java @@ -0,0 +1,47 @@ +// 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.math.geometry.proto; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.proto.Geometry2D.ProtobufPose2d; +import edu.wpi.first.util.protobuf.Protobuf; +import us.hebi.quickbuf.Descriptors.Descriptor; + +public class Pose2dProto implements Protobuf { + @Override + public Class getTypeClass() { + return Pose2d.class; + } + + @Override + public Descriptor getDescriptor() { + return ProtobufPose2d.getDescriptor(); + } + + @Override + public Protobuf[] getNested() { + return new Protobuf[] {Translation2d.proto, Rotation2d.proto}; + } + + @Override + public ProtobufPose2d createMessage() { + return ProtobufPose2d.newInstance(); + } + + @Override + public Pose2d unpack(ProtobufPose2d msg) { + return new Pose2d( + Translation2d.proto.unpack(msg.getTranslation()), + Rotation2d.proto.unpack(msg.getRotation())); + } + + @Override + public void pack(ProtobufPose2d msg, Pose2d value) { + Translation2d.proto.pack(msg.getMutableTranslation(), value.getTranslation()); + Rotation2d.proto.pack(msg.getMutableRotation(), value.getRotation()); + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/proto/Pose3dProto.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/proto/Pose3dProto.java new file mode 100644 index 00000000000..69d6ba29264 --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/proto/Pose3dProto.java @@ -0,0 +1,47 @@ +// 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.math.geometry.proto; + +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.proto.Geometry3D.ProtobufPose3d; +import edu.wpi.first.util.protobuf.Protobuf; +import us.hebi.quickbuf.Descriptors.Descriptor; + +public class Pose3dProto implements Protobuf { + @Override + public Class getTypeClass() { + return Pose3d.class; + } + + @Override + public Descriptor getDescriptor() { + return ProtobufPose3d.getDescriptor(); + } + + @Override + public Protobuf[] getNested() { + return new Protobuf[] {Translation3d.proto, Rotation3d.proto}; + } + + @Override + public ProtobufPose3d createMessage() { + return ProtobufPose3d.newInstance(); + } + + @Override + public Pose3d unpack(ProtobufPose3d msg) { + return new Pose3d( + Translation3d.proto.unpack(msg.getTranslation()), + Rotation3d.proto.unpack(msg.getRotation())); + } + + @Override + public void pack(ProtobufPose3d msg, Pose3d value) { + Translation3d.proto.pack(msg.getMutableTranslation(), value.getTranslation()); + Rotation3d.proto.pack(msg.getMutableRotation(), value.getRotation()); + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/proto/QuaternionProto.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/proto/QuaternionProto.java new file mode 100644 index 00000000000..c464ba270c7 --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/proto/QuaternionProto.java @@ -0,0 +1,40 @@ +// 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.math.geometry.proto; + +import edu.wpi.first.math.geometry.Quaternion; +import edu.wpi.first.math.proto.Geometry3D.ProtobufQuaternion; +import edu.wpi.first.util.protobuf.Protobuf; +import us.hebi.quickbuf.Descriptors.Descriptor; + +public class QuaternionProto implements Protobuf { + @Override + public Class getTypeClass() { + return Quaternion.class; + } + + @Override + public Descriptor getDescriptor() { + return ProtobufQuaternion.getDescriptor(); + } + + @Override + public ProtobufQuaternion createMessage() { + return ProtobufQuaternion.newInstance(); + } + + @Override + public Quaternion unpack(ProtobufQuaternion msg) { + return new Quaternion(msg.getW(), msg.getX(), msg.getY(), msg.getZ()); + } + + @Override + public void pack(ProtobufQuaternion msg, Quaternion value) { + msg.setW(value.getW()); + msg.setX(value.getX()); + msg.setY(value.getY()); + msg.setZ(value.getZ()); + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/proto/Rotation2dProto.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/proto/Rotation2dProto.java new file mode 100644 index 00000000000..98ad5c7bcfe --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/proto/Rotation2dProto.java @@ -0,0 +1,37 @@ +// 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.math.geometry.proto; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.proto.Geometry2D.ProtobufRotation2d; +import edu.wpi.first.util.protobuf.Protobuf; +import us.hebi.quickbuf.Descriptors.Descriptor; + +public class Rotation2dProto implements Protobuf { + @Override + public Class getTypeClass() { + return Rotation2d.class; + } + + @Override + public Descriptor getDescriptor() { + return ProtobufRotation2d.getDescriptor(); + } + + @Override + public ProtobufRotation2d createMessage() { + return ProtobufRotation2d.newInstance(); + } + + @Override + public Rotation2d unpack(ProtobufRotation2d msg) { + return new Rotation2d(msg.getValue()); + } + + @Override + public void pack(ProtobufRotation2d msg, Rotation2d value) { + msg.setValue(value.getRadians()); + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/proto/Rotation3dProto.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/proto/Rotation3dProto.java new file mode 100644 index 00000000000..9c9b01a93b4 --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/proto/Rotation3dProto.java @@ -0,0 +1,43 @@ +// 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.math.geometry.proto; + +import edu.wpi.first.math.geometry.Quaternion; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.proto.Geometry3D.ProtobufRotation3d; +import edu.wpi.first.util.protobuf.Protobuf; +import us.hebi.quickbuf.Descriptors.Descriptor; + +public class Rotation3dProto implements Protobuf { + @Override + public Class getTypeClass() { + return Rotation3d.class; + } + + @Override + public Descriptor getDescriptor() { + return ProtobufRotation3d.getDescriptor(); + } + + @Override + public Protobuf[] getNested() { + return new Protobuf[] {Quaternion.proto}; + } + + @Override + public ProtobufRotation3d createMessage() { + return ProtobufRotation3d.newInstance(); + } + + @Override + public Rotation3d unpack(ProtobufRotation3d msg) { + return new Rotation3d(Quaternion.proto.unpack(msg.getQ())); + } + + @Override + public void pack(ProtobufRotation3d msg, Rotation3d value) { + Quaternion.proto.pack(msg.getMutableQ(), value.getQuaternion()); + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/proto/Transform2dProto.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/proto/Transform2dProto.java new file mode 100644 index 00000000000..ecc05287cf6 --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/proto/Transform2dProto.java @@ -0,0 +1,47 @@ +// 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.math.geometry.proto; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.proto.Geometry2D.ProtobufTransform2d; +import edu.wpi.first.util.protobuf.Protobuf; +import us.hebi.quickbuf.Descriptors.Descriptor; + +public class Transform2dProto implements Protobuf { + @Override + public Class getTypeClass() { + return Transform2d.class; + } + + @Override + public Descriptor getDescriptor() { + return ProtobufTransform2d.getDescriptor(); + } + + @Override + public Protobuf[] getNested() { + return new Protobuf[] {Translation2d.proto, Rotation2d.proto}; + } + + @Override + public ProtobufTransform2d createMessage() { + return ProtobufTransform2d.newInstance(); + } + + @Override + public Transform2d unpack(ProtobufTransform2d msg) { + return new Transform2d( + Translation2d.proto.unpack(msg.getTranslation()), + Rotation2d.proto.unpack(msg.getRotation())); + } + + @Override + public void pack(ProtobufTransform2d msg, Transform2d value) { + Translation2d.proto.pack(msg.getMutableTranslation(), value.getTranslation()); + Rotation2d.proto.pack(msg.getMutableRotation(), value.getRotation()); + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/proto/Transform3dProto.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/proto/Transform3dProto.java new file mode 100644 index 00000000000..29ea793e600 --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/proto/Transform3dProto.java @@ -0,0 +1,47 @@ +// 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.math.geometry.proto; + +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.proto.Geometry3D.ProtobufTransform3d; +import edu.wpi.first.util.protobuf.Protobuf; +import us.hebi.quickbuf.Descriptors.Descriptor; + +public class Transform3dProto implements Protobuf { + @Override + public Class getTypeClass() { + return Transform3d.class; + } + + @Override + public Descriptor getDescriptor() { + return ProtobufTransform3d.getDescriptor(); + } + + @Override + public Protobuf[] getNested() { + return new Protobuf[] {Translation3d.proto, Rotation3d.proto}; + } + + @Override + public ProtobufTransform3d createMessage() { + return ProtobufTransform3d.newInstance(); + } + + @Override + public Transform3d unpack(ProtobufTransform3d msg) { + return new Transform3d( + Translation3d.proto.unpack(msg.getTranslation()), + Rotation3d.proto.unpack(msg.getRotation())); + } + + @Override + public void pack(ProtobufTransform3d msg, Transform3d value) { + Translation3d.proto.pack(msg.getMutableTranslation(), value.getTranslation()); + Rotation3d.proto.pack(msg.getMutableRotation(), value.getRotation()); + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/proto/Translation2dProto.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/proto/Translation2dProto.java new file mode 100644 index 00000000000..a63fec35bdd --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/proto/Translation2dProto.java @@ -0,0 +1,38 @@ +// 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.math.geometry.proto; + +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.proto.Geometry2D.ProtobufTranslation2d; +import edu.wpi.first.util.protobuf.Protobuf; +import us.hebi.quickbuf.Descriptors.Descriptor; + +public class Translation2dProto implements Protobuf { + @Override + public Class getTypeClass() { + return Translation2d.class; + } + + @Override + public Descriptor getDescriptor() { + return ProtobufTranslation2d.getDescriptor(); + } + + @Override + public ProtobufTranslation2d createMessage() { + return ProtobufTranslation2d.newInstance(); + } + + @Override + public Translation2d unpack(ProtobufTranslation2d msg) { + return new Translation2d(msg.getX(), msg.getY()); + } + + @Override + public void pack(ProtobufTranslation2d msg, Translation2d value) { + msg.setX(value.getX()); + msg.setY(value.getY()); + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/proto/Translation3dProto.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/proto/Translation3dProto.java new file mode 100644 index 00000000000..b854a343f5e --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/proto/Translation3dProto.java @@ -0,0 +1,39 @@ +// 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.math.geometry.proto; + +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.proto.Geometry3D.ProtobufTranslation3d; +import edu.wpi.first.util.protobuf.Protobuf; +import us.hebi.quickbuf.Descriptors.Descriptor; + +public class Translation3dProto implements Protobuf { + @Override + public Class getTypeClass() { + return Translation3d.class; + } + + @Override + public Descriptor getDescriptor() { + return ProtobufTranslation3d.getDescriptor(); + } + + @Override + public ProtobufTranslation3d createMessage() { + return ProtobufTranslation3d.newInstance(); + } + + @Override + public Translation3d unpack(ProtobufTranslation3d msg) { + return new Translation3d(msg.getX(), msg.getY(), msg.getZ()); + } + + @Override + public void pack(ProtobufTranslation3d msg, Translation3d value) { + msg.setX(value.getX()); + msg.setY(value.getY()); + msg.setZ(value.getZ()); + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/proto/Twist2dProto.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/proto/Twist2dProto.java new file mode 100644 index 00000000000..bc9da6b3418 --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/proto/Twist2dProto.java @@ -0,0 +1,39 @@ +// 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.math.geometry.proto; + +import edu.wpi.first.math.geometry.Twist2d; +import edu.wpi.first.math.proto.Geometry2D.ProtobufTwist2d; +import edu.wpi.first.util.protobuf.Protobuf; +import us.hebi.quickbuf.Descriptors.Descriptor; + +public class Twist2dProto implements Protobuf { + @Override + public Class getTypeClass() { + return Twist2d.class; + } + + @Override + public Descriptor getDescriptor() { + return ProtobufTwist2d.getDescriptor(); + } + + @Override + public ProtobufTwist2d createMessage() { + return ProtobufTwist2d.newInstance(); + } + + @Override + public Twist2d unpack(ProtobufTwist2d msg) { + return new Twist2d(msg.getDx(), msg.getDy(), msg.getDtheta()); + } + + @Override + public void pack(ProtobufTwist2d msg, Twist2d value) { + msg.setDx(value.dx); + msg.setDy(value.dy); + msg.setDtheta(value.dtheta); + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/proto/Twist3dProto.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/proto/Twist3dProto.java new file mode 100644 index 00000000000..356c18f7a18 --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/proto/Twist3dProto.java @@ -0,0 +1,43 @@ +// 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.math.geometry.proto; + +import edu.wpi.first.math.geometry.Twist3d; +import edu.wpi.first.math.proto.Geometry3D.ProtobufTwist3d; +import edu.wpi.first.util.protobuf.Protobuf; +import us.hebi.quickbuf.Descriptors.Descriptor; + +public class Twist3dProto implements Protobuf { + @Override + public Class getTypeClass() { + return Twist3d.class; + } + + @Override + public Descriptor getDescriptor() { + return ProtobufTwist3d.getDescriptor(); + } + + @Override + public ProtobufTwist3d createMessage() { + return ProtobufTwist3d.newInstance(); + } + + @Override + public Twist3d unpack(ProtobufTwist3d msg) { + return new Twist3d( + msg.getDx(), msg.getDy(), msg.getDz(), msg.getRx(), msg.getRy(), msg.getRz()); + } + + @Override + public void pack(ProtobufTwist3d msg, Twist3d value) { + msg.setDx(value.dx); + msg.setDy(value.dy); + msg.setDz(value.dz); + msg.setRx(value.rx); + msg.setRy(value.ry); + msg.setRz(value.rz); + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/struct/Pose2dStruct.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/struct/Pose2dStruct.java new file mode 100644 index 00000000000..8435354db50 --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/struct/Pose2dStruct.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.math.geometry.struct; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.util.struct.Struct; +import java.nio.ByteBuffer; + +public class Pose2dStruct implements Struct { + @Override + public Class getTypeClass() { + return Pose2d.class; + } + + @Override + public String getTypeString() { + return "struct:Pose2d"; + } + + @Override + public int getSize() { + return Translation2d.struct.getSize() + Rotation2d.struct.getSize(); + } + + @Override + public String getSchema() { + return "Translation2d translation;Rotation2d rotation"; + } + + @Override + public Struct[] getNested() { + return new Struct[] {Translation2d.struct, Rotation2d.struct}; + } + + @Override + public Pose2d unpack(ByteBuffer bb) { + Translation2d translation = Translation2d.struct.unpack(bb); + Rotation2d rotation = Rotation2d.struct.unpack(bb); + return new Pose2d(translation, rotation); + } + + @Override + public void pack(ByteBuffer bb, Pose2d value) { + Translation2d.struct.pack(bb, value.getTranslation()); + Rotation2d.struct.pack(bb, value.getRotation()); + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/struct/Pose3dStruct.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/struct/Pose3dStruct.java new file mode 100644 index 00000000000..db380f6f904 --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/struct/Pose3dStruct.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.math.geometry.struct; + +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.util.struct.Struct; +import java.nio.ByteBuffer; + +public class Pose3dStruct implements Struct { + @Override + public Class getTypeClass() { + return Pose3d.class; + } + + @Override + public String getTypeString() { + return "struct:Pose3d"; + } + + @Override + public int getSize() { + return Translation3d.struct.getSize() + Rotation3d.struct.getSize(); + } + + @Override + public String getSchema() { + return "Translation3d translation;Rotation3d rotation"; + } + + @Override + public Struct[] getNested() { + return new Struct[] {Translation3d.struct, Rotation3d.struct}; + } + + @Override + public Pose3d unpack(ByteBuffer bb) { + Translation3d translation = Translation3d.struct.unpack(bb); + Rotation3d rotation = Rotation3d.struct.unpack(bb); + return new Pose3d(translation, rotation); + } + + @Override + public void pack(ByteBuffer bb, Pose3d value) { + Translation3d.struct.pack(bb, value.getTranslation()); + Rotation3d.struct.pack(bb, value.getRotation()); + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/struct/QuaternionStruct.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/struct/QuaternionStruct.java new file mode 100644 index 00000000000..fec8452b0ef --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/struct/QuaternionStruct.java @@ -0,0 +1,48 @@ +// 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.math.geometry.struct; + +import edu.wpi.first.math.geometry.Quaternion; +import edu.wpi.first.util.struct.Struct; +import java.nio.ByteBuffer; + +public class QuaternionStruct implements Struct { + @Override + public Class getTypeClass() { + return Quaternion.class; + } + + @Override + public String getTypeString() { + return "struct:Quaternion"; + } + + @Override + public int getSize() { + return kSizeDouble * 4; + } + + @Override + public String getSchema() { + return "double w;double x;double y;double z"; + } + + @Override + public Quaternion unpack(ByteBuffer bb) { + double w = bb.getDouble(); + double x = bb.getDouble(); + double y = bb.getDouble(); + double z = bb.getDouble(); + return new Quaternion(w, x, y, z); + } + + @Override + public void pack(ByteBuffer bb, Quaternion value) { + bb.putDouble(value.getW()); + bb.putDouble(value.getX()); + bb.putDouble(value.getY()); + bb.putDouble(value.getZ()); + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/struct/Rotation2dStruct.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/struct/Rotation2dStruct.java new file mode 100644 index 00000000000..b22e3d15eb1 --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/struct/Rotation2dStruct.java @@ -0,0 +1,42 @@ +// 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.math.geometry.struct; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.util.struct.Struct; +import java.nio.ByteBuffer; + +public class Rotation2dStruct implements Struct { + @Override + public Class getTypeClass() { + return Rotation2d.class; + } + + @Override + public String getTypeString() { + return "struct:Rotation2d"; + } + + @Override + public int getSize() { + return kSizeDouble; + } + + @Override + public String getSchema() { + return "double value"; + } + + @Override + public Rotation2d unpack(ByteBuffer bb) { + double value = bb.getDouble(); + return new Rotation2d(value); + } + + @Override + public void pack(ByteBuffer bb, Rotation2d value) { + bb.putDouble(value.getRadians()); + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/struct/Rotation3dStruct.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/struct/Rotation3dStruct.java new file mode 100644 index 00000000000..4184990a22f --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/struct/Rotation3dStruct.java @@ -0,0 +1,48 @@ +// 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.math.geometry.struct; + +import edu.wpi.first.math.geometry.Quaternion; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.util.struct.Struct; +import java.nio.ByteBuffer; + +public class Rotation3dStruct implements Struct { + @Override + public Class getTypeClass() { + return Rotation3d.class; + } + + @Override + public String getTypeString() { + return "struct:Rotation3d"; + } + + @Override + public int getSize() { + return Quaternion.struct.getSize(); + } + + @Override + public String getSchema() { + return "Quaternion q"; + } + + @Override + public Struct[] getNested() { + return new Struct[] {Quaternion.struct}; + } + + @Override + public Rotation3d unpack(ByteBuffer bb) { + Quaternion q = Quaternion.struct.unpack(bb); + return new Rotation3d(q); + } + + @Override + public void pack(ByteBuffer bb, Rotation3d value) { + Quaternion.struct.pack(bb, value.getQuaternion()); + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/struct/Transform2dStruct.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/struct/Transform2dStruct.java new file mode 100644 index 00000000000..298d6db4e28 --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/struct/Transform2dStruct.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.math.geometry.struct; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.util.struct.Struct; +import java.nio.ByteBuffer; + +public class Transform2dStruct implements Struct { + @Override + public Class getTypeClass() { + return Transform2d.class; + } + + @Override + public String getTypeString() { + return "struct:Transform2d"; + } + + @Override + public int getSize() { + return Translation2d.struct.getSize() + Rotation2d.struct.getSize(); + } + + @Override + public String getSchema() { + return "Translation2d translation;Rotation2d rotation"; + } + + @Override + public Struct[] getNested() { + return new Struct[] {Translation2d.struct, Rotation2d.struct}; + } + + @Override + public Transform2d unpack(ByteBuffer bb) { + Translation2d translation = Translation2d.struct.unpack(bb); + Rotation2d rotation = Rotation2d.struct.unpack(bb); + return new Transform2d(translation, rotation); + } + + @Override + public void pack(ByteBuffer bb, Transform2d value) { + Translation2d.struct.pack(bb, value.getTranslation()); + Rotation2d.struct.pack(bb, value.getRotation()); + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/struct/Transform3dStruct.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/struct/Transform3dStruct.java new file mode 100644 index 00000000000..2ad5bc153b8 --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/struct/Transform3dStruct.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.math.geometry.struct; + +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.util.struct.Struct; +import java.nio.ByteBuffer; + +public class Transform3dStruct implements Struct { + @Override + public Class getTypeClass() { + return Transform3d.class; + } + + @Override + public String getTypeString() { + return "struct:Transform3d"; + } + + @Override + public int getSize() { + return Translation3d.struct.getSize() + Rotation3d.struct.getSize(); + } + + @Override + public String getSchema() { + return "Translation3d translation;Rotation3d rotation"; + } + + @Override + public Struct[] getNested() { + return new Struct[] {Translation3d.struct, Rotation3d.struct}; + } + + @Override + public Transform3d unpack(ByteBuffer bb) { + Translation3d translation = Translation3d.struct.unpack(bb); + Rotation3d rotation = Rotation3d.struct.unpack(bb); + return new Transform3d(translation, rotation); + } + + @Override + public void pack(ByteBuffer bb, Transform3d value) { + Translation3d.struct.pack(bb, value.getTranslation()); + Rotation3d.struct.pack(bb, value.getRotation()); + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/struct/Translation2dStruct.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/struct/Translation2dStruct.java new file mode 100644 index 00000000000..aac179aacbc --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/struct/Translation2dStruct.java @@ -0,0 +1,44 @@ +// 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.math.geometry.struct; + +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.util.struct.Struct; +import java.nio.ByteBuffer; + +public class Translation2dStruct implements Struct { + @Override + public Class getTypeClass() { + return Translation2d.class; + } + + @Override + public String getTypeString() { + return "struct:Translation2d"; + } + + @Override + public int getSize() { + return kSizeDouble * 2; + } + + @Override + public String getSchema() { + return "double x;double y"; + } + + @Override + public Translation2d unpack(ByteBuffer bb) { + double x = bb.getDouble(); + double y = bb.getDouble(); + return new Translation2d(x, y); + } + + @Override + public void pack(ByteBuffer bb, Translation2d value) { + bb.putDouble(value.getX()); + bb.putDouble(value.getY()); + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/struct/Translation3dStruct.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/struct/Translation3dStruct.java new file mode 100644 index 00000000000..f4e10299fda --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/struct/Translation3dStruct.java @@ -0,0 +1,46 @@ +// 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.math.geometry.struct; + +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.util.struct.Struct; +import java.nio.ByteBuffer; + +public class Translation3dStruct implements Struct { + @Override + public Class getTypeClass() { + return Translation3d.class; + } + + @Override + public String getTypeString() { + return "struct:Translation3d"; + } + + @Override + public int getSize() { + return kSizeDouble * 3; + } + + @Override + public String getSchema() { + return "double x;double y;double z"; + } + + @Override + public Translation3d unpack(ByteBuffer bb) { + double x = bb.getDouble(); + double y = bb.getDouble(); + double z = bb.getDouble(); + return new Translation3d(x, y, z); + } + + @Override + public void pack(ByteBuffer bb, Translation3d value) { + bb.putDouble(value.getX()); + bb.putDouble(value.getY()); + bb.putDouble(value.getZ()); + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/struct/Twist2dStruct.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/struct/Twist2dStruct.java new file mode 100644 index 00000000000..7797f3ac9d1 --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/struct/Twist2dStruct.java @@ -0,0 +1,46 @@ +// 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.math.geometry.struct; + +import edu.wpi.first.math.geometry.Twist2d; +import edu.wpi.first.util.struct.Struct; +import java.nio.ByteBuffer; + +public class Twist2dStruct implements Struct { + @Override + public Class getTypeClass() { + return Twist2d.class; + } + + @Override + public String getTypeString() { + return "struct:Twist2d"; + } + + @Override + public int getSize() { + return kSizeDouble * 3; + } + + @Override + public String getSchema() { + return "double dx;double dy;double dtheta"; + } + + @Override + public Twist2d unpack(ByteBuffer bb) { + double dx = bb.getDouble(); + double dy = bb.getDouble(); + double dtheta = bb.getDouble(); + return new Twist2d(dx, dy, dtheta); + } + + @Override + public void pack(ByteBuffer bb, Twist2d value) { + bb.putDouble(value.dx); + bb.putDouble(value.dy); + bb.putDouble(value.dtheta); + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/struct/Twist3dStruct.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/struct/Twist3dStruct.java new file mode 100644 index 00000000000..68926a4c41f --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/struct/Twist3dStruct.java @@ -0,0 +1,52 @@ +// 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.math.geometry.struct; + +import edu.wpi.first.math.geometry.Twist3d; +import edu.wpi.first.util.struct.Struct; +import java.nio.ByteBuffer; + +public class Twist3dStruct implements Struct { + @Override + public Class getTypeClass() { + return Twist3d.class; + } + + @Override + public String getTypeString() { + return "struct:Twist3d"; + } + + @Override + public int getSize() { + return kSizeDouble * 6; + } + + @Override + public String getSchema() { + return "double dx;double dy;double dz;double rx;double ry;double rz"; + } + + @Override + public Twist3d unpack(ByteBuffer bb) { + double dx = bb.getDouble(); + double dy = bb.getDouble(); + double dz = bb.getDouble(); + double rx = bb.getDouble(); + double ry = bb.getDouble(); + double rz = bb.getDouble(); + return new Twist3d(dx, dy, dz, rx, ry, rz); + } + + @Override + public void pack(ByteBuffer bb, Twist3d value) { + bb.putDouble(value.dx); + bb.putDouble(value.dy); + bb.putDouble(value.dz); + bb.putDouble(value.rx); + bb.putDouble(value.ry); + bb.putDouble(value.rz); + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/ChassisSpeeds.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/ChassisSpeeds.java index ffbce71cbf0..759398a04c8 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/ChassisSpeeds.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/ChassisSpeeds.java @@ -4,9 +4,20 @@ package edu.wpi.first.math.kinematics; +import static edu.wpi.first.units.Units.MetersPerSecond; +import static edu.wpi.first.units.Units.RadiansPerSecond; +import static edu.wpi.first.units.Units.Seconds; + import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.proto.ChassisSpeedsProto; +import edu.wpi.first.math.kinematics.struct.ChassisSpeedsStruct; +import edu.wpi.first.units.Angle; +import edu.wpi.first.units.Distance; +import edu.wpi.first.units.Measure; +import edu.wpi.first.units.Time; +import edu.wpi.first.units.Velocity; /** * Represents the speed of a robot chassis. Although this class contains similar members compared to @@ -27,6 +38,9 @@ public class ChassisSpeeds { /** Represents the angular velocity of the robot frame. (CCW is +) */ public double omegaRadiansPerSecond; + public static final ChassisSpeedsProto proto = new ChassisSpeedsProto(); + public static final ChassisSpeedsStruct struct = new ChassisSpeedsStruct(); + /** Constructs a ChassisSpeeds with zeros for dx, dy, and theta. */ public ChassisSpeeds() {} @@ -44,6 +58,20 @@ public ChassisSpeeds( this.omegaRadiansPerSecond = omegaRadiansPerSecond; } + /** + * Constructs a ChassisSpeeds object. + * + * @param vx Forward velocity. + * @param vy Sideways velocity. + * @param omega Angular velocity. + */ + public ChassisSpeeds( + Measure> vx, + Measure> vy, + Measure> omega) { + this(vx.in(MetersPerSecond), vy.in(MetersPerSecond), omega.in(RadiansPerSecond)); + } + /** * Discretizes a continuous-time chassis speed. * @@ -86,6 +114,32 @@ public static ChassisSpeeds discretize( *

This is useful for compensating for translational skew when translating and rotating a * swerve drivetrain. * + * @param vx Forward velocity. + * @param vy Sideways velocity. + * @param omega Angular velocity. + * @param dt The duration of the timestep the speeds should be applied for. + * @return Discretized ChassisSpeeds. + */ + public static ChassisSpeeds discretize( + Measure> vx, + Measure> vy, + Measure> omega, + Measure

This function converts a continous-time chassis speed into a discrete-time one such that + * when the discrete-time chassis speed is applied for one timestep, the robot moves as if the + * velocity components are independent (i.e., the robot moves v_x * dt along the x-axis, v_y * dt + * along the y-axis, and omega * dt around the z-axis). + * + *

This is useful for compensating for translational skew when translating and rotating a + * swerve drivetrain. + * * @param continuousSpeeds The continuous speeds. * @param dtSeconds The duration of the timestep the speeds should be applied for. * @return Discretized ChassisSpeeds. @@ -123,6 +177,29 @@ public static ChassisSpeeds fromFieldRelativeSpeeds( return new ChassisSpeeds(rotated.getX(), rotated.getY(), omegaRadiansPerSecond); } + /** + * Converts a user provided field-relative set of speeds into a robot-relative ChassisSpeeds + * object. + * + * @param vx The component of speed in the x direction relative to the field. Positive x is away + * from your alliance wall. + * @param vy The component of speed in the y direction relative to the field. Positive y is to + * your left when standing behind the alliance wall. + * @param omega The angular rate of the robot. + * @param robotAngle The angle of the robot as measured by a gyroscope. The robot's angle is + * considered to be zero when it is facing directly away from your alliance station wall. + * Remember that this should be CCW positive. + * @return ChassisSpeeds object representing the speeds in the robot's frame of reference. + */ + public static ChassisSpeeds fromFieldRelativeSpeeds( + Measure> vx, + Measure> vy, + Measure> omega, + Rotation2d robotAngle) { + return fromFieldRelativeSpeeds( + vx.in(MetersPerSecond), vy.in(MetersPerSecond), omega.in(RadiansPerSecond), robotAngle); + } + /** * Converts a user provided field-relative ChassisSpeeds object into a robot-relative * ChassisSpeeds object. @@ -168,6 +245,29 @@ public static ChassisSpeeds fromRobotRelativeSpeeds( return new ChassisSpeeds(rotated.getX(), rotated.getY(), omegaRadiansPerSecond); } + /** + * Converts a user provided robot-relative set of speeds into a field-relative ChassisSpeeds + * object. + * + * @param vx The component of speed in the x direction relative to the robot. Positive x is + * towards the robot's front. + * @param vy The component of speed in the y direction relative to the robot. Positive y is + * towards the robot's left. + * @param omega The angular rate of the robot. + * @param robotAngle The angle of the robot as measured by a gyroscope. The robot's angle is + * considered to be zero when it is facing directly away from your alliance station wall. + * Remember that this should be CCW positive. + * @return ChassisSpeeds object representing the speeds in the field's frame of reference. + */ + public static ChassisSpeeds fromRobotRelativeSpeeds( + Measure> vx, + Measure> vy, + Measure> omega, + Rotation2d robotAngle) { + return fromRobotRelativeSpeeds( + vx.in(MetersPerSecond), vy.in(MetersPerSecond), omega.in(RadiansPerSecond), robotAngle); + } + /** * Converts a user provided robot-relative ChassisSpeeds object into a field-relative * ChassisSpeeds object. diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveKinematics.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveKinematics.java index a1fb2db3714..b892daa5745 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveKinematics.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveKinematics.java @@ -4,9 +4,15 @@ package edu.wpi.first.math.kinematics; +import static edu.wpi.first.units.Units.Meters; + import edu.wpi.first.math.MathSharedStore; import edu.wpi.first.math.MathUsageId; import edu.wpi.first.math.geometry.Twist2d; +import edu.wpi.first.math.kinematics.proto.DifferentialDriveKinematicsProto; +import edu.wpi.first.math.kinematics.struct.DifferentialDriveKinematicsStruct; +import edu.wpi.first.units.Distance; +import edu.wpi.first.units.Measure; /** * Helper class that converts a chassis velocity (dx and dtheta components) to left and right wheel @@ -20,6 +26,11 @@ public class DifferentialDriveKinematics implements Kinematics { public final double trackWidthMeters; + public static final DifferentialDriveKinematicsProto proto = + new DifferentialDriveKinematicsProto(); + public static final DifferentialDriveKinematicsStruct struct = + new DifferentialDriveKinematicsStruct(); + /** * Constructs a differential drive kinematics object. * @@ -32,6 +43,17 @@ public DifferentialDriveKinematics(double trackWidthMeters) { MathSharedStore.reportUsage(MathUsageId.kKinematics_DifferentialDrive, 1); } + /** + * Constructs a differential drive kinematics object. + * + * @param trackWidth The track width of the drivetrain. Theoretically, this is the distance + * between the left wheels and right wheels. However, the empirical value may be larger than + * the physical measured value due to scrubbing effects. + */ + public DifferentialDriveKinematics(Measure trackWidth) { + this(trackWidth.in(Meters)); + } + /** * Returns a chassis speed from left and right component velocities using forward kinematics. * diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveOdometry.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveOdometry.java index db5e8a33875..87d7bbf77fc 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveOdometry.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveOdometry.java @@ -4,10 +4,14 @@ package edu.wpi.first.math.kinematics; +import static edu.wpi.first.units.Units.Meters; + import edu.wpi.first.math.MathSharedStore; import edu.wpi.first.math.MathUsageId; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.units.Distance; +import edu.wpi.first.units.Measure; /** * Class for differential drive odometry. Odometry allows you to track the robot's position on the @@ -41,6 +45,22 @@ public DifferentialDriveOdometry( MathSharedStore.reportUsage(MathUsageId.kOdometry_DifferentialDrive, 1); } + /** + * Constructs a DifferentialDriveOdometry object. + * + * @param gyroAngle The angle reported by the gyroscope. + * @param leftDistance The distance traveled by the left encoder. + * @param rightDistance The distance traveled by the right encoder. + * @param initialPoseMeters The starting position of the robot on the field. + */ + public DifferentialDriveOdometry( + Rotation2d gyroAngle, + Measure leftDistance, + Measure rightDistance, + Pose2d initialPoseMeters) { + this(gyroAngle, leftDistance.in(Meters), rightDistance.in(Meters), initialPoseMeters); + } + /** * Constructs a DifferentialDriveOdometry object. * @@ -53,6 +73,18 @@ public DifferentialDriveOdometry( this(gyroAngle, leftDistanceMeters, rightDistanceMeters, new Pose2d()); } + /** + * Constructs a DifferentialDriveOdometry object. + * + * @param gyroAngle The angle reported by the gyroscope. + * @param leftDistance The distance traveled by the left encoder. + * @param rightDistance The distance traveled by the right encoder. + */ + public DifferentialDriveOdometry( + Rotation2d gyroAngle, Measure leftDistance, Measure rightDistance) { + this(gyroAngle, leftDistance, rightDistance, new Pose2d()); + } + /** * Resets the robot's position on the field. * @@ -75,6 +107,25 @@ public void resetPosition( poseMeters); } + /** + * Resets the robot's position on the field. + * + *

The gyroscope angle does not need to be reset here on the user's robot code. The library + * automatically takes care of offsetting the gyro angle. + * + * @param gyroAngle The angle reported by the gyroscope. + * @param leftDistance The distance traveled by the left encoder. + * @param rightDistance The distance traveled by the right encoder. + * @param poseMeters The position on the field that your robot is at. + */ + public void resetPosition( + Rotation2d gyroAngle, + Measure leftDistance, + Measure rightDistance, + Pose2d poseMeters) { + resetPosition(gyroAngle, leftDistance.in(Meters), rightDistance.in(Meters), poseMeters); + } + /** * Updates the robot position on the field using distance measurements from encoders. This method * is more numerically accurate than using velocities to integrate the pose and is also 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 18866afe974..c0a211e049b 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 @@ -4,7 +4,11 @@ package edu.wpi.first.math.kinematics; +import static edu.wpi.first.units.Units.Meters; + import edu.wpi.first.math.MathUtil; +import edu.wpi.first.units.Distance; +import edu.wpi.first.units.Measure; import java.util.Objects; public class DifferentialDriveWheelPositions @@ -26,6 +30,16 @@ public DifferentialDriveWheelPositions(double leftMeters, double rightMeters) { this.rightMeters = rightMeters; } + /** + * Constructs a DifferentialDriveWheelPositions. + * + * @param left Distance measured by the left side. + * @param right Distance measured by the right side. + */ + public DifferentialDriveWheelPositions(Measure left, Measure right) { + this(left.in(Meters), right.in(Meters)); + } + @Override public boolean equals(Object obj) { if (obj instanceof DifferentialDriveWheelPositions) { diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveWheelSpeeds.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveWheelSpeeds.java index ec874b65f33..17a48e226f3 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveWheelSpeeds.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveWheelSpeeds.java @@ -4,6 +4,14 @@ package edu.wpi.first.math.kinematics; +import static edu.wpi.first.units.Units.MetersPerSecond; + +import edu.wpi.first.math.kinematics.proto.DifferentialDriveWheelSpeedsProto; +import edu.wpi.first.math.kinematics.struct.DifferentialDriveWheelSpeedsStruct; +import edu.wpi.first.units.Distance; +import edu.wpi.first.units.Measure; +import edu.wpi.first.units.Velocity; + /** Represents the wheel speeds for a differential drive drivetrain. */ public class DifferentialDriveWheelSpeeds { /** Speed of the left side of the robot. */ @@ -12,6 +20,11 @@ public class DifferentialDriveWheelSpeeds { /** Speed of the right side of the robot. */ public double rightMetersPerSecond; + public static final DifferentialDriveWheelSpeedsProto proto = + new DifferentialDriveWheelSpeedsProto(); + public static final DifferentialDriveWheelSpeedsStruct struct = + new DifferentialDriveWheelSpeedsStruct(); + /** Constructs a DifferentialDriveWheelSpeeds with zeros for left and right speeds. */ public DifferentialDriveWheelSpeeds() {} @@ -26,6 +39,17 @@ public DifferentialDriveWheelSpeeds(double leftMetersPerSecond, double rightMete this.rightMetersPerSecond = rightMetersPerSecond; } + /** + * Constructs a DifferentialDriveWheelSpeeds. + * + * @param left The left speed. + * @param right The right speed. + */ + public DifferentialDriveWheelSpeeds( + Measure> left, Measure> right) { + this(left.in(MetersPerSecond), right.in(MetersPerSecond)); + } + /** * Renormalizes the wheel speeds if any either side is above the specified maximum. * @@ -46,6 +70,20 @@ public void desaturate(double attainableMaxSpeedMetersPerSecond) { } } + /** + * Renormalizes the wheel speeds if any either side is above the specified maximum. + * + *

Sometimes, after inverse kinematics, the requested speed from one or more wheels may be + * above the max attainable speed for the driving motor on that wheel. To fix this issue, one can + * reduce all the wheel speeds to make sure that all requested module speeds are at-or-below the + * absolute threshold, while maintaining the ratio of speeds between wheels. + * + * @param attainableMaxSpeed The absolute max speed that a wheel can reach. + */ + public void desaturate(Measure> attainableMaxSpeed) { + desaturate(attainableMaxSpeed.in(MetersPerSecond)); + } + /** * Adds two DifferentialDriveWheelSpeeds and returns the sum. * diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveKinematics.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveKinematics.java index 76c857adcc1..c9ff593a9fd 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveKinematics.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveKinematics.java @@ -8,6 +8,8 @@ import edu.wpi.first.math.MathUsageId; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.geometry.Twist2d; +import edu.wpi.first.math.kinematics.proto.MecanumDriveKinematicsProto; +import edu.wpi.first.math.kinematics.struct.MecanumDriveKinematicsStruct; import org.ejml.simple.SimpleMatrix; /** @@ -42,6 +44,9 @@ public class MecanumDriveKinematics private Translation2d m_prevCoR = new Translation2d(); + public static final MecanumDriveKinematicsProto proto = new MecanumDriveKinematicsProto(); + public static final MecanumDriveKinematicsStruct struct = new MecanumDriveKinematicsStruct(); + /** * Constructs a mecanum drive kinematics object. * @@ -207,4 +212,20 @@ private void setInverseKinematics( m_inverseKinematics.setRow(2, 0, 1, 1, rl.getX() - rl.getY()); m_inverseKinematics.setRow(3, 0, 1, -1, -(rr.getX() + rr.getY())); } + + public Translation2d getFrontLeft() { + return m_frontLeftWheelMeters; + } + + public Translation2d getFrontRight() { + return m_frontRightWheelMeters; + } + + public Translation2d getRearLeft() { + return m_rearLeftWheelMeters; + } + + public Translation2d getRearRight() { + return m_rearRightWheelMeters; + } } diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveWheelPositions.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveWheelPositions.java index 2b284cb27b4..5515802fc40 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveWheelPositions.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveWheelPositions.java @@ -4,7 +4,13 @@ package edu.wpi.first.math.kinematics; +import static edu.wpi.first.units.Units.Meters; + import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.kinematics.proto.MecanumDriveWheelPositionsProto; +import edu.wpi.first.math.kinematics.struct.MecanumDriveWheelPositionsStruct; +import edu.wpi.first.units.Distance; +import edu.wpi.first.units.Measure; import java.util.Objects; public class MecanumDriveWheelPositions implements WheelPositions { @@ -20,6 +26,10 @@ public class MecanumDriveWheelPositions implements WheelPositions frontLeft, + Measure frontRight, + Measure rearLeft, + Measure rearRight) { + this(frontLeft.in(Meters), frontRight.in(Meters), rearLeft.in(Meters), rearRight.in(Meters)); + } + @Override public boolean equals(Object obj) { if (obj instanceof MecanumDriveWheelPositions) { diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveWheelSpeeds.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveWheelSpeeds.java index 63cef1838f0..64d712aa10f 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveWheelSpeeds.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveWheelSpeeds.java @@ -4,6 +4,13 @@ package edu.wpi.first.math.kinematics; +import static edu.wpi.first.units.Units.MetersPerSecond; + +import edu.wpi.first.math.kinematics.proto.MecanumDriveWheelSpeedsProto; +import edu.wpi.first.math.kinematics.struct.MecanumDriveWheelSpeedsStruct; +import edu.wpi.first.units.Distance; +import edu.wpi.first.units.Measure; +import edu.wpi.first.units.Velocity; import java.util.stream.DoubleStream; public class MecanumDriveWheelSpeeds { @@ -19,6 +26,9 @@ public class MecanumDriveWheelSpeeds { /** Speed of the rear right wheel. */ public double rearRightMetersPerSecond; + public static final MecanumDriveWheelSpeedsStruct struct = new MecanumDriveWheelSpeedsStruct(); + public static final MecanumDriveWheelSpeedsProto proto = new MecanumDriveWheelSpeedsProto(); + /** Constructs a MecanumDriveWheelSpeeds with zeros for all member fields. */ public MecanumDriveWheelSpeeds() {} @@ -41,6 +51,26 @@ public MecanumDriveWheelSpeeds( this.rearRightMetersPerSecond = rearRightMetersPerSecond; } + /** + * Constructs a MecanumDriveWheelSpeeds. + * + * @param frontLeft Speed of the front left wheel. + * @param frontRight Speed of the front right wheel. + * @param rearLeft Speed of the rear left wheel. + * @param rearRight Speed of the rear right wheel. + */ + public MecanumDriveWheelSpeeds( + Measure> frontLeft, + Measure> frontRight, + Measure> rearLeft, + Measure> rearRight) { + this( + frontLeft.in(MetersPerSecond), + frontRight.in(MetersPerSecond), + rearLeft.in(MetersPerSecond), + rearRight.in(MetersPerSecond)); + } + /** * Renormalizes the wheel speeds if any individual speed is above the specified maximum. * @@ -73,6 +103,20 @@ public void desaturate(double attainableMaxSpeedMetersPerSecond) { } } + /** + * Renormalizes the wheel speeds if any individual speed is above the specified maximum. + * + *

Sometimes, after inverse kinematics, the requested speed from one or more wheels may be + * above the max attainable speed for the driving motor on that wheel. To fix this issue, one can + * reduce all the wheel speeds to make sure that all requested module speeds are at-or-below the + * absolute threshold, while maintaining the ratio of speeds between wheels. + * + * @param attainableMaxSpeed The absolute max speed that a wheel can reach. + */ + public void desaturate(Measure> attainableMaxSpeed) { + desaturate(attainableMaxSpeed.in(MetersPerSecond)); + } + /** * Adds two MecanumDriveWheelSpeeds and returns the sum. * diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveKinematics.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveKinematics.java index aa5338e6eb2..2498b337427 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveKinematics.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveKinematics.java @@ -4,11 +4,18 @@ package edu.wpi.first.math.kinematics; +import static edu.wpi.first.units.Units.MetersPerSecond; +import static edu.wpi.first.units.Units.RadiansPerSecond; + import edu.wpi.first.math.MathSharedStore; import edu.wpi.first.math.MathUsageId; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.geometry.Twist2d; +import edu.wpi.first.units.Angle; +import edu.wpi.first.units.Distance; +import edu.wpi.first.units.Measure; +import edu.wpi.first.units.Velocity; import java.util.Arrays; import org.ejml.simple.SimpleMatrix; @@ -302,6 +309,23 @@ public static void desaturateWheelSpeeds( } } + /** + * Renormalizes the wheel speeds if any individual speed is above the specified maximum. + * + *

Sometimes, after inverse kinematics, the requested speed from one or more modules may be + * above the max attainable speed for the driving motor on that module. To fix this issue, one can + * reduce all the wheel speeds to make sure that all requested module speeds are at-or-below the + * absolute threshold, while maintaining the ratio of speeds between modules. + * + * @param moduleStates Reference to array of module states. The array will be mutated with the + * normalized speeds! + * @param attainableMaxSpeed The absolute max speed that a module can reach. + */ + public static void desaturateWheelSpeeds( + SwerveModuleState[] moduleStates, Measure> attainableMaxSpeed) { + desaturateWheelSpeeds(moduleStates, attainableMaxSpeed.in(MetersPerSecond)); + } + /** * Renormalizes the wheel speeds if any individual speed is above the specified maximum, as well * as getting rid of joystick saturation at edges of joystick. @@ -348,4 +372,36 @@ public static void desaturateWheelSpeeds( moduleState.speedMetersPerSecond *= scale; } } + + /** + * Renormalizes the wheel speeds if any individual speed is above the specified maximum, as well + * as getting rid of joystick saturation at edges of joystick. + * + *

Sometimes, after inverse kinematics, the requested speed from one or more modules may be + * above the max attainable speed for the driving motor on that module. To fix this issue, one can + * reduce all the wheel speeds to make sure that all requested module speeds are at-or-below the + * absolute threshold, while maintaining the ratio of speeds between modules. + * + * @param moduleStates Reference to array of module states. The array will be mutated with the + * normalized speeds! + * @param desiredChassisSpeed The desired speed of the robot + * @param attainableMaxModuleSpeed The absolute max speed that a module can reach + * @param attainableMaxTranslationalSpeed The absolute max speed that your robot can reach while + * translating + * @param attainableMaxRotationalVelocity The absolute max speed the robot can reach while + * rotating + */ + public static void desaturateWheelSpeeds( + SwerveModuleState[] moduleStates, + ChassisSpeeds desiredChassisSpeed, + Measure> attainableMaxModuleSpeed, + Measure> attainableMaxTranslationalSpeed, + Measure> attainableMaxRotationalVelocity) { + desaturateWheelSpeeds( + moduleStates, + desiredChassisSpeed, + attainableMaxModuleSpeed.in(MetersPerSecond), + attainableMaxTranslationalSpeed.in(MetersPerSecond), + attainableMaxRotationalVelocity.in(RadiansPerSecond)); + } } diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveModulePosition.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveModulePosition.java index d058764f3b3..9583dbd3134 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveModulePosition.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveModulePosition.java @@ -4,9 +4,15 @@ package edu.wpi.first.math.kinematics; +import static edu.wpi.first.units.Units.Meters; + import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.interpolation.Interpolatable; +import edu.wpi.first.math.kinematics.proto.SwerveModulePositionProto; +import edu.wpi.first.math.kinematics.struct.SwerveModulePositionStruct; +import edu.wpi.first.units.Distance; +import edu.wpi.first.units.Measure; import java.util.Objects; /** Represents the state of one swerve module. */ @@ -18,6 +24,9 @@ public class SwerveModulePosition /** Angle of the module. */ public Rotation2d angle = Rotation2d.fromDegrees(0); + public static final SwerveModulePositionStruct struct = new SwerveModulePositionStruct(); + public static final SwerveModulePositionProto proto = new SwerveModulePositionProto(); + /** Constructs a SwerveModulePosition with zeros for distance and angle. */ public SwerveModulePosition() {} @@ -32,6 +41,16 @@ public SwerveModulePosition(double distanceMeters, Rotation2d angle) { this.angle = angle; } + /** + * Constructs a SwerveModulePosition. + * + * @param distance The distance measured by the wheel of the module. + * @param angle The angle of the module. + */ + public SwerveModulePosition(Measure distance, Rotation2d angle) { + this(distance.in(Meters), angle); + } + @Override public boolean equals(Object obj) { if (obj instanceof SwerveModulePosition) { diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveModuleState.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveModuleState.java index 10dee4fea11..bbc3b205e59 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveModuleState.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveModuleState.java @@ -4,7 +4,14 @@ package edu.wpi.first.math.kinematics; +import static edu.wpi.first.units.Units.MetersPerSecond; + import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.proto.SwerveModuleStateProto; +import edu.wpi.first.math.kinematics.struct.SwerveModuleStateStruct; +import edu.wpi.first.units.Distance; +import edu.wpi.first.units.Measure; +import edu.wpi.first.units.Velocity; import java.util.Objects; /** Represents the state of one swerve module. */ @@ -15,6 +22,9 @@ public class SwerveModuleState implements Comparable { /** Angle of the module. */ public Rotation2d angle = Rotation2d.fromDegrees(0); + public static final SwerveModuleStateStruct struct = new SwerveModuleStateStruct(); + public static final SwerveModuleStateProto proto = new SwerveModuleStateProto(); + /** Constructs a SwerveModuleState with zeros for speed and angle. */ public SwerveModuleState() {} @@ -29,6 +39,16 @@ public SwerveModuleState(double speedMetersPerSecond, Rotation2d angle) { this.angle = angle; } + /** + * Constructs a SwerveModuleState. + * + * @param speed The speed of the wheel of the module. + * @param angle The angle of the module. + */ + public SwerveModuleState(Measure> speed, Rotation2d angle) { + this(speed.in(MetersPerSecond), angle); + } + @Override public boolean equals(Object obj) { if (obj instanceof SwerveModuleState) { diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/ChassisSpeedsProto.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/ChassisSpeedsProto.java new file mode 100644 index 00000000000..b20d23bbc9f --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/ChassisSpeedsProto.java @@ -0,0 +1,39 @@ +// 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.math.kinematics.proto; + +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.proto.Kinematics.ProtobufChassisSpeeds; +import edu.wpi.first.util.protobuf.Protobuf; +import us.hebi.quickbuf.Descriptors.Descriptor; + +public class ChassisSpeedsProto implements Protobuf { + @Override + public Class getTypeClass() { + return ChassisSpeeds.class; + } + + @Override + public Descriptor getDescriptor() { + return ProtobufChassisSpeeds.getDescriptor(); + } + + @Override + public ProtobufChassisSpeeds createMessage() { + return ProtobufChassisSpeeds.newInstance(); + } + + @Override + public ChassisSpeeds unpack(ProtobufChassisSpeeds msg) { + return new ChassisSpeeds(msg.getVx(), msg.getVy(), msg.getOmega()); + } + + @Override + public void pack(ProtobufChassisSpeeds msg, ChassisSpeeds value) { + msg.setVx(value.vxMetersPerSecond); + msg.setVy(value.vyMetersPerSecond); + msg.setOmega(value.omegaRadiansPerSecond); + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/DifferentialDriveKinematicsProto.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/DifferentialDriveKinematicsProto.java new file mode 100644 index 00000000000..d1a8969d162 --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/DifferentialDriveKinematicsProto.java @@ -0,0 +1,38 @@ +// 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.math.kinematics.proto; + +import edu.wpi.first.math.kinematics.DifferentialDriveKinematics; +import edu.wpi.first.math.proto.Kinematics.ProtobufDifferentialDriveKinematics; +import edu.wpi.first.util.protobuf.Protobuf; +import us.hebi.quickbuf.Descriptors.Descriptor; + +public class DifferentialDriveKinematicsProto + implements Protobuf { + @Override + public Class getTypeClass() { + return DifferentialDriveKinematics.class; + } + + @Override + public Descriptor getDescriptor() { + return ProtobufDifferentialDriveKinematics.getDescriptor(); + } + + @Override + public ProtobufDifferentialDriveKinematics createMessage() { + return ProtobufDifferentialDriveKinematics.newInstance(); + } + + @Override + public DifferentialDriveKinematics unpack(ProtobufDifferentialDriveKinematics msg) { + return new DifferentialDriveKinematics(msg.getTrackWidth()); + } + + @Override + public void pack(ProtobufDifferentialDriveKinematics msg, DifferentialDriveKinematics value) { + msg.setTrackWidth(value.trackWidthMeters); + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/DifferentialDriveWheelSpeedsProto.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/DifferentialDriveWheelSpeedsProto.java new file mode 100644 index 00000000000..e30df565749 --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/DifferentialDriveWheelSpeedsProto.java @@ -0,0 +1,39 @@ +// 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.math.kinematics.proto; + +import edu.wpi.first.math.kinematics.DifferentialDriveWheelSpeeds; +import edu.wpi.first.math.proto.Kinematics.ProtobufDifferentialDriveWheelSpeeds; +import edu.wpi.first.util.protobuf.Protobuf; +import us.hebi.quickbuf.Descriptors.Descriptor; + +public class DifferentialDriveWheelSpeedsProto + implements Protobuf { + @Override + public Class getTypeClass() { + return DifferentialDriveWheelSpeeds.class; + } + + @Override + public Descriptor getDescriptor() { + return ProtobufDifferentialDriveWheelSpeeds.getDescriptor(); + } + + @Override + public ProtobufDifferentialDriveWheelSpeeds createMessage() { + return ProtobufDifferentialDriveWheelSpeeds.newInstance(); + } + + @Override + public DifferentialDriveWheelSpeeds unpack(ProtobufDifferentialDriveWheelSpeeds msg) { + return new DifferentialDriveWheelSpeeds(msg.getLeft(), msg.getRight()); + } + + @Override + public void pack(ProtobufDifferentialDriveWheelSpeeds msg, DifferentialDriveWheelSpeeds value) { + msg.setLeft(value.leftMetersPerSecond); + msg.setRight(value.rightMetersPerSecond); + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/MecanumDriveKinematicsProto.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/MecanumDriveKinematicsProto.java new file mode 100644 index 00000000000..f5b8f3bdfb0 --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/MecanumDriveKinematicsProto.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.math.kinematics.proto; + +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.MecanumDriveKinematics; +import edu.wpi.first.math.proto.Kinematics.ProtobufMecanumDriveKinematics; +import edu.wpi.first.util.protobuf.Protobuf; +import us.hebi.quickbuf.Descriptors.Descriptor; + +public class MecanumDriveKinematicsProto + implements Protobuf { + @Override + public Class getTypeClass() { + return MecanumDriveKinematics.class; + } + + @Override + public Descriptor getDescriptor() { + return ProtobufMecanumDriveKinematics.getDescriptor(); + } + + @Override + public Protobuf[] getNested() { + return new Protobuf[] {Translation2d.proto}; + } + + @Override + public ProtobufMecanumDriveKinematics createMessage() { + return ProtobufMecanumDriveKinematics.newInstance(); + } + + @Override + public MecanumDriveKinematics unpack(ProtobufMecanumDriveKinematics msg) { + return new MecanumDriveKinematics( + Translation2d.proto.unpack(msg.getFrontLeft()), + Translation2d.proto.unpack(msg.getFrontRight()), + Translation2d.proto.unpack(msg.getRearLeft()), + Translation2d.proto.unpack(msg.getRearRight())); + } + + @Override + public void pack(ProtobufMecanumDriveKinematics msg, MecanumDriveKinematics value) { + Translation2d.proto.pack(msg.getMutableFrontLeft(), value.getFrontLeft()); + Translation2d.proto.pack(msg.getMutableFrontRight(), value.getFrontRight()); + Translation2d.proto.pack(msg.getMutableRearLeft(), value.getRearLeft()); + Translation2d.proto.pack(msg.getMutableRearRight(), value.getRearRight()); + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/MecanumDriveWheelPositionsProto.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/MecanumDriveWheelPositionsProto.java new file mode 100644 index 00000000000..d4f0f5a3ce4 --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/MecanumDriveWheelPositionsProto.java @@ -0,0 +1,42 @@ +// 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.math.kinematics.proto; + +import edu.wpi.first.math.kinematics.MecanumDriveWheelPositions; +import edu.wpi.first.math.proto.Kinematics.ProtobufMecanumDriveWheelPositions; +import edu.wpi.first.util.protobuf.Protobuf; +import us.hebi.quickbuf.Descriptors.Descriptor; + +public class MecanumDriveWheelPositionsProto + implements Protobuf { + @Override + public Class getTypeClass() { + return MecanumDriveWheelPositions.class; + } + + @Override + public Descriptor getDescriptor() { + return ProtobufMecanumDriveWheelPositions.getDescriptor(); + } + + @Override + public ProtobufMecanumDriveWheelPositions createMessage() { + return ProtobufMecanumDriveWheelPositions.newInstance(); + } + + @Override + public MecanumDriveWheelPositions unpack(ProtobufMecanumDriveWheelPositions msg) { + return new MecanumDriveWheelPositions( + msg.getFrontLeft(), msg.getFrontRight(), msg.getRearLeft(), msg.getRearRight()); + } + + @Override + public void pack(ProtobufMecanumDriveWheelPositions msg, MecanumDriveWheelPositions value) { + msg.setFrontLeft(value.frontLeftMeters); + msg.setFrontRight(value.frontRightMeters); + msg.setRearLeft(value.rearLeftMeters); + msg.setRearRight(value.rearRightMeters); + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/MecanumDriveWheelSpeedsProto.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/MecanumDriveWheelSpeedsProto.java new file mode 100644 index 00000000000..85e8fd10a35 --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/MecanumDriveWheelSpeedsProto.java @@ -0,0 +1,42 @@ +// 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.math.kinematics.proto; + +import edu.wpi.first.math.kinematics.MecanumDriveWheelSpeeds; +import edu.wpi.first.math.proto.Kinematics.ProtobufMecanumDriveWheelSpeeds; +import edu.wpi.first.util.protobuf.Protobuf; +import us.hebi.quickbuf.Descriptors.Descriptor; + +public class MecanumDriveWheelSpeedsProto + implements Protobuf { + @Override + public Class getTypeClass() { + return MecanumDriveWheelSpeeds.class; + } + + @Override + public Descriptor getDescriptor() { + return ProtobufMecanumDriveWheelSpeeds.getDescriptor(); + } + + @Override + public ProtobufMecanumDriveWheelSpeeds createMessage() { + return ProtobufMecanumDriveWheelSpeeds.newInstance(); + } + + @Override + public MecanumDriveWheelSpeeds unpack(ProtobufMecanumDriveWheelSpeeds msg) { + return new MecanumDriveWheelSpeeds( + msg.getFrontLeft(), msg.getFrontRight(), msg.getRearLeft(), msg.getRearRight()); + } + + @Override + public void pack(ProtobufMecanumDriveWheelSpeeds msg, MecanumDriveWheelSpeeds value) { + msg.setFrontLeft(value.frontLeftMetersPerSecond); + msg.setFrontRight(value.frontRightMetersPerSecond); + msg.setRearLeft(value.rearLeftMetersPerSecond); + msg.setRearRight(value.rearRightMetersPerSecond); + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/SwerveModulePositionProto.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/SwerveModulePositionProto.java new file mode 100644 index 00000000000..fe5ec5fc661 --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/SwerveModulePositionProto.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.math.kinematics.proto; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.math.proto.Kinematics.ProtobufSwerveModulePosition; +import edu.wpi.first.util.protobuf.Protobuf; +import us.hebi.quickbuf.Descriptors.Descriptor; + +public class SwerveModulePositionProto + implements Protobuf { + @Override + public Class getTypeClass() { + return SwerveModulePosition.class; + } + + @Override + public Descriptor getDescriptor() { + return ProtobufSwerveModulePosition.getDescriptor(); + } + + @Override + public Protobuf[] getNested() { + return new Protobuf[] {Rotation2d.proto}; + } + + @Override + public ProtobufSwerveModulePosition createMessage() { + return ProtobufSwerveModulePosition.newInstance(); + } + + @Override + public SwerveModulePosition unpack(ProtobufSwerveModulePosition msg) { + return new SwerveModulePosition(msg.getDistance(), Rotation2d.proto.unpack(msg.getAngle())); + } + + @Override + public void pack(ProtobufSwerveModulePosition msg, SwerveModulePosition value) { + msg.setDistance(value.distanceMeters); + Rotation2d.proto.pack(msg.getMutableAngle(), value.angle); + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/SwerveModuleStateProto.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/SwerveModuleStateProto.java new file mode 100644 index 00000000000..dc30f91117b --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/SwerveModuleStateProto.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.math.kinematics.proto; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.math.proto.Kinematics.ProtobufSwerveModuleState; +import edu.wpi.first.util.protobuf.Protobuf; +import us.hebi.quickbuf.Descriptors.Descriptor; + +public class SwerveModuleStateProto + implements Protobuf { + @Override + public Class getTypeClass() { + return SwerveModuleState.class; + } + + @Override + public Descriptor getDescriptor() { + return ProtobufSwerveModuleState.getDescriptor(); + } + + @Override + public Protobuf[] getNested() { + return new Protobuf[] {Rotation2d.proto}; + } + + @Override + public ProtobufSwerveModuleState createMessage() { + return ProtobufSwerveModuleState.newInstance(); + } + + @Override + public SwerveModuleState unpack(ProtobufSwerveModuleState msg) { + return new SwerveModuleState(msg.getSpeed(), Rotation2d.proto.unpack(msg.getAngle())); + } + + @Override + public void pack(ProtobufSwerveModuleState msg, SwerveModuleState value) { + msg.setSpeed(value.speedMetersPerSecond); + Rotation2d.proto.pack(msg.getMutableAngle(), value.angle); + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/ChassisSpeedsStruct.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/ChassisSpeedsStruct.java new file mode 100644 index 00000000000..ea26f86e629 --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/ChassisSpeedsStruct.java @@ -0,0 +1,46 @@ +// 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.math.kinematics.struct; + +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.util.struct.Struct; +import java.nio.ByteBuffer; + +public class ChassisSpeedsStruct implements Struct { + @Override + public Class getTypeClass() { + return ChassisSpeeds.class; + } + + @Override + public String getTypeString() { + return "struct:ChassisSpeeds"; + } + + @Override + public int getSize() { + return kSizeDouble * 3; + } + + @Override + public String getSchema() { + return "double vx;double vy;double omega"; + } + + @Override + public ChassisSpeeds unpack(ByteBuffer bb) { + double vx = bb.getDouble(); + double vy = bb.getDouble(); + double omega = bb.getDouble(); + return new ChassisSpeeds(vx, vy, omega); + } + + @Override + public void pack(ByteBuffer bb, ChassisSpeeds value) { + bb.putDouble(value.vxMetersPerSecond); + bb.putDouble(value.vyMetersPerSecond); + bb.putDouble(value.omegaRadiansPerSecond); + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/DifferentialDriveKinematicsStruct.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/DifferentialDriveKinematicsStruct.java new file mode 100644 index 00000000000..1ddadaac1ff --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/DifferentialDriveKinematicsStruct.java @@ -0,0 +1,42 @@ +// 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.math.kinematics.struct; + +import edu.wpi.first.math.kinematics.DifferentialDriveKinematics; +import edu.wpi.first.util.struct.Struct; +import java.nio.ByteBuffer; + +public class DifferentialDriveKinematicsStruct implements Struct { + @Override + public Class getTypeClass() { + return DifferentialDriveKinematics.class; + } + + @Override + public String getTypeString() { + return "struct:DifferentialDriveKinematics"; + } + + @Override + public int getSize() { + return kSizeDouble; + } + + @Override + public String getSchema() { + return "double track_width"; + } + + @Override + public DifferentialDriveKinematics unpack(ByteBuffer bb) { + double trackWidth = bb.getDouble(); + return new DifferentialDriveKinematics(trackWidth); + } + + @Override + public void pack(ByteBuffer bb, DifferentialDriveKinematics value) { + bb.putDouble(value.trackWidthMeters); + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/DifferentialDriveWheelSpeedsStruct.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/DifferentialDriveWheelSpeedsStruct.java new file mode 100644 index 00000000000..c1242479056 --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/DifferentialDriveWheelSpeedsStruct.java @@ -0,0 +1,44 @@ +// 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.math.kinematics.struct; + +import edu.wpi.first.math.kinematics.DifferentialDriveWheelSpeeds; +import edu.wpi.first.util.struct.Struct; +import java.nio.ByteBuffer; + +public class DifferentialDriveWheelSpeedsStruct implements Struct { + @Override + public Class getTypeClass() { + return DifferentialDriveWheelSpeeds.class; + } + + @Override + public String getTypeString() { + return "struct:DifferentialDriveWheelSpeeds"; + } + + @Override + public int getSize() { + return kSizeDouble * 2; + } + + @Override + public String getSchema() { + return "double left;double right"; + } + + @Override + public DifferentialDriveWheelSpeeds unpack(ByteBuffer bb) { + double left = bb.getDouble(); + double right = bb.getDouble(); + return new DifferentialDriveWheelSpeeds(left, right); + } + + @Override + public void pack(ByteBuffer bb, DifferentialDriveWheelSpeeds value) { + bb.putDouble(value.leftMetersPerSecond); + bb.putDouble(value.rightMetersPerSecond); + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/MecanumDriveKinematicsStruct.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/MecanumDriveKinematicsStruct.java new file mode 100644 index 00000000000..227f80ed402 --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/MecanumDriveKinematicsStruct.java @@ -0,0 +1,55 @@ +// 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.math.kinematics.struct; + +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.MecanumDriveKinematics; +import edu.wpi.first.util.struct.Struct; +import java.nio.ByteBuffer; + +public class MecanumDriveKinematicsStruct implements Struct { + @Override + public Class getTypeClass() { + return MecanumDriveKinematics.class; + } + + @Override + public String getTypeString() { + return "struct:MecanumDriveKinematics"; + } + + @Override + public int getSize() { + return 4 * Translation2d.struct.getSize(); + } + + @Override + public String getSchema() { + return "Translation2d front_left;Translation2d front_right;Translation2d rear_left;" + + "Translation2d rear_right"; + } + + @Override + public Struct[] getNested() { + return new Struct[] {Translation2d.struct}; + } + + @Override + public MecanumDriveKinematics unpack(ByteBuffer bb) { + Translation2d frontLeft = Translation2d.struct.unpack(bb); + Translation2d frontRight = Translation2d.struct.unpack(bb); + Translation2d rearLeft = Translation2d.struct.unpack(bb); + Translation2d rearRight = Translation2d.struct.unpack(bb); + return new MecanumDriveKinematics(frontLeft, frontRight, rearLeft, rearRight); + } + + @Override + public void pack(ByteBuffer bb, MecanumDriveKinematics value) { + Translation2d.struct.pack(bb, value.getFrontLeft()); + Translation2d.struct.pack(bb, value.getFrontRight()); + Translation2d.struct.pack(bb, value.getRearLeft()); + Translation2d.struct.pack(bb, value.getRearRight()); + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/MecanumDriveWheelPositionsStruct.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/MecanumDriveWheelPositionsStruct.java new file mode 100644 index 00000000000..9f8b1824712 --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/MecanumDriveWheelPositionsStruct.java @@ -0,0 +1,48 @@ +// 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.math.kinematics.struct; + +import edu.wpi.first.math.kinematics.MecanumDriveWheelPositions; +import edu.wpi.first.util.struct.Struct; +import java.nio.ByteBuffer; + +public class MecanumDriveWheelPositionsStruct implements Struct { + @Override + public Class getTypeClass() { + return MecanumDriveWheelPositions.class; + } + + @Override + public String getTypeString() { + return "struct:MecanumDriveWheelPositions"; + } + + @Override + public int getSize() { + return kSizeDouble * 4; + } + + @Override + public String getSchema() { + return "double front_left;double front_right;double rear_left;double rear_right"; + } + + @Override + public MecanumDriveWheelPositions unpack(ByteBuffer bb) { + double frontLeft = bb.getDouble(); + double frontRight = bb.getDouble(); + double rearLeft = bb.getDouble(); + double rearRight = bb.getDouble(); + return new MecanumDriveWheelPositions(frontLeft, frontRight, rearLeft, rearRight); + } + + @Override + public void pack(ByteBuffer bb, MecanumDriveWheelPositions value) { + bb.putDouble(value.frontLeftMeters); + bb.putDouble(value.frontRightMeters); + bb.putDouble(value.rearLeftMeters); + bb.putDouble(value.rearRightMeters); + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/MecanumDriveWheelSpeedsStruct.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/MecanumDriveWheelSpeedsStruct.java new file mode 100644 index 00000000000..46d12763cd4 --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/MecanumDriveWheelSpeedsStruct.java @@ -0,0 +1,48 @@ +// 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.math.kinematics.struct; + +import edu.wpi.first.math.kinematics.MecanumDriveWheelSpeeds; +import edu.wpi.first.util.struct.Struct; +import java.nio.ByteBuffer; + +public class MecanumDriveWheelSpeedsStruct implements Struct { + @Override + public Class getTypeClass() { + return MecanumDriveWheelSpeeds.class; + } + + @Override + public String getTypeString() { + return "struct:MecanumDriveWheelSpeeds"; + } + + @Override + public int getSize() { + return kSizeDouble * 4; + } + + @Override + public String getSchema() { + return "double front_left;double front_right;double rear_left;double rear_right"; + } + + @Override + public MecanumDriveWheelSpeeds unpack(ByteBuffer bb) { + double frontLeft = bb.getDouble(); + double frontRight = bb.getDouble(); + double rearLeft = bb.getDouble(); + double rearRight = bb.getDouble(); + return new MecanumDriveWheelSpeeds(frontLeft, frontRight, rearLeft, rearRight); + } + + @Override + public void pack(ByteBuffer bb, MecanumDriveWheelSpeeds value) { + bb.putDouble(value.frontLeftMetersPerSecond); + bb.putDouble(value.frontRightMetersPerSecond); + bb.putDouble(value.rearLeftMetersPerSecond); + bb.putDouble(value.rearRightMetersPerSecond); + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/SwerveModulePositionStruct.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/SwerveModulePositionStruct.java new file mode 100644 index 00000000000..21567328e94 --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/SwerveModulePositionStruct.java @@ -0,0 +1,50 @@ +// 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.math.kinematics.struct; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.util.struct.Struct; +import java.nio.ByteBuffer; + +public class SwerveModulePositionStruct implements Struct { + @Override + public Class getTypeClass() { + return SwerveModulePosition.class; + } + + @Override + public String getTypeString() { + return "struct:SwerveModulePosition"; + } + + @Override + public int getSize() { + return kSizeDouble + Rotation2d.struct.getSize(); + } + + @Override + public String getSchema() { + return "double distance;Rotation2d angle"; + } + + @Override + public Struct[] getNested() { + return new Struct[] {Rotation2d.struct}; + } + + @Override + public SwerveModulePosition unpack(ByteBuffer bb) { + double distance = bb.getDouble(); + Rotation2d angle = Rotation2d.struct.unpack(bb); + return new SwerveModulePosition(distance, angle); + } + + @Override + public void pack(ByteBuffer bb, SwerveModulePosition value) { + bb.putDouble(value.distanceMeters); + Rotation2d.struct.pack(bb, value.angle); + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/SwerveModuleStateStruct.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/SwerveModuleStateStruct.java new file mode 100644 index 00000000000..51dd61f045b --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/SwerveModuleStateStruct.java @@ -0,0 +1,50 @@ +// 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.math.kinematics.struct; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.util.struct.Struct; +import java.nio.ByteBuffer; + +public class SwerveModuleStateStruct implements Struct { + @Override + public Class getTypeClass() { + return SwerveModuleState.class; + } + + @Override + public String getTypeString() { + return "struct:SwerveModuleState"; + } + + @Override + public int getSize() { + return kSizeDouble + Rotation2d.struct.getSize(); + } + + @Override + public String getSchema() { + return "double speed;Rotation2d angle"; + } + + @Override + public Struct[] getNested() { + return new Struct[] {Rotation2d.struct}; + } + + @Override + public SwerveModuleState unpack(ByteBuffer bb) { + double speed = bb.getDouble(); + Rotation2d angle = Rotation2d.struct.unpack(bb); + return new SwerveModuleState(speed, angle); + } + + @Override + public void pack(ByteBuffer bb, SwerveModuleState value) { + bb.putDouble(value.speedMetersPerSecond); + Rotation2d.struct.pack(bb, value.angle); + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/math/system/plant/DCMotor.java b/wpimath/src/main/java/edu/wpi/first/math/system/plant/DCMotor.java index 0c6e3346b2c..9f9cafef7ab 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/system/plant/DCMotor.java +++ b/wpimath/src/main/java/edu/wpi/first/math/system/plant/DCMotor.java @@ -4,6 +4,8 @@ package edu.wpi.first.math.system.plant; +import edu.wpi.first.math.system.plant.proto.DCMotorProto; +import edu.wpi.first.math.system.plant.struct.DCMotorStruct; import edu.wpi.first.math.util.Units; /** Holds the constants for a DC motor. */ @@ -17,6 +19,9 @@ public class DCMotor { public final double KvRadPerSecPerVolt; public final double KtNMPerAmp; + public static final DCMotorProto proto = new DCMotorProto(); + public static final DCMotorStruct struct = new DCMotorStruct(); + /** * Constructs a DC motor. * 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 8377022af3f..3ab305872db 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 @@ -4,6 +4,7 @@ package edu.wpi.first.math.system.plant; +import edu.wpi.first.math.MatBuilder; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.Nat; import edu.wpi.first.math.VecBuilder; @@ -23,37 +24,34 @@ private LinearSystemId() { * @param motor The motor (or gearbox) attached to the carriage. * @param massKg The mass of the elevator carriage, in kilograms. * @param radiusMeters The radius of the elevator's driving drum, in meters. - * @param G The reduction between motor and drum, as a ratio of output to input. + * @param gearing The reduction between motor and drum, as a ratio of output to input. * @return A LinearSystem representing the given characterized constants. - * @throws IllegalArgumentException if massKg <= 0, radiusMeters <= 0, or G <= 0. + * @throws IllegalArgumentException if massKg <= 0, radiusMeters <= 0, or gearing <= 0. */ public static LinearSystem createElevatorSystem( - DCMotor motor, double massKg, double radiusMeters, double G) { + DCMotor motor, double massKg, double radiusMeters, double gearing) { if (massKg <= 0.0) { throw new IllegalArgumentException("massKg must be greater than zero."); } if (radiusMeters <= 0.0) { throw new IllegalArgumentException("radiusMeters must be greater than zero."); } - if (G <= 0) { - throw new IllegalArgumentException("G must be greater than zero."); + if (gearing <= 0) { + throw new IllegalArgumentException("gearing must be greater than zero."); } return new LinearSystem<>( - Matrix.mat(Nat.N2(), Nat.N2()) - .fill( - 0, - 1, - 0, - -Math.pow(G, 2) - * motor.KtNMPerAmp - / (motor.rOhms - * radiusMeters - * radiusMeters - * massKg - * motor.KvRadPerSecPerVolt)), - VecBuilder.fill(0, G * motor.KtNMPerAmp / (motor.rOhms * radiusMeters * massKg)), - Matrix.mat(Nat.N1(), Nat.N2()).fill(1, 0), + MatBuilder.fill( + Nat.N2(), + Nat.N2(), + 0, + 1, + 0, + -Math.pow(gearing, 2) + * motor.KtNMPerAmp + / (motor.rOhms * radiusMeters * radiusMeters * massKg * motor.KvRadPerSecPerVolt)), + VecBuilder.fill(0, gearing * motor.KtNMPerAmp / (motor.rOhms * radiusMeters * massKg)), + MatBuilder.fill(Nat.N1(), Nat.N2(), 1, 0), new Matrix<>(Nat.N1(), Nat.N1())); } @@ -63,26 +61,26 @@ public static LinearSystem createElevatorSystem( * * @param motor The motor (or gearbox) attached to the flywheel. * @param JKgMetersSquared The moment of inertia J of the flywheel. - * @param G The reduction between motor and drum, as a ratio of output to input. + * @param gearing The reduction between motor and drum, as a ratio of output to input. * @return A LinearSystem representing the given characterized constants. - * @throws IllegalArgumentException if JKgMetersSquared <= 0 or G <= 0. + * @throws IllegalArgumentException if JKgMetersSquared <= 0 or gearing <= 0. */ public static LinearSystem createFlywheelSystem( - DCMotor motor, double JKgMetersSquared, double G) { + DCMotor motor, double JKgMetersSquared, double gearing) { if (JKgMetersSquared <= 0.0) { throw new IllegalArgumentException("J must be greater than zero."); } - if (G <= 0.0) { - throw new IllegalArgumentException("G must be greater than zero."); + if (gearing <= 0.0) { + throw new IllegalArgumentException("gearing must be greater than zero."); } return new LinearSystem<>( VecBuilder.fill( - -G - * G + -gearing + * gearing * motor.KtNMPerAmp / (motor.KvRadPerSecPerVolt * motor.rOhms * JKgMetersSquared)), - VecBuilder.fill(G * motor.KtNMPerAmp / (motor.rOhms * JKgMetersSquared)), + VecBuilder.fill(gearing * motor.KtNMPerAmp / (motor.rOhms * JKgMetersSquared)), Matrix.eye(Nat.N1()), new Matrix<>(Nat.N1(), Nat.N1())); } @@ -94,30 +92,31 @@ public static LinearSystem createFlywheelSystem( * * @param motor The motor (or gearbox) attached to system. * @param JKgMetersSquared The moment of inertia J of the DC motor. - * @param G The reduction between motor and drum, as a ratio of output to input. + * @param gearing The reduction between motor and drum, as a ratio of output to input. * @return A LinearSystem representing the given characterized constants. - * @throws IllegalArgumentException if JKgMetersSquared <= 0 or G <= 0. + * @throws IllegalArgumentException if JKgMetersSquared <= 0 or gearing <= 0. */ public static LinearSystem createDCMotorSystem( - DCMotor motor, double JKgMetersSquared, double G) { + DCMotor motor, double JKgMetersSquared, double gearing) { if (JKgMetersSquared <= 0.0) { throw new IllegalArgumentException("J must be greater than zero."); } - if (G <= 0.0) { - throw new IllegalArgumentException("G must be greater than zero."); + if (gearing <= 0.0) { + throw new IllegalArgumentException("gearing must be greater than zero."); } return new LinearSystem<>( - Matrix.mat(Nat.N2(), Nat.N2()) - .fill( - 0, - 1, - 0, - -G - * G - * motor.KtNMPerAmp - / (motor.KvRadPerSecPerVolt * motor.rOhms * JKgMetersSquared)), - VecBuilder.fill(0, G * motor.KtNMPerAmp / (motor.rOhms * JKgMetersSquared)), + MatBuilder.fill( + Nat.N2(), + Nat.N2(), + 0, + 1, + 0, + -gearing + * gearing + * motor.KtNMPerAmp + / (motor.KvRadPerSecPerVolt * motor.rOhms * JKgMetersSquared)), + VecBuilder.fill(0, gearing * motor.KtNMPerAmp / (motor.rOhms * JKgMetersSquared)), Matrix.eye(Nat.N2()), new Matrix<>(Nat.N2(), Nat.N1())); } @@ -149,7 +148,7 @@ public static LinearSystem createDCMotorSystem(double kV, double kA) } return new LinearSystem<>( - Matrix.mat(Nat.N2(), Nat.N2()).fill(0, 1, 0, -kV / kA), + MatBuilder.fill(Nat.N2(), Nat.N2(), 0, 1, 0, -kV / kA), VecBuilder.fill(0, 1 / kA), Matrix.eye(Nat.N2()), new Matrix<>(Nat.N2(), Nat.N1())); @@ -165,9 +164,10 @@ public static LinearSystem createDCMotorSystem(double kV, double kA) * @param rMeters The radius of the wheels in meters. * @param rbMeters The radius of the base (half the track width) in meters. * @param JKgMetersSquared The moment of inertia of the robot. - * @param G The gearing reduction as output over input. + * @param gearing The gearing reduction as output over input. * @return A LinearSystem representing a differential drivetrain. - * @throws IllegalArgumentException if m <= 0, r <= 0, rb <= 0, J <= 0, or G <= 0. + * @throws IllegalArgumentException if m <= 0, r <= 0, rb <= 0, J <= 0, or gearing + * <= 0. */ public static LinearSystem createDrivetrainVelocitySystem( DCMotor motor, @@ -175,7 +175,7 @@ public static LinearSystem createDrivetrainVelocitySystem( double rMeters, double rbMeters, double JKgMetersSquared, - double G) { + double gearing) { if (massKg <= 0.0) { throw new IllegalArgumentException("massKg must be greater than zero."); } @@ -188,20 +188,22 @@ public static LinearSystem createDrivetrainVelocitySystem( if (JKgMetersSquared <= 0.0) { throw new IllegalArgumentException("JKgMetersSquared must be greater than zero."); } - if (G <= 0.0) { - throw new IllegalArgumentException("G must be greater than zero."); + if (gearing <= 0.0) { + throw new IllegalArgumentException("gearing must be greater than zero."); } var C1 = - -(G * G) * motor.KtNMPerAmp / (motor.KvRadPerSecPerVolt * motor.rOhms * rMeters * rMeters); - var C2 = G * motor.KtNMPerAmp / (motor.rOhms * rMeters); + -(gearing * gearing) + * motor.KtNMPerAmp + / (motor.KvRadPerSecPerVolt * motor.rOhms * rMeters * rMeters); + var C2 = gearing * motor.KtNMPerAmp / (motor.rOhms * rMeters); final double C3 = 1 / massKg + rbMeters * rbMeters / JKgMetersSquared; final double C4 = 1 / massKg - rbMeters * rbMeters / JKgMetersSquared; - var A = Matrix.mat(Nat.N2(), Nat.N2()).fill(C3 * C1, C4 * C1, C4 * C1, C3 * C1); - var B = Matrix.mat(Nat.N2(), Nat.N2()).fill(C3 * C2, C4 * C2, C4 * C2, C3 * C2); - var C = Matrix.mat(Nat.N2(), Nat.N2()).fill(1.0, 0.0, 0.0, 1.0); - var D = Matrix.mat(Nat.N2(), Nat.N2()).fill(0.0, 0.0, 0.0, 0.0); + var A = MatBuilder.fill(Nat.N2(), Nat.N2(), C3 * C1, C4 * C1, C4 * C1, C3 * C1); + var B = MatBuilder.fill(Nat.N2(), Nat.N2(), C3 * C2, C4 * C2, C4 * C2, C3 * C2); + var C = MatBuilder.fill(Nat.N2(), Nat.N2(), 1.0, 0.0, 0.0, 1.0); + var D = MatBuilder.fill(Nat.N2(), Nat.N2(), 0.0, 0.0, 0.0, 0.0); return new LinearSystem<>(A, B, C, D); } @@ -212,30 +214,31 @@ public static LinearSystem createDrivetrainVelocitySystem( * * @param motor The motor (or gearbox) attached to the arm. * @param JKgSquaredMeters The moment of inertia J of the arm. - * @param G The gearing between the motor and arm, in output over input. Most of the time this - * will be greater than 1. + * @param gearing The gearing between the motor and arm, in output over input. Most of the time + * this will be greater than 1. * @return A LinearSystem representing the given characterized constants. */ public static LinearSystem createSingleJointedArmSystem( - DCMotor motor, double JKgSquaredMeters, double G) { + DCMotor motor, double JKgSquaredMeters, double gearing) { if (JKgSquaredMeters <= 0.0) { throw new IllegalArgumentException("JKgSquaredMeters must be greater than zero."); } - if (G <= 0.0) { - throw new IllegalArgumentException("G must be greater than zero."); + if (gearing <= 0.0) { + throw new IllegalArgumentException("gearing must be greater than zero."); } return new LinearSystem<>( - Matrix.mat(Nat.N2(), Nat.N2()) - .fill( - 0, - 1, - 0, - -Math.pow(G, 2) - * motor.KtNMPerAmp - / (motor.KvRadPerSecPerVolt * motor.rOhms * JKgSquaredMeters)), - VecBuilder.fill(0, G * motor.KtNMPerAmp / (motor.rOhms * JKgSquaredMeters)), - Matrix.mat(Nat.N1(), Nat.N2()).fill(1, 0), + MatBuilder.fill( + Nat.N2(), + Nat.N2(), + 0, + 1, + 0, + -Math.pow(gearing, 2) + * motor.KtNMPerAmp + / (motor.KvRadPerSecPerVolt * motor.rOhms * JKgSquaredMeters)), + VecBuilder.fill(0, gearing * motor.KtNMPerAmp / (motor.rOhms * JKgSquaredMeters)), + MatBuilder.fill(Nat.N1(), Nat.N2(), 1, 0), new Matrix<>(Nat.N1(), Nat.N1())); } @@ -299,9 +302,9 @@ public static LinearSystem identifyPositionSystem(double kV, double } return new LinearSystem<>( - Matrix.mat(Nat.N2(), Nat.N2()).fill(0.0, 1.0, 0.0, -kV / kA), + MatBuilder.fill(Nat.N2(), Nat.N2(), 0.0, 1.0, 0.0, -kV / kA), VecBuilder.fill(0.0, 1.0 / kA), - Matrix.mat(Nat.N1(), Nat.N2()).fill(1.0, 0.0), + MatBuilder.fill(Nat.N1(), Nat.N2(), 1.0, 0.0), VecBuilder.fill(0.0)); } @@ -344,10 +347,10 @@ public static LinearSystem identifyDrivetrainSystem( final double B2 = 0.5 * (1.0 / kALinear - 1.0 / kAAngular); return new LinearSystem<>( - Matrix.mat(Nat.N2(), Nat.N2()).fill(A1, A2, A2, A1), - Matrix.mat(Nat.N2(), Nat.N2()).fill(B1, B2, B2, B1), - Matrix.mat(Nat.N2(), Nat.N2()).fill(1, 0, 0, 1), - Matrix.mat(Nat.N2(), Nat.N2()).fill(0, 0, 0, 0)); + MatBuilder.fill(Nat.N2(), Nat.N2(), A1, A2, A2, A1), + MatBuilder.fill(Nat.N2(), Nat.N2(), B1, B2, B2, B1), + MatBuilder.fill(Nat.N2(), Nat.N2(), 1, 0, 0, 1), + MatBuilder.fill(Nat.N2(), Nat.N2(), 0, 0, 0, 0)); } /** diff --git a/wpimath/src/main/java/edu/wpi/first/math/system/plant/proto/DCMotorProto.java b/wpimath/src/main/java/edu/wpi/first/math/system/plant/proto/DCMotorProto.java new file mode 100644 index 00000000000..f9e4aa39576 --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/system/plant/proto/DCMotorProto.java @@ -0,0 +1,47 @@ +// 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.math.system.plant.proto; + +import edu.wpi.first.math.proto.Plant.ProtobufDCMotor; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.util.protobuf.Protobuf; +import us.hebi.quickbuf.Descriptors.Descriptor; + +public class DCMotorProto implements Protobuf { + @Override + public Class getTypeClass() { + return DCMotor.class; + } + + @Override + public Descriptor getDescriptor() { + return ProtobufDCMotor.getDescriptor(); + } + + @Override + public ProtobufDCMotor createMessage() { + return ProtobufDCMotor.newInstance(); + } + + @Override + public DCMotor unpack(ProtobufDCMotor msg) { + return new DCMotor( + msg.getNominalVoltage(), + msg.getStallTorque(), + msg.getStallCurrent(), + msg.getFreeCurrent(), + msg.getFreeSpeed(), + 1); + } + + @Override + public void pack(ProtobufDCMotor msg, DCMotor value) { + msg.setNominalVoltage(value.nominalVoltageVolts); + msg.setStallTorque(value.stallTorqueNewtonMeters); + msg.setStallCurrent(value.stallCurrentAmps); + msg.setFreeCurrent(value.freeCurrentAmps); + msg.setFreeSpeed(value.freeSpeedRadPerSec); + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/math/system/plant/struct/DCMotorStruct.java b/wpimath/src/main/java/edu/wpi/first/math/system/plant/struct/DCMotorStruct.java new file mode 100644 index 00000000000..da159fc63ad --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/system/plant/struct/DCMotorStruct.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.math.system.plant.struct; + +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.util.struct.Struct; +import java.nio.ByteBuffer; + +public class DCMotorStruct implements Struct { + @Override + public Class getTypeClass() { + return DCMotor.class; + } + + @Override + public String getTypeString() { + return "struct:DCMotor"; + } + + @Override + public int getSize() { + return kSizeDouble * 5; + } + + @Override + public String getSchema() { + return "double nominal_voltage;double stall_torque;double stall_current;" + + "double free_current;double free_speed"; + } + + @Override + public DCMotor unpack(ByteBuffer bb) { + double nominalVoltage = bb.getDouble(); + double stallTorque = bb.getDouble(); + double stallCurrent = bb.getDouble(); + double freeCurrent = bb.getDouble(); + double freeSpeed = bb.getDouble(); + return new DCMotor(nominalVoltage, stallTorque, stallCurrent, freeCurrent, freeSpeed, 1); + } + + @Override + public void pack(ByteBuffer bb, DCMotor value) { + bb.putDouble(value.nominalVoltageVolts); + bb.putDouble(value.stallTorqueNewtonMeters); + bb.putDouble(value.stallCurrentAmps); + bb.putDouble(value.freeCurrentAmps); + bb.putDouble(value.freeSpeedRadPerSec); + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/math/trajectory/Trajectory.java b/wpimath/src/main/java/edu/wpi/first/math/trajectory/Trajectory.java index f7120be7df1..bc2dea3a4f2 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/trajectory/Trajectory.java +++ b/wpimath/src/main/java/edu/wpi/first/math/trajectory/Trajectory.java @@ -7,6 +7,8 @@ import com.fasterxml.jackson.annotation.JsonProperty; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.math.trajectory.proto.TrajectoryProto; +import edu.wpi.first.math.trajectory.proto.TrajectoryStateProto; import java.util.ArrayList; import java.util.List; import java.util.Objects; @@ -17,6 +19,8 @@ * represent the pose, curvature, time elapsed, velocity, and acceleration at that point. */ public class Trajectory { + public static final TrajectoryProto proto = new TrajectoryProto(); + private final double m_totalTimeSeconds; private final List m_states; @@ -264,6 +268,8 @@ public Trajectory concatenate(Trajectory other) { * represent the pose, curvature, time elapsed, velocity, and acceleration at that point. */ public static class State { + public static final TrajectoryStateProto proto = new TrajectoryStateProto(); + // The time elapsed since the beginning of the trajectory. @JsonProperty("time") public double timeSeconds; diff --git a/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrajectoryConfig.java b/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrajectoryConfig.java index fbf734fd31c..931efe5aab8 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrajectoryConfig.java +++ b/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrajectoryConfig.java @@ -4,6 +4,9 @@ package edu.wpi.first.math.trajectory; +import static edu.wpi.first.units.Units.MetersPerSecond; +import static edu.wpi.first.units.Units.MetersPerSecondPerSecond; + import edu.wpi.first.math.kinematics.DifferentialDriveKinematics; import edu.wpi.first.math.kinematics.MecanumDriveKinematics; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; @@ -11,6 +14,9 @@ import edu.wpi.first.math.trajectory.constraint.MecanumDriveKinematicsConstraint; import edu.wpi.first.math.trajectory.constraint.SwerveDriveKinematicsConstraint; import edu.wpi.first.math.trajectory.constraint.TrajectoryConstraint; +import edu.wpi.first.units.Distance; +import edu.wpi.first.units.Measure; +import edu.wpi.first.units.Velocity; import java.util.ArrayList; import java.util.List; @@ -43,6 +49,18 @@ public TrajectoryConfig( m_constraints = new ArrayList<>(); } + /** + * Constructs the trajectory configuration class. + * + * @param maxVelocity The max velocity for the trajectory. + * @param maxAcceleration The max acceleration for the trajectory. + */ + public TrajectoryConfig( + Measure> maxVelocity, + Measure>> maxAcceleration) { + this(maxVelocity.in(MetersPerSecond), maxAcceleration.in(MetersPerSecondPerSecond)); + } + /** * Adds a user-defined constraint to the trajectory. * @@ -121,6 +139,16 @@ public TrajectoryConfig setStartVelocity(double startVelocityMetersPerSecond) { return this; } + /** + * Sets the start velocity of the trajectory. + * + * @param startVelocity The start velocity of the trajectory. + * @return Instance of the current config object. + */ + public TrajectoryConfig setStartVelocity(Measure> startVelocity) { + return setStartVelocity(startVelocity.in(MetersPerSecond)); + } + /** * Returns the starting velocity of the trajectory. * @@ -141,6 +169,16 @@ public TrajectoryConfig setEndVelocity(double endVelocityMetersPerSecond) { return this; } + /** + * Sets the end velocity of the trajectory. + * + * @param endVelocity The end velocity of the trajectory. + * @return Instance of the current config object. + */ + public TrajectoryConfig setEndVelocity(Measure> endVelocity) { + return setEndVelocity(endVelocity.in(MetersPerSecond)); + } + /** * Returns the maximum velocity of the trajectory. * diff --git a/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrapezoidProfile.java b/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrapezoidProfile.java index d29ed107c99..f14aea78f0a 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrapezoidProfile.java +++ b/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrapezoidProfile.java @@ -6,6 +6,9 @@ import edu.wpi.first.math.MathSharedStore; import edu.wpi.first.math.MathUsageId; +import edu.wpi.first.units.Measure; +import edu.wpi.first.units.Unit; +import edu.wpi.first.units.Velocity; import java.util.Objects; /** @@ -67,6 +70,11 @@ public Constraints(double maxVelocity, double maxAcceleration) { this.maxAcceleration = maxAcceleration; MathSharedStore.reportUsage(MathUsageId.kTrajectory_TrapezoidProfile, 1); } + + public > Constraints( + Measure> maxVelocity, Measure>> maxAcceleration) { + this(maxVelocity.baseUnitMagnitude(), maxAcceleration.baseUnitMagnitude()); + } } public static class State { @@ -81,6 +89,10 @@ public State(double position, double velocity) { this.velocity = velocity; } + public > State(Measure position, Measure> velocity) { + this(position.baseUnitMagnitude(), velocity.baseUnitMagnitude()); + } + @Override public boolean equals(Object other) { if (other instanceof State) { diff --git a/wpimath/src/main/java/edu/wpi/first/math/trajectory/proto/TrajectoryProto.java b/wpimath/src/main/java/edu/wpi/first/math/trajectory/proto/TrajectoryProto.java new file mode 100644 index 00000000000..92e5ac7d2cd --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/trajectory/proto/TrajectoryProto.java @@ -0,0 +1,52 @@ +// 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.math.trajectory.proto; + +import edu.wpi.first.math.proto.Trajectory.ProtobufTrajectory; +import edu.wpi.first.math.proto.Trajectory.ProtobufTrajectoryState; +import edu.wpi.first.math.trajectory.Trajectory; +import edu.wpi.first.util.protobuf.Protobuf; +import java.util.ArrayList; +import us.hebi.quickbuf.Descriptors.Descriptor; + +public class TrajectoryProto implements Protobuf { + @Override + public Class getTypeClass() { + return Trajectory.class; + } + + @Override + public Descriptor getDescriptor() { + return ProtobufTrajectory.getDescriptor(); + } + + @Override + public Protobuf[] getNested() { + return new Protobuf[] {Trajectory.State.proto}; + } + + @Override + public ProtobufTrajectory createMessage() { + return ProtobufTrajectory.newInstance(); + } + + @Override + public Trajectory unpack(ProtobufTrajectory msg) { + ArrayList states = new ArrayList<>(msg.getStates().length()); + for (ProtobufTrajectoryState protoState : msg.getStates()) { + states.add(Trajectory.State.proto.unpack(protoState)); + } + + return new Trajectory(states); + } + + @Override + public void pack(ProtobufTrajectory msg, Trajectory value) { + var states = msg.getMutableStates().reserve(value.getStates().size()); + for (var item : value.getStates()) { + Trajectory.State.proto.pack(states.next(), item); + } + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/math/trajectory/proto/TrajectoryStateProto.java b/wpimath/src/main/java/edu/wpi/first/math/trajectory/proto/TrajectoryStateProto.java new file mode 100644 index 00000000000..e8900bfbd7b --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/trajectory/proto/TrajectoryStateProto.java @@ -0,0 +1,52 @@ +// 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.math.trajectory.proto; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.proto.Trajectory.ProtobufTrajectoryState; +import edu.wpi.first.math.trajectory.Trajectory; +import edu.wpi.first.util.protobuf.Protobuf; +import us.hebi.quickbuf.Descriptors.Descriptor; + +public class TrajectoryStateProto implements Protobuf { + @Override + public Class getTypeClass() { + return Trajectory.State.class; + } + + @Override + public Descriptor getDescriptor() { + return ProtobufTrajectoryState.getDescriptor(); + } + + @Override + public Protobuf[] getNested() { + return new Protobuf[] {Pose2d.proto}; + } + + @Override + public ProtobufTrajectoryState createMessage() { + return ProtobufTrajectoryState.newInstance(); + } + + @Override + public Trajectory.State unpack(ProtobufTrajectoryState msg) { + return new Trajectory.State( + msg.getTime(), + msg.getVelocity(), + msg.getAcceleration(), + Pose2d.proto.unpack(msg.getPose()), + msg.getCurvature()); + } + + @Override + public void pack(ProtobufTrajectoryState msg, Trajectory.State value) { + msg.setTime(value.timeSeconds); + msg.setVelocity(value.velocityMetersPerSecond); + msg.setAcceleration(value.accelerationMetersPerSecondSq); + Pose2d.proto.pack(msg.getMutablePose(), value.poseMeters); + msg.setCurvature(value.curvatureRadPerMeter); + } +} diff --git a/wpimath/src/main/native/cpp/StateSpaceUtil.cpp b/wpimath/src/main/native/cpp/StateSpaceUtil.cpp index 758d2e772ac..5dcf1a1798a 100644 --- a/wpimath/src/main/native/cpp/StateSpaceUtil.cpp +++ b/wpimath/src/main/native/cpp/StateSpaceUtil.cpp @@ -18,21 +18,12 @@ Eigen::Vector4d PoseTo4dVector(const Pose2d& pose) { pose.Rotation().Sin()}; } -template <> -bool IsStabilizable<1, 1>(const Matrixd<1, 1>& A, const Matrixd<1, 1>& B) { - return detail::IsStabilizableImpl<1, 1>(A, B); -} - -template <> -bool IsStabilizable<2, 1>(const Matrixd<2, 2>& A, const Matrixd<2, 1>& B) { - return detail::IsStabilizableImpl<2, 1>(A, B); -} - -template <> -bool IsStabilizable(const Eigen::MatrixXd& A, - const Eigen::MatrixXd& B) { - return detail::IsStabilizableImpl(A, B); -} +template bool IsStabilizable<1, 1>(const Matrixd<1, 1>& A, + const Matrixd<1, 1>& B); +template bool IsStabilizable<2, 1>(const Matrixd<2, 2>& A, + const Matrixd<2, 1>& B); +template bool IsStabilizable( + const Eigen::MatrixXd& A, const Eigen::MatrixXd& B); Eigen::Vector3d PoseToVector(const Pose2d& pose) { return Eigen::Vector3d{pose.X().value(), pose.Y().value(), diff --git a/wpimath/src/main/native/cpp/controller/proto/ArmFeedforwardProto.cpp b/wpimath/src/main/native/cpp/controller/proto/ArmFeedforwardProto.cpp new file mode 100644 index 00000000000..37a55cf631e --- /dev/null +++ b/wpimath/src/main/native/cpp/controller/proto/ArmFeedforwardProto.cpp @@ -0,0 +1,33 @@ +// 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. + +#include "frc/controller/proto/ArmFeedforwardProto.h" + +#include "controller.pb.h" + +google::protobuf::Message* wpi::Protobuf::New( + google::protobuf::Arena* arena) { + return google::protobuf::Arena::CreateMessage< + wpi::proto::ProtobufArmFeedforward>(arena); +} + +frc::ArmFeedforward wpi::Protobuf::Unpack( + const google::protobuf::Message& msg) { + auto m = static_cast(&msg); + return frc::ArmFeedforward{ + units::volt_t{m->ks()}, + units::volt_t{m->kg()}, + units::unit_t{m->kv()}, + units::unit_t{m->ka()}, + }; +} + +void wpi::Protobuf::Pack( + google::protobuf::Message* msg, const frc::ArmFeedforward& value) { + auto m = static_cast(msg); + m->set_ks(value.kS.value()); + m->set_kg(value.kG.value()); + m->set_kv(value.kV.value()); + m->set_ka(value.kA.value()); +} diff --git a/wpimath/src/main/native/cpp/controller/proto/DifferentialDriveWheelVoltagesProto.cpp b/wpimath/src/main/native/cpp/controller/proto/DifferentialDriveWheelVoltagesProto.cpp new file mode 100644 index 00000000000..b0b16b955d6 --- /dev/null +++ b/wpimath/src/main/native/cpp/controller/proto/DifferentialDriveWheelVoltagesProto.cpp @@ -0,0 +1,34 @@ +// 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. + +#include "frc/controller/proto/DifferentialDriveWheelVoltagesProto.h" + +#include "controller.pb.h" + +google::protobuf::Message* wpi::Protobuf< + frc::DifferentialDriveWheelVoltages>::New(google::protobuf::Arena* arena) { + return google::protobuf::Arena::CreateMessage< + wpi::proto::ProtobufDifferentialDriveWheelVoltages>(arena); +} + +frc::DifferentialDriveWheelVoltages +wpi::Protobuf::Unpack( + const google::protobuf::Message& msg) { + auto m = + static_cast( + &msg); + return frc::DifferentialDriveWheelVoltages{ + units::volt_t{m->left()}, + units::volt_t{m->right()}, + }; +} + +void wpi::Protobuf::Pack( + google::protobuf::Message* msg, + const frc::DifferentialDriveWheelVoltages& value) { + auto m = + static_cast(msg); + m->set_left(value.left.value()); + m->set_right(value.right.value()); +} diff --git a/wpimath/src/main/native/cpp/controller/proto/ElevatorFeedforwardProto.cpp b/wpimath/src/main/native/cpp/controller/proto/ElevatorFeedforwardProto.cpp new file mode 100644 index 00000000000..4b4bce2df6a --- /dev/null +++ b/wpimath/src/main/native/cpp/controller/proto/ElevatorFeedforwardProto.cpp @@ -0,0 +1,33 @@ +// 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. + +#include "frc/controller/proto/ElevatorFeedforwardProto.h" + +#include "controller.pb.h" + +google::protobuf::Message* wpi::Protobuf::New( + google::protobuf::Arena* arena) { + return google::protobuf::Arena::CreateMessage< + wpi::proto::ProtobufElevatorFeedforward>(arena); +} + +frc::ElevatorFeedforward wpi::Protobuf::Unpack( + const google::protobuf::Message& msg) { + auto m = static_cast(&msg); + return frc::ElevatorFeedforward{ + units::volt_t{m->ks()}, + units::volt_t{m->kg()}, + units::unit_t{m->kv()}, + units::unit_t{m->ka()}, + }; +} + +void wpi::Protobuf::Pack( + google::protobuf::Message* msg, const frc::ElevatorFeedforward& value) { + auto m = static_cast(msg); + m->set_ks(value.kS()); + m->set_kg(value.kG()); + m->set_kv(value.kV()); + m->set_ka(value.kA()); +} diff --git a/wpimath/src/main/native/cpp/controller/struct/ArmFeedforwardStruct.cpp b/wpimath/src/main/native/cpp/controller/struct/ArmFeedforwardStruct.cpp new file mode 100644 index 00000000000..0e6e8165ca4 --- /dev/null +++ b/wpimath/src/main/native/cpp/controller/struct/ArmFeedforwardStruct.cpp @@ -0,0 +1,33 @@ +// 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. + +#include "frc/controller/struct/ArmFeedforwardStruct.h" + +namespace { +constexpr size_t kKsOff = 0; +constexpr size_t kKgOff = kKsOff + 8; +constexpr size_t kKvOff = kKgOff + 8; +constexpr size_t kKaOff = kKvOff + 8; +} // namespace + +using StructType = wpi::Struct; + +frc::ArmFeedforward StructType::Unpack(std::span data) { + return frc::ArmFeedforward{ + units::volt_t{wpi::UnpackStruct(data)}, + units::volt_t{wpi::UnpackStruct(data)}, + units::unit_t{ + wpi::UnpackStruct(data)}, + units::unit_t{ + wpi::UnpackStruct(data)}, + }; +} + +void StructType::Pack(std::span data, + const frc::ArmFeedforward& value) { + wpi::PackStruct(data, value.kS()); + wpi::PackStruct(data, value.kG()); + wpi::PackStruct(data, value.kV()); + wpi::PackStruct(data, value.kA()); +} diff --git a/wpimath/src/main/native/cpp/controller/struct/DifferentialDriveWheelVoltagesStruct.cpp b/wpimath/src/main/native/cpp/controller/struct/DifferentialDriveWheelVoltagesStruct.cpp new file mode 100644 index 00000000000..c056638eaf3 --- /dev/null +++ b/wpimath/src/main/native/cpp/controller/struct/DifferentialDriveWheelVoltagesStruct.cpp @@ -0,0 +1,26 @@ +// 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. + +#include "frc/controller/struct/DifferentialDriveWheelVoltagesStruct.h" + +namespace { +constexpr size_t kLeftOff = 0; +constexpr size_t kRightOff = kLeftOff + 8; +} // namespace + +using StructType = wpi::Struct; + +frc::DifferentialDriveWheelVoltages StructType::Unpack( + std::span data) { + return frc::DifferentialDriveWheelVoltages{ + units::volt_t{wpi::UnpackStruct(data)}, + units::volt_t{wpi::UnpackStruct(data)}, + }; +} + +void StructType::Pack(std::span data, + const frc::DifferentialDriveWheelVoltages& value) { + wpi::PackStruct(data, value.left.value()); + wpi::PackStruct(data, value.right.value()); +} diff --git a/wpimath/src/main/native/cpp/controller/struct/ElevatorFeedforwardStruct.cpp b/wpimath/src/main/native/cpp/controller/struct/ElevatorFeedforwardStruct.cpp new file mode 100644 index 00000000000..0572181c7f5 --- /dev/null +++ b/wpimath/src/main/native/cpp/controller/struct/ElevatorFeedforwardStruct.cpp @@ -0,0 +1,34 @@ +// 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. + +#include "frc/controller/struct/ElevatorFeedforwardStruct.h" + +namespace { +constexpr size_t kKsOff = 0; +constexpr size_t kKgOff = kKsOff + 8; +constexpr size_t kKvOff = kKgOff + 8; +constexpr size_t kKaOff = kKvOff + 8; +} // namespace + +using StructType = wpi::Struct; + +frc::ElevatorFeedforward StructType::Unpack( + std::span data) { + return frc::ElevatorFeedforward{ + units::volt_t{wpi::UnpackStruct(data)}, + units::volt_t{wpi::UnpackStruct(data)}, + units::unit_t{ + wpi::UnpackStruct(data)}, + units::unit_t{ + wpi::UnpackStruct(data)}, + }; +} + +void StructType::Pack(std::span data, + const frc::ElevatorFeedforward& value) { + wpi::PackStruct(data, value.kS()); + wpi::PackStruct(data, value.kG()); + wpi::PackStruct(data, value.kV()); + wpi::PackStruct(data, value.kA()); +} diff --git a/wpimath/src/main/native/cpp/geometry/Pose2d.cpp b/wpimath/src/main/native/cpp/geometry/Pose2d.cpp index 2e152165274..53779d05c83 100644 --- a/wpimath/src/main/native/cpp/geometry/Pose2d.cpp +++ b/wpimath/src/main/native/cpp/geometry/Pose2d.cpp @@ -9,7 +9,6 @@ #include #include "frc/MathUtil.h" -#include "geometry2d.pb.h" using namespace frc; @@ -109,23 +108,3 @@ void frc::from_json(const wpi::json& json, Pose2d& pose) { pose = Pose2d{json.at("translation").get(), json.at("rotation").get()}; } - -google::protobuf::Message* wpi::Protobuf::New( - google::protobuf::Arena* arena) { - return google::protobuf::Arena::CreateMessage( - arena); -} - -frc::Pose2d wpi::Protobuf::Unpack( - const google::protobuf::Message& msg) { - auto m = static_cast(&msg); - return Pose2d{wpi::UnpackProtobuf(m->translation()), - wpi::UnpackProtobuf(m->rotation())}; -} - -void wpi::Protobuf::Pack(google::protobuf::Message* msg, - const frc::Pose2d& value) { - auto m = static_cast(msg); - wpi::PackProtobuf(m->mutable_translation(), value.Translation()); - wpi::PackProtobuf(m->mutable_rotation(), value.Rotation()); -} diff --git a/wpimath/src/main/native/cpp/geometry/Pose3d.cpp b/wpimath/src/main/native/cpp/geometry/Pose3d.cpp index ffbaecbff43..90f7b491f5a 100644 --- a/wpimath/src/main/native/cpp/geometry/Pose3d.cpp +++ b/wpimath/src/main/native/cpp/geometry/Pose3d.cpp @@ -189,23 +189,3 @@ void frc::from_json(const wpi::json& json, Pose3d& pose) { pose = Pose3d{json.at("translation").get(), json.at("rotation").get()}; } - -google::protobuf::Message* wpi::Protobuf::New( - google::protobuf::Arena* arena) { - return google::protobuf::Arena::CreateMessage( - arena); -} - -frc::Pose3d wpi::Protobuf::Unpack( - const google::protobuf::Message& msg) { - auto m = static_cast(&msg); - return Pose3d{wpi::UnpackProtobuf(m->translation()), - wpi::UnpackProtobuf(m->rotation())}; -} - -void wpi::Protobuf::Pack(google::protobuf::Message* msg, - const frc::Pose3d& value) { - auto m = static_cast(msg); - wpi::PackProtobuf(m->mutable_translation(), value.Translation()); - wpi::PackProtobuf(m->mutable_rotation(), value.Rotation()); -} diff --git a/wpimath/src/main/native/cpp/geometry/Quaternion.cpp b/wpimath/src/main/native/cpp/geometry/Quaternion.cpp index 37afbb88a32..3aad64c377e 100644 --- a/wpimath/src/main/native/cpp/geometry/Quaternion.cpp +++ b/wpimath/src/main/native/cpp/geometry/Quaternion.cpp @@ -232,24 +232,3 @@ void frc::from_json(const wpi::json& json, Quaternion& quaternion) { Quaternion{json.at("W").get(), json.at("X").get(), json.at("Y").get(), json.at("Z").get()}; } - -google::protobuf::Message* wpi::Protobuf::New( - google::protobuf::Arena* arena) { - return google::protobuf::Arena::CreateMessage( - arena); -} - -frc::Quaternion wpi::Protobuf::Unpack( - const google::protobuf::Message& msg) { - auto m = static_cast(&msg); - return frc::Quaternion{m->w(), m->x(), m->y(), m->z()}; -} - -void wpi::Protobuf::Pack(google::protobuf::Message* msg, - const frc::Quaternion& value) { - auto m = static_cast(msg); - m->set_w(value.W()); - m->set_x(value.X()); - m->set_y(value.Y()); - m->set_z(value.Z()); -} diff --git a/wpimath/src/main/native/cpp/geometry/Rotation2d.cpp b/wpimath/src/main/native/cpp/geometry/Rotation2d.cpp index 05a644ea2bd..69193028f77 100644 --- a/wpimath/src/main/native/cpp/geometry/Rotation2d.cpp +++ b/wpimath/src/main/native/cpp/geometry/Rotation2d.cpp @@ -20,21 +20,3 @@ void frc::to_json(wpi::json& json, const Rotation2d& rotation) { void frc::from_json(const wpi::json& json, Rotation2d& rotation) { rotation = Rotation2d{units::radian_t{json.at("radians").get()}}; } - -google::protobuf::Message* wpi::Protobuf::New( - google::protobuf::Arena* arena) { - return google::protobuf::Arena::CreateMessage( - arena); -} - -frc::Rotation2d wpi::Protobuf::Unpack( - const google::protobuf::Message& msg) { - auto m = static_cast(&msg); - return frc::Rotation2d{units::radian_t{m->value()}}; -} - -void wpi::Protobuf::Pack(google::protobuf::Message* msg, - const frc::Rotation2d& value) { - auto m = static_cast(msg); - m->set_value(value.Radians().value()); -} diff --git a/wpimath/src/main/native/cpp/geometry/Rotation3d.cpp b/wpimath/src/main/native/cpp/geometry/Rotation3d.cpp index 3a688700453..072d023cb28 100644 --- a/wpimath/src/main/native/cpp/geometry/Rotation3d.cpp +++ b/wpimath/src/main/native/cpp/geometry/Rotation3d.cpp @@ -262,21 +262,3 @@ void frc::to_json(wpi::json& json, const Rotation3d& rotation) { void frc::from_json(const wpi::json& json, Rotation3d& rotation) { rotation = Rotation3d{json.at("quaternion").get()}; } - -google::protobuf::Message* wpi::Protobuf::New( - google::protobuf::Arena* arena) { - return google::protobuf::Arena::CreateMessage( - arena); -} - -frc::Rotation3d wpi::Protobuf::Unpack( - const google::protobuf::Message& msg) { - auto m = static_cast(&msg); - return Rotation3d{wpi::UnpackProtobuf(m->q())}; -} - -void wpi::Protobuf::Pack(google::protobuf::Message* msg, - const frc::Rotation3d& value) { - auto m = static_cast(msg); - wpi::PackProtobuf(m->mutable_q(), value.GetQuaternion()); -} diff --git a/wpimath/src/main/native/cpp/geometry/Transform2d.cpp b/wpimath/src/main/native/cpp/geometry/Transform2d.cpp index 77f3ceed2d4..157359bfbc5 100644 --- a/wpimath/src/main/native/cpp/geometry/Transform2d.cpp +++ b/wpimath/src/main/native/cpp/geometry/Transform2d.cpp @@ -22,23 +22,3 @@ Transform2d::Transform2d(Pose2d initial, Pose2d final) { Transform2d Transform2d::operator+(const Transform2d& other) const { return Transform2d{Pose2d{}, Pose2d{}.TransformBy(*this).TransformBy(other)}; } - -google::protobuf::Message* wpi::Protobuf::New( - google::protobuf::Arena* arena) { - return google::protobuf::Arena::CreateMessage< - wpi::proto::ProtobufTransform2d>(arena); -} - -frc::Transform2d wpi::Protobuf::Unpack( - const google::protobuf::Message& msg) { - auto m = static_cast(&msg); - return Transform2d{wpi::UnpackProtobuf(m->translation()), - wpi::UnpackProtobuf(m->rotation())}; -} - -void wpi::Protobuf::Pack(google::protobuf::Message* msg, - const frc::Transform2d& value) { - auto m = static_cast(msg); - wpi::PackProtobuf(m->mutable_translation(), value.Translation()); - wpi::PackProtobuf(m->mutable_rotation(), value.Rotation()); -} diff --git a/wpimath/src/main/native/cpp/geometry/Transform3d.cpp b/wpimath/src/main/native/cpp/geometry/Transform3d.cpp index de6c2533c97..556a3027d0b 100644 --- a/wpimath/src/main/native/cpp/geometry/Transform3d.cpp +++ b/wpimath/src/main/native/cpp/geometry/Transform3d.cpp @@ -5,7 +5,6 @@ #include "frc/geometry/Transform3d.h" #include "frc/geometry/Pose3d.h" -#include "geometry3d.pb.h" using namespace frc; @@ -36,23 +35,3 @@ Transform3d Transform3d::Inverse() const { Transform3d Transform3d::operator+(const Transform3d& other) const { return Transform3d{Pose3d{}, Pose3d{}.TransformBy(*this).TransformBy(other)}; } - -google::protobuf::Message* wpi::Protobuf::New( - google::protobuf::Arena* arena) { - return google::protobuf::Arena::CreateMessage< - wpi::proto::ProtobufTransform3d>(arena); -} - -frc::Transform3d wpi::Protobuf::Unpack( - const google::protobuf::Message& msg) { - auto m = static_cast(&msg); - return Transform3d{wpi::UnpackProtobuf(m->translation()), - wpi::UnpackProtobuf(m->rotation())}; -} - -void wpi::Protobuf::Pack(google::protobuf::Message* msg, - const frc::Transform3d& value) { - auto m = static_cast(msg); - wpi::PackProtobuf(m->mutable_translation(), value.Translation()); - wpi::PackProtobuf(m->mutable_rotation(), value.Rotation()); -} diff --git a/wpimath/src/main/native/cpp/geometry/Translation2d.cpp b/wpimath/src/main/native/cpp/geometry/Translation2d.cpp index 6d5f3153ae1..c875582820e 100644 --- a/wpimath/src/main/native/cpp/geometry/Translation2d.cpp +++ b/wpimath/src/main/native/cpp/geometry/Translation2d.cpp @@ -49,22 +49,3 @@ void frc::from_json(const wpi::json& json, Translation2d& translation) { translation = Translation2d{units::meter_t{json.at("x").get()}, units::meter_t{json.at("y").get()}}; } - -google::protobuf::Message* wpi::Protobuf::New( - google::protobuf::Arena* arena) { - return google::protobuf::Arena::CreateMessage< - wpi::proto::ProtobufTranslation2d>(arena); -} - -frc::Translation2d wpi::Protobuf::Unpack( - const google::protobuf::Message& msg) { - auto m = static_cast(&msg); - return frc::Translation2d{units::meter_t{m->x()}, units::meter_t{m->y()}}; -} - -void wpi::Protobuf::Pack(google::protobuf::Message* msg, - const frc::Translation2d& value) { - auto m = static_cast(msg); - m->set_x(value.X().value()); - m->set_y(value.Y().value()); -} diff --git a/wpimath/src/main/native/cpp/geometry/Translation3d.cpp b/wpimath/src/main/native/cpp/geometry/Translation3d.cpp index 90e94ae8258..ecfee1ca3a2 100644 --- a/wpimath/src/main/native/cpp/geometry/Translation3d.cpp +++ b/wpimath/src/main/native/cpp/geometry/Translation3d.cpp @@ -53,24 +53,3 @@ void frc::from_json(const wpi::json& json, Translation3d& translation) { units::meter_t{json.at("y").get()}, units::meter_t{json.at("z").get()}}; } - -google::protobuf::Message* wpi::Protobuf::New( - google::protobuf::Arena* arena) { - return google::protobuf::Arena::CreateMessage< - wpi::proto::ProtobufTranslation3d>(arena); -} - -frc::Translation3d wpi::Protobuf::Unpack( - const google::protobuf::Message& msg) { - auto m = static_cast(&msg); - return frc::Translation3d{units::meter_t{m->x()}, units::meter_t{m->y()}, - units::meter_t{m->z()}}; -} - -void wpi::Protobuf::Pack(google::protobuf::Message* msg, - const frc::Translation3d& value) { - auto m = static_cast(msg); - m->set_x(value.X().value()); - m->set_y(value.Y().value()); - m->set_z(value.Z().value()); -} diff --git a/wpimath/src/main/native/cpp/geometry/proto/Pose2dProto.cpp b/wpimath/src/main/native/cpp/geometry/proto/Pose2dProto.cpp new file mode 100644 index 00000000000..2b8bf5ff665 --- /dev/null +++ b/wpimath/src/main/native/cpp/geometry/proto/Pose2dProto.cpp @@ -0,0 +1,29 @@ +// 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. + +#include "frc/geometry/proto/Pose2dProto.h" + +#include "geometry2d.pb.h" + +google::protobuf::Message* wpi::Protobuf::New( + google::protobuf::Arena* arena) { + return google::protobuf::Arena::CreateMessage( + arena); +} + +frc::Pose2d wpi::Protobuf::Unpack( + const google::protobuf::Message& msg) { + auto m = static_cast(&msg); + return frc::Pose2d{ + wpi::UnpackProtobuf(m->translation()), + wpi::UnpackProtobuf(m->rotation()), + }; +} + +void wpi::Protobuf::Pack(google::protobuf::Message* msg, + const frc::Pose2d& value) { + auto m = static_cast(msg); + wpi::PackProtobuf(m->mutable_translation(), value.Translation()); + wpi::PackProtobuf(m->mutable_rotation(), value.Rotation()); +} diff --git a/wpimath/src/main/native/cpp/geometry/proto/Pose3dProto.cpp b/wpimath/src/main/native/cpp/geometry/proto/Pose3dProto.cpp new file mode 100644 index 00000000000..581cafb0f48 --- /dev/null +++ b/wpimath/src/main/native/cpp/geometry/proto/Pose3dProto.cpp @@ -0,0 +1,29 @@ +// 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. + +#include "frc/geometry/proto/Pose3dProto.h" + +#include "geometry3d.pb.h" + +google::protobuf::Message* wpi::Protobuf::New( + google::protobuf::Arena* arena) { + return google::protobuf::Arena::CreateMessage( + arena); +} + +frc::Pose3d wpi::Protobuf::Unpack( + const google::protobuf::Message& msg) { + auto m = static_cast(&msg); + return frc::Pose3d{ + wpi::UnpackProtobuf(m->translation()), + wpi::UnpackProtobuf(m->rotation()), + }; +} + +void wpi::Protobuf::Pack(google::protobuf::Message* msg, + const frc::Pose3d& value) { + auto m = static_cast(msg); + wpi::PackProtobuf(m->mutable_translation(), value.Translation()); + wpi::PackProtobuf(m->mutable_rotation(), value.Rotation()); +} diff --git a/wpimath/src/main/native/cpp/geometry/proto/QuaternionProto.cpp b/wpimath/src/main/native/cpp/geometry/proto/QuaternionProto.cpp new file mode 100644 index 00000000000..aadefa03918 --- /dev/null +++ b/wpimath/src/main/native/cpp/geometry/proto/QuaternionProto.cpp @@ -0,0 +1,33 @@ +// 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. + +#include "frc/geometry/proto/QuaternionProto.h" + +#include "geometry3d.pb.h" + +google::protobuf::Message* wpi::Protobuf::New( + google::protobuf::Arena* arena) { + return google::protobuf::Arena::CreateMessage( + arena); +} + +frc::Quaternion wpi::Protobuf::Unpack( + const google::protobuf::Message& msg) { + auto m = static_cast(&msg); + return frc::Quaternion{ + m->w(), + m->x(), + m->y(), + m->z(), + }; +} + +void wpi::Protobuf::Pack(google::protobuf::Message* msg, + const frc::Quaternion& value) { + auto m = static_cast(msg); + m->set_w(value.W()); + m->set_x(value.X()); + m->set_y(value.Y()); + m->set_z(value.Z()); +} diff --git a/wpimath/src/main/native/cpp/geometry/proto/Rotation2dProto.cpp b/wpimath/src/main/native/cpp/geometry/proto/Rotation2dProto.cpp new file mode 100644 index 00000000000..c9005f2bf87 --- /dev/null +++ b/wpimath/src/main/native/cpp/geometry/proto/Rotation2dProto.cpp @@ -0,0 +1,27 @@ +// 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. + +#include "frc/geometry/proto/Rotation2dProto.h" + +#include "geometry2d.pb.h" + +google::protobuf::Message* wpi::Protobuf::New( + google::protobuf::Arena* arena) { + return google::protobuf::Arena::CreateMessage( + arena); +} + +frc::Rotation2d wpi::Protobuf::Unpack( + const google::protobuf::Message& msg) { + auto m = static_cast(&msg); + return frc::Rotation2d{ + units::radian_t{m->value()}, + }; +} + +void wpi::Protobuf::Pack(google::protobuf::Message* msg, + const frc::Rotation2d& value) { + auto m = static_cast(msg); + m->set_value(value.Radians().value()); +} diff --git a/wpimath/src/main/native/cpp/geometry/proto/Rotation3dProto.cpp b/wpimath/src/main/native/cpp/geometry/proto/Rotation3dProto.cpp new file mode 100644 index 00000000000..72645e1952b --- /dev/null +++ b/wpimath/src/main/native/cpp/geometry/proto/Rotation3dProto.cpp @@ -0,0 +1,27 @@ +// 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. + +#include "frc/geometry/proto/Rotation3dProto.h" + +#include "geometry3d.pb.h" + +google::protobuf::Message* wpi::Protobuf::New( + google::protobuf::Arena* arena) { + return google::protobuf::Arena::CreateMessage( + arena); +} + +frc::Rotation3d wpi::Protobuf::Unpack( + const google::protobuf::Message& msg) { + auto m = static_cast(&msg); + return frc::Rotation3d{ + wpi::UnpackProtobuf(m->q()), + }; +} + +void wpi::Protobuf::Pack(google::protobuf::Message* msg, + const frc::Rotation3d& value) { + auto m = static_cast(msg); + wpi::PackProtobuf(m->mutable_q(), value.GetQuaternion()); +} diff --git a/wpimath/src/main/native/cpp/geometry/proto/Transform2dProto.cpp b/wpimath/src/main/native/cpp/geometry/proto/Transform2dProto.cpp new file mode 100644 index 00000000000..4d38b812786 --- /dev/null +++ b/wpimath/src/main/native/cpp/geometry/proto/Transform2dProto.cpp @@ -0,0 +1,29 @@ +// 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. + +#include "frc/geometry/proto/Transform2dProto.h" + +#include "geometry2d.pb.h" + +google::protobuf::Message* wpi::Protobuf::New( + google::protobuf::Arena* arena) { + return google::protobuf::Arena::CreateMessage< + wpi::proto::ProtobufTransform2d>(arena); +} + +frc::Transform2d wpi::Protobuf::Unpack( + const google::protobuf::Message& msg) { + auto m = static_cast(&msg); + return frc::Transform2d{ + wpi::UnpackProtobuf(m->translation()), + wpi::UnpackProtobuf(m->rotation()), + }; +} + +void wpi::Protobuf::Pack(google::protobuf::Message* msg, + const frc::Transform2d& value) { + auto m = static_cast(msg); + wpi::PackProtobuf(m->mutable_translation(), value.Translation()); + wpi::PackProtobuf(m->mutable_rotation(), value.Rotation()); +} diff --git a/wpimath/src/main/native/cpp/geometry/proto/Transform3dProto.cpp b/wpimath/src/main/native/cpp/geometry/proto/Transform3dProto.cpp new file mode 100644 index 00000000000..4a8fbfd326d --- /dev/null +++ b/wpimath/src/main/native/cpp/geometry/proto/Transform3dProto.cpp @@ -0,0 +1,29 @@ +// 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. + +#include "frc/geometry/proto/Transform3dProto.h" + +#include "geometry3d.pb.h" + +google::protobuf::Message* wpi::Protobuf::New( + google::protobuf::Arena* arena) { + return google::protobuf::Arena::CreateMessage< + wpi::proto::ProtobufTransform3d>(arena); +} + +frc::Transform3d wpi::Protobuf::Unpack( + const google::protobuf::Message& msg) { + auto m = static_cast(&msg); + return frc::Transform3d{ + wpi::UnpackProtobuf(m->translation()), + wpi::UnpackProtobuf(m->rotation()), + }; +} + +void wpi::Protobuf::Pack(google::protobuf::Message* msg, + const frc::Transform3d& value) { + auto m = static_cast(msg); + wpi::PackProtobuf(m->mutable_translation(), value.Translation()); + wpi::PackProtobuf(m->mutable_rotation(), value.Rotation()); +} diff --git a/wpimath/src/main/native/cpp/geometry/proto/Translation2dProto.cpp b/wpimath/src/main/native/cpp/geometry/proto/Translation2dProto.cpp new file mode 100644 index 00000000000..739dc99f34b --- /dev/null +++ b/wpimath/src/main/native/cpp/geometry/proto/Translation2dProto.cpp @@ -0,0 +1,29 @@ +// 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. + +#include "frc/geometry/proto/Translation2dProto.h" + +#include "geometry2d.pb.h" + +google::protobuf::Message* wpi::Protobuf::New( + google::protobuf::Arena* arena) { + return google::protobuf::Arena::CreateMessage< + wpi::proto::ProtobufTranslation2d>(arena); +} + +frc::Translation2d wpi::Protobuf::Unpack( + const google::protobuf::Message& msg) { + auto m = static_cast(&msg); + return frc::Translation2d{ + units::meter_t{m->x()}, + units::meter_t{m->y()}, + }; +} + +void wpi::Protobuf::Pack(google::protobuf::Message* msg, + const frc::Translation2d& value) { + auto m = static_cast(msg); + m->set_x(value.X().value()); + m->set_y(value.Y().value()); +} diff --git a/wpimath/src/main/native/cpp/geometry/proto/Translation3dProto.cpp b/wpimath/src/main/native/cpp/geometry/proto/Translation3dProto.cpp new file mode 100644 index 00000000000..6285b2b8528 --- /dev/null +++ b/wpimath/src/main/native/cpp/geometry/proto/Translation3dProto.cpp @@ -0,0 +1,31 @@ +// 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. + +#include "frc/geometry/proto/Translation3dProto.h" + +#include "geometry3d.pb.h" + +google::protobuf::Message* wpi::Protobuf::New( + google::protobuf::Arena* arena) { + return google::protobuf::Arena::CreateMessage< + wpi::proto::ProtobufTranslation3d>(arena); +} + +frc::Translation3d wpi::Protobuf::Unpack( + const google::protobuf::Message& msg) { + auto m = static_cast(&msg); + return frc::Translation3d{ + units::meter_t{m->x()}, + units::meter_t{m->y()}, + units::meter_t{m->z()}, + }; +} + +void wpi::Protobuf::Pack(google::protobuf::Message* msg, + const frc::Translation3d& value) { + auto m = static_cast(msg); + m->set_x(value.X().value()); + m->set_y(value.Y().value()); + m->set_z(value.Z().value()); +} diff --git a/wpimath/src/main/native/cpp/geometry/Twist2d.cpp b/wpimath/src/main/native/cpp/geometry/proto/Twist2dProto.cpp similarity index 83% rename from wpimath/src/main/native/cpp/geometry/Twist2d.cpp rename to wpimath/src/main/native/cpp/geometry/proto/Twist2dProto.cpp index 6c106eb436e..2590fc96a38 100644 --- a/wpimath/src/main/native/cpp/geometry/Twist2d.cpp +++ b/wpimath/src/main/native/cpp/geometry/proto/Twist2dProto.cpp @@ -2,12 +2,10 @@ // 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. -#include "frc/geometry/Twist2d.h" +#include "frc/geometry/proto/Twist2dProto.h" #include "geometry2d.pb.h" -using namespace frc; - google::protobuf::Message* wpi::Protobuf::New( google::protobuf::Arena* arena) { return google::protobuf::Arena::CreateMessage( @@ -17,8 +15,11 @@ google::protobuf::Message* wpi::Protobuf::New( frc::Twist2d wpi::Protobuf::Unpack( const google::protobuf::Message& msg) { auto m = static_cast(&msg); - return frc::Twist2d{units::meter_t{m->dx()}, units::meter_t{m->dy()}, - units::radian_t{m->dtheta()}}; + return frc::Twist2d{ + units::meter_t{m->dx()}, + units::meter_t{m->dy()}, + units::radian_t{m->dtheta()}, + }; } void wpi::Protobuf::Pack(google::protobuf::Message* msg, diff --git a/wpimath/src/main/native/cpp/geometry/Twist3d.cpp b/wpimath/src/main/native/cpp/geometry/proto/Twist3dProto.cpp similarity index 77% rename from wpimath/src/main/native/cpp/geometry/Twist3d.cpp rename to wpimath/src/main/native/cpp/geometry/proto/Twist3dProto.cpp index 4f4ce86b4bd..e2d91e4e16b 100644 --- a/wpimath/src/main/native/cpp/geometry/Twist3d.cpp +++ b/wpimath/src/main/native/cpp/geometry/proto/Twist3dProto.cpp @@ -2,12 +2,10 @@ // 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. -#include "frc/geometry/Twist3d.h" +#include "frc/geometry/proto/Twist3dProto.h" #include "geometry3d.pb.h" -using namespace frc; - google::protobuf::Message* wpi::Protobuf::New( google::protobuf::Arena* arena) { return google::protobuf::Arena::CreateMessage( @@ -17,9 +15,11 @@ google::protobuf::Message* wpi::Protobuf::New( frc::Twist3d wpi::Protobuf::Unpack( const google::protobuf::Message& msg) { auto m = static_cast(&msg); - return frc::Twist3d{units::meter_t{m->dx()}, units::meter_t{m->dy()}, - units::meter_t{m->dz()}, units::radian_t{m->rx()}, - units::radian_t{m->ry()}, units::radian_t{m->rz()}}; + return frc::Twist3d{ + units::meter_t{m->dx()}, units::meter_t{m->dy()}, + units::meter_t{m->dz()}, units::radian_t{m->rx()}, + units::radian_t{m->ry()}, units::radian_t{m->rz()}, + }; } void wpi::Protobuf::Pack(google::protobuf::Message* msg, diff --git a/wpimath/src/main/native/cpp/geometry/struct/Pose2dStruct.cpp b/wpimath/src/main/native/cpp/geometry/struct/Pose2dStruct.cpp new file mode 100644 index 00000000000..68e63476ac5 --- /dev/null +++ b/wpimath/src/main/native/cpp/geometry/struct/Pose2dStruct.cpp @@ -0,0 +1,32 @@ +// 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. + +#include "frc/geometry/struct/Pose2dStruct.h" + +namespace { +constexpr size_t kTranslationOff = 0; +constexpr size_t kRotationOff = + kTranslationOff + wpi::Struct::kSize; +} // namespace + +using StructType = wpi::Struct; + +frc::Pose2d StructType::Unpack(std::span data) { + return frc::Pose2d{ + wpi::UnpackStruct(data), + wpi::UnpackStruct(data), + }; +} + +void StructType::Pack(std::span data, + const frc::Pose2d& value) { + wpi::PackStruct(data, value.Translation()); + wpi::PackStruct(data, value.Rotation()); +} + +void StructType::ForEachNested( + std::invocable auto fn) { + wpi::ForEachStructSchema(fn); + wpi::ForEachStructSchema(fn); +} diff --git a/wpimath/src/main/native/cpp/geometry/struct/Pose3dStruct.cpp b/wpimath/src/main/native/cpp/geometry/struct/Pose3dStruct.cpp new file mode 100644 index 00000000000..37995781e1f --- /dev/null +++ b/wpimath/src/main/native/cpp/geometry/struct/Pose3dStruct.cpp @@ -0,0 +1,32 @@ +// 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. + +#include "frc/geometry/struct/Pose3dStruct.h" + +namespace { +constexpr size_t kTranslationOff = 0; +constexpr size_t kRotationOff = + kTranslationOff + wpi::Struct::kSize; +} // namespace + +using StructType = wpi::Struct; + +frc::Pose3d StructType::Unpack(std::span data) { + return frc::Pose3d{ + wpi::UnpackStruct(data), + wpi::UnpackStruct(data), + }; +} + +void StructType::Pack(std::span data, + const frc::Pose3d& value) { + wpi::PackStruct(data, value.Translation()); + wpi::PackStruct(data, value.Rotation()); +} + +void StructType::ForEachNested( + std::invocable auto fn) { + wpi::ForEachStructSchema(fn); + wpi::ForEachStructSchema(fn); +} diff --git a/wpimath/src/main/native/cpp/geometry/struct/QuaternionStruct.cpp b/wpimath/src/main/native/cpp/geometry/struct/QuaternionStruct.cpp new file mode 100644 index 00000000000..140b6faca64 --- /dev/null +++ b/wpimath/src/main/native/cpp/geometry/struct/QuaternionStruct.cpp @@ -0,0 +1,31 @@ +// 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. + +#include "frc/geometry/struct/QuaternionStruct.h" + +namespace { +constexpr size_t kWOff = 0; +constexpr size_t kXOff = kWOff + 8; +constexpr size_t kYOff = kXOff + 8; +constexpr size_t kZOff = kYOff + 8; +} // namespace + +using StructType = wpi::Struct; + +frc::Quaternion StructType::Unpack(std::span data) { + return frc::Quaternion{ + wpi::UnpackStruct(data), + wpi::UnpackStruct(data), + wpi::UnpackStruct(data), + wpi::UnpackStruct(data), + }; +} + +void StructType::Pack(std::span data, + const frc::Quaternion& value) { + wpi::PackStruct(data, value.W()); + wpi::PackStruct(data, value.X()); + wpi::PackStruct(data, value.Y()); + wpi::PackStruct(data, value.Z()); +} diff --git a/wpimath/src/main/native/cpp/geometry/struct/Rotation2dStruct.cpp b/wpimath/src/main/native/cpp/geometry/struct/Rotation2dStruct.cpp new file mode 100644 index 00000000000..8027858cb2c --- /dev/null +++ b/wpimath/src/main/native/cpp/geometry/struct/Rotation2dStruct.cpp @@ -0,0 +1,22 @@ +// 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. + +#include "frc/geometry/struct/Rotation2dStruct.h" + +namespace { +constexpr size_t kValueOff = 0; +} // namespace + +using StructType = wpi::Struct; + +frc::Rotation2d StructType::Unpack(std::span data) { + return frc::Rotation2d{ + units::radian_t{wpi::UnpackStruct(data)}, + }; +} + +void StructType::Pack(std::span data, + const frc::Rotation2d& value) { + wpi::PackStruct(data, value.Radians().value()); +} diff --git a/wpimath/src/main/native/cpp/geometry/struct/Rotation3dStruct.cpp b/wpimath/src/main/native/cpp/geometry/struct/Rotation3dStruct.cpp new file mode 100644 index 00000000000..e2a122d5612 --- /dev/null +++ b/wpimath/src/main/native/cpp/geometry/struct/Rotation3dStruct.cpp @@ -0,0 +1,27 @@ +// 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. + +#include "frc/geometry/struct/Rotation3dStruct.h" + +namespace { +constexpr size_t kQOff = 0; +} // namespace + +using StructType = wpi::Struct; + +frc::Rotation3d StructType::Unpack(std::span data) { + return frc::Rotation3d{ + wpi::UnpackStruct(data), + }; +} + +void StructType::Pack(std::span data, + const frc::Rotation3d& value) { + wpi::PackStruct(data, value.GetQuaternion()); +} + +void StructType::ForEachNested( + std::invocable auto fn) { + wpi::ForEachStructSchema(fn); +} diff --git a/wpimath/src/main/native/cpp/geometry/struct/Transform2dStruct.cpp b/wpimath/src/main/native/cpp/geometry/struct/Transform2dStruct.cpp new file mode 100644 index 00000000000..25ef5119a0a --- /dev/null +++ b/wpimath/src/main/native/cpp/geometry/struct/Transform2dStruct.cpp @@ -0,0 +1,32 @@ +// 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. + +#include "frc/geometry/struct/Transform2dStruct.h" + +namespace { +constexpr size_t kTranslationOff = 0; +constexpr size_t kRotationOff = + kTranslationOff + wpi::Struct::kSize; +} // namespace + +using StructType = wpi::Struct; + +frc::Transform2d StructType::Unpack(std::span data) { + return frc::Transform2d{ + wpi::UnpackStruct(data), + wpi::UnpackStruct(data), + }; +} + +void StructType::Pack(std::span data, + const frc::Transform2d& value) { + wpi::PackStruct(data, value.Translation()); + wpi::PackStruct(data, value.Rotation()); +} + +void StructType::ForEachNested( + std::invocable auto fn) { + wpi::ForEachStructSchema(fn); + wpi::ForEachStructSchema(fn); +} diff --git a/wpimath/src/main/native/cpp/geometry/struct/Transform3dStruct.cpp b/wpimath/src/main/native/cpp/geometry/struct/Transform3dStruct.cpp new file mode 100644 index 00000000000..5f169c5b2a1 --- /dev/null +++ b/wpimath/src/main/native/cpp/geometry/struct/Transform3dStruct.cpp @@ -0,0 +1,32 @@ +// 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. + +#include "frc/geometry/struct/Transform3dStruct.h" + +namespace { +constexpr size_t kTranslationOff = 0; +constexpr size_t kRotationOff = + kTranslationOff + wpi::Struct::kSize; +} // namespace + +using StructType = wpi::Struct; + +frc::Transform3d StructType::Unpack(std::span data) { + return frc::Transform3d{ + wpi::UnpackStruct(data), + wpi::UnpackStruct(data), + }; +} + +void StructType::Pack(std::span data, + const frc::Transform3d& value) { + wpi::PackStruct(data, value.Translation()); + wpi::PackStruct(data, value.Rotation()); +} + +void StructType::ForEachNested( + std::invocable auto fn) { + wpi::ForEachStructSchema(fn); + wpi::ForEachStructSchema(fn); +} diff --git a/wpimath/src/main/native/cpp/geometry/struct/Translation2dStruct.cpp b/wpimath/src/main/native/cpp/geometry/struct/Translation2dStruct.cpp new file mode 100644 index 00000000000..e9deb8ea29b --- /dev/null +++ b/wpimath/src/main/native/cpp/geometry/struct/Translation2dStruct.cpp @@ -0,0 +1,25 @@ +// 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. + +#include "frc/geometry/struct/Translation2dStruct.h" + +namespace { +constexpr size_t kXOff = 0; +constexpr size_t kYOff = kXOff + 8; +} // namespace + +using StructType = wpi::Struct; + +frc::Translation2d StructType::Unpack(std::span data) { + return frc::Translation2d{ + units::meter_t{wpi::UnpackStruct(data)}, + units::meter_t{wpi::UnpackStruct(data)}, + }; +} + +void StructType::Pack(std::span data, + const frc::Translation2d& value) { + wpi::PackStruct(data, value.X().value()); + wpi::PackStruct(data, value.Y().value()); +} diff --git a/wpimath/src/main/native/cpp/geometry/struct/Translation3dStruct.cpp b/wpimath/src/main/native/cpp/geometry/struct/Translation3dStruct.cpp new file mode 100644 index 00000000000..d747cb3c227 --- /dev/null +++ b/wpimath/src/main/native/cpp/geometry/struct/Translation3dStruct.cpp @@ -0,0 +1,28 @@ +// 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. + +#include "frc/geometry/struct/Translation3dStruct.h" + +namespace { +constexpr size_t kXOff = 0; +constexpr size_t kYOff = kXOff + 8; +constexpr size_t kZOff = kYOff + 8; +} // namespace + +using StructType = wpi::Struct; + +frc::Translation3d StructType::Unpack(std::span data) { + return frc::Translation3d{ + units::meter_t{wpi::UnpackStruct(data)}, + units::meter_t{wpi::UnpackStruct(data)}, + units::meter_t{wpi::UnpackStruct(data)}, + }; +} + +void StructType::Pack(std::span data, + const frc::Translation3d& value) { + wpi::PackStruct(data, value.X().value()); + wpi::PackStruct(data, value.Y().value()); + wpi::PackStruct(data, value.Z().value()); +} diff --git a/wpimath/src/main/native/cpp/geometry/struct/Twist2dStruct.cpp b/wpimath/src/main/native/cpp/geometry/struct/Twist2dStruct.cpp new file mode 100644 index 00000000000..9ad58199e4e --- /dev/null +++ b/wpimath/src/main/native/cpp/geometry/struct/Twist2dStruct.cpp @@ -0,0 +1,28 @@ +// 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. + +#include "frc/geometry/struct/Twist2dStruct.h" + +namespace { +constexpr size_t kDxOff = 0; +constexpr size_t kDyOff = kDxOff + 8; +constexpr size_t kDthetaOff = kDyOff + 8; +} // namespace + +using StructType = wpi::Struct; + +frc::Twist2d StructType::Unpack(std::span data) { + return frc::Twist2d{ + units::meter_t{wpi::UnpackStruct(data)}, + units::meter_t{wpi::UnpackStruct(data)}, + units::radian_t{wpi::UnpackStruct(data)}, + }; +} + +void StructType::Pack(std::span data, + const frc::Twist2d& value) { + wpi::PackStruct(data, value.dx.value()); + wpi::PackStruct(data, value.dy.value()); + wpi::PackStruct(data, value.dtheta.value()); +} diff --git a/wpimath/src/main/native/cpp/geometry/struct/Twist3dStruct.cpp b/wpimath/src/main/native/cpp/geometry/struct/Twist3dStruct.cpp new file mode 100644 index 00000000000..f434a8f1a9a --- /dev/null +++ b/wpimath/src/main/native/cpp/geometry/struct/Twist3dStruct.cpp @@ -0,0 +1,37 @@ +// 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. + +#include "frc/geometry/struct/Twist3dStruct.h" + +namespace { +constexpr size_t kDxOff = 0; +constexpr size_t kDyOff = kDxOff + 8; +constexpr size_t kDzOff = kDyOff + 8; +constexpr size_t kRxOff = kDzOff + 8; +constexpr size_t kRyOff = kRxOff + 8; +constexpr size_t kRzOff = kRyOff + 8; +} // namespace + +using StructType = wpi::Struct; + +frc::Twist3d StructType::Unpack(std::span data) { + return frc::Twist3d{ + units::meter_t{wpi::UnpackStruct(data)}, + units::meter_t{wpi::UnpackStruct(data)}, + units::meter_t{wpi::UnpackStruct(data)}, + units::radian_t{wpi::UnpackStruct(data)}, + units::radian_t{wpi::UnpackStruct(data)}, + units::radian_t{wpi::UnpackStruct(data)}, + }; +} + +void StructType::Pack(std::span data, + const frc::Twist3d& value) { + wpi::PackStruct(data, value.dx.value()); + wpi::PackStruct(data, value.dy.value()); + wpi::PackStruct(data, value.dz.value()); + wpi::PackStruct(data, value.rx.value()); + wpi::PackStruct(data, value.ry.value()); + wpi::PackStruct(data, value.rz.value()); +} diff --git a/wpimath/src/main/native/cpp/kinematics/proto/ChassisSpeedsProto.cpp b/wpimath/src/main/native/cpp/kinematics/proto/ChassisSpeedsProto.cpp new file mode 100644 index 00000000000..097a2fba2e5 --- /dev/null +++ b/wpimath/src/main/native/cpp/kinematics/proto/ChassisSpeedsProto.cpp @@ -0,0 +1,31 @@ +// 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. + +#include "frc/kinematics/proto/ChassisSpeedsProto.h" + +#include "kinematics.pb.h" + +google::protobuf::Message* wpi::Protobuf::New( + google::protobuf::Arena* arena) { + return google::protobuf::Arena::CreateMessage< + wpi::proto::ProtobufChassisSpeeds>(arena); +} + +frc::ChassisSpeeds wpi::Protobuf::Unpack( + const google::protobuf::Message& msg) { + auto m = static_cast(&msg); + return frc::ChassisSpeeds{ + units::meters_per_second_t{m->vx()}, + units::meters_per_second_t{m->vy()}, + units::radians_per_second_t{m->omega()}, + }; +} + +void wpi::Protobuf::Pack(google::protobuf::Message* msg, + const frc::ChassisSpeeds& value) { + auto m = static_cast(msg); + m->set_vx(value.vx.value()); + m->set_vy(value.vy.value()); + m->set_omega(value.omega.value()); +} diff --git a/wpimath/src/main/native/cpp/kinematics/proto/DifferentialDriveKinematicsProto.cpp b/wpimath/src/main/native/cpp/kinematics/proto/DifferentialDriveKinematicsProto.cpp new file mode 100644 index 00000000000..a79e565572a --- /dev/null +++ b/wpimath/src/main/native/cpp/kinematics/proto/DifferentialDriveKinematicsProto.cpp @@ -0,0 +1,30 @@ +// 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. + +#include "frc/kinematics/proto/DifferentialDriveKinematicsProto.h" + +#include "kinematics.pb.h" + +google::protobuf::Message* wpi::Protobuf::New( + google::protobuf::Arena* arena) { + return google::protobuf::Arena::CreateMessage< + wpi::proto::ProtobufDifferentialDriveKinematics>(arena); +} + +frc::DifferentialDriveKinematics +wpi::Protobuf::Unpack( + const google::protobuf::Message& msg) { + auto m = + static_cast(&msg); + return frc::DifferentialDriveKinematics{ + units::meter_t{m->track_width()}, + }; +} + +void wpi::Protobuf::Pack( + google::protobuf::Message* msg, + const frc::DifferentialDriveKinematics& value) { + auto m = static_cast(msg); + m->set_track_width(value.trackWidth.value()); +} diff --git a/wpimath/src/main/native/cpp/kinematics/proto/DifferentialDriveWheelSpeedsProto.cpp b/wpimath/src/main/native/cpp/kinematics/proto/DifferentialDriveWheelSpeedsProto.cpp new file mode 100644 index 00000000000..cb38b844c47 --- /dev/null +++ b/wpimath/src/main/native/cpp/kinematics/proto/DifferentialDriveWheelSpeedsProto.cpp @@ -0,0 +1,32 @@ +// 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. + +#include "frc/kinematics/proto/DifferentialDriveWheelSpeedsProto.h" + +#include "kinematics.pb.h" + +google::protobuf::Message* wpi::Protobuf< + frc::DifferentialDriveWheelSpeeds>::New(google::protobuf::Arena* arena) { + return google::protobuf::Arena::CreateMessage< + wpi::proto::ProtobufDifferentialDriveWheelSpeeds>(arena); +} + +frc::DifferentialDriveWheelSpeeds +wpi::Protobuf::Unpack( + const google::protobuf::Message& msg) { + auto m = static_cast( + &msg); + return frc::DifferentialDriveWheelSpeeds{ + units::meters_per_second_t{m->left()}, + units::meters_per_second_t{m->right()}, + }; +} + +void wpi::Protobuf::Pack( + google::protobuf::Message* msg, + const frc::DifferentialDriveWheelSpeeds& value) { + auto m = static_cast(msg); + m->set_left(value.left.value()); + m->set_right(value.right.value()); +} diff --git a/wpimath/src/main/native/cpp/kinematics/proto/MecanumDriveKinematicsProto.cpp b/wpimath/src/main/native/cpp/kinematics/proto/MecanumDriveKinematicsProto.cpp new file mode 100644 index 00000000000..5b3c027e6ef --- /dev/null +++ b/wpimath/src/main/native/cpp/kinematics/proto/MecanumDriveKinematicsProto.cpp @@ -0,0 +1,33 @@ +// 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. + +#include "frc/kinematics/proto/MecanumDriveKinematicsProto.h" + +#include "kinematics.pb.h" + +google::protobuf::Message* wpi::Protobuf::New( + google::protobuf::Arena* arena) { + return google::protobuf::Arena::CreateMessage< + wpi::proto::ProtobufMecanumDriveKinematics>(arena); +} + +frc::MecanumDriveKinematics wpi::Protobuf::Unpack( + const google::protobuf::Message& msg) { + auto m = static_cast(&msg); + return frc::MecanumDriveKinematics{ + wpi::UnpackProtobuf(m->front_left()), + wpi::UnpackProtobuf(m->front_right()), + wpi::UnpackProtobuf(m->rear_left()), + wpi::UnpackProtobuf(m->rear_right()), + }; +} + +void wpi::Protobuf::Pack( + google::protobuf::Message* msg, const frc::MecanumDriveKinematics& value) { + auto m = static_cast(msg); + wpi::PackProtobuf(m->mutable_front_left(), value.GetFrontLeftWheel()); + wpi::PackProtobuf(m->mutable_front_right(), value.GetFrontRightWheel()); + wpi::PackProtobuf(m->mutable_rear_left(), value.GetRearLeftWheel()); + wpi::PackProtobuf(m->mutable_rear_right(), value.GetRearRightWheel()); +} diff --git a/wpimath/src/main/native/cpp/kinematics/proto/MecanumDriveWheelPositionsProto.cpp b/wpimath/src/main/native/cpp/kinematics/proto/MecanumDriveWheelPositionsProto.cpp new file mode 100644 index 00000000000..94ca982cfa6 --- /dev/null +++ b/wpimath/src/main/native/cpp/kinematics/proto/MecanumDriveWheelPositionsProto.cpp @@ -0,0 +1,36 @@ +// 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. + +#include "frc/kinematics/proto/MecanumDriveWheelPositionsProto.h" + +#include "kinematics.pb.h" + +google::protobuf::Message* wpi::Protobuf::New( + google::protobuf::Arena* arena) { + return google::protobuf::Arena::CreateMessage< + wpi::proto::ProtobufMecanumDriveWheelPositions>(arena); +} + +frc::MecanumDriveWheelPositions +wpi::Protobuf::Unpack( + const google::protobuf::Message& msg) { + auto m = + static_cast(&msg); + return frc::MecanumDriveWheelPositions{ + units::meter_t{m->front_left()}, + units::meter_t{m->front_right()}, + units::meter_t{m->rear_left()}, + units::meter_t{m->rear_right()}, + }; +} + +void wpi::Protobuf::Pack( + google::protobuf::Message* msg, + const frc::MecanumDriveWheelPositions& value) { + auto m = static_cast(msg); + m->set_front_left(value.frontLeft.value()); + m->set_front_right(value.frontRight.value()); + m->set_rear_left(value.rearLeft.value()); + m->set_rear_right(value.rearRight.value()); +} diff --git a/wpimath/src/main/native/cpp/kinematics/proto/MecanumDriveWheelSpeedsProto.cpp b/wpimath/src/main/native/cpp/kinematics/proto/MecanumDriveWheelSpeedsProto.cpp new file mode 100644 index 00000000000..049a088eda0 --- /dev/null +++ b/wpimath/src/main/native/cpp/kinematics/proto/MecanumDriveWheelSpeedsProto.cpp @@ -0,0 +1,35 @@ +// 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. + +#include "frc/kinematics/proto/MecanumDriveWheelSpeedsProto.h" + +#include "kinematics.pb.h" + +google::protobuf::Message* wpi::Protobuf::New( + google::protobuf::Arena* arena) { + return google::protobuf::Arena::CreateMessage< + wpi::proto::ProtobufMecanumDriveWheelSpeeds>(arena); +} + +frc::MecanumDriveWheelSpeeds +wpi::Protobuf::Unpack( + const google::protobuf::Message& msg) { + auto m = + static_cast(&msg); + return frc::MecanumDriveWheelSpeeds{ + units::meters_per_second_t{m->front_left()}, + units::meters_per_second_t{m->front_right()}, + units::meters_per_second_t{m->rear_left()}, + units::meters_per_second_t{m->rear_right()}, + }; +} + +void wpi::Protobuf::Pack( + google::protobuf::Message* msg, const frc::MecanumDriveWheelSpeeds& value) { + auto m = static_cast(msg); + m->set_front_left(value.frontLeft.value()); + m->set_front_right(value.frontRight.value()); + m->set_rear_left(value.rearLeft.value()); + m->set_rear_right(value.rearRight.value()); +} diff --git a/wpimath/src/main/native/cpp/kinematics/proto/SwerveModulePositionProto.cpp b/wpimath/src/main/native/cpp/kinematics/proto/SwerveModulePositionProto.cpp new file mode 100644 index 00000000000..4e85ec8eb4b --- /dev/null +++ b/wpimath/src/main/native/cpp/kinematics/proto/SwerveModulePositionProto.cpp @@ -0,0 +1,29 @@ +// 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. + +#include "frc/kinematics/proto/SwerveModulePositionProto.h" + +#include "kinematics.pb.h" + +google::protobuf::Message* wpi::Protobuf::New( + google::protobuf::Arena* arena) { + return google::protobuf::Arena::CreateMessage< + wpi::proto::ProtobufSwerveModulePosition>(arena); +} + +frc::SwerveModulePosition wpi::Protobuf::Unpack( + const google::protobuf::Message& msg) { + auto m = static_cast(&msg); + return frc::SwerveModulePosition{ + units::meter_t{m->distance()}, + wpi::UnpackProtobuf(m->angle()), + }; +} + +void wpi::Protobuf::Pack( + google::protobuf::Message* msg, const frc::SwerveModulePosition& value) { + auto m = static_cast(msg); + m->set_distance(value.distance.value()); + wpi::PackProtobuf(m->mutable_angle(), value.angle); +} diff --git a/wpimath/src/main/native/cpp/kinematics/proto/SwerveModuleStateProto.cpp b/wpimath/src/main/native/cpp/kinematics/proto/SwerveModuleStateProto.cpp new file mode 100644 index 00000000000..f5d5a0fa1d6 --- /dev/null +++ b/wpimath/src/main/native/cpp/kinematics/proto/SwerveModuleStateProto.cpp @@ -0,0 +1,29 @@ +// 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. + +#include "frc/kinematics/proto/SwerveModuleStateProto.h" + +#include "kinematics.pb.h" + +google::protobuf::Message* wpi::Protobuf::New( + google::protobuf::Arena* arena) { + return google::protobuf::Arena::CreateMessage< + wpi::proto::ProtobufSwerveModuleState>(arena); +} + +frc::SwerveModuleState wpi::Protobuf::Unpack( + const google::protobuf::Message& msg) { + auto m = static_cast(&msg); + return frc::SwerveModuleState{ + units::meters_per_second_t{m->speed()}, + wpi::UnpackProtobuf(m->angle()), + }; +} + +void wpi::Protobuf::Pack( + google::protobuf::Message* msg, const frc::SwerveModuleState& value) { + auto m = static_cast(msg); + m->set_speed(value.speed.value()); + wpi::PackProtobuf(m->mutable_angle(), value.angle); +} diff --git a/wpimath/src/main/native/cpp/kinematics/struct/ChassisSpeedsStruct.cpp b/wpimath/src/main/native/cpp/kinematics/struct/ChassisSpeedsStruct.cpp new file mode 100644 index 00000000000..cb17a0c86d3 --- /dev/null +++ b/wpimath/src/main/native/cpp/kinematics/struct/ChassisSpeedsStruct.cpp @@ -0,0 +1,28 @@ +// 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. + +#include "frc/kinematics/struct/ChassisSpeedsStruct.h" + +namespace { +constexpr size_t kVxOff = 0; +constexpr size_t kVyOff = kVxOff + 8; +constexpr size_t kOmegaOff = kVyOff + 8; +} // namespace + +using StructType = wpi::Struct; + +frc::ChassisSpeeds StructType::Unpack(std::span data) { + return frc::ChassisSpeeds{ + units::meters_per_second_t{wpi::UnpackStruct(data)}, + units::meters_per_second_t{wpi::UnpackStruct(data)}, + units::radians_per_second_t{wpi::UnpackStruct(data)}, + }; +} + +void StructType::Pack(std::span data, + const frc::ChassisSpeeds& value) { + wpi::PackStruct(data, value.vx.value()); + wpi::PackStruct(data, value.vy.value()); + wpi::PackStruct(data, value.omega.value()); +} diff --git a/wpimath/src/main/native/cpp/kinematics/struct/DifferentialDriveKinematicsStruct.cpp b/wpimath/src/main/native/cpp/kinematics/struct/DifferentialDriveKinematicsStruct.cpp new file mode 100644 index 00000000000..9ce43860f14 --- /dev/null +++ b/wpimath/src/main/native/cpp/kinematics/struct/DifferentialDriveKinematicsStruct.cpp @@ -0,0 +1,23 @@ +// 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. + +#include "frc/kinematics/struct/DifferentialDriveKinematicsStruct.h" + +namespace { +constexpr size_t kTrackWidthOff = 0; +} // namespace + +using StructType = wpi::Struct; + +frc::DifferentialDriveKinematics StructType::Unpack( + std::span data) { + return frc::DifferentialDriveKinematics{ + units::meter_t{wpi::UnpackStruct(data)}, + }; +} + +void StructType::Pack(std::span data, + const frc::DifferentialDriveKinematics& value) { + wpi::PackStruct(data, value.trackWidth.value()); +} diff --git a/wpimath/src/main/native/cpp/kinematics/struct/DifferentialDriveWheelSpeedsStruct.cpp b/wpimath/src/main/native/cpp/kinematics/struct/DifferentialDriveWheelSpeedsStruct.cpp new file mode 100644 index 00000000000..35c157c83d3 --- /dev/null +++ b/wpimath/src/main/native/cpp/kinematics/struct/DifferentialDriveWheelSpeedsStruct.cpp @@ -0,0 +1,26 @@ +// 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. + +#include "frc/kinematics/struct/DifferentialDriveWheelSpeedsStruct.h" + +namespace { +constexpr size_t kLeftOff = 0; +constexpr size_t kRightOff = kLeftOff + 8; +} // namespace + +using StructType = wpi::Struct; + +frc::DifferentialDriveWheelSpeeds StructType::Unpack( + std::span data) { + return frc::DifferentialDriveWheelSpeeds{ + units::meters_per_second_t{wpi::UnpackStruct(data)}, + units::meters_per_second_t{wpi::UnpackStruct(data)}, + }; +} + +void StructType::Pack(std::span data, + const frc::DifferentialDriveWheelSpeeds& value) { + wpi::PackStruct(data, value.left.value()); + wpi::PackStruct(data, value.right.value()); +} diff --git a/wpimath/src/main/native/cpp/kinematics/struct/MecanumDriveKinematicsStruct.cpp b/wpimath/src/main/native/cpp/kinematics/struct/MecanumDriveKinematicsStruct.cpp new file mode 100644 index 00000000000..135618e5add --- /dev/null +++ b/wpimath/src/main/native/cpp/kinematics/struct/MecanumDriveKinematicsStruct.cpp @@ -0,0 +1,40 @@ +// 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. + +#include "frc/kinematics/struct/MecanumDriveKinematicsStruct.h" + +namespace { +constexpr size_t kFrontLeftOff = 0; +constexpr size_t kFrontRightOff = + kFrontLeftOff + wpi::Struct::kSize; +constexpr size_t kRearLeftOff = + kFrontRightOff + wpi::Struct::kSize; +constexpr size_t kRearRightOff = + kRearLeftOff + wpi::Struct::kSize; +} // namespace + +using StructType = wpi::Struct; + +frc::MecanumDriveKinematics StructType::Unpack( + std::span data) { + return frc::MecanumDriveKinematics{ + wpi::UnpackStruct(data), + wpi::UnpackStruct(data), + wpi::UnpackStruct(data), + wpi::UnpackStruct(data), + }; +} + +void StructType::Pack(std::span data, + const frc::MecanumDriveKinematics& value) { + wpi::PackStruct(data, value.GetFrontLeftWheel()); + wpi::PackStruct(data, value.GetFrontRightWheel()); + wpi::PackStruct(data, value.GetRearLeftWheel()); + wpi::PackStruct(data, value.GetRearRightWheel()); +} + +void StructType::ForEachNested( + std::invocable auto fn) { + wpi::ForEachStructSchema(fn); +} diff --git a/wpimath/src/main/native/cpp/kinematics/struct/MecanumDriveWheelPositionsStruct.cpp b/wpimath/src/main/native/cpp/kinematics/struct/MecanumDriveWheelPositionsStruct.cpp new file mode 100644 index 00000000000..d75df6e7dc4 --- /dev/null +++ b/wpimath/src/main/native/cpp/kinematics/struct/MecanumDriveWheelPositionsStruct.cpp @@ -0,0 +1,32 @@ +// 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. + +#include "frc/kinematics/struct/MecanumDriveWheelPositionsStruct.h" + +namespace { +constexpr size_t kFrontLeftOff = 0; +constexpr size_t kFrontRightOff = kFrontLeftOff + 8; +constexpr size_t kRearLeftOff = kFrontRightOff + 8; +constexpr size_t kRearRightOff = kRearLeftOff + 8; +} // namespace + +using StructType = wpi::Struct; + +frc::MecanumDriveWheelPositions StructType::Unpack( + std::span data) { + return frc::MecanumDriveWheelPositions{ + units::meter_t{wpi::UnpackStruct(data)}, + units::meter_t{wpi::UnpackStruct(data)}, + units::meter_t{wpi::UnpackStruct(data)}, + units::meter_t{wpi::UnpackStruct(data)}, + }; +} + +void StructType::Pack(std::span data, + const frc::MecanumDriveWheelPositions& value) { + wpi::PackStruct(data, value.frontLeft.value()); + wpi::PackStruct(data, value.frontRight.value()); + wpi::PackStruct(data, value.rearLeft.value()); + wpi::PackStruct(data, value.rearRight.value()); +} diff --git a/wpimath/src/main/native/cpp/kinematics/struct/MecanumDriveWheelSpeedsStruct.cpp b/wpimath/src/main/native/cpp/kinematics/struct/MecanumDriveWheelSpeedsStruct.cpp new file mode 100644 index 00000000000..ef173f4f648 --- /dev/null +++ b/wpimath/src/main/native/cpp/kinematics/struct/MecanumDriveWheelSpeedsStruct.cpp @@ -0,0 +1,35 @@ +// 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. + +#include "frc/kinematics/struct/MecanumDriveWheelSpeedsStruct.h" + +namespace { +constexpr size_t kFrontLeftOff = 0; +constexpr size_t kFrontRightOff = kFrontLeftOff + 8; +constexpr size_t kRearLeftOff = kFrontRightOff + 8; +constexpr size_t kRearRightOff = kRearLeftOff + 8; +} // namespace + +using StructType = wpi::Struct; + +frc::MecanumDriveWheelSpeeds StructType::Unpack( + std::span data) { + return frc::MecanumDriveWheelSpeeds{ + units::meters_per_second_t{ + wpi::UnpackStruct(data)}, + units::meters_per_second_t{ + wpi::UnpackStruct(data)}, + units::meters_per_second_t{wpi::UnpackStruct(data)}, + units::meters_per_second_t{ + wpi::UnpackStruct(data)}, + }; +} + +void StructType::Pack(std::span data, + const frc::MecanumDriveWheelSpeeds& value) { + wpi::PackStruct(data, value.frontLeft.value()); + wpi::PackStruct(data, value.frontRight.value()); + wpi::PackStruct(data, value.rearLeft.value()); + wpi::PackStruct(data, value.rearRight.value()); +} diff --git a/wpimath/src/main/native/cpp/kinematics/struct/SwerveModulePositionStruct.cpp b/wpimath/src/main/native/cpp/kinematics/struct/SwerveModulePositionStruct.cpp new file mode 100644 index 00000000000..56de1ed1694 --- /dev/null +++ b/wpimath/src/main/native/cpp/kinematics/struct/SwerveModulePositionStruct.cpp @@ -0,0 +1,31 @@ +// 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. + +#include "frc/kinematics/struct/SwerveModulePositionStruct.h" + +namespace { +constexpr size_t kDistanceOff = 0; +constexpr size_t kAngleOff = kDistanceOff + 8; +} // namespace + +using StructType = wpi::Struct; + +frc::SwerveModulePosition StructType::Unpack( + std::span data) { + return frc::SwerveModulePosition{ + units::meter_t{wpi::UnpackStruct(data)}, + wpi::UnpackStruct(data), + }; +} + +void StructType::Pack(std::span data, + const frc::SwerveModulePosition& value) { + wpi::PackStruct(data, value.distance.value()); + wpi::PackStruct(data, value.angle); +} + +void StructType::ForEachNested( + std::invocable auto fn) { + wpi::ForEachStructSchema(fn); +} diff --git a/wpimath/src/main/native/cpp/kinematics/struct/SwerveModuleStateStruct.cpp b/wpimath/src/main/native/cpp/kinematics/struct/SwerveModuleStateStruct.cpp new file mode 100644 index 00000000000..396b223f180 --- /dev/null +++ b/wpimath/src/main/native/cpp/kinematics/struct/SwerveModuleStateStruct.cpp @@ -0,0 +1,31 @@ +// 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. + +#include "frc/kinematics/struct/SwerveModuleStateStruct.h" + +namespace { +constexpr size_t kSpeedOff = 0; +constexpr size_t kAngleOff = kSpeedOff + 8; +} // namespace + +using StructType = wpi::Struct; + +frc::SwerveModuleState StructType::Unpack( + std::span data) { + return frc::SwerveModuleState{ + units::meters_per_second_t{wpi::UnpackStruct(data)}, + wpi::UnpackStruct(data), + }; +} + +void StructType::Pack(std::span data, + const frc::SwerveModuleState& value) { + wpi::PackStruct(data, value.speed.value()); + wpi::PackStruct(data, value.angle); +} + +void StructType::ForEachNested( + std::invocable auto fn) { + wpi::ForEachStructSchema(fn); +} diff --git a/wpimath/src/main/native/cpp/system/plant/LinearSystemId.cpp b/wpimath/src/main/native/cpp/system/plant/LinearSystemId.cpp index 3527092a972..11c65eada35 100644 --- a/wpimath/src/main/native/cpp/system/plant/LinearSystemId.cpp +++ b/wpimath/src/main/native/cpp/system/plant/LinearSystemId.cpp @@ -9,23 +9,24 @@ using namespace frc; LinearSystem<2, 1, 1> LinearSystemId::ElevatorSystem(DCMotor motor, units::kilogram_t mass, units::meter_t radius, - double G) { + double gearing) { if (mass <= 0_kg) { throw std::domain_error("mass must be greater than zero."); } if (radius <= 0_m) { throw std::domain_error("radius must be greater than zero."); } - if (G <= 0.0) { - throw std::domain_error("G must be greater than zero."); + if (gearing <= 0.0) { + throw std::domain_error("gearing must be greater than zero."); } Matrixd<2, 2> A{ {0.0, 1.0}, - {0.0, (-std::pow(G, 2) * motor.Kt / + {0.0, (-std::pow(gearing, 2) * motor.Kt / (motor.R * units::math::pow<2>(radius) * mass * motor.Kv)) .value()}}; - Matrixd<2, 1> B{0.0, (G * motor.Kt / (motor.R * radius * mass)).value()}; + Matrixd<2, 1> B{0.0, + (gearing * motor.Kt / (motor.R * radius * mass)).value()}; Matrixd<1, 2> C{1.0, 0.0}; Matrixd<1, 1> D{0.0}; @@ -33,18 +34,19 @@ LinearSystem<2, 1, 1> LinearSystemId::ElevatorSystem(DCMotor motor, } LinearSystem<2, 1, 1> LinearSystemId::SingleJointedArmSystem( - DCMotor motor, units::kilogram_square_meter_t J, double G) { + DCMotor motor, units::kilogram_square_meter_t J, double gearing) { if (J <= 0_kg_sq_m) { throw std::domain_error("J must be greater than zero."); } - if (G <= 0.0) { - throw std::domain_error("G must be greater than zero."); + if (gearing <= 0.0) { + throw std::domain_error("gearing must be greater than zero."); } Matrixd<2, 2> A{ {0.0, 1.0}, - {0.0, (-std::pow(G, 2) * motor.Kt / (motor.Kv * motor.R * J)).value()}}; - Matrixd<2, 1> B{0.0, (G * motor.Kt / (motor.R * J)).value()}; + {0.0, + (-std::pow(gearing, 2) * motor.Kt / (motor.Kv * motor.R * J)).value()}}; + Matrixd<2, 1> B{0.0, (gearing * motor.Kt / (motor.R * J)).value()}; Matrixd<1, 2> C{1.0, 0.0}; Matrixd<1, 1> D{0.0}; @@ -119,17 +121,17 @@ LinearSystem<2, 2, 2> LinearSystemId::IdentifyDrivetrainSystem( } LinearSystem<1, 1, 1> LinearSystemId::FlywheelSystem( - DCMotor motor, units::kilogram_square_meter_t J, double G) { + DCMotor motor, units::kilogram_square_meter_t J, double gearing) { if (J <= 0_kg_sq_m) { throw std::domain_error("J must be greater than zero."); } - if (G <= 0.0) { - throw std::domain_error("G must be greater than zero."); + if (gearing <= 0.0) { + throw std::domain_error("gearing must be greater than zero."); } Matrixd<1, 1> A{ - (-std::pow(G, 2) * motor.Kt / (motor.Kv * motor.R * J)).value()}; - Matrixd<1, 1> B{(G * motor.Kt / (motor.R * J)).value()}; + (-std::pow(gearing, 2) * motor.Kt / (motor.Kv * motor.R * J)).value()}; + Matrixd<1, 1> B{(gearing * motor.Kt / (motor.R * J)).value()}; Matrixd<1, 1> C{1.0}; Matrixd<1, 1> D{0.0}; @@ -137,18 +139,19 @@ LinearSystem<1, 1, 1> LinearSystemId::FlywheelSystem( } LinearSystem<2, 1, 2> LinearSystemId::DCMotorSystem( - DCMotor motor, units::kilogram_square_meter_t J, double G) { + DCMotor motor, units::kilogram_square_meter_t J, double gearing) { if (J <= 0_kg_sq_m) { throw std::domain_error("J must be greater than zero."); } - if (G <= 0.0) { - throw std::domain_error("G must be greater than zero."); + if (gearing <= 0.0) { + throw std::domain_error("gearing must be greater than zero."); } Matrixd<2, 2> A{ {0.0, 1.0}, - {0.0, (-std::pow(G, 2) * motor.Kt / (motor.Kv * motor.R * J)).value()}}; - Matrixd<2, 1> B{0.0, (G * motor.Kt / (motor.R * J)).value()}; + {0.0, + (-std::pow(gearing, 2) * motor.Kt / (motor.Kv * motor.R * J)).value()}}; + Matrixd<2, 1> B{0.0, (gearing * motor.Kt / (motor.R * J)).value()}; Matrixd<2, 2> C{{1.0, 0.0}, {0.0, 1.0}}; Matrixd<2, 1> D{0.0, 0.0}; @@ -157,7 +160,7 @@ LinearSystem<2, 1, 2> LinearSystemId::DCMotorSystem( LinearSystem<2, 2, 2> LinearSystemId::DrivetrainVelocitySystem( const DCMotor& motor, units::kilogram_t mass, units::meter_t r, - units::meter_t rb, units::kilogram_square_meter_t J, double G) { + units::meter_t rb, units::kilogram_square_meter_t J, double gearing) { if (mass <= 0_kg) { throw std::domain_error("mass must be greater than zero."); } @@ -170,13 +173,13 @@ LinearSystem<2, 2, 2> LinearSystemId::DrivetrainVelocitySystem( if (J <= 0_kg_sq_m) { throw std::domain_error("J must be greater than zero."); } - if (G <= 0.0) { - throw std::domain_error("G must be greater than zero."); + if (gearing <= 0.0) { + throw std::domain_error("gearing must be greater than zero."); } - auto C1 = -std::pow(G, 2) * motor.Kt / + auto C1 = -std::pow(gearing, 2) * motor.Kt / (motor.Kv * motor.R * units::math::pow<2>(r)); - auto C2 = G * motor.Kt / (motor.R * r); + auto C2 = gearing * motor.Kt / (motor.R * r); Matrixd<2, 2> A{{((1 / mass + units::math::pow<2>(rb) / J) * C1).value(), ((1 / mass - units::math::pow<2>(rb) / J) * C1).value()}, diff --git a/wpimath/src/main/native/cpp/system/plant/proto/DCMotorProto.cpp b/wpimath/src/main/native/cpp/system/plant/proto/DCMotorProto.cpp new file mode 100644 index 00000000000..83632702090 --- /dev/null +++ b/wpimath/src/main/native/cpp/system/plant/proto/DCMotorProto.cpp @@ -0,0 +1,35 @@ +// 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. + +#include "frc/system/plant/proto/DCMotorProto.h" + +#include "plant.pb.h" + +google::protobuf::Message* wpi::Protobuf::New( + google::protobuf::Arena* arena) { + return google::protobuf::Arena::CreateMessage( + arena); +} + +frc::DCMotor wpi::Protobuf::Unpack( + const google::protobuf::Message& msg) { + auto m = static_cast(&msg); + return frc::DCMotor{ + units::volt_t{m->nominal_voltage()}, + units::newton_meter_t{m->stall_torque()}, + units::ampere_t{m->stall_current()}, + units::ampere_t{m->free_current()}, + units::radians_per_second_t{m->free_speed()}, + }; +} + +void wpi::Protobuf::Pack(google::protobuf::Message* msg, + const frc::DCMotor& value) { + auto m = static_cast(msg); + m->set_nominal_voltage(value.nominalVoltage.value()); + m->set_stall_torque(value.stallTorque.value()); + m->set_stall_current(value.stallCurrent.value()); + m->set_free_current(value.freeCurrent.value()); + m->set_free_speed(value.freeSpeed.value()); +} diff --git a/wpimath/src/main/native/cpp/system/plant/struct/DCMotorStruct.cpp b/wpimath/src/main/native/cpp/system/plant/struct/DCMotorStruct.cpp new file mode 100644 index 00000000000..91438ca2c1f --- /dev/null +++ b/wpimath/src/main/native/cpp/system/plant/struct/DCMotorStruct.cpp @@ -0,0 +1,35 @@ +// 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. + +#include "frc/system/plant/struct/DCMotorStruct.h" + +namespace { +constexpr size_t kNominalVoltageOff = 0; +constexpr size_t kStallTorqueOff = kNominalVoltageOff + 8; +constexpr size_t kStallCurrentOff = kStallTorqueOff + 8; +constexpr size_t kFreeCurrentOff = kStallCurrentOff + 8; +constexpr size_t kFreeSpeedOff = kFreeCurrentOff + 8; +} // namespace + +using StructType = wpi::Struct; + +frc::DCMotor StructType::Unpack(std::span data) { + return frc::DCMotor{ + units::volt_t{wpi::UnpackStruct(data)}, + units::newton_meter_t{wpi::UnpackStruct(data)}, + units::ampere_t{wpi::UnpackStruct(data)}, + units::ampere_t{wpi::UnpackStruct(data)}, + units::radians_per_second_t{ + wpi::UnpackStruct(data)}, + }; +} + +void StructType::Pack(std::span data, + const frc::DCMotor& value) { + wpi::PackStruct(data, value.nominalVoltage.value()); + wpi::PackStruct(data, value.stallTorque.value()); + wpi::PackStruct(data, value.stallCurrent.value()); + wpi::PackStruct(data, value.freeCurrent.value()); + wpi::PackStruct(data, value.freeSpeed.value()); +} diff --git a/wpimath/src/main/native/cpp/trajectory/proto/TrajectoryProto.cpp b/wpimath/src/main/native/cpp/trajectory/proto/TrajectoryProto.cpp new file mode 100644 index 00000000000..d5bbe2b3b57 --- /dev/null +++ b/wpimath/src/main/native/cpp/trajectory/proto/TrajectoryProto.cpp @@ -0,0 +1,33 @@ +// 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. + +#include "frc/trajectory/proto/TrajectoryProto.h" + +#include "trajectory.pb.h" + +google::protobuf::Message* wpi::Protobuf::New( + google::protobuf::Arena* arena) { + return google::protobuf::Arena::CreateMessage( + arena); +} + +frc::Trajectory wpi::Protobuf::Unpack( + const google::protobuf::Message& msg) { + auto m = static_cast(&msg); + std::vector states; + states.reserve(m->states().size()); + for (const auto& protoState : m->states()) { + states.push_back(wpi::UnpackProtobuf(protoState)); + } + return frc::Trajectory{states}; +} + +void wpi::Protobuf::Pack(google::protobuf::Message* msg, + const frc::Trajectory& value) { + auto m = static_cast(msg); + m->mutable_states()->Reserve(value.States().size()); + for (const auto& state : value.States()) { + wpi::PackProtobuf(m->add_states(), state); + } +} diff --git a/wpimath/src/main/native/cpp/trajectory/proto/TrajectoryStateProto.cpp b/wpimath/src/main/native/cpp/trajectory/proto/TrajectoryStateProto.cpp new file mode 100644 index 00000000000..f2b80abe8ab --- /dev/null +++ b/wpimath/src/main/native/cpp/trajectory/proto/TrajectoryStateProto.cpp @@ -0,0 +1,35 @@ +// 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. + +#include "frc/trajectory/proto/TrajectoryStateProto.h" + +#include "trajectory.pb.h" + +google::protobuf::Message* wpi::Protobuf::New( + google::protobuf::Arena* arena) { + return google::protobuf::Arena::CreateMessage< + wpi::proto::ProtobufTrajectoryState>(arena); +} + +frc::Trajectory::State wpi::Protobuf::Unpack( + const google::protobuf::Message& msg) { + auto m = static_cast(&msg); + return frc::Trajectory::State{ + units::second_t{m->time()}, + units::meters_per_second_t{m->velocity()}, + units::meters_per_second_squared_t{m->acceleration()}, + wpi::UnpackProtobuf(m->pose()), + units::curvature_t{m->curvature()}, + }; +} + +void wpi::Protobuf::Pack( + google::protobuf::Message* msg, const frc::Trajectory::State& value) { + auto m = static_cast(msg); + m->set_time(value.t.value()); + m->set_velocity(value.velocity.value()); + m->set_acceleration(value.acceleration.value()); + wpi::PackProtobuf(m->mutable_pose(), value.pose); + m->set_curvature(value.curvature.value()); +} diff --git a/wpimath/src/main/native/include/frc/StateSpaceUtil.h b/wpimath/src/main/native/include/frc/StateSpaceUtil.h index 3aa2e753303..e2b638ca24b 100644 --- a/wpimath/src/main/native/include/frc/StateSpaceUtil.h +++ b/wpimath/src/main/native/include/frc/StateSpaceUtil.h @@ -12,87 +12,13 @@ #include #include +#include #include -#include #include "frc/EigenCore.h" #include "frc/geometry/Pose2d.h" namespace frc { -namespace detail { - -template -void CostMatrixImpl(Matrix& result, T elem, Ts... elems) { - if (elem == std::numeric_limits::infinity()) { - result(result.rows() - (sizeof...(Ts) + 1)) = 0.0; - } else { - result(result.rows() - (sizeof...(Ts) + 1)) = 1.0 / std::pow(elem, 2); - } - if constexpr (sizeof...(Ts) > 0) { - CostMatrixImpl(result, elems...); - } -} - -template -void CovMatrixImpl(Matrix& result, T elem, Ts... elems) { - result(result.rows() - (sizeof...(Ts) + 1)) = std::pow(elem, 2); - if constexpr (sizeof...(Ts) > 0) { - CovMatrixImpl(result, elems...); - } -} - -template -void WhiteNoiseVectorImpl(Matrix& result, T elem, Ts... elems) { - std::random_device rd; - std::mt19937 gen{rd()}; - std::normal_distribution<> distr{0.0, elem}; - - result(result.rows() - (sizeof...(Ts) + 1)) = distr(gen); - if constexpr (sizeof...(Ts) > 0) { - WhiteNoiseVectorImpl(result, elems...); - } -} - -template -bool IsStabilizableImpl(const Matrixd& A, - const Matrixd& B) { - Eigen::EigenSolver> es{A, false}; - - for (int i = 0; i < A.rows(); ++i) { - if (std::norm(es.eigenvalues()[i]) < 1) { - continue; - } - - if constexpr (States != Eigen::Dynamic && Inputs != Eigen::Dynamic) { - Eigen::Matrix, States, States + Inputs> E; - E << es.eigenvalues()[i] * Eigen::Matrix, States, - States>::Identity() - - A, - B; - - Eigen::ColPivHouseholderQR< - Eigen::Matrix, States, States + Inputs>> - qr{E}; - if (qr.rank() < States) { - return false; - } - } else { - Eigen::MatrixXcd E{A.rows(), A.rows() + B.cols()}; - E << es.eigenvalues()[i] * - Eigen::MatrixXcd::Identity(A.rows(), A.rows()) - - A, - B; - - Eigen::ColPivHouseholderQR qr{E}; - if (qr.rank() < A.rows()) { - return false; - } - } - } - return true; -} - -} // namespace detail /** * Creates a cost matrix from the given vector for use with LQR. @@ -110,7 +36,16 @@ bool IsStabilizableImpl(const Matrixd& A, template ... Ts> Matrixd MakeCostMatrix(Ts... tolerances) { Eigen::DiagonalMatrix result; - detail::CostMatrixImpl(result.diagonal(), tolerances...); + auto& diag = result.diagonal(); + wpi::for_each( + [&](int i, double tolerance) { + if (tolerance == std::numeric_limits::infinity()) { + diag(i) = 0.0; + } else { + diag(i) = 1.0 / std::pow(tolerance, 2); + } + }, + tolerances...); return result; } @@ -129,7 +64,9 @@ Matrixd MakeCostMatrix(Ts... tolerances) { template ... Ts> Matrixd MakeCovMatrix(Ts... stdDevs) { Eigen::DiagonalMatrix result; - detail::CovMatrixImpl(result.diagonal(), stdDevs...); + auto& diag = result.diagonal(); + wpi::for_each([&](int i, double stdDev) { diag(i) = std::pow(stdDev, 2); }, + stdDevs...); return result; } @@ -150,7 +87,7 @@ template Matrixd MakeCostMatrix(const std::array& costs) { Eigen::DiagonalMatrix result; auto& diag = result.diagonal(); - for (size_t i = 0; i < N; ++i) { + for (size_t i = 0; i < costs.size(); ++i) { if (costs[i] == std::numeric_limits::infinity()) { diag(i) = 0.0; } else { @@ -183,9 +120,23 @@ Matrixd MakeCovMatrix(const std::array& stdDevs) { } template ... Ts> -Matrixd MakeWhiteNoiseVector(Ts... stdDevs) { - Matrixd result; - detail::WhiteNoiseVectorImpl(result, stdDevs...); +Vectord MakeWhiteNoiseVector(Ts... stdDevs) { + std::random_device rd; + std::mt19937 gen{rd()}; + + Vectord result; + wpi::for_each( + [&](int i, double stdDev) { + // Passing a standard deviation of 0.0 to std::normal_distribution is + // undefined behavior + if (stdDev == 0.0) { + result(i) = 0.0; + } else { + std::normal_distribution distr{0.0, stdDev}; + result(i) = distr(gen); + } + }, + stdDevs...); return result; } @@ -203,7 +154,7 @@ Vectord MakeWhiteNoiseVector(const std::array& stdDevs) { std::mt19937 gen{rd()}; Vectord result; - for (int i = 0; i < N; ++i) { + for (size_t i = 0; i < stdDevs.size(); ++i) { // Passing a standard deviation of 0.0 to std::normal_distribution is // undefined behavior if (stdDevs[i] == 0.0) { @@ -251,9 +202,50 @@ Eigen::Vector4d PoseTo4dVector(const Pose2d& pose); template bool IsStabilizable(const Matrixd& A, const Matrixd& B) { - return detail::IsStabilizableImpl(A, B); + Eigen::EigenSolver> es{A, false}; + + for (int i = 0; i < A.rows(); ++i) { + if (std::norm(es.eigenvalues()[i]) < 1) { + continue; + } + + if constexpr (States != Eigen::Dynamic && Inputs != Eigen::Dynamic) { + Eigen::Matrix, States, States + Inputs> E; + E << es.eigenvalues()[i] * Eigen::Matrix, States, + States>::Identity() - + A, + B; + + Eigen::ColPivHouseholderQR< + Eigen::Matrix, States, States + Inputs>> + qr{E}; + if (qr.rank() < States) { + return false; + } + } else { + Eigen::MatrixXcd E{A.rows(), A.rows() + B.cols()}; + E << es.eigenvalues()[i] * + Eigen::MatrixXcd::Identity(A.rows(), A.rows()) - + A, + B; + + Eigen::ColPivHouseholderQR qr{E}; + if (qr.rank() < A.rows()) { + return false; + } + } + } + return true; } +extern template WPILIB_DLLEXPORT bool IsStabilizable<1, 1>( + const Matrixd<1, 1>& A, const Matrixd<1, 1>& B); +extern template WPILIB_DLLEXPORT bool IsStabilizable<2, 1>( + const Matrixd<2, 2>& A, const Matrixd<2, 1>& B); +extern template WPILIB_DLLEXPORT bool +IsStabilizable(const Eigen::MatrixXd& A, + const Eigen::MatrixXd& B); + /** * Returns true if (A, C) is a detectable pair. * @@ -269,28 +261,9 @@ bool IsStabilizable(const Matrixd& A, template bool IsDetectable(const Matrixd& A, const Matrixd& C) { - return detail::IsStabilizableImpl(A.transpose(), - C.transpose()); + return IsStabilizable(A.transpose(), C.transpose()); } -// Template specializations are used here to make common state-input pairs -// compile faster. -template <> -WPILIB_DLLEXPORT bool IsStabilizable<1, 1>(const Matrixd<1, 1>& A, - const Matrixd<1, 1>& B); - -// Template specializations are used here to make common state-input pairs -// compile faster. -template <> -WPILIB_DLLEXPORT bool IsStabilizable<2, 1>(const Matrixd<2, 2>& A, - const Matrixd<2, 1>& B); - -// Template specializations are used here to make common state-input pairs -// compile faster. -template <> -WPILIB_DLLEXPORT bool IsStabilizable( - const Eigen::MatrixXd& A, const Eigen::MatrixXd& B); - /** * Converts a Pose2d into a vector of [x, y, theta]. * diff --git a/wpimath/src/main/native/include/frc/controller/ArmFeedforward.h b/wpimath/src/main/native/include/frc/controller/ArmFeedforward.h index 5621803f228..39f4f90e593 100644 --- a/wpimath/src/main/native/include/frc/controller/ArmFeedforward.h +++ b/wpimath/src/main/native/include/frc/controller/ArmFeedforward.h @@ -169,3 +169,6 @@ class WPILIB_DLLEXPORT ArmFeedforward { units::unit_t kA{0}; }; } // namespace frc + +#include "frc/controller/proto/ArmFeedforwardProto.h" +#include "frc/controller/struct/ArmFeedforwardStruct.h" diff --git a/wpimath/src/main/native/include/frc/controller/DifferentialDriveWheelVoltages.h b/wpimath/src/main/native/include/frc/controller/DifferentialDriveWheelVoltages.h index 48f341e6d66..235a95877e8 100644 --- a/wpimath/src/main/native/include/frc/controller/DifferentialDriveWheelVoltages.h +++ b/wpimath/src/main/native/include/frc/controller/DifferentialDriveWheelVoltages.h @@ -17,3 +17,6 @@ struct DifferentialDriveWheelVoltages { }; } // namespace frc + +#include "frc/controller/proto/DifferentialDriveWheelVoltagesProto.h" +#include "frc/controller/struct/DifferentialDriveWheelVoltagesStruct.h" diff --git a/wpimath/src/main/native/include/frc/controller/ElevatorFeedforward.h b/wpimath/src/main/native/include/frc/controller/ElevatorFeedforward.h index 62a7bad8676..4d308f4ec7b 100644 --- a/wpimath/src/main/native/include/frc/controller/ElevatorFeedforward.h +++ b/wpimath/src/main/native/include/frc/controller/ElevatorFeedforward.h @@ -176,3 +176,6 @@ class ElevatorFeedforward { units::unit_t kA{0}; }; } // namespace frc + +#include "frc/controller/proto/ElevatorFeedforwardProto.h" +#include "frc/controller/struct/ElevatorFeedforwardStruct.h" diff --git a/wpimath/src/main/native/include/frc/controller/proto/ArmFeedforwardProto.h b/wpimath/src/main/native/include/frc/controller/proto/ArmFeedforwardProto.h new file mode 100644 index 00000000000..bc893aa8cb2 --- /dev/null +++ b/wpimath/src/main/native/include/frc/controller/proto/ArmFeedforwardProto.h @@ -0,0 +1,18 @@ +// 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 +#include + +#include "frc/controller/ArmFeedforward.h" + +template <> +struct WPILIB_DLLEXPORT wpi::Protobuf { + static google::protobuf::Message* New(google::protobuf::Arena* arena); + static frc::ArmFeedforward Unpack(const google::protobuf::Message& msg); + static void Pack(google::protobuf::Message* msg, + const frc::ArmFeedforward& value); +}; diff --git a/wpimath/src/main/native/include/frc/controller/proto/DifferentialDriveWheelVoltagesProto.h b/wpimath/src/main/native/include/frc/controller/proto/DifferentialDriveWheelVoltagesProto.h new file mode 100644 index 00000000000..486fd177d5b --- /dev/null +++ b/wpimath/src/main/native/include/frc/controller/proto/DifferentialDriveWheelVoltagesProto.h @@ -0,0 +1,19 @@ +// 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 +#include + +#include "frc/controller/DifferentialDriveWheelVoltages.h" + +template <> +struct WPILIB_DLLEXPORT wpi::Protobuf { + static google::protobuf::Message* New(google::protobuf::Arena* arena); + static frc::DifferentialDriveWheelVoltages Unpack( + const google::protobuf::Message& msg); + static void Pack(google::protobuf::Message* msg, + const frc::DifferentialDriveWheelVoltages& value); +}; diff --git a/wpimath/src/main/native/include/frc/controller/proto/ElevatorFeedforwardProto.h b/wpimath/src/main/native/include/frc/controller/proto/ElevatorFeedforwardProto.h new file mode 100644 index 00000000000..377f62aaa3b --- /dev/null +++ b/wpimath/src/main/native/include/frc/controller/proto/ElevatorFeedforwardProto.h @@ -0,0 +1,18 @@ +// 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 +#include + +#include "frc/controller/ElevatorFeedforward.h" + +template <> +struct WPILIB_DLLEXPORT wpi::Protobuf { + static google::protobuf::Message* New(google::protobuf::Arena* arena); + static frc::ElevatorFeedforward Unpack(const google::protobuf::Message& msg); + static void Pack(google::protobuf::Message* msg, + const frc::ElevatorFeedforward& value); +}; diff --git a/wpimath/src/main/native/include/frc/controller/struct/ArmFeedforwardStruct.h b/wpimath/src/main/native/include/frc/controller/struct/ArmFeedforwardStruct.h new file mode 100644 index 00000000000..3f427da9d87 --- /dev/null +++ b/wpimath/src/main/native/include/frc/controller/struct/ArmFeedforwardStruct.h @@ -0,0 +1,22 @@ +// 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 +#include + +#include "frc/controller/ArmFeedforward.h" + +template <> +struct WPILIB_DLLEXPORT wpi::Struct { + static constexpr std::string_view kTypeString = "struct:ArmFeedforward"; + static constexpr size_t kSize = 32; + static constexpr std::string_view kSchema = + "double ks;double kg;double kv;double ka"; + + static frc::ArmFeedforward Unpack(std::span data); + static void Pack(std::span data, + const frc::ArmFeedforward& value); +}; diff --git a/wpimath/src/main/native/include/frc/controller/struct/DifferentialDriveWheelVoltagesStruct.h b/wpimath/src/main/native/include/frc/controller/struct/DifferentialDriveWheelVoltagesStruct.h new file mode 100644 index 00000000000..dae20ed4893 --- /dev/null +++ b/wpimath/src/main/native/include/frc/controller/struct/DifferentialDriveWheelVoltagesStruct.h @@ -0,0 +1,23 @@ +// 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 +#include + +#include "frc/controller/DifferentialDriveWheelVoltages.h" + +template <> +struct WPILIB_DLLEXPORT wpi::Struct { + static constexpr std::string_view kTypeString = + "struct:DifferentialDriveWheelVoltages"; + static constexpr size_t kSize = 16; + static constexpr std::string_view kSchema = "double left;double right"; + + static frc::DifferentialDriveWheelVoltages Unpack( + std::span data); + static void Pack(std::span data, + const frc::DifferentialDriveWheelVoltages& value); +}; diff --git a/wpimath/src/main/native/include/frc/controller/struct/ElevatorFeedforwardStruct.h b/wpimath/src/main/native/include/frc/controller/struct/ElevatorFeedforwardStruct.h new file mode 100644 index 00000000000..89c03f8fea9 --- /dev/null +++ b/wpimath/src/main/native/include/frc/controller/struct/ElevatorFeedforwardStruct.h @@ -0,0 +1,22 @@ +// 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 +#include + +#include "frc/controller/ElevatorFeedforward.h" + +template <> +struct WPILIB_DLLEXPORT wpi::Struct { + static constexpr std::string_view kTypeString = "struct:ElevatorFeedforward"; + static constexpr size_t kSize = 32; + static constexpr std::string_view kSchema = + "double ks;double kg;double kv;double ka"; + + static frc::ElevatorFeedforward Unpack(std::span data); + static void Pack(std::span data, + const frc::ElevatorFeedforward& value); +}; diff --git a/wpimath/src/main/native/include/frc/geometry/Pose2d.h b/wpimath/src/main/native/include/frc/geometry/Pose2d.h index 970f7929614..65a146c3dd7 100644 --- a/wpimath/src/main/native/include/frc/geometry/Pose2d.h +++ b/wpimath/src/main/native/include/frc/geometry/Pose2d.h @@ -9,8 +9,6 @@ #include #include -#include -#include #include "frc/geometry/Rotation2d.h" #include "frc/geometry/Transform2d.h" @@ -215,38 +213,6 @@ void from_json(const wpi::json& json, Pose2d& pose); } // namespace frc -template <> -struct wpi::Struct { - static constexpr std::string_view kTypeString = "struct:Pose2d"; - static constexpr size_t kSize = wpi::Struct::kSize + - wpi::Struct::kSize; - static constexpr std::string_view kSchema = - "Translation2d translation;Rotation2d rotation"; - static frc::Pose2d Unpack(std::span data) { - return {wpi::UnpackStruct(data), - wpi::UnpackStruct(data)}; - } - static void Pack(std::span data, const frc::Pose2d& value) { - wpi::PackStruct<0>(data, value.Translation()); - wpi::PackStruct(data, value.Rotation()); - } - static void ForEachNested( - std::invocable auto fn) { - wpi::ForEachStructSchema(fn); - wpi::ForEachStructSchema(fn); - } - - private: - static constexpr size_t kRotationOff = wpi::Struct::kSize; -}; - -static_assert(wpi::HasNestedStruct); - -template <> -struct WPILIB_DLLEXPORT wpi::Protobuf { - static google::protobuf::Message* New(google::protobuf::Arena* arena); - static frc::Pose2d Unpack(const google::protobuf::Message& msg); - static void Pack(google::protobuf::Message* msg, const frc::Pose2d& value); -}; - +#include "frc/geometry/proto/Pose2dProto.h" +#include "frc/geometry/struct/Pose2dStruct.h" #include "frc/geometry/Pose2d.inc" diff --git a/wpimath/src/main/native/include/frc/geometry/Pose3d.h b/wpimath/src/main/native/include/frc/geometry/Pose3d.h index b5a0e031ba8..526dced399d 100644 --- a/wpimath/src/main/native/include/frc/geometry/Pose3d.h +++ b/wpimath/src/main/native/include/frc/geometry/Pose3d.h @@ -6,8 +6,6 @@ #include #include -#include -#include #include "frc/geometry/Pose2d.h" #include "frc/geometry/Rotation3d.h" @@ -217,36 +215,5 @@ void from_json(const wpi::json& json, Pose3d& pose); } // namespace frc -template <> -struct wpi::Struct { - static constexpr std::string_view kTypeString = "struct:Pose3d"; - static constexpr size_t kSize = wpi::Struct::kSize + - wpi::Struct::kSize; - static constexpr std::string_view kSchema = - "Translation3d translation;Rotation3d rotation"; - static frc::Pose3d Unpack(std::span data) { - return {wpi::UnpackStruct(data), - wpi::UnpackStruct(data)}; - } - static void Pack(std::span data, const frc::Pose3d& value) { - wpi::PackStruct<0>(data, value.Translation()); - wpi::PackStruct(data, value.Rotation()); - } - static void ForEachNested( - std::invocable auto fn) { - wpi::ForEachStructSchema(fn); - wpi::ForEachStructSchema(fn); - } - - private: - static constexpr size_t kRotationOff = wpi::Struct::kSize; -}; - -static_assert(wpi::HasNestedStruct); - -template <> -struct WPILIB_DLLEXPORT wpi::Protobuf { - static google::protobuf::Message* New(google::protobuf::Arena* arena); - static frc::Pose3d Unpack(const google::protobuf::Message& msg); - static void Pack(google::protobuf::Message* msg, const frc::Pose3d& value); -}; +#include "frc/geometry/proto/Pose3dProto.h" +#include "frc/geometry/struct/Pose3dStruct.h" diff --git a/wpimath/src/main/native/include/frc/geometry/Quaternion.h b/wpimath/src/main/native/include/frc/geometry/Quaternion.h index 63a3af2784d..a9526bc3791 100644 --- a/wpimath/src/main/native/include/frc/geometry/Quaternion.h +++ b/wpimath/src/main/native/include/frc/geometry/Quaternion.h @@ -7,8 +7,6 @@ #include #include #include -#include -#include namespace frc { @@ -190,31 +188,5 @@ void from_json(const wpi::json& json, Quaternion& quaternion); } // namespace frc -template <> -struct wpi::Struct { - static constexpr std::string_view kTypeString = "struct:Quaternion"; - static constexpr size_t kSize = 32; - static constexpr std::string_view kSchema = - "double w;double x;double y;double z"; - static frc::Quaternion Unpack(std::span data) { - return {wpi::UnpackStruct(data), - wpi::UnpackStruct(data), - wpi::UnpackStruct(data), - wpi::UnpackStruct(data)}; - } - static void Pack(std::span data, const frc::Quaternion& value) { - wpi::PackStruct<0>(data, value.W()); - wpi::PackStruct<8>(data, value.X()); - wpi::PackStruct<16>(data, value.Y()); - wpi::PackStruct<24>(data, value.Z()); - } -}; - -template <> -struct WPILIB_DLLEXPORT wpi::Protobuf { - static constexpr std::string_view kTypeString = "proto:Quaternion"; - static google::protobuf::Message* New(google::protobuf::Arena* arena); - static frc::Quaternion Unpack(const google::protobuf::Message& msg); - static void Pack(google::protobuf::Message* msg, - const frc::Quaternion& value); -}; +#include "frc/geometry/proto/QuaternionProto.h" +#include "frc/geometry/struct/QuaternionStruct.h" diff --git a/wpimath/src/main/native/include/frc/geometry/Rotation2d.h b/wpimath/src/main/native/include/frc/geometry/Rotation2d.h index 96041a49799..8f89556dd8d 100644 --- a/wpimath/src/main/native/include/frc/geometry/Rotation2d.h +++ b/wpimath/src/main/native/include/frc/geometry/Rotation2d.h @@ -6,8 +6,6 @@ #include #include -#include -#include #include "units/angle.h" @@ -178,25 +176,6 @@ void from_json(const wpi::json& json, Rotation2d& rotation); } // namespace frc -template <> -struct wpi::Struct { - static constexpr std::string_view kTypeString = "struct:Rotation2d"; - static constexpr size_t kSize = 8; - static constexpr std::string_view kSchema = "double value"; - static frc::Rotation2d Unpack(std::span data) { - return units::radian_t{wpi::UnpackStruct(data)}; - } - static void Pack(std::span data, const frc::Rotation2d& value) { - wpi::PackStruct(data, value.Radians().value()); - } -}; - -template <> -struct WPILIB_DLLEXPORT wpi::Protobuf { - static google::protobuf::Message* New(google::protobuf::Arena* arena); - static frc::Rotation2d Unpack(const google::protobuf::Message& msg); - static void Pack(google::protobuf::Message* msg, - const frc::Rotation2d& value); -}; - +#include "frc/geometry/proto/Rotation2dProto.h" +#include "frc/geometry/struct/Rotation2dStruct.h" #include "frc/geometry/Rotation2d.inc" diff --git a/wpimath/src/main/native/include/frc/geometry/Rotation3d.h b/wpimath/src/main/native/include/frc/geometry/Rotation3d.h index 6d04715316e..98abe007a87 100644 --- a/wpimath/src/main/native/include/frc/geometry/Rotation3d.h +++ b/wpimath/src/main/native/include/frc/geometry/Rotation3d.h @@ -7,8 +7,6 @@ #include #include #include -#include -#include #include "frc/geometry/Quaternion.h" #include "frc/geometry/Rotation2d.h" @@ -197,30 +195,5 @@ void from_json(const wpi::json& json, Rotation3d& rotation); } // namespace frc -template <> -struct wpi::Struct { - static constexpr std::string_view kTypeString = "struct:Rotation3d"; - static constexpr size_t kSize = wpi::Struct::kSize; - static constexpr std::string_view kSchema = "Quaternion q"; - static frc::Rotation3d Unpack(std::span data) { - return frc::Rotation3d{wpi::UnpackStruct(data)}; - } - static void Pack(std::span data, - const frc::Rotation3d& value) { - wpi::PackStruct<0>(data, value.GetQuaternion()); - } - static void ForEachNested( - std::invocable auto fn) { - wpi::ForEachStructSchema(fn); - } -}; - -static_assert(wpi::HasNestedStruct); - -template <> -struct WPILIB_DLLEXPORT wpi::Protobuf { - static google::protobuf::Message* New(google::protobuf::Arena* arena); - static frc::Rotation3d Unpack(const google::protobuf::Message& msg); - static void Pack(google::protobuf::Message* msg, - const frc::Rotation3d& value); -}; +#include "frc/geometry/proto/Rotation3dProto.h" +#include "frc/geometry/struct/Rotation3dStruct.h" diff --git a/wpimath/src/main/native/include/frc/geometry/Transform2d.h b/wpimath/src/main/native/include/frc/geometry/Transform2d.h index 593dce03ab5..dccc0e1eeec 100644 --- a/wpimath/src/main/native/include/frc/geometry/Transform2d.h +++ b/wpimath/src/main/native/include/frc/geometry/Transform2d.h @@ -5,8 +5,6 @@ #pragma once #include -#include -#include #include "frc/geometry/Translation2d.h" @@ -126,40 +124,6 @@ class WPILIB_DLLEXPORT Transform2d { }; } // namespace frc -template <> -struct wpi::Struct { - static constexpr std::string_view kTypeString = "struct:Transform2d"; - static constexpr size_t kSize = wpi::Struct::kSize + - wpi::Struct::kSize; - static constexpr std::string_view kSchema = - "Translation2d translation;Rotation2d rotation"; - static frc::Transform2d Unpack(std::span data) { - return {wpi::UnpackStruct(data), - wpi::UnpackStruct(data)}; - } - static void Pack(std::span data, - const frc::Transform2d& value) { - wpi::PackStruct<0>(data, value.Translation()); - wpi::PackStruct(data, value.Rotation()); - } - static void ForEachNested( - std::invocable auto fn) { - wpi::ForEachStructSchema(fn); - wpi::ForEachStructSchema(fn); - } - - private: - static constexpr size_t kRotationOff = wpi::Struct::kSize; -}; - -static_assert(wpi::HasNestedStruct); - -template <> -struct WPILIB_DLLEXPORT wpi::Protobuf { - static google::protobuf::Message* New(google::protobuf::Arena* arena); - static frc::Transform2d Unpack(const google::protobuf::Message& msg); - static void Pack(google::protobuf::Message* msg, - const frc::Transform2d& value); -}; - +#include "frc/geometry/proto/Transform2dProto.h" +#include "frc/geometry/struct/Transform2dStruct.h" #include "frc/geometry/Transform2d.inc" diff --git a/wpimath/src/main/native/include/frc/geometry/Transform3d.h b/wpimath/src/main/native/include/frc/geometry/Transform3d.h index d5af97d4731..94ecfe37eda 100644 --- a/wpimath/src/main/native/include/frc/geometry/Transform3d.h +++ b/wpimath/src/main/native/include/frc/geometry/Transform3d.h @@ -5,8 +5,6 @@ #pragma once #include -#include -#include #include "frc/geometry/Translation3d.h" @@ -132,38 +130,5 @@ class WPILIB_DLLEXPORT Transform3d { }; } // namespace frc -template <> -struct wpi::Struct { - static constexpr std::string_view kTypeString = "struct:Transform3d"; - static constexpr size_t kSize = wpi::Struct::kSize + - wpi::Struct::kSize; - static constexpr std::string_view kSchema = - "Translation3d translation;Rotation3d rotation"; - static frc::Transform3d Unpack(std::span data) { - return {wpi::UnpackStruct(data), - wpi::UnpackStruct(data)}; - } - static void Pack(std::span data, - const frc::Transform3d& value) { - wpi::PackStruct<0>(data, value.Translation()); - wpi::PackStruct(data, value.Rotation()); - } - static void ForEachNested( - std::invocable auto fn) { - wpi::ForEachStructSchema(fn); - wpi::ForEachStructSchema(fn); - } - - private: - static constexpr size_t kRotationOff = wpi::Struct::kSize; -}; - -static_assert(wpi::HasNestedStruct); - -template <> -struct WPILIB_DLLEXPORT wpi::Protobuf { - static google::protobuf::Message* New(google::protobuf::Arena* arena); - static frc::Transform3d Unpack(const google::protobuf::Message& msg); - static void Pack(google::protobuf::Message* msg, - const frc::Transform3d& value); -}; +#include "frc/geometry/proto/Transform3dProto.h" +#include "frc/geometry/struct/Transform3dStruct.h" diff --git a/wpimath/src/main/native/include/frc/geometry/Translation2d.h b/wpimath/src/main/native/include/frc/geometry/Translation2d.h index 1568586163f..a99492dcebd 100644 --- a/wpimath/src/main/native/include/frc/geometry/Translation2d.h +++ b/wpimath/src/main/native/include/frc/geometry/Translation2d.h @@ -9,8 +9,6 @@ #include #include -#include -#include #include "frc/geometry/Rotation2d.h" #include "units/length.h" @@ -200,28 +198,6 @@ void from_json(const wpi::json& json, Translation2d& state); } // namespace frc -template <> -struct wpi::Struct { - static constexpr std::string_view kTypeString = "struct:Translation2d"; - static constexpr size_t kSize = 16; - static constexpr std::string_view kSchema = "double x;double y"; - static frc::Translation2d Unpack(std::span data) { - return {units::meter_t{wpi::UnpackStruct(data)}, - units::meter_t{wpi::UnpackStruct(data)}}; - } - static void Pack(std::span data, - const frc::Translation2d& value) { - wpi::PackStruct<0>(data, value.X().value()); - wpi::PackStruct<8>(data, value.Y().value()); - } -}; - -template <> -struct WPILIB_DLLEXPORT wpi::Protobuf { - static google::protobuf::Message* New(google::protobuf::Arena* arena); - static frc::Translation2d Unpack(const google::protobuf::Message& msg); - static void Pack(google::protobuf::Message* msg, - const frc::Translation2d& value); -}; - +#include "frc/geometry/proto/Translation2dProto.h" +#include "frc/geometry/struct/Translation2dStruct.h" #include "frc/geometry/Translation2d.inc" diff --git a/wpimath/src/main/native/include/frc/geometry/Translation3d.h b/wpimath/src/main/native/include/frc/geometry/Translation3d.h index 47ba1f6bad5..b0b3359d96e 100644 --- a/wpimath/src/main/native/include/frc/geometry/Translation3d.h +++ b/wpimath/src/main/native/include/frc/geometry/Translation3d.h @@ -6,8 +6,6 @@ #include #include -#include -#include #include "frc/geometry/Rotation3d.h" #include "frc/geometry/Translation2d.h" @@ -185,30 +183,6 @@ void from_json(const wpi::json& json, Translation3d& state); } // namespace frc -template <> -struct wpi::Struct { - static constexpr std::string_view kTypeString = "struct:Translation3d"; - static constexpr size_t kSize = 24; - static constexpr std::string_view kSchema = "double x;double y;double z"; - static frc::Translation3d Unpack(std::span data) { - return {units::meter_t{wpi::UnpackStruct(data)}, - units::meter_t{wpi::UnpackStruct(data)}, - units::meter_t{wpi::UnpackStruct(data)}}; - } - static void Pack(std::span data, - const frc::Translation3d& value) { - wpi::PackStruct<0>(data, value.X().value()); - wpi::PackStruct<8>(data, value.Y().value()); - wpi::PackStruct<16>(data, value.Z().value()); - } -}; - -template <> -struct WPILIB_DLLEXPORT wpi::Protobuf { - static google::protobuf::Message* New(google::protobuf::Arena* arena); - static frc::Translation3d Unpack(const google::protobuf::Message& msg); - static void Pack(google::protobuf::Message* msg, - const frc::Translation3d& value); -}; - +#include "frc/geometry/proto/Translation3dProto.h" +#include "frc/geometry/struct/Translation3dStruct.h" #include "frc/geometry/Translation3d.inc" diff --git a/wpimath/src/main/native/include/frc/geometry/Twist2d.h b/wpimath/src/main/native/include/frc/geometry/Twist2d.h index 257f1b65e78..d13cc4ec80b 100644 --- a/wpimath/src/main/native/include/frc/geometry/Twist2d.h +++ b/wpimath/src/main/native/include/frc/geometry/Twist2d.h @@ -5,8 +5,6 @@ #pragma once #include -#include -#include #include "units/angle.h" #include "units/length.h" @@ -60,27 +58,5 @@ struct WPILIB_DLLEXPORT Twist2d { }; } // namespace frc -template <> -struct wpi::Struct { - static constexpr std::string_view kTypeString = "struct:Twist2d"; - static constexpr size_t kSize = 24; - static constexpr std::string_view kSchema = - "double dx;double dy;double dtheta"; - static frc::Twist2d Unpack(std::span data) { - return {units::meter_t{wpi::UnpackStruct(data)}, - units::meter_t{wpi::UnpackStruct(data)}, - units::radian_t{wpi::UnpackStruct(data)}}; - } - static void Pack(std::span data, const frc::Twist2d& value) { - wpi::PackStruct<0>(data, value.dx.value()); - wpi::PackStruct<8>(data, value.dy.value()); - wpi::PackStruct<16>(data, value.dtheta.value()); - } -}; - -template <> -struct WPILIB_DLLEXPORT wpi::Protobuf { - static google::protobuf::Message* New(google::protobuf::Arena* arena); - static frc::Twist2d Unpack(const google::protobuf::Message& msg); - static void Pack(google::protobuf::Message* msg, const frc::Twist2d& value); -}; +#include "frc/geometry/proto/Twist2dProto.h" +#include "frc/geometry/struct/Twist2dStruct.h" diff --git a/wpimath/src/main/native/include/frc/geometry/Twist3d.h b/wpimath/src/main/native/include/frc/geometry/Twist3d.h index 4d902dfcbba..3262367dc0f 100644 --- a/wpimath/src/main/native/include/frc/geometry/Twist3d.h +++ b/wpimath/src/main/native/include/frc/geometry/Twist3d.h @@ -5,8 +5,6 @@ #pragma once #include -#include -#include #include "frc/geometry/Rotation3d.h" #include "units/angle.h" @@ -80,34 +78,5 @@ struct WPILIB_DLLEXPORT Twist3d { }; } // namespace frc -template <> -struct wpi::Struct { - static constexpr std::string_view kTypeString = "struct:Twist3d"; - static constexpr size_t kSize = 48; - static constexpr std::string_view kSchema = - "double dx;double dy;double dz;double rx;double ry;double rz"; - static frc::Twist3d Unpack(std::span data) { - return {units::meter_t{wpi::UnpackStruct(data)}, - units::meter_t{wpi::UnpackStruct(data)}, - units::meter_t{wpi::UnpackStruct(data)}, - units::radian_t{wpi::UnpackStruct(data)}, - units::radian_t{wpi::UnpackStruct(data)}, - units::radian_t{wpi::UnpackStruct(data)}}; - } - static void Pack(std::span data, const frc::Twist3d& value) { - wpi::PackStruct<0>(data, value.dx.value()); - wpi::PackStruct<8>(data, value.dy.value()); - wpi::PackStruct<16>(data, value.dz.value()); - wpi::PackStruct<24>(data, value.rx.value()); - wpi::PackStruct<32>(data, value.ry.value()); - wpi::PackStruct<40>(data, value.rz.value()); - } -}; - -template <> -struct WPILIB_DLLEXPORT wpi::Protobuf { - static constexpr std::string_view kTypeString = "proto:Twist3d"; - static google::protobuf::Message* New(google::protobuf::Arena* arena); - static frc::Twist3d Unpack(const google::protobuf::Message& msg); - static void Pack(google::protobuf::Message* msg, const frc::Twist3d& value); -}; +#include "frc/geometry/proto/Twist3dProto.h" +#include "frc/geometry/struct/Twist3dStruct.h" diff --git a/wpimath/src/main/native/include/frc/geometry/proto/Pose2dProto.h b/wpimath/src/main/native/include/frc/geometry/proto/Pose2dProto.h new file mode 100644 index 00000000000..9934b4ae83d --- /dev/null +++ b/wpimath/src/main/native/include/frc/geometry/proto/Pose2dProto.h @@ -0,0 +1,17 @@ +// 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 +#include + +#include "frc/geometry/Pose2d.h" + +template <> +struct WPILIB_DLLEXPORT wpi::Protobuf { + static google::protobuf::Message* New(google::protobuf::Arena* arena); + static frc::Pose2d Unpack(const google::protobuf::Message& msg); + static void Pack(google::protobuf::Message* msg, const frc::Pose2d& value); +}; diff --git a/wpimath/src/main/native/include/frc/geometry/proto/Pose3dProto.h b/wpimath/src/main/native/include/frc/geometry/proto/Pose3dProto.h new file mode 100644 index 00000000000..9941568d258 --- /dev/null +++ b/wpimath/src/main/native/include/frc/geometry/proto/Pose3dProto.h @@ -0,0 +1,17 @@ +// 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 +#include + +#include "frc/geometry/Pose3d.h" + +template <> +struct WPILIB_DLLEXPORT wpi::Protobuf { + static google::protobuf::Message* New(google::protobuf::Arena* arena); + static frc::Pose3d Unpack(const google::protobuf::Message& msg); + static void Pack(google::protobuf::Message* msg, const frc::Pose3d& value); +}; diff --git a/wpimath/src/main/native/include/frc/geometry/proto/QuaternionProto.h b/wpimath/src/main/native/include/frc/geometry/proto/QuaternionProto.h new file mode 100644 index 00000000000..77f371d67e7 --- /dev/null +++ b/wpimath/src/main/native/include/frc/geometry/proto/QuaternionProto.h @@ -0,0 +1,18 @@ +// 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 +#include + +#include "frc/geometry/Quaternion.h" + +template <> +struct WPILIB_DLLEXPORT wpi::Protobuf { + static google::protobuf::Message* New(google::protobuf::Arena* arena); + static frc::Quaternion Unpack(const google::protobuf::Message& msg); + static void Pack(google::protobuf::Message* msg, + const frc::Quaternion& value); +}; diff --git a/wpimath/src/main/native/include/frc/geometry/proto/Rotation2dProto.h b/wpimath/src/main/native/include/frc/geometry/proto/Rotation2dProto.h new file mode 100644 index 00000000000..33c348e6fc3 --- /dev/null +++ b/wpimath/src/main/native/include/frc/geometry/proto/Rotation2dProto.h @@ -0,0 +1,18 @@ +// 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 +#include + +#include "frc/geometry/Rotation2d.h" + +template <> +struct WPILIB_DLLEXPORT wpi::Protobuf { + static google::protobuf::Message* New(google::protobuf::Arena* arena); + static frc::Rotation2d Unpack(const google::protobuf::Message& msg); + static void Pack(google::protobuf::Message* msg, + const frc::Rotation2d& value); +}; diff --git a/wpimath/src/main/native/include/frc/geometry/proto/Rotation3dProto.h b/wpimath/src/main/native/include/frc/geometry/proto/Rotation3dProto.h new file mode 100644 index 00000000000..f6f5662357a --- /dev/null +++ b/wpimath/src/main/native/include/frc/geometry/proto/Rotation3dProto.h @@ -0,0 +1,18 @@ +// 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 +#include + +#include "frc/geometry/Rotation3d.h" + +template <> +struct WPILIB_DLLEXPORT wpi::Protobuf { + static google::protobuf::Message* New(google::protobuf::Arena* arena); + static frc::Rotation3d Unpack(const google::protobuf::Message& msg); + static void Pack(google::protobuf::Message* msg, + const frc::Rotation3d& value); +}; diff --git a/wpimath/src/main/native/include/frc/geometry/proto/Transform2dProto.h b/wpimath/src/main/native/include/frc/geometry/proto/Transform2dProto.h new file mode 100644 index 00000000000..5ff19bd2268 --- /dev/null +++ b/wpimath/src/main/native/include/frc/geometry/proto/Transform2dProto.h @@ -0,0 +1,18 @@ +// 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 +#include + +#include "frc/geometry/Transform2d.h" + +template <> +struct WPILIB_DLLEXPORT wpi::Protobuf { + static google::protobuf::Message* New(google::protobuf::Arena* arena); + static frc::Transform2d Unpack(const google::protobuf::Message& msg); + static void Pack(google::protobuf::Message* msg, + const frc::Transform2d& value); +}; diff --git a/wpimath/src/main/native/include/frc/geometry/proto/Transform3dProto.h b/wpimath/src/main/native/include/frc/geometry/proto/Transform3dProto.h new file mode 100644 index 00000000000..d27fb9a0b86 --- /dev/null +++ b/wpimath/src/main/native/include/frc/geometry/proto/Transform3dProto.h @@ -0,0 +1,18 @@ +// 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 +#include + +#include "frc/geometry/Transform3d.h" + +template <> +struct WPILIB_DLLEXPORT wpi::Protobuf { + static google::protobuf::Message* New(google::protobuf::Arena* arena); + static frc::Transform3d Unpack(const google::protobuf::Message& msg); + static void Pack(google::protobuf::Message* msg, + const frc::Transform3d& value); +}; diff --git a/wpimath/src/main/native/include/frc/geometry/proto/Translation2dProto.h b/wpimath/src/main/native/include/frc/geometry/proto/Translation2dProto.h new file mode 100644 index 00000000000..6516fc8d88a --- /dev/null +++ b/wpimath/src/main/native/include/frc/geometry/proto/Translation2dProto.h @@ -0,0 +1,18 @@ +// 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 +#include + +#include "frc/geometry/Translation2d.h" + +template <> +struct WPILIB_DLLEXPORT wpi::Protobuf { + static google::protobuf::Message* New(google::protobuf::Arena* arena); + static frc::Translation2d Unpack(const google::protobuf::Message& msg); + static void Pack(google::protobuf::Message* msg, + const frc::Translation2d& value); +}; diff --git a/wpimath/src/main/native/include/frc/geometry/proto/Translation3dProto.h b/wpimath/src/main/native/include/frc/geometry/proto/Translation3dProto.h new file mode 100644 index 00000000000..2d317fc5afb --- /dev/null +++ b/wpimath/src/main/native/include/frc/geometry/proto/Translation3dProto.h @@ -0,0 +1,18 @@ +// 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 +#include + +#include "frc/geometry/Translation3d.h" + +template <> +struct WPILIB_DLLEXPORT wpi::Protobuf { + static google::protobuf::Message* New(google::protobuf::Arena* arena); + static frc::Translation3d Unpack(const google::protobuf::Message& msg); + static void Pack(google::protobuf::Message* msg, + const frc::Translation3d& value); +}; diff --git a/wpimath/src/main/native/include/frc/geometry/proto/Twist2dProto.h b/wpimath/src/main/native/include/frc/geometry/proto/Twist2dProto.h new file mode 100644 index 00000000000..328798e14cc --- /dev/null +++ b/wpimath/src/main/native/include/frc/geometry/proto/Twist2dProto.h @@ -0,0 +1,17 @@ +// 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 +#include + +#include "frc/geometry/Twist2d.h" + +template <> +struct WPILIB_DLLEXPORT wpi::Protobuf { + static google::protobuf::Message* New(google::protobuf::Arena* arena); + static frc::Twist2d Unpack(const google::protobuf::Message& msg); + static void Pack(google::protobuf::Message* msg, const frc::Twist2d& value); +}; diff --git a/wpimath/src/main/native/include/frc/geometry/proto/Twist3dProto.h b/wpimath/src/main/native/include/frc/geometry/proto/Twist3dProto.h new file mode 100644 index 00000000000..24f496935ed --- /dev/null +++ b/wpimath/src/main/native/include/frc/geometry/proto/Twist3dProto.h @@ -0,0 +1,17 @@ +// 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 +#include + +#include "frc/geometry/Twist3d.h" + +template <> +struct WPILIB_DLLEXPORT wpi::Protobuf { + static google::protobuf::Message* New(google::protobuf::Arena* arena); + static frc::Twist3d Unpack(const google::protobuf::Message& msg); + static void Pack(google::protobuf::Message* msg, const frc::Twist3d& value); +}; diff --git a/wpimath/src/main/native/include/frc/geometry/struct/Pose2dStruct.h b/wpimath/src/main/native/include/frc/geometry/struct/Pose2dStruct.h new file mode 100644 index 00000000000..ee517fb4919 --- /dev/null +++ b/wpimath/src/main/native/include/frc/geometry/struct/Pose2dStruct.h @@ -0,0 +1,26 @@ +// 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 +#include + +#include "frc/geometry/Pose2d.h" + +template <> +struct WPILIB_DLLEXPORT wpi::Struct { + static constexpr std::string_view kTypeString = "struct:Pose2d"; + static constexpr size_t kSize = wpi::Struct::kSize + + wpi::Struct::kSize; + static constexpr std::string_view kSchema = + "Translation2d translation;Rotation2d rotation"; + + static frc::Pose2d Unpack(std::span data); + static void Pack(std::span data, const frc::Pose2d& value); + static void ForEachNested( + std::invocable auto fn); +}; + +static_assert(wpi::HasNestedStruct); diff --git a/wpimath/src/main/native/include/frc/geometry/struct/Pose3dStruct.h b/wpimath/src/main/native/include/frc/geometry/struct/Pose3dStruct.h new file mode 100644 index 00000000000..2f899491a79 --- /dev/null +++ b/wpimath/src/main/native/include/frc/geometry/struct/Pose3dStruct.h @@ -0,0 +1,26 @@ +// 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 +#include + +#include "frc/geometry/Pose3d.h" + +template <> +struct WPILIB_DLLEXPORT wpi::Struct { + static constexpr std::string_view kTypeString = "struct:Pose3d"; + static constexpr size_t kSize = wpi::Struct::kSize + + wpi::Struct::kSize; + static constexpr std::string_view kSchema = + "Translation3d translation;Rotation3d rotation"; + + static frc::Pose3d Unpack(std::span data); + static void Pack(std::span data, const frc::Pose3d& value); + static void ForEachNested( + std::invocable auto fn); +}; + +static_assert(wpi::HasNestedStruct); diff --git a/wpimath/src/main/native/include/frc/geometry/struct/QuaternionStruct.h b/wpimath/src/main/native/include/frc/geometry/struct/QuaternionStruct.h new file mode 100644 index 00000000000..35acd4ba35c --- /dev/null +++ b/wpimath/src/main/native/include/frc/geometry/struct/QuaternionStruct.h @@ -0,0 +1,22 @@ +// 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 +#include + +#include "frc/geometry/Quaternion.h" + +template <> +struct WPILIB_DLLEXPORT wpi::Struct { + static constexpr std::string_view kTypeString = "struct:Quaternion"; + static constexpr size_t kSize = 32; + static constexpr std::string_view kSchema = + "double w;double x;double y;double z"; + + static frc::Quaternion Unpack(std::span data); + static void Pack(std::span data, + const frc::Quaternion& value); +}; diff --git a/wpimath/src/main/native/include/frc/geometry/struct/Rotation2dStruct.h b/wpimath/src/main/native/include/frc/geometry/struct/Rotation2dStruct.h new file mode 100644 index 00000000000..ce4f7a6f025 --- /dev/null +++ b/wpimath/src/main/native/include/frc/geometry/struct/Rotation2dStruct.h @@ -0,0 +1,21 @@ +// 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 +#include + +#include "frc/geometry/Rotation2d.h" + +template <> +struct WPILIB_DLLEXPORT wpi::Struct { + static constexpr std::string_view kTypeString = "struct:Rotation2d"; + static constexpr size_t kSize = 8; + static constexpr std::string_view kSchema = "double value"; + + static frc::Rotation2d Unpack(std::span data); + static void Pack(std::span data, + const frc::Rotation2d& value); +}; diff --git a/wpimath/src/main/native/include/frc/geometry/struct/Rotation3dStruct.h b/wpimath/src/main/native/include/frc/geometry/struct/Rotation3dStruct.h new file mode 100644 index 00000000000..9d73f72d405 --- /dev/null +++ b/wpimath/src/main/native/include/frc/geometry/struct/Rotation3dStruct.h @@ -0,0 +1,25 @@ +// 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 +#include + +#include "frc/geometry/Rotation3d.h" + +template <> +struct WPILIB_DLLEXPORT wpi::Struct { + static constexpr std::string_view kTypeString = "struct:Rotation3d"; + static constexpr size_t kSize = wpi::Struct::kSize; + static constexpr std::string_view kSchema = "Quaternion q"; + + static frc::Rotation3d Unpack(std::span data); + static void Pack(std::span data, + const frc::Rotation3d& value); + static void ForEachNested( + std::invocable auto fn); +}; + +static_assert(wpi::HasNestedStruct); diff --git a/wpimath/src/main/native/include/frc/geometry/struct/Transform2dStruct.h b/wpimath/src/main/native/include/frc/geometry/struct/Transform2dStruct.h new file mode 100644 index 00000000000..55f0ef86043 --- /dev/null +++ b/wpimath/src/main/native/include/frc/geometry/struct/Transform2dStruct.h @@ -0,0 +1,27 @@ +// 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 +#include + +#include "frc/geometry/Transform2d.h" + +template <> +struct WPILIB_DLLEXPORT wpi::Struct { + static constexpr std::string_view kTypeString = "struct:Transform2d"; + static constexpr size_t kSize = wpi::Struct::kSize + + wpi::Struct::kSize; + static constexpr std::string_view kSchema = + "Translation2d translation;Rotation2d rotation"; + + static frc::Transform2d Unpack(std::span data); + static void Pack(std::span data, + const frc::Transform2d& value); + static void ForEachNested( + std::invocable auto fn); +}; + +static_assert(wpi::HasNestedStruct); diff --git a/wpimath/src/main/native/include/frc/geometry/struct/Transform3dStruct.h b/wpimath/src/main/native/include/frc/geometry/struct/Transform3dStruct.h new file mode 100644 index 00000000000..4fb592016cf --- /dev/null +++ b/wpimath/src/main/native/include/frc/geometry/struct/Transform3dStruct.h @@ -0,0 +1,27 @@ +// 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 +#include + +#include "frc/geometry/Transform3d.h" + +template <> +struct WPILIB_DLLEXPORT wpi::Struct { + static constexpr std::string_view kTypeString = "struct:Transform3d"; + static constexpr size_t kSize = wpi::Struct::kSize + + wpi::Struct::kSize; + static constexpr std::string_view kSchema = + "Translation3d translation;Rotation3d rotation"; + + static frc::Transform3d Unpack(std::span data); + static void Pack(std::span data, + const frc::Transform3d& value); + static void ForEachNested( + std::invocable auto fn); +}; + +static_assert(wpi::HasNestedStruct); diff --git a/wpimath/src/main/native/include/frc/geometry/struct/Translation2dStruct.h b/wpimath/src/main/native/include/frc/geometry/struct/Translation2dStruct.h new file mode 100644 index 00000000000..1f6ea397726 --- /dev/null +++ b/wpimath/src/main/native/include/frc/geometry/struct/Translation2dStruct.h @@ -0,0 +1,21 @@ +// 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 +#include + +#include "frc/geometry/Translation2d.h" + +template <> +struct WPILIB_DLLEXPORT wpi::Struct { + static constexpr std::string_view kTypeString = "struct:Translation2d"; + static constexpr size_t kSize = 16; + static constexpr std::string_view kSchema = "double x;double y"; + + static frc::Translation2d Unpack(std::span data); + static void Pack(std::span data, + const frc::Translation2d& value); +}; diff --git a/wpimath/src/main/native/include/frc/geometry/struct/Translation3dStruct.h b/wpimath/src/main/native/include/frc/geometry/struct/Translation3dStruct.h new file mode 100644 index 00000000000..34604c6e1f5 --- /dev/null +++ b/wpimath/src/main/native/include/frc/geometry/struct/Translation3dStruct.h @@ -0,0 +1,21 @@ +// 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 +#include + +#include "frc/geometry/Translation3d.h" + +template <> +struct WPILIB_DLLEXPORT wpi::Struct { + static constexpr std::string_view kTypeString = "struct:Translation3d"; + static constexpr size_t kSize = 24; + static constexpr std::string_view kSchema = "double x;double y;double z"; + + static frc::Translation3d Unpack(std::span data); + static void Pack(std::span data, + const frc::Translation3d& value); +}; diff --git a/wpimath/src/main/native/include/frc/geometry/struct/Twist2dStruct.h b/wpimath/src/main/native/include/frc/geometry/struct/Twist2dStruct.h new file mode 100644 index 00000000000..bda746c6daf --- /dev/null +++ b/wpimath/src/main/native/include/frc/geometry/struct/Twist2dStruct.h @@ -0,0 +1,21 @@ +// 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 +#include + +#include "frc/geometry/Twist2d.h" + +template <> +struct WPILIB_DLLEXPORT wpi::Struct { + static constexpr std::string_view kTypeString = "struct:Twist2d"; + static constexpr size_t kSize = 24; + static constexpr std::string_view kSchema = + "double dx;double dy;double dtheta"; + + static frc::Twist2d Unpack(std::span data); + static void Pack(std::span data, const frc::Twist2d& value); +}; diff --git a/wpimath/src/main/native/include/frc/geometry/struct/Twist3dStruct.h b/wpimath/src/main/native/include/frc/geometry/struct/Twist3dStruct.h new file mode 100644 index 00000000000..7cbe8a5d208 --- /dev/null +++ b/wpimath/src/main/native/include/frc/geometry/struct/Twist3dStruct.h @@ -0,0 +1,21 @@ +// 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 +#include + +#include "frc/geometry/Twist3d.h" + +template <> +struct WPILIB_DLLEXPORT wpi::Struct { + static constexpr std::string_view kTypeString = "struct:Twist3d"; + static constexpr size_t kSize = 48; + static constexpr std::string_view kSchema = + "double dx;double dy;double dz;double rx;double ry;double rz"; + + static frc::Twist3d Unpack(std::span data); + static void Pack(std::span data, const frc::Twist3d& value); +}; diff --git a/wpimath/src/main/native/include/frc/kinematics/ChassisSpeeds.h b/wpimath/src/main/native/include/frc/kinematics/ChassisSpeeds.h index 93c9044dca6..ac117b63e38 100644 --- a/wpimath/src/main/native/include/frc/kinematics/ChassisSpeeds.h +++ b/wpimath/src/main/native/include/frc/kinematics/ChassisSpeeds.h @@ -251,3 +251,6 @@ struct WPILIB_DLLEXPORT ChassisSpeeds { } }; } // namespace frc + +#include "frc/kinematics/proto/ChassisSpeedsProto.h" +#include "frc/kinematics/struct/ChassisSpeedsStruct.h" diff --git a/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveKinematics.h b/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveKinematics.h index 95f53fa3b98..3ea6ee92d3a 100644 --- a/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveKinematics.h +++ b/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveKinematics.h @@ -91,3 +91,6 @@ class WPILIB_DLLEXPORT DifferentialDriveKinematics units::meter_t trackWidth; }; } // namespace frc + +#include "frc/kinematics/proto/DifferentialDriveKinematicsProto.h" +#include "frc/kinematics/struct/DifferentialDriveKinematicsStruct.h" diff --git a/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveWheelSpeeds.h b/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveWheelSpeeds.h index 9d9e705772d..676b8ad59c6 100644 --- a/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveWheelSpeeds.h +++ b/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveWheelSpeeds.h @@ -112,3 +112,6 @@ struct WPILIB_DLLEXPORT DifferentialDriveWheelSpeeds { } }; } // namespace frc + +#include "frc/kinematics/proto/DifferentialDriveWheelSpeedsProto.h" +#include "frc/kinematics/struct/DifferentialDriveWheelSpeedsStruct.h" diff --git a/wpimath/src/main/native/include/frc/kinematics/MecanumDriveKinematics.h b/wpimath/src/main/native/include/frc/kinematics/MecanumDriveKinematics.h index fe6eb5120a5..24f450abf7e 100644 --- a/wpimath/src/main/native/include/frc/kinematics/MecanumDriveKinematics.h +++ b/wpimath/src/main/native/include/frc/kinematics/MecanumDriveKinematics.h @@ -138,6 +138,11 @@ class WPILIB_DLLEXPORT MecanumDriveKinematics */ Twist2d ToTwist2d(const MecanumDriveWheelPositions& wheelDeltas) const; + const Translation2d GetFrontLeftWheel() const { return m_frontLeftWheel; } + const Translation2d GetFrontRightWheel() const { return m_frontRightWheel; } + const Translation2d GetRearLeftWheel() const { return m_rearLeftWheel; } + const Translation2d GetRearRightWheel() const { return m_rearRightWheel; } + private: mutable Matrixd<4, 3> m_inverseKinematics; Eigen::HouseholderQR> m_forwardKinematics; @@ -165,3 +170,6 @@ class WPILIB_DLLEXPORT MecanumDriveKinematics }; } // namespace frc + +#include "frc/kinematics/proto/MecanumDriveKinematicsProto.h" +#include "frc/kinematics/struct/MecanumDriveKinematicsStruct.h" diff --git a/wpimath/src/main/native/include/frc/kinematics/MecanumDriveWheelPositions.h b/wpimath/src/main/native/include/frc/kinematics/MecanumDriveWheelPositions.h index 76c8b9dd96d..77d79192b9d 100644 --- a/wpimath/src/main/native/include/frc/kinematics/MecanumDriveWheelPositions.h +++ b/wpimath/src/main/native/include/frc/kinematics/MecanumDriveWheelPositions.h @@ -60,3 +60,6 @@ struct WPILIB_DLLEXPORT MecanumDriveWheelPositions { } }; } // namespace frc + +#include "frc/kinematics/proto/MecanumDriveWheelPositionsProto.h" +#include "frc/kinematics/struct/MecanumDriveWheelPositionsStruct.h" diff --git a/wpimath/src/main/native/include/frc/kinematics/MecanumDriveWheelSpeeds.h b/wpimath/src/main/native/include/frc/kinematics/MecanumDriveWheelSpeeds.h index 80e84607ed7..5df198c8018 100644 --- a/wpimath/src/main/native/include/frc/kinematics/MecanumDriveWheelSpeeds.h +++ b/wpimath/src/main/native/include/frc/kinematics/MecanumDriveWheelSpeeds.h @@ -120,3 +120,6 @@ struct WPILIB_DLLEXPORT MecanumDriveWheelSpeeds { } }; } // namespace frc + +#include "frc/kinematics/proto/MecanumDriveWheelSpeedsProto.h" +#include "frc/kinematics/struct/MecanumDriveWheelSpeedsStruct.h" diff --git a/wpimath/src/main/native/include/frc/kinematics/SwerveModulePosition.h b/wpimath/src/main/native/include/frc/kinematics/SwerveModulePosition.h index 93f7465a0a6..576bfcf00f9 100644 --- a/wpimath/src/main/native/include/frc/kinematics/SwerveModulePosition.h +++ b/wpimath/src/main/native/include/frc/kinematics/SwerveModulePosition.h @@ -42,3 +42,6 @@ struct WPILIB_DLLEXPORT SwerveModulePosition { } }; } // namespace frc + +#include "frc/kinematics/proto/SwerveModulePositionProto.h" +#include "frc/kinematics/struct/SwerveModulePositionStruct.h" diff --git a/wpimath/src/main/native/include/frc/kinematics/SwerveModuleState.h b/wpimath/src/main/native/include/frc/kinematics/SwerveModuleState.h index 2f95d9b3590..3060d35fc24 100644 --- a/wpimath/src/main/native/include/frc/kinematics/SwerveModuleState.h +++ b/wpimath/src/main/native/include/frc/kinematics/SwerveModuleState.h @@ -47,3 +47,6 @@ struct WPILIB_DLLEXPORT SwerveModuleState { const Rotation2d& currentAngle); }; } // namespace frc + +#include "frc/kinematics/proto/SwerveModuleStateProto.h" +#include "frc/kinematics/struct/SwerveModuleStateStruct.h" diff --git a/wpimath/src/main/native/include/frc/kinematics/proto/ChassisSpeedsProto.h b/wpimath/src/main/native/include/frc/kinematics/proto/ChassisSpeedsProto.h new file mode 100644 index 00000000000..cefc83f8db0 --- /dev/null +++ b/wpimath/src/main/native/include/frc/kinematics/proto/ChassisSpeedsProto.h @@ -0,0 +1,18 @@ +// 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 +#include + +#include "frc/kinematics/ChassisSpeeds.h" + +template <> +struct WPILIB_DLLEXPORT wpi::Protobuf { + static google::protobuf::Message* New(google::protobuf::Arena* arena); + static frc::ChassisSpeeds Unpack(const google::protobuf::Message& msg); + static void Pack(google::protobuf::Message* msg, + const frc::ChassisSpeeds& value); +}; diff --git a/wpimath/src/main/native/include/frc/kinematics/proto/DifferentialDriveKinematicsProto.h b/wpimath/src/main/native/include/frc/kinematics/proto/DifferentialDriveKinematicsProto.h new file mode 100644 index 00000000000..9108f33c9c6 --- /dev/null +++ b/wpimath/src/main/native/include/frc/kinematics/proto/DifferentialDriveKinematicsProto.h @@ -0,0 +1,19 @@ +// 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 +#include + +#include "frc/kinematics/DifferentialDriveKinematics.h" + +template <> +struct WPILIB_DLLEXPORT wpi::Protobuf { + static google::protobuf::Message* New(google::protobuf::Arena* arena); + static frc::DifferentialDriveKinematics Unpack( + const google::protobuf::Message& msg); + static void Pack(google::protobuf::Message* msg, + const frc::DifferentialDriveKinematics& value); +}; diff --git a/wpimath/src/main/native/include/frc/kinematics/proto/DifferentialDriveWheelSpeedsProto.h b/wpimath/src/main/native/include/frc/kinematics/proto/DifferentialDriveWheelSpeedsProto.h new file mode 100644 index 00000000000..f310892538e --- /dev/null +++ b/wpimath/src/main/native/include/frc/kinematics/proto/DifferentialDriveWheelSpeedsProto.h @@ -0,0 +1,19 @@ +// 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 +#include + +#include "frc/kinematics/DifferentialDriveWheelSpeeds.h" + +template <> +struct WPILIB_DLLEXPORT wpi::Protobuf { + static google::protobuf::Message* New(google::protobuf::Arena* arena); + static frc::DifferentialDriveWheelSpeeds Unpack( + const google::protobuf::Message& msg); + static void Pack(google::protobuf::Message* msg, + const frc::DifferentialDriveWheelSpeeds& value); +}; diff --git a/wpimath/src/main/native/include/frc/kinematics/proto/MecanumDriveKinematicsProto.h b/wpimath/src/main/native/include/frc/kinematics/proto/MecanumDriveKinematicsProto.h new file mode 100644 index 00000000000..16e828824f0 --- /dev/null +++ b/wpimath/src/main/native/include/frc/kinematics/proto/MecanumDriveKinematicsProto.h @@ -0,0 +1,19 @@ +// 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 +#include + +#include "frc/kinematics/MecanumDriveKinematics.h" + +template <> +struct WPILIB_DLLEXPORT wpi::Protobuf { + static google::protobuf::Message* New(google::protobuf::Arena* arena); + static frc::MecanumDriveKinematics Unpack( + const google::protobuf::Message& msg); + static void Pack(google::protobuf::Message* msg, + const frc::MecanumDriveKinematics& value); +}; diff --git a/wpimath/src/main/native/include/frc/kinematics/proto/MecanumDriveWheelPositionsProto.h b/wpimath/src/main/native/include/frc/kinematics/proto/MecanumDriveWheelPositionsProto.h new file mode 100644 index 00000000000..a4b4bd6762f --- /dev/null +++ b/wpimath/src/main/native/include/frc/kinematics/proto/MecanumDriveWheelPositionsProto.h @@ -0,0 +1,19 @@ +// 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 +#include + +#include "frc/kinematics/MecanumDriveWheelPositions.h" + +template <> +struct WPILIB_DLLEXPORT wpi::Protobuf { + static google::protobuf::Message* New(google::protobuf::Arena* arena); + static frc::MecanumDriveWheelPositions Unpack( + const google::protobuf::Message& msg); + static void Pack(google::protobuf::Message* msg, + const frc::MecanumDriveWheelPositions& value); +}; diff --git a/wpimath/src/main/native/include/frc/kinematics/proto/MecanumDriveWheelSpeedsProto.h b/wpimath/src/main/native/include/frc/kinematics/proto/MecanumDriveWheelSpeedsProto.h new file mode 100644 index 00000000000..4d099d3de16 --- /dev/null +++ b/wpimath/src/main/native/include/frc/kinematics/proto/MecanumDriveWheelSpeedsProto.h @@ -0,0 +1,19 @@ +// 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 +#include + +#include "frc/kinematics/MecanumDriveWheelSpeeds.h" + +template <> +struct WPILIB_DLLEXPORT wpi::Protobuf { + static google::protobuf::Message* New(google::protobuf::Arena* arena); + static frc::MecanumDriveWheelSpeeds Unpack( + const google::protobuf::Message& msg); + static void Pack(google::protobuf::Message* msg, + const frc::MecanumDriveWheelSpeeds& value); +}; diff --git a/wpimath/src/main/native/include/frc/kinematics/proto/SwerveModulePositionProto.h b/wpimath/src/main/native/include/frc/kinematics/proto/SwerveModulePositionProto.h new file mode 100644 index 00000000000..3e864cbd95b --- /dev/null +++ b/wpimath/src/main/native/include/frc/kinematics/proto/SwerveModulePositionProto.h @@ -0,0 +1,18 @@ +// 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 +#include + +#include "frc/kinematics/SwerveModulePosition.h" + +template <> +struct WPILIB_DLLEXPORT wpi::Protobuf { + static google::protobuf::Message* New(google::protobuf::Arena* arena); + static frc::SwerveModulePosition Unpack(const google::protobuf::Message& msg); + static void Pack(google::protobuf::Message* msg, + const frc::SwerveModulePosition& value); +}; diff --git a/wpimath/src/main/native/include/frc/kinematics/proto/SwerveModuleStateProto.h b/wpimath/src/main/native/include/frc/kinematics/proto/SwerveModuleStateProto.h new file mode 100644 index 00000000000..3e7b3ec6e41 --- /dev/null +++ b/wpimath/src/main/native/include/frc/kinematics/proto/SwerveModuleStateProto.h @@ -0,0 +1,18 @@ +// 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 +#include + +#include "frc/kinematics/SwerveModuleState.h" + +template <> +struct WPILIB_DLLEXPORT wpi::Protobuf { + static google::protobuf::Message* New(google::protobuf::Arena* arena); + static frc::SwerveModuleState Unpack(const google::protobuf::Message& msg); + static void Pack(google::protobuf::Message* msg, + const frc::SwerveModuleState& value); +}; diff --git a/wpimath/src/main/native/include/frc/kinematics/struct/ChassisSpeedsStruct.h b/wpimath/src/main/native/include/frc/kinematics/struct/ChassisSpeedsStruct.h new file mode 100644 index 00000000000..4e6547bd265 --- /dev/null +++ b/wpimath/src/main/native/include/frc/kinematics/struct/ChassisSpeedsStruct.h @@ -0,0 +1,22 @@ +// 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 +#include + +#include "frc/kinematics/ChassisSpeeds.h" + +template <> +struct WPILIB_DLLEXPORT wpi::Struct { + static constexpr std::string_view kTypeString = "struct:ChassisSpeeds"; + static constexpr size_t kSize = 24; + static constexpr std::string_view kSchema = + "double vx;double vy;double omega"; + + static frc::ChassisSpeeds Unpack(std::span data); + static void Pack(std::span data, + const frc::ChassisSpeeds& value); +}; diff --git a/wpimath/src/main/native/include/frc/kinematics/struct/DifferentialDriveKinematicsStruct.h b/wpimath/src/main/native/include/frc/kinematics/struct/DifferentialDriveKinematicsStruct.h new file mode 100644 index 00000000000..ab1e2a89648 --- /dev/null +++ b/wpimath/src/main/native/include/frc/kinematics/struct/DifferentialDriveKinematicsStruct.h @@ -0,0 +1,23 @@ +// 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 +#include + +#include "frc/kinematics/DifferentialDriveKinematics.h" + +template <> +struct WPILIB_DLLEXPORT wpi::Struct { + static constexpr std::string_view kTypeString = + "struct:DifferentialDriveKinematics"; + static constexpr size_t kSize = 8; + static constexpr std::string_view kSchema = "double track_width"; + + static frc::DifferentialDriveKinematics Unpack( + std::span data); + static void Pack(std::span data, + const frc::DifferentialDriveKinematics& value); +}; diff --git a/wpimath/src/main/native/include/frc/kinematics/struct/DifferentialDriveWheelSpeedsStruct.h b/wpimath/src/main/native/include/frc/kinematics/struct/DifferentialDriveWheelSpeedsStruct.h new file mode 100644 index 00000000000..38101603857 --- /dev/null +++ b/wpimath/src/main/native/include/frc/kinematics/struct/DifferentialDriveWheelSpeedsStruct.h @@ -0,0 +1,23 @@ +// 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 +#include + +#include "frc/kinematics/DifferentialDriveWheelSpeeds.h" + +template <> +struct WPILIB_DLLEXPORT wpi::Struct { + static constexpr std::string_view kTypeString = + "struct:DifferentialDriveWheelSpeeds"; + static constexpr size_t kSize = 16; + static constexpr std::string_view kSchema = "double left;double right"; + + static frc::DifferentialDriveWheelSpeeds Unpack( + std::span data); + static void Pack(std::span data, + const frc::DifferentialDriveWheelSpeeds& value); +}; diff --git a/wpimath/src/main/native/include/frc/kinematics/struct/MecanumDriveKinematicsStruct.h b/wpimath/src/main/native/include/frc/kinematics/struct/MecanumDriveKinematicsStruct.h new file mode 100644 index 00000000000..2974c6b1d6c --- /dev/null +++ b/wpimath/src/main/native/include/frc/kinematics/struct/MecanumDriveKinematicsStruct.h @@ -0,0 +1,29 @@ +// 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 +#include + +#include "frc/kinematics/MecanumDriveKinematics.h" + +template <> +struct WPILIB_DLLEXPORT wpi::Struct { + static constexpr std::string_view kTypeString = + "struct:MecanumDriveKinematics"; + static constexpr size_t kSize = 4 * wpi::Struct::kSize; + static constexpr std::string_view kSchema = + "Translation2d front_left;Translation2d front_right;Translation2d " + "rear_left;Translation2d rear_right"; + + static frc::MecanumDriveKinematics Unpack( + std::span data); + static void Pack(std::span data, + const frc::MecanumDriveKinematics& value); + static void ForEachNested( + std::invocable auto fn); +}; + +static_assert(wpi::HasNestedStruct); diff --git a/wpimath/src/main/native/include/frc/kinematics/struct/MecanumDriveWheelPositionsStruct.h b/wpimath/src/main/native/include/frc/kinematics/struct/MecanumDriveWheelPositionsStruct.h new file mode 100644 index 00000000000..8fbc3cb316a --- /dev/null +++ b/wpimath/src/main/native/include/frc/kinematics/struct/MecanumDriveWheelPositionsStruct.h @@ -0,0 +1,24 @@ +// 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 +#include + +#include "frc/kinematics/MecanumDriveWheelPositions.h" + +template <> +struct WPILIB_DLLEXPORT wpi::Struct { + static constexpr std::string_view kTypeString = + "struct:MecanumDriveWheelPositions"; + static constexpr size_t kSize = 32; + static constexpr std::string_view kSchema = + "double front_left;double front_right;double rear_left;double rear_right"; + + static frc::MecanumDriveWheelPositions Unpack( + std::span data); + static void Pack(std::span data, + const frc::MecanumDriveWheelPositions& value); +}; diff --git a/wpimath/src/main/native/include/frc/kinematics/struct/MecanumDriveWheelSpeedsStruct.h b/wpimath/src/main/native/include/frc/kinematics/struct/MecanumDriveWheelSpeedsStruct.h new file mode 100644 index 00000000000..f4cfa0987ef --- /dev/null +++ b/wpimath/src/main/native/include/frc/kinematics/struct/MecanumDriveWheelSpeedsStruct.h @@ -0,0 +1,24 @@ +// 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 +#include + +#include "frc/kinematics/MecanumDriveWheelSpeeds.h" + +template <> +struct WPILIB_DLLEXPORT wpi::Struct { + static constexpr std::string_view kTypeString = + "struct:MecanumDriveWheelSpeeds"; + static constexpr size_t kSize = 32; + static constexpr std::string_view kSchema = + "double front_left;double front_right;double rear_left;double rear_right"; + + static frc::MecanumDriveWheelSpeeds Unpack( + std::span data); + static void Pack(std::span data, + const frc::MecanumDriveWheelSpeeds& value); +}; diff --git a/wpimath/src/main/native/include/frc/kinematics/struct/SwerveModulePositionStruct.h b/wpimath/src/main/native/include/frc/kinematics/struct/SwerveModulePositionStruct.h new file mode 100644 index 00000000000..20b1ff60ccd --- /dev/null +++ b/wpimath/src/main/native/include/frc/kinematics/struct/SwerveModulePositionStruct.h @@ -0,0 +1,26 @@ +// 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 +#include + +#include "frc/kinematics/SwerveModulePosition.h" + +template <> +struct WPILIB_DLLEXPORT wpi::Struct { + static constexpr std::string_view kTypeString = "struct:SwerveModulePosition"; + static constexpr size_t kSize = 8 + wpi::Struct::kSize; + static constexpr std::string_view kSchema = + "double distance;Rotation2d angle"; + + static frc::SwerveModulePosition Unpack(std::span data); + static void Pack(std::span data, + const frc::SwerveModulePosition& value); + static void ForEachNested( + std::invocable auto fn); +}; + +static_assert(wpi::HasNestedStruct); diff --git a/wpimath/src/main/native/include/frc/kinematics/struct/SwerveModuleStateStruct.h b/wpimath/src/main/native/include/frc/kinematics/struct/SwerveModuleStateStruct.h new file mode 100644 index 00000000000..fd854cb54d2 --- /dev/null +++ b/wpimath/src/main/native/include/frc/kinematics/struct/SwerveModuleStateStruct.h @@ -0,0 +1,25 @@ +// 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 +#include + +#include "frc/kinematics/SwerveModuleState.h" + +template <> +struct WPILIB_DLLEXPORT wpi::Struct { + static constexpr std::string_view kTypeString = "struct:SwerveModuleState"; + static constexpr size_t kSize = 8 + wpi::Struct::kSize; + static constexpr std::string_view kSchema = "double speed;Rotation2d angle"; + + static frc::SwerveModuleState Unpack(std::span data); + static void Pack(std::span data, + const frc::SwerveModuleState& value); + static void ForEachNested( + std::invocable auto fn); +}; + +static_assert(wpi::HasNestedStruct); diff --git a/wpimath/src/main/native/include/frc/system/plant/DCMotor.h b/wpimath/src/main/native/include/frc/system/plant/DCMotor.h index ad711ee3cfa..de9c3b1b547 100644 --- a/wpimath/src/main/native/include/frc/system/plant/DCMotor.h +++ b/wpimath/src/main/native/include/frc/system/plant/DCMotor.h @@ -239,3 +239,6 @@ class WPILIB_DLLEXPORT DCMotor { }; } // namespace frc + +#include "frc/system/plant/proto/DCMotorProto.h" +#include "frc/system/plant/struct/DCMotorStruct.h" 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 64f34963a90..af9e799dea0 100644 --- a/wpimath/src/main/native/include/frc/system/plant/LinearSystemId.h +++ b/wpimath/src/main/native/include/frc/system/plant/LinearSystemId.h @@ -38,12 +38,13 @@ class WPILIB_DLLEXPORT LinearSystemId { * @param motor The motor (or gearbox) attached to the carriage. * @param mass The mass of the elevator carriage, in kilograms. * @param radius The radius of the elevator's driving drum, in meters. - * @param G Gear ratio from motor to carriage. - * @throws std::domain_error if mass <= 0, radius <= 0, or G <= 0. + * @param gearing Gear ratio from motor to carriage. + * @throws std::domain_error if mass <= 0, radius <= 0, or gearing <= 0. */ static LinearSystem<2, 1, 1> ElevatorSystem(DCMotor motor, units::kilogram_t mass, - units::meter_t radius, double G); + units::meter_t radius, + double gearing); /** * Create a state-space model of a single-jointed arm system.The states of the @@ -52,11 +53,11 @@ class WPILIB_DLLEXPORT LinearSystemId { * * @param motor The motor (or gearbox) attached to the arm. * @param J The moment of inertia J of the arm. - * @param G Gear ratio from motor to arm. - * @throws std::domain_error if J <= 0 or G <= 0. + * @param gearing Gear ratio from motor to arm. + * @throws std::domain_error if J <= 0 or gearing <= 0. */ static LinearSystem<2, 1, 1> SingleJointedArmSystem( - DCMotor motor, units::kilogram_square_meter_t J, double G); + DCMotor motor, units::kilogram_square_meter_t J, double gearing); /** * Create a state-space model for a 1 DOF velocity system from its kV @@ -198,12 +199,12 @@ class WPILIB_DLLEXPORT LinearSystemId { * * @param motor The motor (or gearbox) attached to the flywheel. * @param J The moment of inertia J of the flywheel. - * @param G Gear ratio from motor to flywheel. - * @throws std::domain_error if J <= 0 or G <= 0. + * @param gearing Gear ratio from motor to flywheel. + * @throws std::domain_error if J <= 0 or gearing <= 0. */ static LinearSystem<1, 1, 1> FlywheelSystem(DCMotor motor, units::kilogram_square_meter_t J, - double G); + double gearing); /** * Create a state-space model of a DC motor system. The states of the system @@ -212,12 +213,12 @@ class WPILIB_DLLEXPORT LinearSystemId { * * @param motor The motor (or gearbox) attached to the system. * @param J the moment of inertia J of the DC motor. - * @param G Gear ratio from motor to output. - * @throws std::domain_error if J <= 0 or G <= 0. + * @param gearing Gear ratio from motor to output. + * @throws std::domain_error if J <= 0 or gearing <= 0. */ static LinearSystem<2, 1, 2> DCMotorSystem(DCMotor motor, units::kilogram_square_meter_t J, - double G); + double gearing); /** * Create a state-space model of a DC motor system from its kV @@ -271,13 +272,13 @@ class WPILIB_DLLEXPORT LinearSystemId { * @param r The radius of the wheels in meters. * @param rb The radius of the base (half of the track width), in meters. * @param J The moment of inertia of the robot. - * @param G Gear ratio from motor to wheel. + * @param gearing Gear ratio from motor to wheel. * @throws std::domain_error if mass <= 0, r <= 0, rb <= 0, J <= 0, or - * G <= 0. + * gearing <= 0. */ static LinearSystem<2, 2, 2> DrivetrainVelocitySystem( const DCMotor& motor, units::kilogram_t mass, units::meter_t r, - units::meter_t rb, units::kilogram_square_meter_t J, double G); + units::meter_t rb, units::kilogram_square_meter_t J, double gearing); }; } // namespace frc diff --git a/wpimath/src/main/native/include/frc/system/plant/proto/DCMotorProto.h b/wpimath/src/main/native/include/frc/system/plant/proto/DCMotorProto.h new file mode 100644 index 00000000000..acae1db6d3c --- /dev/null +++ b/wpimath/src/main/native/include/frc/system/plant/proto/DCMotorProto.h @@ -0,0 +1,17 @@ +// 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 +#include + +#include "frc/system/plant/DCMotor.h" + +template <> +struct WPILIB_DLLEXPORT wpi::Protobuf { + static google::protobuf::Message* New(google::protobuf::Arena* arena); + static frc::DCMotor Unpack(const google::protobuf::Message& msg); + static void Pack(google::protobuf::Message* msg, const frc::DCMotor& value); +}; diff --git a/wpimath/src/main/native/include/frc/system/plant/struct/DCMotorStruct.h b/wpimath/src/main/native/include/frc/system/plant/struct/DCMotorStruct.h new file mode 100644 index 00000000000..c16b65b4d6d --- /dev/null +++ b/wpimath/src/main/native/include/frc/system/plant/struct/DCMotorStruct.h @@ -0,0 +1,22 @@ +// 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 +#include + +#include "frc/system/plant/DCMotor.h" + +template <> +struct WPILIB_DLLEXPORT wpi::Struct { + static constexpr std::string_view kTypeString = "struct:DCMotor"; + static constexpr size_t kSize = 40; + static constexpr std::string_view kSchema = + "double nominal_voltage;double stall_torque;double stall_current;double " + "free_current;double free_speed"; + + static frc::DCMotor Unpack(std::span data); + static void Pack(std::span data, const frc::DCMotor& value); +}; diff --git a/wpimath/src/main/native/include/frc/trajectory/Trajectory.h b/wpimath/src/main/native/include/frc/trajectory/Trajectory.h index ca97593a1ad..2ad972ea490 100644 --- a/wpimath/src/main/native/include/frc/trajectory/Trajectory.h +++ b/wpimath/src/main/native/include/frc/trajectory/Trajectory.h @@ -145,3 +145,6 @@ WPILIB_DLLEXPORT void from_json(const wpi::json& json, Trajectory::State& state); } // namespace frc + +#include "frc/trajectory/proto/TrajectoryProto.h" +#include "frc/trajectory/proto/TrajectoryStateProto.h" diff --git a/wpimath/src/main/native/include/frc/trajectory/proto/TrajectoryProto.h b/wpimath/src/main/native/include/frc/trajectory/proto/TrajectoryProto.h new file mode 100644 index 00000000000..4b485b2a922 --- /dev/null +++ b/wpimath/src/main/native/include/frc/trajectory/proto/TrajectoryProto.h @@ -0,0 +1,18 @@ +// 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 +#include + +#include "frc/trajectory/Trajectory.h" + +template <> +struct WPILIB_DLLEXPORT wpi::Protobuf { + static google::protobuf::Message* New(google::protobuf::Arena* arena); + static frc::Trajectory Unpack(const google::protobuf::Message& msg); + static void Pack(google::protobuf::Message* msg, + const frc::Trajectory& value); +}; diff --git a/wpimath/src/main/native/include/frc/trajectory/proto/TrajectoryStateProto.h b/wpimath/src/main/native/include/frc/trajectory/proto/TrajectoryStateProto.h new file mode 100644 index 00000000000..150837b8d3e --- /dev/null +++ b/wpimath/src/main/native/include/frc/trajectory/proto/TrajectoryStateProto.h @@ -0,0 +1,18 @@ +// 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 +#include + +#include "frc/trajectory/Trajectory.h" + +template <> +struct WPILIB_DLLEXPORT wpi::Protobuf { + static google::protobuf::Message* New(google::protobuf::Arena* arena); + static frc::Trajectory::State Unpack(const google::protobuf::Message& msg); + static void Pack(google::protobuf::Message* msg, + const frc::Trajectory::State& value); +}; diff --git a/wpimath/src/main/proto/plant.proto b/wpimath/src/main/proto/plant.proto index d0d9eabc8a7..86200d13cfa 100644 --- a/wpimath/src/main/proto/plant.proto +++ b/wpimath/src/main/proto/plant.proto @@ -10,7 +10,4 @@ message ProtobufDCMotor { double stall_current = 3; double free_current = 4; double free_speed = 5; - double r = 6; - double kv = 7; - double kt = 8; } diff --git a/wpimath/src/main/proto/trajectory.proto b/wpimath/src/main/proto/trajectory.proto index a37a501d0d3..03ece13af32 100644 --- a/wpimath/src/main/proto/trajectory.proto +++ b/wpimath/src/main/proto/trajectory.proto @@ -15,6 +15,5 @@ message ProtobufTrajectoryState { } message ProtobufTrajectory { - double total_time = 1; repeated ProtobufTrajectoryState states = 2; } diff --git a/wpimath/src/test/java/edu/wpi/first/math/MatrixTest.java b/wpimath/src/test/java/edu/wpi/first/math/MatrixTest.java index 8ee11e5e229..d4bc0356b98 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/MatrixTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/MatrixTest.java @@ -18,29 +18,28 @@ class MatrixTest { @Test void testMatrixMultiplication() { - var mat1 = Matrix.mat(Nat.N2(), Nat.N2()).fill(2.0, 1.0, 0.0, 1.0); - var mat2 = Matrix.mat(Nat.N2(), Nat.N2()).fill(3.0, 0.0, 0.0, 2.5); + var mat1 = MatBuilder.fill(Nat.N2(), Nat.N2(), 2.0, 1.0, 0.0, 1.0); + var mat2 = MatBuilder.fill(Nat.N2(), Nat.N2(), 3.0, 0.0, 0.0, 2.5); Matrix result = mat1.times(mat2); - assertEquals(result, Matrix.mat(Nat.N2(), Nat.N2()).fill(6.0, 2.5, 0.0, 2.5)); + assertEquals(result, MatBuilder.fill(Nat.N2(), Nat.N2(), 6.0, 2.5, 0.0, 2.5)); - var mat3 = Matrix.mat(Nat.N2(), Nat.N3()).fill(1.0, 3.0, 0.5, 2.0, 4.3, 1.2); + var mat3 = MatBuilder.fill(Nat.N2(), Nat.N3(), 1.0, 3.0, 0.5, 2.0, 4.3, 1.2); var mat4 = - Matrix.mat(Nat.N3(), Nat.N4()) - .fill(3.0, 1.5, 2.0, 4.5, 2.3, 1.0, 1.6, 3.1, 5.2, 2.1, 2.0, 1.0); + MatBuilder.fill( + Nat.N3(), Nat.N4(), 3.0, 1.5, 2.0, 4.5, 2.3, 1.0, 1.6, 3.1, 5.2, 2.1, 2.0, 1.0); Matrix result2 = mat3.times(mat4); assertTrue( - Matrix.mat(Nat.N2(), Nat.N4()) - .fill(12.5, 5.55, 7.8, 14.3, 22.13, 9.82, 13.28, 23.53) + MatBuilder.fill(Nat.N2(), Nat.N4(), 12.5, 5.55, 7.8, 14.3, 22.13, 9.82, 13.28, 23.53) .isEqual(result2, 1E-9)); } @Test void testMatrixVectorMultiplication() { - var mat = Matrix.mat(Nat.N2(), Nat.N2()).fill(1.0, 1.0, 0.0, 1.0); + var mat = MatBuilder.fill(Nat.N2(), Nat.N2(), 1.0, 1.0, 0.0, 1.0); var vec = VecBuilder.fill(3.0, 2.0); @@ -54,19 +53,19 @@ void testTranspose() { Matrix transpose = vec.transpose(); - assertEquals(Matrix.mat(Nat.N1(), Nat.N3()).fill(1.0, 2.0, 3.0), transpose); + assertEquals(MatBuilder.fill(Nat.N1(), Nat.N3(), 1.0, 2.0, 3.0), transpose); } @Test void testSolve() { - var mat1 = Matrix.mat(Nat.N2(), Nat.N2()).fill(1.0, 2.0, 3.0, 4.0); + var mat1 = MatBuilder.fill(Nat.N2(), Nat.N2(), 1.0, 2.0, 3.0, 4.0); var vec1 = VecBuilder.fill(1.0, 2.0); var solve1 = mat1.solve(vec1); assertEquals(VecBuilder.fill(0.0, 0.5), solve1); - var mat2 = Matrix.mat(Nat.N3(), Nat.N2()).fill(1.0, 2.0, 3.0, 4.0, 5.0, 6.0); + var mat2 = MatBuilder.fill(Nat.N3(), Nat.N2(), 1.0, 2.0, 3.0, 4.0, 5.0, 6.0); var vec2 = VecBuilder.fill(1.0, 2.0, 3.0); var solve2 = mat2.solve(vec2); @@ -76,7 +75,7 @@ void testSolve() { @Test void testInverse() { - var mat = Matrix.mat(Nat.N3(), Nat.N3()).fill(1.0, 3.0, 2.0, 5.0, 2.0, 1.5, 0.0, 1.3, 2.5); + var mat = MatBuilder.fill(Nat.N3(), Nat.N3(), 1.0, 3.0, 2.0, 5.0, 2.0, 1.5, 0.0, 1.3, 2.5); var inv = mat.inv(); @@ -87,33 +86,33 @@ void testInverse() { @Test void testUninvertableMatrix() { - var singularMatrix = Matrix.mat(Nat.N2(), Nat.N2()).fill(2.0, 1.0, 2.0, 1.0); + var singularMatrix = MatBuilder.fill(Nat.N2(), Nat.N2(), 2.0, 1.0, 2.0, 1.0); assertThrows(SingularMatrixException.class, singularMatrix::inv); } @Test void testMatrixScalarArithmetic() { - var mat = Matrix.mat(Nat.N2(), Nat.N2()).fill(1.0, 2.0, 3.0, 4.0); + var mat = MatBuilder.fill(Nat.N2(), Nat.N2(), 1.0, 2.0, 3.0, 4.0); - assertEquals(Matrix.mat(Nat.N2(), Nat.N2()).fill(3.0, 4.0, 5.0, 6.0), mat.plus(2.0)); + assertEquals(MatBuilder.fill(Nat.N2(), Nat.N2(), 3.0, 4.0, 5.0, 6.0), mat.plus(2.0)); - assertEquals(Matrix.mat(Nat.N2(), Nat.N2()).fill(0.0, 1.0, 2.0, 3.0), mat.minus(1.0)); + assertEquals(MatBuilder.fill(Nat.N2(), Nat.N2(), 0.0, 1.0, 2.0, 3.0), mat.minus(1.0)); - assertEquals(Matrix.mat(Nat.N2(), Nat.N2()).fill(2.0, 4.0, 6.0, 8.0), mat.times(2.0)); + assertEquals(MatBuilder.fill(Nat.N2(), Nat.N2(), 2.0, 4.0, 6.0, 8.0), mat.times(2.0)); - assertTrue(Matrix.mat(Nat.N2(), Nat.N2()).fill(0.5, 1.0, 1.5, 2.0).isEqual(mat.div(2.0), 1E-3)); + assertTrue(MatBuilder.fill(Nat.N2(), Nat.N2(), 0.5, 1.0, 1.5, 2.0).isEqual(mat.div(2.0), 1E-3)); } @Test void testMatrixMatrixArithmetic() { - var mat1 = Matrix.mat(Nat.N2(), Nat.N2()).fill(1.0, 2.0, 3.0, 4.0); + var mat1 = MatBuilder.fill(Nat.N2(), Nat.N2(), 1.0, 2.0, 3.0, 4.0); - var mat2 = Matrix.mat(Nat.N2(), Nat.N2()).fill(5.0, 6.0, 7.0, 8.0); + var mat2 = MatBuilder.fill(Nat.N2(), Nat.N2(), 5.0, 6.0, 7.0, 8.0); - assertEquals(Matrix.mat(Nat.N2(), Nat.N2()).fill(-4.0, -4.0, -4.0, -4.0), mat1.minus(mat2)); + assertEquals(MatBuilder.fill(Nat.N2(), Nat.N2(), -4.0, -4.0, -4.0, -4.0), mat1.minus(mat2)); - assertEquals(Matrix.mat(Nat.N2(), Nat.N2()).fill(6.0, 8.0, 10.0, 12.0), mat1.plus(mat2)); + assertEquals(MatBuilder.fill(Nat.N2(), Nat.N2(), 6.0, 8.0, 10.0, 12.0), mat1.plus(mat2)); } @Test @@ -121,14 +120,14 @@ void testMatrixExponential() { var matrix = Matrix.eye(Nat.N2()); var result = matrix.exp(); - assertTrue(result.isEqual(Matrix.mat(Nat.N2(), Nat.N2()).fill(Math.E, 0, 0, Math.E), 1E-9)); + assertTrue(result.isEqual(MatBuilder.fill(Nat.N2(), Nat.N2(), Math.E, 0, 0, Math.E), 1E-9)); - matrix = Matrix.mat(Nat.N2(), Nat.N2()).fill(1, 2, 3, 4); + matrix = MatBuilder.fill(Nat.N2(), Nat.N2(), 1, 2, 3, 4); result = matrix.times(0.01).exp(); assertTrue( result.isEqual( - Matrix.mat(Nat.N2(), Nat.N2()).fill(1.01035625, 0.02050912, 0.03076368, 1.04111993), + MatBuilder.fill(Nat.N2(), Nat.N2(), 1.01035625, 0.02050912, 0.03076368, 1.04111993), 1E-8)); } } diff --git a/wpimath/src/test/java/edu/wpi/first/math/StateSpaceUtilTest.java b/wpimath/src/test/java/edu/wpi/first/math/StateSpaceUtilTest.java index ee33f01526e..74ff4012694 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/StateSpaceUtilTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/StateSpaceUtilTest.java @@ -61,48 +61,48 @@ void testIsStabilizable() { // First eigenvalue is uncontrollable and unstable. // Second eigenvalue is controllable and stable. - A = Matrix.mat(Nat.N2(), Nat.N2()).fill(1.2, 0, 0, 0.5); + A = MatBuilder.fill(Nat.N2(), Nat.N2(), 1.2, 0, 0, 0.5); assertFalse(StateSpaceUtil.isStabilizable(A, B)); // First eigenvalue is uncontrollable and marginally stable. // Second eigenvalue is controllable and stable. - A = Matrix.mat(Nat.N2(), Nat.N2()).fill(1, 0, 0, 0.5); + A = MatBuilder.fill(Nat.N2(), Nat.N2(), 1, 0, 0, 0.5); assertFalse(StateSpaceUtil.isStabilizable(A, B)); // First eigenvalue is uncontrollable and stable. // Second eigenvalue is controllable and stable. - A = Matrix.mat(Nat.N2(), Nat.N2()).fill(0.2, 0, 0, 0.5); + A = MatBuilder.fill(Nat.N2(), Nat.N2(), 0.2, 0, 0, 0.5); assertTrue(StateSpaceUtil.isStabilizable(A, B)); // First eigenvalue is uncontrollable and stable. // Second eigenvalue is controllable and unstable. - A = Matrix.mat(Nat.N2(), Nat.N2()).fill(0.2, 0, 0, 1.2); + A = MatBuilder.fill(Nat.N2(), Nat.N2(), 0.2, 0, 0, 1.2); assertTrue(StateSpaceUtil.isStabilizable(A, B)); } @Test void testIsDetectable() { Matrix A; - Matrix C = Matrix.mat(Nat.N1(), Nat.N2()).fill(0, 1); + Matrix C = MatBuilder.fill(Nat.N1(), Nat.N2(), 0, 1); // First eigenvalue is unobservable and unstable. // Second eigenvalue is observable and stable. - A = Matrix.mat(Nat.N2(), Nat.N2()).fill(1.2, 0, 0, 0.5); + A = MatBuilder.fill(Nat.N2(), Nat.N2(), 1.2, 0, 0, 0.5); assertFalse(StateSpaceUtil.isDetectable(A, C)); // First eigenvalue is unobservable and marginally stable. // Second eigenvalue is observable and stable. - A = Matrix.mat(Nat.N2(), Nat.N2()).fill(1, 0, 0, 0.5); + A = MatBuilder.fill(Nat.N2(), Nat.N2(), 1, 0, 0, 0.5); assertFalse(StateSpaceUtil.isDetectable(A, C)); // First eigenvalue is unobservable and stable. // Second eigenvalue is observable and stable. - A = Matrix.mat(Nat.N2(), Nat.N2()).fill(0.2, 0, 0, 0.5); + A = MatBuilder.fill(Nat.N2(), Nat.N2(), 0.2, 0, 0, 0.5); assertTrue(StateSpaceUtil.isDetectable(A, C)); // First eigenvalue is unobservable and stable. // Second eigenvalue is observable and unstable. - A = Matrix.mat(Nat.N2(), Nat.N2()).fill(0.2, 0, 0, 1.2); + A = MatBuilder.fill(Nat.N2(), Nat.N2(), 0.2, 0, 0, 1.2); assertTrue(StateSpaceUtil.isDetectable(A, C)); } @@ -143,14 +143,14 @@ void testMatrixExp() { var wrappedResult = wrappedMatrix.exp(); assertTrue( - wrappedResult.isEqual(Matrix.mat(Nat.N2(), Nat.N2()).fill(Math.E, 0, 0, Math.E), 1E-9)); + wrappedResult.isEqual(MatBuilder.fill(Nat.N2(), Nat.N2(), Math.E, 0, 0, Math.E), 1E-9)); - var matrix = Matrix.mat(Nat.N2(), Nat.N2()).fill(1, 2, 3, 4); + var matrix = MatBuilder.fill(Nat.N2(), Nat.N2(), 1, 2, 3, 4); wrappedResult = matrix.times(0.01).exp(); assertTrue( wrappedResult.isEqual( - Matrix.mat(Nat.N2(), Nat.N2()).fill(1.01035625, 0.02050912, 0.03076368, 1.04111993), + MatBuilder.fill(Nat.N2(), Nat.N2(), 1.01035625, 0.02050912, 0.03076368, 1.04111993), 1E-8)); } diff --git a/wpimath/src/test/java/edu/wpi/first/math/controller/ControlAffinePlantInversionFeedforwardTest.java b/wpimath/src/test/java/edu/wpi/first/math/controller/ControlAffinePlantInversionFeedforwardTest.java index fcd9de14163..60300fb838b 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/controller/ControlAffinePlantInversionFeedforwardTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/controller/ControlAffinePlantInversionFeedforwardTest.java @@ -6,6 +6,7 @@ import static org.junit.jupiter.api.Assertions.assertEquals; +import edu.wpi.first.math.MatBuilder; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.Nat; import edu.wpi.first.math.VecBuilder; @@ -35,13 +36,12 @@ void testCalculateState() { } protected Matrix getDynamics(Matrix x, Matrix u) { - return Matrix.mat(Nat.N2(), Nat.N2()) - .fill(1.000, 0, 0, 1.000) + return MatBuilder.fill(Nat.N2(), Nat.N2(), 1.000, 0, 0, 1.000) .times(x) .plus(VecBuilder.fill(0, 1).times(u)); } protected Matrix getStateDynamics(Matrix x) { - return Matrix.mat(Nat.N2(), Nat.N2()).fill(1.000, 0, 0, 1.000).times(x); + return MatBuilder.fill(Nat.N2(), Nat.N2(), 1.000, 0, 0, 1.000).times(x); } } diff --git a/wpimath/src/test/java/edu/wpi/first/math/controller/DifferentialDriveAccelerationLimiterTest.java b/wpimath/src/test/java/edu/wpi/first/math/controller/DifferentialDriveAccelerationLimiterTest.java index faf55634ad6..4ec558d4e77 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/controller/DifferentialDriveAccelerationLimiterTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/controller/DifferentialDriveAccelerationLimiterTest.java @@ -9,8 +9,10 @@ import static org.junit.jupiter.api.Assertions.assertThrows; import static org.junit.jupiter.api.Assertions.assertTrue; -import edu.wpi.first.math.MatBuilder; -import edu.wpi.first.math.Nat; +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N2; import edu.wpi.first.math.system.plant.LinearSystemId; import org.junit.jupiter.api.Test; @@ -26,52 +28,38 @@ void testLowLimits() { var accelLimiter = new DifferentialDriveAccelerationLimiter(plant, trackwidth, maxA, maxAlpha); - var x = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(0.0, 0.0); - var xAccelLimiter = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(0.0, 0.0); + Matrix x = VecBuilder.fill(0.0, 0.0); + Matrix xAccelLimiter = VecBuilder.fill(0.0, 0.0); // Ensure voltage exceeds acceleration before limiting { final var accels = - plant - .getA() - .times(xAccelLimiter) - .plus(plant.getB().times(new MatBuilder<>(Nat.N2(), Nat.N1()).fill(12.0, 12.0))); + plant.getA().times(xAccelLimiter).plus(plant.getB().times(VecBuilder.fill(12.0, 12.0))); final double a = (accels.get(0, 0) + accels.get(1, 0)) / 2.0; assertTrue(Math.abs(a) > maxA); } { final var accels = - plant - .getA() - .times(xAccelLimiter) - .plus(plant.getB().times(new MatBuilder<>(Nat.N2(), Nat.N1()).fill(-12.0, 12.0))); + plant.getA().times(xAccelLimiter).plus(plant.getB().times(VecBuilder.fill(-12.0, 12.0))); final double alpha = (accels.get(1, 0) - accels.get(0, 0)) / trackwidth; assertTrue(Math.abs(alpha) > maxAlpha); } // Forward - var u = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(12.0, 12.0); + var u = VecBuilder.fill(12.0, 12.0); for (double t = 0.0; t < 3.0; t += dt) { x = plant.calculateX(x, u, dt); final var voltages = accelLimiter.calculate( xAccelLimiter.get(0, 0), xAccelLimiter.get(1, 0), u.get(0, 0), u.get(1, 0)); xAccelLimiter = - plant.calculateX( - xAccelLimiter, - new MatBuilder<>(Nat.N2(), Nat.N1()).fill(voltages.left, voltages.right), - dt); + plant.calculateX(xAccelLimiter, VecBuilder.fill(voltages.left, voltages.right), dt); final var accels = plant .getA() .times(xAccelLimiter) - .plus( - plant - .getB() - .times( - new MatBuilder<>(Nat.N2(), Nat.N1()) - .fill(voltages.left, voltages.right))); + .plus(plant.getB().times(VecBuilder.fill(voltages.left, voltages.right))); final double a = (accels.get(0, 0) + accels.get(1, 0)) / 2.0; final double alpha = (accels.get(1, 0) - accels.get(0, 0)) / trackwidth; assertTrue(Math.abs(a) <= maxA); @@ -79,28 +67,20 @@ void testLowLimits() { } // Backward - u = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(-12.0, -12.0); + u = VecBuilder.fill(-12.0, -12.0); for (double t = 0.0; t < 3.0; t += dt) { x = plant.calculateX(x, u, dt); final var voltages = accelLimiter.calculate( xAccelLimiter.get(0, 0), xAccelLimiter.get(1, 0), u.get(0, 0), u.get(1, 0)); xAccelLimiter = - plant.calculateX( - xAccelLimiter, - new MatBuilder<>(Nat.N2(), Nat.N1()).fill(voltages.left, voltages.right), - dt); + plant.calculateX(xAccelLimiter, VecBuilder.fill(voltages.left, voltages.right), dt); final var accels = plant .getA() .times(xAccelLimiter) - .plus( - plant - .getB() - .times( - new MatBuilder<>(Nat.N2(), Nat.N1()) - .fill(voltages.left, voltages.right))); + .plus(plant.getB().times(VecBuilder.fill(voltages.left, voltages.right))); final double a = (accels.get(0, 0) + accels.get(1, 0)) / 2.0; final double alpha = (accels.get(1, 0) - accels.get(0, 0)) / trackwidth; assertTrue(Math.abs(a) <= maxA); @@ -108,28 +88,20 @@ void testLowLimits() { } // Rotate CCW - u = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(-12.0, 12.0); + u = VecBuilder.fill(-12.0, 12.0); for (double t = 0.0; t < 3.0; t += dt) { x = plant.calculateX(x, u, dt); final var voltages = accelLimiter.calculate( xAccelLimiter.get(0, 0), xAccelLimiter.get(1, 0), u.get(0, 0), u.get(1, 0)); xAccelLimiter = - plant.calculateX( - xAccelLimiter, - new MatBuilder<>(Nat.N2(), Nat.N1()).fill(voltages.left, voltages.right), - dt); + plant.calculateX(xAccelLimiter, VecBuilder.fill(voltages.left, voltages.right), dt); final var accels = plant .getA() .times(xAccelLimiter) - .plus( - plant - .getB() - .times( - new MatBuilder<>(Nat.N2(), Nat.N1()) - .fill(voltages.left, voltages.right))); + .plus(plant.getB().times(VecBuilder.fill(voltages.left, voltages.right))); final double a = (accels.get(0, 0) + accels.get(1, 0)) / 2.0; final double alpha = (accels.get(1, 0) - accels.get(0, 0)) / trackwidth; assertTrue(Math.abs(a) <= maxA); @@ -148,21 +120,18 @@ void testHighLimits() { // unconstrained systems should match var accelLimiter = new DifferentialDriveAccelerationLimiter(plant, trackwidth, 1e3, 1e3); - var x = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(0.0, 0.0); - var xAccelLimiter = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(0.0, 0.0); + Matrix x = VecBuilder.fill(0.0, 0.0); + Matrix xAccelLimiter = VecBuilder.fill(0.0, 0.0); // Forward - var u = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(12.0, 12.0); + var u = VecBuilder.fill(12.0, 12.0); for (double t = 0.0; t < 3.0; t += dt) { x = plant.calculateX(x, u, dt); final var voltages = accelLimiter.calculate( xAccelLimiter.get(0, 0), xAccelLimiter.get(1, 0), u.get(0, 0), u.get(1, 0)); xAccelLimiter = - plant.calculateX( - xAccelLimiter, - new MatBuilder<>(Nat.N2(), Nat.N1()).fill(voltages.left, voltages.right), - dt); + plant.calculateX(xAccelLimiter, VecBuilder.fill(voltages.left, voltages.right), dt); assertEquals(x.get(0, 0), xAccelLimiter.get(0, 0), 1e-5); assertEquals(x.get(1, 0), xAccelLimiter.get(1, 0), 1e-5); @@ -171,17 +140,14 @@ void testHighLimits() { // Backward x.fill(0.0); xAccelLimiter.fill(0.0); - u = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(-12.0, -12.0); + u = VecBuilder.fill(-12.0, -12.0); for (double t = 0.0; t < 3.0; t += dt) { x = plant.calculateX(x, u, dt); final var voltages = accelLimiter.calculate( xAccelLimiter.get(0, 0), xAccelLimiter.get(1, 0), u.get(0, 0), u.get(1, 0)); xAccelLimiter = - plant.calculateX( - xAccelLimiter, - new MatBuilder<>(Nat.N2(), Nat.N1()).fill(voltages.left, voltages.right), - dt); + plant.calculateX(xAccelLimiter, VecBuilder.fill(voltages.left, voltages.right), dt); assertEquals(x.get(0, 0), xAccelLimiter.get(0, 0), 1e-5); assertEquals(x.get(1, 0), xAccelLimiter.get(1, 0), 1e-5); @@ -190,17 +156,14 @@ void testHighLimits() { // Rotate CCW x.fill(0.0); xAccelLimiter.fill(0.0); - u = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(-12.0, 12.0); + u = VecBuilder.fill(-12.0, 12.0); for (double t = 0.0; t < 3.0; t += dt) { x = plant.calculateX(x, u, dt); final var voltages = accelLimiter.calculate( xAccelLimiter.get(0, 0), xAccelLimiter.get(1, 0), u.get(0, 0), u.get(1, 0)); xAccelLimiter = - plant.calculateX( - xAccelLimiter, - new MatBuilder<>(Nat.N2(), Nat.N1()).fill(voltages.left, voltages.right), - dt); + plant.calculateX(xAccelLimiter, VecBuilder.fill(voltages.left, voltages.right), dt); assertEquals(x.get(0, 0), xAccelLimiter.get(0, 0), 1e-5); assertEquals(x.get(1, 0), xAccelLimiter.get(1, 0), 1e-5); @@ -220,16 +183,13 @@ void testSeperateMinMaxLowLimits() { var accelLimiter = new DifferentialDriveAccelerationLimiter(plant, trackwidth, minA, maxA, maxAlpha); - var x = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(0.0, 0.0); - var xAccelLimiter = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(0.0, 0.0); + Matrix x = VecBuilder.fill(0.0, 0.0); + Matrix xAccelLimiter = VecBuilder.fill(0.0, 0.0); // Ensure voltage exceeds acceleration before limiting { final var accels = - plant - .getA() - .times(xAccelLimiter) - .plus(plant.getB().times(new MatBuilder<>(Nat.N2(), Nat.N1()).fill(12.0, 12.0))); + plant.getA().times(xAccelLimiter).plus(plant.getB().times(VecBuilder.fill(12.0, 12.0))); final double a = (accels.get(0, 0) + accels.get(1, 0)) / 2.0; assertTrue(Math.abs(a) > maxA); assertTrue(Math.abs(a) > -minA); @@ -237,55 +197,39 @@ void testSeperateMinMaxLowLimits() { // a should always be within [minA, maxA] // Forward - var u = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(12.0, 12.0); + var u = VecBuilder.fill(12.0, 12.0); for (double t = 0.0; t < 3.0; t += dt) { x = plant.calculateX(x, u, dt); final var voltages = accelLimiter.calculate( xAccelLimiter.get(0, 0), xAccelLimiter.get(1, 0), u.get(0, 0), u.get(1, 0)); xAccelLimiter = - plant.calculateX( - xAccelLimiter, - new MatBuilder<>(Nat.N2(), Nat.N1()).fill(voltages.left, voltages.right), - dt); + plant.calculateX(xAccelLimiter, VecBuilder.fill(voltages.left, voltages.right), dt); final var accels = plant .getA() .times(xAccelLimiter) - .plus( - plant - .getB() - .times( - new MatBuilder<>(Nat.N2(), Nat.N1()) - .fill(voltages.left, voltages.right))); + .plus(plant.getB().times(VecBuilder.fill(voltages.left, voltages.right))); final double a = (accels.get(0, 0) + accels.get(1, 0)) / 2.0; assertTrue(minA <= a && a <= maxA); } // Backward - u = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(-12.0, -12.0); + u = VecBuilder.fill(-12.0, -12.0); for (double t = 0.0; t < 3.0; t += dt) { x = plant.calculateX(x, u, dt); final var voltages = accelLimiter.calculate( xAccelLimiter.get(0, 0), xAccelLimiter.get(1, 0), u.get(0, 0), u.get(1, 0)); xAccelLimiter = - plant.calculateX( - xAccelLimiter, - new MatBuilder<>(Nat.N2(), Nat.N1()).fill(voltages.left, voltages.right), - dt); + plant.calculateX(xAccelLimiter, VecBuilder.fill(voltages.left, voltages.right), dt); final var accels = plant .getA() .times(xAccelLimiter) - .plus( - plant - .getB() - .times( - new MatBuilder<>(Nat.N2(), Nat.N1()) - .fill(voltages.left, voltages.right))); + .plus(plant.getB().times(VecBuilder.fill(voltages.left, voltages.right))); final double a = (accels.get(0, 0) + accels.get(1, 0)) / 2.0; assertTrue(minA <= a && a <= maxA); } diff --git a/wpimath/src/test/java/edu/wpi/first/math/controller/ElevatorFeedforwardTest.java b/wpimath/src/test/java/edu/wpi/first/math/controller/ElevatorFeedforwardTest.java index 6c978ac9868..14993029504 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/controller/ElevatorFeedforwardTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/controller/ElevatorFeedforwardTest.java @@ -6,7 +6,7 @@ import static org.junit.jupiter.api.Assertions.assertEquals; -import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.MatBuilder; import edu.wpi.first.math.Nat; import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.numbers.N1; @@ -27,8 +27,8 @@ void testCalculate() { assertEquals(6.5, m_elevatorFF.calculate(2, 1), 0.002); assertEquals(-0.5, m_elevatorFF.calculate(-2, 1), 0.002); - var A = Matrix.mat(Nat.N1(), Nat.N1()).fill(-kv / ka); - var B = Matrix.mat(Nat.N1(), Nat.N1()).fill(1.0 / ka); + var A = MatBuilder.fill(Nat.N1(), Nat.N1(), -kv / ka); + var B = MatBuilder.fill(Nat.N1(), Nat.N1(), 1.0 / ka); final double dt = 0.02; var plantInversion = new LinearPlantInversionFeedforward(A, B, dt); diff --git a/wpimath/src/test/java/edu/wpi/first/math/controller/ImplicitModelFollowerTest.java b/wpimath/src/test/java/edu/wpi/first/math/controller/ImplicitModelFollowerTest.java index 674cba3dc37..cfcc71d1ee2 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/controller/ImplicitModelFollowerTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/controller/ImplicitModelFollowerTest.java @@ -7,8 +7,9 @@ import static org.junit.jupiter.api.Assertions.assertEquals; import static org.junit.jupiter.api.Assertions.assertTrue; -import edu.wpi.first.math.MatBuilder; -import edu.wpi.first.math.Nat; +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N2; import edu.wpi.first.math.system.plant.LinearSystemId; import org.junit.jupiter.api.Test; @@ -22,11 +23,11 @@ void testSameModel() { var imf = new ImplicitModelFollower(plant, plant); - var x = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(0.0, 0.0); - var xImf = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(0.0, 0.0); + Matrix x = VecBuilder.fill(0.0, 0.0); + Matrix xImf = VecBuilder.fill(0.0, 0.0); // Forward - var u = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(12.0, 12.0); + var u = VecBuilder.fill(12.0, 12.0); for (double t = 0.0; t < 3.0; t += dt) { x = plant.calculateX(x, u, dt); xImf = plant.calculateX(xImf, imf.calculate(xImf, u), dt); @@ -36,7 +37,7 @@ void testSameModel() { } // Backward - u = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(-12.0, -12.0); + u = VecBuilder.fill(-12.0, -12.0); for (double t = 0.0; t < 3.0; t += dt) { x = plant.calculateX(x, u, dt); xImf = plant.calculateX(xImf, imf.calculate(xImf, u), dt); @@ -46,7 +47,7 @@ void testSameModel() { } // Rotate CCW - u = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(-12.0, 12.0); + u = VecBuilder.fill(-12.0, 12.0); for (double t = 0.0; t < 3.0; t += dt) { x = plant.calculateX(x, u, dt); xImf = plant.calculateX(xImf, imf.calculate(xImf, u), dt); @@ -67,11 +68,11 @@ void testSlowerRefModel() { var imf = new ImplicitModelFollower(plant, plantRef); - var x = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(0.0, 0.0); - var xImf = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(0.0, 0.0); + Matrix x = VecBuilder.fill(0.0, 0.0); + Matrix xImf = VecBuilder.fill(0.0, 0.0); // Forward - var u = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(12.0, 12.0); + var u = VecBuilder.fill(12.0, 12.0); for (double t = 0.0; t < 3.0; t += dt) { x = plant.calculateX(x, u, dt); xImf = plant.calculateX(xImf, imf.calculate(xImf, u), dt); @@ -83,7 +84,7 @@ void testSlowerRefModel() { // Backward x.fill(0.0); xImf.fill(0.0); - u = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(-12.0, -12.0); + u = VecBuilder.fill(-12.0, -12.0); for (double t = 0.0; t < 3.0; t += dt) { x = plant.calculateX(x, u, dt); xImf = plant.calculateX(xImf, imf.calculate(xImf, u), dt); @@ -95,7 +96,7 @@ void testSlowerRefModel() { // Rotate CCW x.fill(0.0); xImf.fill(0.0); - u = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(-12.0, 12.0); + u = VecBuilder.fill(-12.0, 12.0); for (double t = 0.0; t < 3.0; t += dt) { x = plant.calculateX(x, u, dt); xImf = plant.calculateX(xImf, imf.calculate(xImf, u), dt); diff --git a/wpimath/src/test/java/edu/wpi/first/math/controller/LTVDifferentialDriveControllerTest.java b/wpimath/src/test/java/edu/wpi/first/math/controller/LTVDifferentialDriveControllerTest.java index efb9c5ead84..e977de87926 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/controller/LTVDifferentialDriveControllerTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/controller/LTVDifferentialDriveControllerTest.java @@ -87,9 +87,14 @@ void testReachesReference() { final var trajectory = TrajectoryGenerator.generateTrajectory(waypoints, config); var x = - new MatBuilder<>(Nat.N5(), Nat.N1()) - .fill( - robotPose.getX(), robotPose.getY(), robotPose.getRotation().getRadians(), 0.0, 0.0); + MatBuilder.fill( + Nat.N5(), + Nat.N1(), + robotPose.getX(), + robotPose.getY(), + robotPose.getRotation().getRadians(), + 0.0, + 0.0); final var totalTime = trajectory.getTotalTimeSeconds(); for (int i = 0; i < (totalTime / kDt); ++i) { @@ -105,7 +110,7 @@ void testReachesReference() { NumericalIntegration.rkdp( LTVDifferentialDriveControllerTest::dynamics, x, - new MatBuilder<>(Nat.N2(), Nat.N1()).fill(output.left, output.right), + VecBuilder.fill(output.left, output.right), kDt); } diff --git a/wpimath/src/test/java/edu/wpi/first/math/controller/LinearPlantInversionFeedforwardTest.java b/wpimath/src/test/java/edu/wpi/first/math/controller/LinearPlantInversionFeedforwardTest.java index 1aba6d79273..eb8d36a503b 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/controller/LinearPlantInversionFeedforwardTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/controller/LinearPlantInversionFeedforwardTest.java @@ -6,6 +6,7 @@ import static org.junit.jupiter.api.Assertions.assertEquals; +import edu.wpi.first.math.MatBuilder; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.Nat; import edu.wpi.first.math.VecBuilder; @@ -16,7 +17,7 @@ class LinearPlantInversionFeedforwardTest { @Test void testCalculate() { - Matrix A = Matrix.mat(Nat.N2(), Nat.N2()).fill(1, 0, 0, 1); + Matrix A = MatBuilder.fill(Nat.N2(), Nat.N2(), 1, 0, 0, 1); Matrix B = VecBuilder.fill(0, 1); LinearPlantInversionFeedforward feedforward = diff --git a/wpimath/src/test/java/edu/wpi/first/math/controller/LinearQuadraticRegulatorTest.java b/wpimath/src/test/java/edu/wpi/first/math/controller/LinearQuadraticRegulatorTest.java index 634ed1cda76..e915388cd3d 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/controller/LinearQuadraticRegulatorTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/controller/LinearQuadraticRegulatorTest.java @@ -6,6 +6,7 @@ import static org.junit.jupiter.api.Assertions.assertEquals; +import edu.wpi.first.math.MatBuilder; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.Nat; import edu.wpi.first.math.Num; @@ -111,10 +112,10 @@ Matrix getImplicitModel @Test void testMatrixOverloadsWithSingleIntegrator() { - var A = Matrix.mat(Nat.N2(), Nat.N2()).fill(0, 0, 0, 0); - var B = Matrix.mat(Nat.N2(), Nat.N2()).fill(1, 0, 0, 1); - var Q = Matrix.mat(Nat.N2(), Nat.N2()).fill(1, 0, 0, 1); - var R = Matrix.mat(Nat.N2(), Nat.N2()).fill(1, 0, 0, 1); + var A = MatBuilder.fill(Nat.N2(), Nat.N2(), 0, 0, 0, 0); + var B = MatBuilder.fill(Nat.N2(), Nat.N2(), 1, 0, 0, 1); + var Q = MatBuilder.fill(Nat.N2(), Nat.N2(), 1, 0, 0, 1); + var R = MatBuilder.fill(Nat.N2(), Nat.N2(), 1, 0, 0, 1); // QR overload var K = new LinearQuadraticRegulator<>(A, B, Q, R, 0.005).getK(); @@ -124,7 +125,7 @@ void testMatrixOverloadsWithSingleIntegrator() { assertEquals(0.99750312499512261, K.get(1, 1), 1e-10); // QRN overload - var N = Matrix.mat(Nat.N2(), Nat.N2()).fill(1, 0, 0, 1); + var N = MatBuilder.fill(Nat.N2(), Nat.N2(), 1, 0, 0, 1); var Kimf = new LinearQuadraticRegulator<>(A, B, Q, R, N, 0.005).getK(); assertEquals(1.0, Kimf.get(0, 0), 1e-10); assertEquals(0.0, Kimf.get(0, 1), 1e-10); @@ -137,10 +138,10 @@ void testMatrixOverloadsWithDoubleIntegrator() { double Kv = 3.02; double Ka = 0.642; - var A = Matrix.mat(Nat.N2(), Nat.N2()).fill(0, 1, 0, -Kv / Ka); - var B = Matrix.mat(Nat.N2(), Nat.N1()).fill(0, 1.0 / Ka); - var Q = Matrix.mat(Nat.N2(), Nat.N2()).fill(1, 0, 0, 0.2); - var R = Matrix.mat(Nat.N1(), Nat.N1()).fill(0.25); + var A = MatBuilder.fill(Nat.N2(), Nat.N2(), 0, 1, 0, -Kv / Ka); + var B = MatBuilder.fill(Nat.N2(), Nat.N1(), 0, 1.0 / Ka); + var Q = MatBuilder.fill(Nat.N2(), Nat.N2(), 1, 0, 0, 0.2); + var R = MatBuilder.fill(Nat.N1(), Nat.N1(), 0.25); // QR overload var K = new LinearQuadraticRegulator<>(A, B, Q, R, 0.005).getK(); @@ -148,7 +149,7 @@ void testMatrixOverloadsWithDoubleIntegrator() { assertEquals(0.51182128351092726, K.get(0, 1), 1e-10); // QRN overload - var Aref = Matrix.mat(Nat.N2(), Nat.N2()).fill(0, 1, 0, -Kv / (Ka * 5.0)); + var Aref = MatBuilder.fill(Nat.N2(), Nat.N2(), 0, 1, 0, -Kv / (Ka * 5.0)); var Kimf = getImplicitModelFollowingK(A, B, Q, R, Aref, 0.005); assertEquals(0.0, Kimf.get(0, 0), 1e-10); assertEquals(-6.9190500116751458e-05, Kimf.get(0, 1), 1e-10); diff --git a/wpimath/src/test/java/edu/wpi/first/math/controller/SimpleMotorFeedforwardTest.java b/wpimath/src/test/java/edu/wpi/first/math/controller/SimpleMotorFeedforwardTest.java index fd4e428877d..e690dafb2c9 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/controller/SimpleMotorFeedforwardTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/controller/SimpleMotorFeedforwardTest.java @@ -6,7 +6,7 @@ import static org.junit.jupiter.api.Assertions.assertEquals; -import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.MatBuilder; import edu.wpi.first.math.Nat; import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.numbers.N1; @@ -20,8 +20,8 @@ void testCalculate() { double Ka = 0.6; double dt = 0.02; - var A = Matrix.mat(Nat.N1(), Nat.N1()).fill(-Kv / Ka); - var B = Matrix.mat(Nat.N1(), Nat.N1()).fill(1.0 / Ka); + var A = MatBuilder.fill(Nat.N1(), Nat.N1(), -Kv / Ka); + var B = MatBuilder.fill(Nat.N1(), Nat.N1(), 1.0 / Ka); var plantInversion = new LinearPlantInversionFeedforward(A, B, dt); var simpleMotor = new SimpleMotorFeedforward(Ks, Kv, Ka); diff --git a/wpimath/src/test/java/edu/wpi/first/math/controller/proto/ArmFeedforwardProtoTest.java b/wpimath/src/test/java/edu/wpi/first/math/controller/proto/ArmFeedforwardProtoTest.java new file mode 100644 index 00000000000..5c036d5e5f4 --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/controller/proto/ArmFeedforwardProtoTest.java @@ -0,0 +1,27 @@ +// 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.math.controller.proto; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import edu.wpi.first.math.controller.ArmFeedforward; +import edu.wpi.first.math.proto.Controller.ProtobufArmFeedforward; +import org.junit.jupiter.api.Test; + +class ArmFeedforwardProtoTest { + private static final ArmFeedforward DATA = new ArmFeedforward(0.174, 0.229, 4.4, 4.4); + + @Test + void testRoundtrip() { + ProtobufArmFeedforward proto = ArmFeedforward.proto.createMessage(); + ArmFeedforward.proto.pack(proto, DATA); + + ArmFeedforward data = ArmFeedforward.proto.unpack(proto); + assertEquals(DATA.ks, data.ks); + assertEquals(DATA.kg, data.kg); + assertEquals(DATA.kv, data.kv); + assertEquals(DATA.ka, data.ka); + } +} diff --git a/wpimath/src/test/java/edu/wpi/first/math/controller/proto/DifferentialDriveWheelVoltagesProtoTest.java b/wpimath/src/test/java/edu/wpi/first/math/controller/proto/DifferentialDriveWheelVoltagesProtoTest.java new file mode 100644 index 00000000000..c3bf58720f8 --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/controller/proto/DifferentialDriveWheelVoltagesProtoTest.java @@ -0,0 +1,27 @@ +// 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.math.controller.proto; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import edu.wpi.first.math.controller.DifferentialDriveWheelVoltages; +import edu.wpi.first.math.proto.Controller.ProtobufDifferentialDriveWheelVoltages; +import org.junit.jupiter.api.Test; + +class DifferentialDriveWheelVoltagesProtoTest { + private static final DifferentialDriveWheelVoltages DATA = + new DifferentialDriveWheelVoltages(0.174, 0.191); + + @Test + void testRoundtrip() { + ProtobufDifferentialDriveWheelVoltages proto = + DifferentialDriveWheelVoltages.proto.createMessage(); + DifferentialDriveWheelVoltages.proto.pack(proto, DATA); + + DifferentialDriveWheelVoltages data = DifferentialDriveWheelVoltages.proto.unpack(proto); + assertEquals(DATA.left, data.left); + assertEquals(DATA.right, data.right); + } +} diff --git a/wpimath/src/test/java/edu/wpi/first/math/controller/proto/ElevatorFeedforwardProtoTest.java b/wpimath/src/test/java/edu/wpi/first/math/controller/proto/ElevatorFeedforwardProtoTest.java new file mode 100644 index 00000000000..e624afff74e --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/controller/proto/ElevatorFeedforwardProtoTest.java @@ -0,0 +1,27 @@ +// 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.math.controller.proto; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import edu.wpi.first.math.controller.ElevatorFeedforward; +import edu.wpi.first.math.proto.Controller.ProtobufElevatorFeedforward; +import org.junit.jupiter.api.Test; + +class ElevatorFeedforwardProtoTest { + private static final ElevatorFeedforward DATA = new ElevatorFeedforward(1.91, 1.1, 1.1, 0.229); + + @Test + void testRoundtrip() { + ProtobufElevatorFeedforward proto = ElevatorFeedforward.proto.createMessage(); + ElevatorFeedforward.proto.pack(proto, DATA); + + ElevatorFeedforward data = ElevatorFeedforward.proto.unpack(proto); + assertEquals(DATA.ks, data.ks); + assertEquals(DATA.kg, data.kg); + assertEquals(DATA.kv, data.kv); + assertEquals(DATA.ka, data.ka); + } +} diff --git a/wpimath/src/test/java/edu/wpi/first/math/controller/struct/ArmFeedforwardStructTest.java b/wpimath/src/test/java/edu/wpi/first/math/controller/struct/ArmFeedforwardStructTest.java new file mode 100644 index 00000000000..42a452dbe7e --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/controller/struct/ArmFeedforwardStructTest.java @@ -0,0 +1,30 @@ +// 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.math.controller.struct; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import edu.wpi.first.math.controller.ArmFeedforward; +import java.nio.ByteBuffer; +import java.nio.ByteOrder; +import org.junit.jupiter.api.Test; + +class ArmFeedforwardStructTest { + private static final ArmFeedforward DATA = new ArmFeedforward(0.174, 0.229, 4.4, 4.4); + + @Test + void testRoundtrip() { + ByteBuffer buffer = ByteBuffer.allocate(ArmFeedforward.struct.getSize()); + buffer.order(ByteOrder.LITTLE_ENDIAN); + ArmFeedforward.struct.pack(buffer, DATA); + buffer.rewind(); + + ArmFeedforward data = ArmFeedforward.struct.unpack(buffer); + assertEquals(DATA.ks, data.ks); + assertEquals(DATA.kg, data.kg); + assertEquals(DATA.kv, data.kv); + assertEquals(DATA.ka, data.ka); + } +} diff --git a/wpimath/src/test/java/edu/wpi/first/math/controller/struct/DifferentialDriveWheelVoltagesStructTest.java b/wpimath/src/test/java/edu/wpi/first/math/controller/struct/DifferentialDriveWheelVoltagesStructTest.java new file mode 100644 index 00000000000..0c6ea66a5fe --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/controller/struct/DifferentialDriveWheelVoltagesStructTest.java @@ -0,0 +1,29 @@ +// 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.math.controller.struct; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import edu.wpi.first.math.controller.DifferentialDriveWheelVoltages; +import java.nio.ByteBuffer; +import java.nio.ByteOrder; +import org.junit.jupiter.api.Test; + +class DifferentialDriveWheelVoltagesStructTest { + private static final DifferentialDriveWheelVoltages DATA = + new DifferentialDriveWheelVoltages(0.174, 0.191); + + @Test + void testRoundtrip() { + ByteBuffer buffer = ByteBuffer.allocate(DifferentialDriveWheelVoltages.struct.getSize()); + buffer.order(ByteOrder.LITTLE_ENDIAN); + DifferentialDriveWheelVoltages.struct.pack(buffer, DATA); + buffer.rewind(); + + DifferentialDriveWheelVoltages data = DifferentialDriveWheelVoltages.struct.unpack(buffer); + assertEquals(DATA.left, data.left); + assertEquals(DATA.right, data.right); + } +} diff --git a/wpimath/src/test/java/edu/wpi/first/math/controller/struct/ElevatorFeedforwardStructTest.java b/wpimath/src/test/java/edu/wpi/first/math/controller/struct/ElevatorFeedforwardStructTest.java new file mode 100644 index 00000000000..a5a83520de5 --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/controller/struct/ElevatorFeedforwardStructTest.java @@ -0,0 +1,30 @@ +// 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.math.controller.struct; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import edu.wpi.first.math.controller.ElevatorFeedforward; +import java.nio.ByteBuffer; +import java.nio.ByteOrder; +import org.junit.jupiter.api.Test; + +class ElevatorFeedforwardStructTest { + private static final ElevatorFeedforward DATA = new ElevatorFeedforward(1.91, 1.1, 1.1, 0.229); + + @Test + void testRoundtrip() { + ByteBuffer buffer = ByteBuffer.allocate(ElevatorFeedforward.struct.getSize()); + buffer.order(ByteOrder.LITTLE_ENDIAN); + ElevatorFeedforward.struct.pack(buffer, DATA); + buffer.rewind(); + + ElevatorFeedforward data = ElevatorFeedforward.struct.unpack(buffer); + assertEquals(DATA.ks, data.ks); + assertEquals(DATA.kg, data.kg); + assertEquals(DATA.kv, data.kv); + assertEquals(DATA.ka, data.ka); + } +} diff --git a/wpimath/src/test/java/edu/wpi/first/math/estimator/AngleStatisticsTest.java b/wpimath/src/test/java/edu/wpi/first/math/estimator/AngleStatisticsTest.java index c36e72f663b..eb30e494cc0 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/estimator/AngleStatisticsTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/estimator/AngleStatisticsTest.java @@ -6,6 +6,7 @@ import static org.junit.jupiter.api.Assertions.assertTrue; +import edu.wpi.first.math.MatBuilder; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.Nat; import edu.wpi.first.math.VecBuilder; @@ -16,7 +17,7 @@ class AngleStatisticsTest { void testMean() { // 3 states, 2 sigmas var sigmas = - Matrix.mat(Nat.N3(), Nat.N2()).fill(1, 1.2, Math.toRadians(359), Math.toRadians(3), 1, 2); + MatBuilder.fill(Nat.N3(), Nat.N2(), 1, 1.2, Math.toRadians(359), Math.toRadians(3), 1, 2); // Weights need to produce the mean of the sigmas var weights = new Matrix<>(Nat.N2(), Nat.N1()); weights.fill(1.0 / sigmas.getNumCols()); diff --git a/wpimath/src/test/java/edu/wpi/first/math/estimator/KalmanFilterTest.java b/wpimath/src/test/java/edu/wpi/first/math/estimator/KalmanFilterTest.java index 848fe9392af..f5f09255688 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/estimator/KalmanFilterTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/estimator/KalmanFilterTest.java @@ -7,6 +7,7 @@ import static org.junit.jupiter.api.Assertions.assertDoesNotThrow; import static org.junit.jupiter.api.Assertions.assertEquals; +import edu.wpi.first.math.MatBuilder; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.Nat; import edu.wpi.first.math.VecBuilder; @@ -47,16 +48,16 @@ private static void createElevator() { // Y is [x, y, theta]ᵀ and u is [ax, ay, alpha}ᵀ LinearSystem m_swerveObserverSystem = new LinearSystem<>( - Matrix.mat(Nat.N6(), Nat.N6()) - .fill( // A - 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0), - Matrix.mat(Nat.N6(), Nat.N3()) - .fill( // B - 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 1), - Matrix.mat(Nat.N3(), Nat.N6()) - .fill( // C - 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0), + MatBuilder.fill( + Nat.N6(), Nat.N6(), // A + 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0), + MatBuilder.fill( + Nat.N6(), Nat.N3(), // B + 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 1), + MatBuilder.fill( + Nat.N3(), Nat.N6(), // C + 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0), new Matrix<>(Nat.N3(), Nat.N3())); // D @Test diff --git a/wpimath/src/test/java/edu/wpi/first/math/estimator/MerweScaledSigmaPointsTest.java b/wpimath/src/test/java/edu/wpi/first/math/estimator/MerweScaledSigmaPointsTest.java index 2131c35d5d5..468eb513601 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/estimator/MerweScaledSigmaPointsTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/estimator/MerweScaledSigmaPointsTest.java @@ -6,7 +6,7 @@ import static org.junit.jupiter.api.Assertions.assertTrue; -import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.MatBuilder; import edu.wpi.first.math.Nat; import edu.wpi.first.math.VecBuilder; import org.junit.jupiter.api.Test; @@ -17,13 +17,23 @@ void testZeroMeanPoints() { var merweScaledSigmaPoints = new MerweScaledSigmaPoints<>(Nat.N2()); var points = merweScaledSigmaPoints.squareRootSigmaPoints( - VecBuilder.fill(0, 0), Matrix.mat(Nat.N2(), Nat.N2()).fill(1, 0, 0, 1)); + VecBuilder.fill(0, 0), MatBuilder.fill(Nat.N2(), Nat.N2(), 1, 0, 0, 1)); assertTrue( points.isEqual( - Matrix.mat(Nat.N2(), Nat.N5()) - .fill( - 0.0, 0.00173205, 0.0, -0.00173205, 0.0, 0.0, 0.0, 0.00173205, 0.0, -0.00173205), + MatBuilder.fill( + Nat.N2(), + Nat.N5(), + 0.0, + 0.00173205, + 0.0, + -0.00173205, + 0.0, + 0.0, + 0.0, + 0.00173205, + 0.0, + -0.00173205), 1E-6)); } @@ -32,12 +42,23 @@ void testNonzeroMeanPoints() { var merweScaledSigmaPoints = new MerweScaledSigmaPoints<>(Nat.N2()); var points = merweScaledSigmaPoints.squareRootSigmaPoints( - VecBuilder.fill(1, 2), Matrix.mat(Nat.N2(), Nat.N2()).fill(1, 0, 0, Math.sqrt(10))); + VecBuilder.fill(1, 2), MatBuilder.fill(Nat.N2(), Nat.N2(), 1, 0, 0, Math.sqrt(10))); assertTrue( points.isEqual( - Matrix.mat(Nat.N2(), Nat.N5()) - .fill(1.0, 1.00173205, 1.0, 0.99826795, 1.0, 2.0, 2.0, 2.00547723, 2.0, 1.99452277), + MatBuilder.fill( + Nat.N2(), + Nat.N5(), + 1.0, + 1.00173205, + 1.0, + 0.99826795, + 1.0, + 2.0, + 2.0, + 2.00547723, + 2.0, + 1.99452277), 1E-6)); } } diff --git a/wpimath/src/test/java/edu/wpi/first/math/estimator/UnscentedKalmanFilterTest.java b/wpimath/src/test/java/edu/wpi/first/math/estimator/UnscentedKalmanFilterTest.java index a8e4d3b25e3..7faa82a3ead 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/estimator/UnscentedKalmanFilterTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/estimator/UnscentedKalmanFilterTest.java @@ -8,6 +8,7 @@ import static org.junit.jupiter.api.Assertions.assertEquals; import static org.junit.jupiter.api.Assertions.assertTrue; +import edu.wpi.first.math.MatBuilder; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.Nat; import edu.wpi.first.math.StateSpaceUtil; @@ -264,7 +265,7 @@ void testRoundTripP() { VecBuilder.fill(0.0, 0.0), dtSeconds); - var P = Matrix.mat(Nat.N2(), Nat.N2()).fill(2.0, 1.0, 1.0, 2.0); + var P = MatBuilder.fill(Nat.N2(), Nat.N2(), 2.0, 1.0, 1.0, 2.0); observer.setP(P); assertTrue(observer.getP().isEqual(P, 1e-9)); 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 d57362d349e..2c683c23ba5 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 @@ -8,12 +8,22 @@ import static org.junit.jupiter.api.Assertions.assertEquals; import static org.junit.jupiter.api.Assertions.assertNotEquals; +import edu.wpi.first.units.Units; import java.util.List; import org.junit.jupiter.api.Test; class Pose2dTest { private static final double kEpsilon = 1E-9; + @Test + void testNewWithMeasures() { + var pose = new Pose2d(Units.Inches.of(6), Units.Inches.of(8), Rotation2d.fromDegrees(45)); + + assertEquals(0.1524, pose.getX(), kEpsilon); + assertEquals(0.2032, pose.getY(), kEpsilon); + assertEquals(Math.PI / 4, pose.getRotation().getRadians(), kEpsilon); + } + @Test void testRotateBy() { final double x = 1.0; diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/Rotation2dTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/Rotation2dTest.java index 6702f1d1863..7f3e469fada 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/geometry/Rotation2dTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/Rotation2dTest.java @@ -8,11 +8,19 @@ import static org.junit.jupiter.api.Assertions.assertEquals; import static org.junit.jupiter.api.Assertions.assertNotEquals; +import edu.wpi.first.units.Units; import org.junit.jupiter.api.Test; class Rotation2dTest { private static final double kEpsilon = 1E-9; + @Test + void testNewWithMeasures() { + var rot = new Rotation2d(Units.Degrees.of(45)); + + assertEquals(Math.PI / 4, rot.getRadians(), kEpsilon); + } + @Test void testRadiansToDegrees() { var rot1 = Rotation2d.fromRadians(Math.PI / 3); diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/Rotation3dTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/Rotation3dTest.java index d80344d93f4..008a65fabff 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/geometry/Rotation3dTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/Rotation3dTest.java @@ -93,8 +93,7 @@ void testInitRotationMatrix() { assertEquals(expected2, rot2); // Matrix that isn't orthogonal - final var R3 = - new MatBuilder<>(Nat.N3(), Nat.N3()).fill(1.0, 0.0, 0.0, 1.0, 0.0, 0.0, 1.0, 0.0, 0.0); + final var R3 = MatBuilder.fill(Nat.N3(), Nat.N3(), 1.0, 0.0, 0.0, 1.0, 0.0, 0.0, 1.0, 0.0, 0.0); assertThrows(IllegalArgumentException.class, () -> new Rotation3d(R3)); // Matrix that's orthogonal but not special orthogonal diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/Transform2dTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/Transform2dTest.java index 93d8a0e9af6..5817f4e2825 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/geometry/Transform2dTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/Transform2dTest.java @@ -6,9 +6,22 @@ import static org.junit.jupiter.api.Assertions.assertEquals; +import edu.wpi.first.units.Units; import org.junit.jupiter.api.Test; class Transform2dTest { + private static final double kEpsilon = 1E-9; + + @Test + void testNewWithMeasures() { + var transform = + new Transform2d(Units.Inches.of(6), Units.Inches.of(8), Rotation2d.fromDegrees(45)); + + assertEquals(0.1524, transform.getX(), kEpsilon); + assertEquals(0.2032, transform.getY(), kEpsilon); + assertEquals(Math.PI / 4, transform.getRotation().getRadians(), kEpsilon); + } + @Test void testInverse() { var initial = new Pose2d(new Translation2d(1.0, 2.0), Rotation2d.fromDegrees(45.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 762425b5437..cd68dbec77d 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 @@ -4,6 +4,7 @@ package edu.wpi.first.math.geometry; +import static edu.wpi.first.units.Units.Inches; import static org.junit.jupiter.api.Assertions.assertAll; import static org.junit.jupiter.api.Assertions.assertEquals; import static org.junit.jupiter.api.Assertions.assertNotEquals; @@ -15,6 +16,15 @@ class Translation3dTest { private static final double kEpsilon = 1E-9; + @Test + void testNewWithMeasures() { + var translation = new Translation3d(Inches.of(6), Inches.of(8), Inches.of(16)); + + assertEquals(0.1524, translation.getX(), kEpsilon); + assertEquals(0.2032, translation.getY(), kEpsilon); + assertEquals(0.4064, translation.getZ(), kEpsilon); + } + @Test void testSum() { var one = new Translation3d(1.0, 3.0, 5.0); diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/proto/Pose2dProtoTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/proto/Pose2dProtoTest.java new file mode 100644 index 00000000000..05ba185cf7a --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/proto/Pose2dProtoTest.java @@ -0,0 +1,28 @@ +// 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.math.geometry.proto; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.proto.Geometry2D.ProtobufPose2d; +import org.junit.jupiter.api.Test; + +class Pose2dProtoTest { + private static final Pose2d DATA = + new Pose2d(new Translation2d(0.191, 2.2), new Rotation2d(22.9)); + + @Test + void testRoundtrip() { + ProtobufPose2d proto = Pose2d.proto.createMessage(); + Pose2d.proto.pack(proto, DATA); + + Pose2d data = Pose2d.proto.unpack(proto); + assertEquals(DATA.getTranslation(), data.getTranslation()); + assertEquals(DATA.getRotation(), data.getRotation()); + } +} diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/proto/Pose3dProtoTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/proto/Pose3dProtoTest.java new file mode 100644 index 00000000000..1043408c6c2 --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/proto/Pose3dProtoTest.java @@ -0,0 +1,31 @@ +// 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.math.geometry.proto; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Quaternion; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.proto.Geometry3D.ProtobufPose3d; +import org.junit.jupiter.api.Test; + +class Pose3dProtoTest { + private static final Pose3d DATA = + new Pose3d( + new Translation3d(1.1, 2.2, 1.1), + new Rotation3d(new Quaternion(1.91, 0.3504, 3.3, 1.74))); + + @Test + void testRoundtrip() { + ProtobufPose3d proto = Pose3d.proto.createMessage(); + Pose3d.proto.pack(proto, DATA); + + Pose3d data = Pose3d.proto.unpack(proto); + assertEquals(DATA.getTranslation(), data.getTranslation()); + assertEquals(DATA.getRotation(), data.getRotation()); + } +} diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/proto/QuaternionProtoTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/proto/QuaternionProtoTest.java new file mode 100644 index 00000000000..9129c19e33e --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/proto/QuaternionProtoTest.java @@ -0,0 +1,27 @@ +// 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.math.geometry.proto; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import edu.wpi.first.math.geometry.Quaternion; +import edu.wpi.first.math.proto.Geometry3D.ProtobufQuaternion; +import org.junit.jupiter.api.Test; + +class QuaternionProtoTest { + private static final Quaternion DATA = new Quaternion(1.1, 0.191, 35.04, 19.1); + + @Test + void testRoundtrip() { + ProtobufQuaternion proto = Quaternion.proto.createMessage(); + Quaternion.proto.pack(proto, DATA); + + Quaternion data = Quaternion.proto.unpack(proto); + assertEquals(DATA.getW(), data.getW()); + assertEquals(DATA.getX(), data.getX()); + assertEquals(DATA.getY(), data.getY()); + assertEquals(DATA.getZ(), data.getZ()); + } +} diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/proto/Rotation2dProtoTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/proto/Rotation2dProtoTest.java new file mode 100644 index 00000000000..6021ec08381 --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/proto/Rotation2dProtoTest.java @@ -0,0 +1,24 @@ +// 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.math.geometry.proto; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.proto.Geometry2D.ProtobufRotation2d; +import org.junit.jupiter.api.Test; + +class Rotation2dProtoTest { + private static final Rotation2d DATA = new Rotation2d(1.91); + + @Test + void testRoundtrip() { + ProtobufRotation2d proto = Rotation2d.proto.createMessage(); + Rotation2d.proto.pack(proto, DATA); + + Rotation2d data = Rotation2d.proto.unpack(proto); + assertEquals(DATA.getRadians(), data.getRadians()); + } +} diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/proto/Rotation3dProtoTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/proto/Rotation3dProtoTest.java new file mode 100644 index 00000000000..5860114bf33 --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/proto/Rotation3dProtoTest.java @@ -0,0 +1,25 @@ +// 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.math.geometry.proto; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import edu.wpi.first.math.geometry.Quaternion; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.proto.Geometry3D.ProtobufRotation3d; +import org.junit.jupiter.api.Test; + +class Rotation3dProtoTest { + private static final Rotation3d DATA = new Rotation3d(new Quaternion(2.29, 0.191, 0.191, 17.4)); + + @Test + void testRoundtrip() { + ProtobufRotation3d proto = Rotation3d.proto.createMessage(); + Rotation3d.proto.pack(proto, DATA); + + Rotation3d data = Rotation3d.proto.unpack(proto); + assertEquals(DATA.getQuaternion(), data.getQuaternion()); + } +} diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/proto/Transform2dProtoTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/proto/Transform2dProtoTest.java new file mode 100644 index 00000000000..78294a105f0 --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/proto/Transform2dProtoTest.java @@ -0,0 +1,28 @@ +// 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.math.geometry.proto; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.proto.Geometry2D.ProtobufTransform2d; +import org.junit.jupiter.api.Test; + +class Transform2dProtoTest { + private static final Transform2d DATA = + new Transform2d(new Translation2d(0.191, 2.2), new Rotation2d(4.4)); + + @Test + void testRoundtrip() { + ProtobufTransform2d proto = Transform2d.proto.createMessage(); + Transform2d.proto.pack(proto, DATA); + + Transform2d data = Transform2d.proto.unpack(proto); + assertEquals(DATA.getTranslation(), data.getTranslation()); + assertEquals(DATA.getRotation(), data.getRotation()); + } +} diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/proto/Transform3dProtoTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/proto/Transform3dProtoTest.java new file mode 100644 index 00000000000..fd72334520a --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/proto/Transform3dProtoTest.java @@ -0,0 +1,31 @@ +// 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.math.geometry.proto; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import edu.wpi.first.math.geometry.Quaternion; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.proto.Geometry3D.ProtobufTransform3d; +import org.junit.jupiter.api.Test; + +class Transform3dProtoTest { + private static final Transform3d DATA = + new Transform3d( + new Translation3d(0.3504, 22.9, 3.504), + new Rotation3d(new Quaternion(0.3504, 35.04, 2.29, 0.3504))); + + @Test + void testRoundtrip() { + ProtobufTransform3d proto = Transform3d.proto.createMessage(); + Transform3d.proto.pack(proto, DATA); + + Transform3d data = Transform3d.proto.unpack(proto); + assertEquals(DATA.getTranslation(), data.getTranslation()); + assertEquals(DATA.getRotation(), data.getRotation()); + } +} diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/proto/Translation2dProtoTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/proto/Translation2dProtoTest.java new file mode 100644 index 00000000000..07b9080194e --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/proto/Translation2dProtoTest.java @@ -0,0 +1,25 @@ +// 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.math.geometry.proto; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.proto.Geometry2D.ProtobufTranslation2d; +import org.junit.jupiter.api.Test; + +class Translation2dProtoTest { + private static final Translation2d DATA = new Translation2d(3.504, 22.9); + + @Test + void testRoundtrip() { + ProtobufTranslation2d proto = Translation2d.proto.createMessage(); + Translation2d.proto.pack(proto, DATA); + + Translation2d data = Translation2d.proto.unpack(proto); + assertEquals(DATA.getX(), data.getX()); + assertEquals(DATA.getY(), data.getY()); + } +} diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/proto/Translation3dProtoTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/proto/Translation3dProtoTest.java new file mode 100644 index 00000000000..d94e1770099 --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/proto/Translation3dProtoTest.java @@ -0,0 +1,26 @@ +// 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.math.geometry.proto; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.proto.Geometry3D.ProtobufTranslation3d; +import org.junit.jupiter.api.Test; + +class Translation3dProtoTest { + private static final Translation3d DATA = new Translation3d(35.04, 22.9, 3.504); + + @Test + void testRoundtrip() { + ProtobufTranslation3d proto = Translation3d.proto.createMessage(); + Translation3d.proto.pack(proto, DATA); + + Translation3d data = Translation3d.proto.unpack(proto); + assertEquals(DATA.getX(), data.getX()); + assertEquals(DATA.getY(), data.getY()); + assertEquals(DATA.getZ(), data.getZ()); + } +} diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/proto/Twist2dProtoTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/proto/Twist2dProtoTest.java new file mode 100644 index 00000000000..e7a0631add0 --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/proto/Twist2dProtoTest.java @@ -0,0 +1,26 @@ +// 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.math.geometry.proto; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import edu.wpi.first.math.geometry.Twist2d; +import edu.wpi.first.math.proto.Geometry2D.ProtobufTwist2d; +import org.junit.jupiter.api.Test; + +class Twist2dProtoTest { + private static final Twist2d DATA = new Twist2d(2.29, 35.04, 35.04); + + @Test + void testRoundtrip() { + ProtobufTwist2d proto = Twist2d.proto.createMessage(); + Twist2d.proto.pack(proto, DATA); + + Twist2d data = Twist2d.proto.unpack(proto); + assertEquals(DATA.dx, data.dx); + assertEquals(DATA.dy, data.dy); + assertEquals(DATA.dtheta, data.dtheta); + } +} diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/proto/Twist3dProtoTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/proto/Twist3dProtoTest.java new file mode 100644 index 00000000000..70e0f079579 --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/proto/Twist3dProtoTest.java @@ -0,0 +1,29 @@ +// 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.math.geometry.proto; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import edu.wpi.first.math.geometry.Twist3d; +import edu.wpi.first.math.proto.Geometry3D.ProtobufTwist3d; +import org.junit.jupiter.api.Test; + +class Twist3dProtoTest { + private static final Twist3d DATA = new Twist3d(1.1, 2.29, 35.04, 0.174, 19.1, 4.4); + + @Test + void testRoundtrip() { + ProtobufTwist3d proto = Twist3d.proto.createMessage(); + Twist3d.proto.pack(proto, DATA); + + Twist3d data = Twist3d.proto.unpack(proto); + assertEquals(DATA.dx, data.dx); + assertEquals(DATA.dy, data.dy); + assertEquals(DATA.dz, data.dz); + assertEquals(DATA.rx, data.rx); + assertEquals(DATA.ry, data.ry); + assertEquals(DATA.rz, data.rz); + } +} diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/struct/Pose2dStructTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/struct/Pose2dStructTest.java new file mode 100644 index 00000000000..0a8c62ab284 --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/struct/Pose2dStructTest.java @@ -0,0 +1,31 @@ +// 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.math.geometry.struct; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import java.nio.ByteBuffer; +import java.nio.ByteOrder; +import org.junit.jupiter.api.Test; + +class Pose2dStructTest { + private static final Pose2d DATA = + new Pose2d(new Translation2d(0.191, 2.2), new Rotation2d(22.9)); + + @Test + void testRoundtrip() { + ByteBuffer buffer = ByteBuffer.allocate(Pose2d.struct.getSize()); + buffer.order(ByteOrder.LITTLE_ENDIAN); + Pose2d.struct.pack(buffer, DATA); + buffer.rewind(); + + Pose2d data = Pose2d.struct.unpack(buffer); + assertEquals(DATA.getTranslation(), data.getTranslation()); + assertEquals(DATA.getRotation(), data.getRotation()); + } +} diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/struct/Pose3dStructTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/struct/Pose3dStructTest.java new file mode 100644 index 00000000000..e2aac4631c4 --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/struct/Pose3dStructTest.java @@ -0,0 +1,34 @@ +// 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.math.geometry.struct; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Quaternion; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Translation3d; +import java.nio.ByteBuffer; +import java.nio.ByteOrder; +import org.junit.jupiter.api.Test; + +class Pose3dStructTest { + private static final Pose3d DATA = + new Pose3d( + new Translation3d(1.1, 2.2, 1.1), + new Rotation3d(new Quaternion(1.91, 0.3504, 3.3, 1.74))); + + @Test + void testRoundtrip() { + ByteBuffer buffer = ByteBuffer.allocate(Pose3d.struct.getSize()); + buffer.order(ByteOrder.LITTLE_ENDIAN); + Pose3d.struct.pack(buffer, DATA); + buffer.rewind(); + + Pose3d data = Pose3d.struct.unpack(buffer); + assertEquals(DATA.getTranslation(), data.getTranslation()); + assertEquals(DATA.getRotation(), data.getRotation()); + } +} diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/struct/QuaternionStructTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/struct/QuaternionStructTest.java new file mode 100644 index 00000000000..41d4604673d --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/struct/QuaternionStructTest.java @@ -0,0 +1,30 @@ +// 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.math.geometry.struct; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import edu.wpi.first.math.geometry.Quaternion; +import java.nio.ByteBuffer; +import java.nio.ByteOrder; +import org.junit.jupiter.api.Test; + +class QuaternionStructTest { + private static final Quaternion DATA = new Quaternion(1.1, 0.191, 35.04, 19.1); + + @Test + void testRoundtrip() { + ByteBuffer buffer = ByteBuffer.allocate(Quaternion.struct.getSize()); + buffer.order(ByteOrder.LITTLE_ENDIAN); + Quaternion.struct.pack(buffer, DATA); + buffer.rewind(); + + Quaternion data = Quaternion.struct.unpack(buffer); + assertEquals(DATA.getW(), data.getW()); + assertEquals(DATA.getX(), data.getX()); + assertEquals(DATA.getY(), data.getY()); + assertEquals(DATA.getZ(), data.getZ()); + } +} diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/struct/Rotation2dStructTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/struct/Rotation2dStructTest.java new file mode 100644 index 00000000000..62569b57e1d --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/struct/Rotation2dStructTest.java @@ -0,0 +1,27 @@ +// 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.math.geometry.struct; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import edu.wpi.first.math.geometry.Rotation2d; +import java.nio.ByteBuffer; +import java.nio.ByteOrder; +import org.junit.jupiter.api.Test; + +class Rotation2dStructTest { + private static final Rotation2d DATA = new Rotation2d(1.91); + + @Test + void testRoundtrip() { + ByteBuffer buffer = ByteBuffer.allocate(Rotation2d.struct.getSize()); + buffer.order(ByteOrder.LITTLE_ENDIAN); + Rotation2d.struct.pack(buffer, DATA); + buffer.rewind(); + + Rotation2d data = Rotation2d.struct.unpack(buffer); + assertEquals(DATA.getRadians(), data.getRadians()); + } +} diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/struct/Rotation3dStructTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/struct/Rotation3dStructTest.java new file mode 100644 index 00000000000..7f275e74e42 --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/struct/Rotation3dStructTest.java @@ -0,0 +1,28 @@ +// 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.math.geometry.struct; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import edu.wpi.first.math.geometry.Quaternion; +import edu.wpi.first.math.geometry.Rotation3d; +import java.nio.ByteBuffer; +import java.nio.ByteOrder; +import org.junit.jupiter.api.Test; + +class Rotation3dStructTest { + private static final Rotation3d DATA = new Rotation3d(new Quaternion(2.29, 0.191, 0.191, 17.4)); + + @Test + void testRoundtrip() { + ByteBuffer buffer = ByteBuffer.allocate(Rotation3d.struct.getSize()); + buffer.order(ByteOrder.LITTLE_ENDIAN); + Rotation3d.struct.pack(buffer, DATA); + buffer.rewind(); + + Rotation3d data = Rotation3d.struct.unpack(buffer); + assertEquals(DATA.getQuaternion(), data.getQuaternion()); + } +} diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/struct/Transform2dStructTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/struct/Transform2dStructTest.java new file mode 100644 index 00000000000..cf583d4ce52 --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/struct/Transform2dStructTest.java @@ -0,0 +1,31 @@ +// 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.math.geometry.struct; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.math.geometry.Translation2d; +import java.nio.ByteBuffer; +import java.nio.ByteOrder; +import org.junit.jupiter.api.Test; + +class Transform2dStructTest { + private static final Transform2d DATA = + new Transform2d(new Translation2d(0.191, 2.2), new Rotation2d(4.4)); + + @Test + void testRoundtrip() { + ByteBuffer buffer = ByteBuffer.allocate(Transform2d.struct.getSize()); + buffer.order(ByteOrder.LITTLE_ENDIAN); + Transform2d.struct.pack(buffer, DATA); + buffer.rewind(); + + Transform2d data = Transform2d.struct.unpack(buffer); + assertEquals(DATA.getTranslation(), data.getTranslation()); + assertEquals(DATA.getRotation(), data.getRotation()); + } +} diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/struct/Transform3dStructTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/struct/Transform3dStructTest.java new file mode 100644 index 00000000000..10a5100d9c8 --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/struct/Transform3dStructTest.java @@ -0,0 +1,34 @@ +// 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.math.geometry.struct; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import edu.wpi.first.math.geometry.Quaternion; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.Translation3d; +import java.nio.ByteBuffer; +import java.nio.ByteOrder; +import org.junit.jupiter.api.Test; + +class Transform3dStructTest { + private static final Transform3d DATA = + new Transform3d( + new Translation3d(0.3504, 22.9, 3.504), + new Rotation3d(new Quaternion(0.3504, 35.04, 2.29, 0.3504))); + + @Test + void testRoundtrip() { + ByteBuffer buffer = ByteBuffer.allocate(Transform3d.struct.getSize()); + buffer.order(ByteOrder.LITTLE_ENDIAN); + Transform3d.struct.pack(buffer, DATA); + buffer.rewind(); + + Transform3d data = Transform3d.struct.unpack(buffer); + assertEquals(DATA.getTranslation(), data.getTranslation()); + assertEquals(DATA.getRotation(), data.getRotation()); + } +} diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/struct/Translation2dStructTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/struct/Translation2dStructTest.java new file mode 100644 index 00000000000..366df176571 --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/struct/Translation2dStructTest.java @@ -0,0 +1,28 @@ +// 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.math.geometry.struct; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import edu.wpi.first.math.geometry.Translation2d; +import java.nio.ByteBuffer; +import java.nio.ByteOrder; +import org.junit.jupiter.api.Test; + +class Translation2dStructTest { + private static final Translation2d DATA = new Translation2d(3.504, 22.9); + + @Test + void testRoundtrip() { + ByteBuffer buffer = ByteBuffer.allocate(Translation2d.struct.getSize()); + buffer.order(ByteOrder.LITTLE_ENDIAN); + Translation2d.struct.pack(buffer, DATA); + buffer.rewind(); + + Translation2d data = Translation2d.struct.unpack(buffer); + assertEquals(DATA.getX(), data.getX()); + assertEquals(DATA.getY(), data.getY()); + } +} diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/struct/Translation3dStructTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/struct/Translation3dStructTest.java new file mode 100644 index 00000000000..fc489819175 --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/struct/Translation3dStructTest.java @@ -0,0 +1,29 @@ +// 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.math.geometry.struct; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import edu.wpi.first.math.geometry.Translation3d; +import java.nio.ByteBuffer; +import java.nio.ByteOrder; +import org.junit.jupiter.api.Test; + +class Translation3dStructTest { + private static final Translation3d DATA = new Translation3d(35.04, 22.9, 3.504); + + @Test + void testRoundtrip() { + ByteBuffer buffer = ByteBuffer.allocate(Translation3d.struct.getSize()); + buffer.order(ByteOrder.LITTLE_ENDIAN); + Translation3d.struct.pack(buffer, DATA); + buffer.rewind(); + + Translation3d data = Translation3d.struct.unpack(buffer); + assertEquals(DATA.getX(), data.getX()); + assertEquals(DATA.getY(), data.getY()); + assertEquals(DATA.getZ(), data.getZ()); + } +} diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/struct/Twist2dStructTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/struct/Twist2dStructTest.java new file mode 100644 index 00000000000..6d4d174c50d --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/struct/Twist2dStructTest.java @@ -0,0 +1,29 @@ +// 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.math.geometry.struct; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import edu.wpi.first.math.geometry.Twist2d; +import java.nio.ByteBuffer; +import java.nio.ByteOrder; +import org.junit.jupiter.api.Test; + +class Twist2dStructTest { + private static final Twist2d DATA = new Twist2d(2.29, 35.04, 35.04); + + @Test + void testRoundtrip() { + ByteBuffer buffer = ByteBuffer.allocate(Twist2d.struct.getSize()); + buffer.order(ByteOrder.LITTLE_ENDIAN); + Twist2d.struct.pack(buffer, DATA); + buffer.rewind(); + + Twist2d data = Twist2d.struct.unpack(buffer); + assertEquals(DATA.dx, data.dx); + assertEquals(DATA.dy, data.dy); + assertEquals(DATA.dtheta, data.dtheta); + } +} diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/struct/Twist3dStructTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/struct/Twist3dStructTest.java new file mode 100644 index 00000000000..131eb2f6ac9 --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/struct/Twist3dStructTest.java @@ -0,0 +1,32 @@ +// 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.math.geometry.struct; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import edu.wpi.first.math.geometry.Twist3d; +import java.nio.ByteBuffer; +import java.nio.ByteOrder; +import org.junit.jupiter.api.Test; + +class Twist3dStructTest { + private static final Twist3d DATA = new Twist3d(1.1, 2.29, 35.04, 0.174, 19.1, 4.4); + + @Test + void testRoundtrip() { + ByteBuffer buffer = ByteBuffer.allocate(Twist3d.struct.getSize()); + buffer.order(ByteOrder.LITTLE_ENDIAN); + Twist3d.struct.pack(buffer, DATA); + buffer.rewind(); + + Twist3d data = Twist3d.struct.unpack(buffer); + assertEquals(DATA.dx, data.dx); + assertEquals(DATA.dy, data.dy); + assertEquals(DATA.dz, data.dz); + assertEquals(DATA.rx, data.rx); + assertEquals(DATA.ry, data.ry); + assertEquals(DATA.rz, data.rz); + } +} diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/ChassisSpeedsTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/ChassisSpeedsTest.java index 24013427bf2..53af4077592 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/kinematics/ChassisSpeedsTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/ChassisSpeedsTest.java @@ -4,6 +4,8 @@ package edu.wpi.first.math.kinematics; +import static edu.wpi.first.units.Units.InchesPerSecond; +import static edu.wpi.first.units.Units.RPM; import static org.junit.jupiter.api.Assertions.assertAll; import static org.junit.jupiter.api.Assertions.assertEquals; @@ -44,6 +46,19 @@ void testDiscretize() { kEpsilon)); } + @Test + void testMeasureConstructor() { + var vx = InchesPerSecond.of(14.52); + var vy = InchesPerSecond.zero(); + var omega = RPM.of(0.02); + var speeds = new ChassisSpeeds(vx, vy, omega); + + assertAll( + () -> assertEquals(0.368808, speeds.vxMetersPerSecond, kEpsilon), + () -> assertEquals(0, speeds.vyMetersPerSecond, kEpsilon), + () -> assertEquals(0.002094395102, speeds.omegaRadiansPerSecond, kEpsilon)); + } + @Test void testFromFieldRelativeSpeeds() { final var chassisSpeeds = diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/proto/ChassisSpeedsProtoTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/proto/ChassisSpeedsProtoTest.java new file mode 100644 index 00000000000..055e8e7d5a8 --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/proto/ChassisSpeedsProtoTest.java @@ -0,0 +1,26 @@ +// 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.math.kinematics.proto; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.proto.Kinematics.ProtobufChassisSpeeds; +import org.junit.jupiter.api.Test; + +class ChassisSpeedsProtoTest { + private static final ChassisSpeeds DATA = new ChassisSpeeds(2.29, 2.2, 0.3504); + + @Test + void testRoundtrip() { + ProtobufChassisSpeeds proto = ChassisSpeeds.proto.createMessage(); + ChassisSpeeds.proto.pack(proto, DATA); + + ChassisSpeeds data = ChassisSpeeds.proto.unpack(proto); + assertEquals(DATA.vxMetersPerSecond, data.vxMetersPerSecond); + assertEquals(DATA.vyMetersPerSecond, data.vyMetersPerSecond); + assertEquals(DATA.omegaRadiansPerSecond, data.omegaRadiansPerSecond); + } +} diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/proto/DifferentialDriveKinematicsProtoTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/proto/DifferentialDriveKinematicsProtoTest.java new file mode 100644 index 00000000000..0d426ad2401 --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/proto/DifferentialDriveKinematicsProtoTest.java @@ -0,0 +1,24 @@ +// 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.math.kinematics.proto; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import edu.wpi.first.math.kinematics.DifferentialDriveKinematics; +import edu.wpi.first.math.proto.Kinematics.ProtobufDifferentialDriveKinematics; +import org.junit.jupiter.api.Test; + +class DifferentialDriveKinematicsProtoTest { + private static final DifferentialDriveKinematics DATA = new DifferentialDriveKinematics(1.74); + + @Test + void testRoundtrip() { + ProtobufDifferentialDriveKinematics proto = DifferentialDriveKinematics.proto.createMessage(); + DifferentialDriveKinematics.proto.pack(proto, DATA); + + DifferentialDriveKinematics data = DifferentialDriveKinematics.proto.unpack(proto); + assertEquals(DATA.trackWidthMeters, data.trackWidthMeters); + } +} diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/proto/DifferentialDriveWheelSpeedsProtoTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/proto/DifferentialDriveWheelSpeedsProtoTest.java new file mode 100644 index 00000000000..1f0b237d3fc --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/proto/DifferentialDriveWheelSpeedsProtoTest.java @@ -0,0 +1,26 @@ +// 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.math.kinematics.proto; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import edu.wpi.first.math.kinematics.DifferentialDriveWheelSpeeds; +import edu.wpi.first.math.proto.Kinematics.ProtobufDifferentialDriveWheelSpeeds; +import org.junit.jupiter.api.Test; + +class DifferentialDriveWheelSpeedsProtoTest { + private static final DifferentialDriveWheelSpeeds DATA = + new DifferentialDriveWheelSpeeds(1.74, 35.04); + + @Test + void testRoundtrip() { + ProtobufDifferentialDriveWheelSpeeds proto = DifferentialDriveWheelSpeeds.proto.createMessage(); + DifferentialDriveWheelSpeeds.proto.pack(proto, DATA); + + DifferentialDriveWheelSpeeds data = DifferentialDriveWheelSpeeds.proto.unpack(proto); + assertEquals(DATA.leftMetersPerSecond, data.leftMetersPerSecond); + assertEquals(DATA.rightMetersPerSecond, data.rightMetersPerSecond); + } +} diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/proto/MecanumDriveKinematicsProtoTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/proto/MecanumDriveKinematicsProtoTest.java new file mode 100644 index 00000000000..a94cfab933e --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/proto/MecanumDriveKinematicsProtoTest.java @@ -0,0 +1,33 @@ +// 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.math.kinematics.proto; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.MecanumDriveKinematics; +import edu.wpi.first.math.proto.Kinematics.ProtobufMecanumDriveKinematics; +import org.junit.jupiter.api.Test; + +class MecanumDriveKinematicsProtoTest { + private static final MecanumDriveKinematics DATA = + new MecanumDriveKinematics( + new Translation2d(19.1, 2.2), + new Translation2d(35.04, 1.91), + new Translation2d(1.74, 3.504), + new Translation2d(3.504, 1.91)); + + @Test + void testRoundtrip() { + ProtobufMecanumDriveKinematics proto = MecanumDriveKinematics.proto.createMessage(); + MecanumDriveKinematics.proto.pack(proto, DATA); + + MecanumDriveKinematics data = MecanumDriveKinematics.proto.unpack(proto); + assertEquals(DATA.getFrontLeft(), data.getFrontLeft()); + assertEquals(DATA.getFrontRight(), data.getFrontRight()); + assertEquals(DATA.getRearLeft(), data.getRearLeft()); + assertEquals(DATA.getRearRight(), data.getRearRight()); + } +} diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/proto/MecanumDriveWheelPositionsProtoTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/proto/MecanumDriveWheelPositionsProtoTest.java new file mode 100644 index 00000000000..3e4551d176e --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/proto/MecanumDriveWheelPositionsProtoTest.java @@ -0,0 +1,28 @@ +// 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.math.kinematics.proto; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import edu.wpi.first.math.kinematics.MecanumDriveWheelPositions; +import edu.wpi.first.math.proto.Kinematics.ProtobufMecanumDriveWheelPositions; +import org.junit.jupiter.api.Test; + +class MecanumDriveWheelPositionsProtoTest { + private static final MecanumDriveWheelPositions DATA = + new MecanumDriveWheelPositions(17.4, 2.29, 22.9, 1.74); + + @Test + void testRoundtrip() { + ProtobufMecanumDriveWheelPositions proto = MecanumDriveWheelPositions.proto.createMessage(); + MecanumDriveWheelPositions.proto.pack(proto, DATA); + + MecanumDriveWheelPositions data = MecanumDriveWheelPositions.proto.unpack(proto); + assertEquals(DATA.frontLeftMeters, data.frontLeftMeters); + assertEquals(DATA.frontRightMeters, data.frontRightMeters); + assertEquals(DATA.rearLeftMeters, data.rearLeftMeters); + assertEquals(DATA.rearRightMeters, data.rearRightMeters); + } +} diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/proto/MecanumDriveWheelSpeedsProtoTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/proto/MecanumDriveWheelSpeedsProtoTest.java new file mode 100644 index 00000000000..3e1d589bcca --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/proto/MecanumDriveWheelSpeedsProtoTest.java @@ -0,0 +1,28 @@ +// 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.math.kinematics.proto; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import edu.wpi.first.math.kinematics.MecanumDriveWheelSpeeds; +import edu.wpi.first.math.proto.Kinematics.ProtobufMecanumDriveWheelSpeeds; +import org.junit.jupiter.api.Test; + +class MecanumDriveWheelSpeedsProtoTest { + private static final MecanumDriveWheelSpeeds DATA = + new MecanumDriveWheelSpeeds(2.29, 17.4, 4.4, 0.229); + + @Test + void testRoundtrip() { + ProtobufMecanumDriveWheelSpeeds proto = MecanumDriveWheelSpeeds.proto.createMessage(); + MecanumDriveWheelSpeeds.proto.pack(proto, DATA); + + MecanumDriveWheelSpeeds data = MecanumDriveWheelSpeeds.proto.unpack(proto); + assertEquals(DATA.frontLeftMetersPerSecond, data.frontLeftMetersPerSecond); + assertEquals(DATA.frontRightMetersPerSecond, data.frontRightMetersPerSecond); + assertEquals(DATA.rearLeftMetersPerSecond, data.rearLeftMetersPerSecond); + assertEquals(DATA.rearRightMetersPerSecond, data.rearRightMetersPerSecond); + } +} diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/proto/SwerveModulePositionProtoTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/proto/SwerveModulePositionProtoTest.java new file mode 100644 index 00000000000..459d11958e9 --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/proto/SwerveModulePositionProtoTest.java @@ -0,0 +1,27 @@ +// 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.math.kinematics.proto; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.math.proto.Kinematics.ProtobufSwerveModulePosition; +import org.junit.jupiter.api.Test; + +class SwerveModulePositionProtoTest { + private static final SwerveModulePosition DATA = + new SwerveModulePosition(3.504, new Rotation2d(17.4)); + + @Test + void testRoundtrip() { + ProtobufSwerveModulePosition proto = SwerveModulePosition.proto.createMessage(); + SwerveModulePosition.proto.pack(proto, DATA); + + SwerveModulePosition data = SwerveModulePosition.proto.unpack(proto); + assertEquals(DATA.distanceMeters, data.distanceMeters); + assertEquals(DATA.angle, data.angle); + } +} diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/proto/SwerveModuleStateProtoTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/proto/SwerveModuleStateProtoTest.java new file mode 100644 index 00000000000..269b7b18b27 --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/proto/SwerveModuleStateProtoTest.java @@ -0,0 +1,26 @@ +// 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.math.kinematics.proto; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.math.proto.Kinematics.ProtobufSwerveModuleState; +import org.junit.jupiter.api.Test; + +class SwerveModuleStateProtoTest { + private static final SwerveModuleState DATA = new SwerveModuleState(22.9, new Rotation2d(3.3)); + + @Test + void testRoundtrip() { + ProtobufSwerveModuleState proto = SwerveModuleState.proto.createMessage(); + SwerveModuleState.proto.pack(proto, DATA); + + SwerveModuleState data = SwerveModuleState.proto.unpack(proto); + assertEquals(DATA.speedMetersPerSecond, data.speedMetersPerSecond); + assertEquals(DATA.angle, data.angle); + } +} diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/struct/ChassisSpeedsStructTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/struct/ChassisSpeedsStructTest.java new file mode 100644 index 00000000000..16ccdb9dfc3 --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/struct/ChassisSpeedsStructTest.java @@ -0,0 +1,29 @@ +// 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.math.kinematics.struct; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import java.nio.ByteBuffer; +import java.nio.ByteOrder; +import org.junit.jupiter.api.Test; + +class ChassisSpeedsStructTest { + private static final ChassisSpeeds DATA = new ChassisSpeeds(2.29, 2.2, 0.3504); + + @Test + void testRoundtrip() { + ByteBuffer buffer = ByteBuffer.allocate(ChassisSpeeds.struct.getSize()); + buffer.order(ByteOrder.LITTLE_ENDIAN); + ChassisSpeeds.struct.pack(buffer, DATA); + buffer.rewind(); + + ChassisSpeeds data = ChassisSpeeds.struct.unpack(buffer); + assertEquals(DATA.vxMetersPerSecond, data.vxMetersPerSecond); + assertEquals(DATA.vyMetersPerSecond, data.vyMetersPerSecond); + assertEquals(DATA.omegaRadiansPerSecond, data.omegaRadiansPerSecond); + } +} diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/struct/DifferentialDriveKinematicsStructTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/struct/DifferentialDriveKinematicsStructTest.java new file mode 100644 index 00000000000..ffcf7e7c1f0 --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/struct/DifferentialDriveKinematicsStructTest.java @@ -0,0 +1,27 @@ +// 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.math.kinematics.struct; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import edu.wpi.first.math.kinematics.DifferentialDriveKinematics; +import java.nio.ByteBuffer; +import java.nio.ByteOrder; +import org.junit.jupiter.api.Test; + +class DifferentialDriveKinematicsStructTest { + private static final DifferentialDriveKinematics DATA = new DifferentialDriveKinematics(1.74); + + @Test + void testRoundtrip() { + ByteBuffer buffer = ByteBuffer.allocate(DifferentialDriveKinematics.struct.getSize()); + buffer.order(ByteOrder.LITTLE_ENDIAN); + DifferentialDriveKinematics.struct.pack(buffer, DATA); + buffer.rewind(); + + DifferentialDriveKinematics data = DifferentialDriveKinematics.struct.unpack(buffer); + assertEquals(DATA.trackWidthMeters, data.trackWidthMeters); + } +} diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/struct/DifferentialDriveWheelSpeedsStructTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/struct/DifferentialDriveWheelSpeedsStructTest.java new file mode 100644 index 00000000000..720f941c064 --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/struct/DifferentialDriveWheelSpeedsStructTest.java @@ -0,0 +1,29 @@ +// 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.math.kinematics.struct; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import edu.wpi.first.math.kinematics.DifferentialDriveWheelSpeeds; +import java.nio.ByteBuffer; +import java.nio.ByteOrder; +import org.junit.jupiter.api.Test; + +class DifferentialDriveWheelSpeedsStructTest { + private static final DifferentialDriveWheelSpeeds DATA = + new DifferentialDriveWheelSpeeds(1.74, 35.04); + + @Test + void testRoundtrip() { + ByteBuffer buffer = ByteBuffer.allocate(DifferentialDriveWheelSpeeds.struct.getSize()); + buffer.order(ByteOrder.LITTLE_ENDIAN); + DifferentialDriveWheelSpeeds.struct.pack(buffer, DATA); + buffer.rewind(); + + DifferentialDriveWheelSpeeds data = DifferentialDriveWheelSpeeds.struct.unpack(buffer); + assertEquals(DATA.leftMetersPerSecond, data.leftMetersPerSecond); + assertEquals(DATA.rightMetersPerSecond, data.rightMetersPerSecond); + } +} diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/struct/MecanumDriveKinematicsStructTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/struct/MecanumDriveKinematicsStructTest.java new file mode 100644 index 00000000000..a8d36f6a208 --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/struct/MecanumDriveKinematicsStructTest.java @@ -0,0 +1,36 @@ +// 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.math.kinematics.struct; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.MecanumDriveKinematics; +import java.nio.ByteBuffer; +import java.nio.ByteOrder; +import org.junit.jupiter.api.Test; + +class MecanumDriveKinematicsStructTest { + private static final MecanumDriveKinematics DATA = + new MecanumDriveKinematics( + new Translation2d(19.1, 2.2), + new Translation2d(35.04, 1.91), + new Translation2d(1.74, 3.504), + new Translation2d(3.504, 1.91)); + + @Test + void testRoundtrip() { + ByteBuffer buffer = ByteBuffer.allocate(MecanumDriveKinematics.struct.getSize()); + buffer.order(ByteOrder.LITTLE_ENDIAN); + MecanumDriveKinematics.struct.pack(buffer, DATA); + buffer.rewind(); + + MecanumDriveKinematics data = MecanumDriveKinematics.struct.unpack(buffer); + assertEquals(DATA.getFrontLeft(), data.getFrontLeft()); + assertEquals(DATA.getFrontRight(), data.getFrontRight()); + assertEquals(DATA.getRearLeft(), data.getRearLeft()); + assertEquals(DATA.getRearRight(), data.getRearRight()); + } +} diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/struct/MecanumDriveWheelPositionsStructTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/struct/MecanumDriveWheelPositionsStructTest.java new file mode 100644 index 00000000000..69f8e034011 --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/struct/MecanumDriveWheelPositionsStructTest.java @@ -0,0 +1,31 @@ +// 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.math.kinematics.struct; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import edu.wpi.first.math.kinematics.MecanumDriveWheelPositions; +import java.nio.ByteBuffer; +import java.nio.ByteOrder; +import org.junit.jupiter.api.Test; + +class MecanumDriveWheelPositionsStructTest { + private static final MecanumDriveWheelPositions DATA = + new MecanumDriveWheelPositions(17.4, 2.29, 22.9, 1.74); + + @Test + void testRoundtrip() { + ByteBuffer buffer = ByteBuffer.allocate(MecanumDriveWheelPositions.struct.getSize()); + buffer.order(ByteOrder.LITTLE_ENDIAN); + MecanumDriveWheelPositions.struct.pack(buffer, DATA); + buffer.rewind(); + + MecanumDriveWheelPositions data = MecanumDriveWheelPositions.struct.unpack(buffer); + assertEquals(DATA.frontLeftMeters, data.frontLeftMeters); + assertEquals(DATA.frontRightMeters, data.frontRightMeters); + assertEquals(DATA.rearLeftMeters, data.rearLeftMeters); + assertEquals(DATA.rearRightMeters, data.rearRightMeters); + } +} diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/struct/MecanumDriveWheelSpeedsStructTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/struct/MecanumDriveWheelSpeedsStructTest.java new file mode 100644 index 00000000000..b962acd80aa --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/struct/MecanumDriveWheelSpeedsStructTest.java @@ -0,0 +1,31 @@ +// 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.math.kinematics.struct; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import edu.wpi.first.math.kinematics.MecanumDriveWheelSpeeds; +import java.nio.ByteBuffer; +import java.nio.ByteOrder; +import org.junit.jupiter.api.Test; + +class MecanumDriveWheelSpeedsStructTest { + private static final MecanumDriveWheelSpeeds DATA = + new MecanumDriveWheelSpeeds(2.29, 17.4, 4.4, 0.229); + + @Test + void testRoundtrip() { + ByteBuffer buffer = ByteBuffer.allocate(MecanumDriveWheelSpeeds.struct.getSize()); + buffer.order(ByteOrder.LITTLE_ENDIAN); + MecanumDriveWheelSpeeds.struct.pack(buffer, DATA); + buffer.rewind(); + + MecanumDriveWheelSpeeds data = MecanumDriveWheelSpeeds.struct.unpack(buffer); + assertEquals(DATA.frontLeftMetersPerSecond, data.frontLeftMetersPerSecond); + assertEquals(DATA.frontRightMetersPerSecond, data.frontRightMetersPerSecond); + assertEquals(DATA.rearLeftMetersPerSecond, data.rearLeftMetersPerSecond); + assertEquals(DATA.rearRightMetersPerSecond, data.rearRightMetersPerSecond); + } +} diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/struct/SwerveModulePositionStructTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/struct/SwerveModulePositionStructTest.java new file mode 100644 index 00000000000..819d98170cf --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/struct/SwerveModulePositionStructTest.java @@ -0,0 +1,30 @@ +// 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.math.kinematics.struct; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import java.nio.ByteBuffer; +import java.nio.ByteOrder; +import org.junit.jupiter.api.Test; + +class SwerveModulePositionStructTest { + private static final SwerveModulePosition DATA = + new SwerveModulePosition(3.504, new Rotation2d(17.4)); + + @Test + void testRoundtrip() { + ByteBuffer buffer = ByteBuffer.allocate(SwerveModulePosition.struct.getSize()); + buffer.order(ByteOrder.LITTLE_ENDIAN); + SwerveModulePosition.struct.pack(buffer, DATA); + buffer.rewind(); + + SwerveModulePosition data = SwerveModulePosition.struct.unpack(buffer); + assertEquals(DATA.distanceMeters, data.distanceMeters); + assertEquals(DATA.angle, data.angle); + } +} diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/struct/SwerveModuleStateStructTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/struct/SwerveModuleStateStructTest.java new file mode 100644 index 00000000000..625514c04b9 --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/struct/SwerveModuleStateStructTest.java @@ -0,0 +1,29 @@ +// 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.math.kinematics.struct; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import java.nio.ByteBuffer; +import java.nio.ByteOrder; +import org.junit.jupiter.api.Test; + +class SwerveModuleStateStructTest { + private static final SwerveModuleState DATA = new SwerveModuleState(22.9, new Rotation2d(3.3)); + + @Test + void testRoundtrip() { + ByteBuffer buffer = ByteBuffer.allocate(SwerveModuleState.struct.getSize()); + buffer.order(ByteOrder.LITTLE_ENDIAN); + SwerveModuleState.struct.pack(buffer, DATA); + buffer.rewind(); + + SwerveModuleState data = SwerveModuleState.struct.unpack(buffer); + assertEquals(DATA.speedMetersPerSecond, data.speedMetersPerSecond); + assertEquals(DATA.angle, data.angle); + } +} diff --git a/wpimath/src/test/java/edu/wpi/first/math/system/DiscretizationTest.java b/wpimath/src/test/java/edu/wpi/first/math/system/DiscretizationTest.java index 7f629331026..830135d2b37 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/system/DiscretizationTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/system/DiscretizationTest.java @@ -19,7 +19,7 @@ class DiscretizationTest { // analytically, @Test void testDiscretizeA() { - final var contA = new MatBuilder<>(Nat.N2(), Nat.N2()).fill(0, 1, 0, 0); + final var contA = MatBuilder.fill(Nat.N2(), Nat.N2(), 0, 1, 0, 0); final var x0 = VecBuilder.fill(1, 1); final var discA = Discretization.discretizeA(contA, 1.0); @@ -37,8 +37,8 @@ void testDiscretizeA() { // analytically, @Test void testDiscretizeAB() { - final var contA = new MatBuilder<>(Nat.N2(), Nat.N2()).fill(0, 1, 0, 0); - final var contB = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(0, 1); + final var contA = MatBuilder.fill(Nat.N2(), Nat.N2(), 0, 1, 0, 0); + final var contB = MatBuilder.fill(Nat.N2(), Nat.N1(), 0, 1); final var x0 = VecBuilder.fill(1, 1); final var u = VecBuilder.fill(1); @@ -63,8 +63,8 @@ void testDiscretizeAB() { // 0 @Test void testDiscretizeSlowModelAQ() { - final var contA = new MatBuilder<>(Nat.N2(), Nat.N2()).fill(0, 1, 0, 0); - final var contQ = new MatBuilder<>(Nat.N2(), Nat.N2()).fill(1, 0, 0, 1); + final var contA = MatBuilder.fill(Nat.N2(), Nat.N2(), 0, 1, 0, 0); + final var contQ = MatBuilder.fill(Nat.N2(), Nat.N2(), 1, 0, 0, 1); final double dt = 1.0; @@ -95,8 +95,8 @@ void testDiscretizeSlowModelAQ() { // 0 @Test void testDiscretizeFastModelAQ() { - final var contA = new MatBuilder<>(Nat.N2(), Nat.N2()).fill(0, 1, 0, -1406.29); - final var contQ = new MatBuilder<>(Nat.N2(), Nat.N2()).fill(0.0025, 0, 0, 1); + final var contA = MatBuilder.fill(Nat.N2(), Nat.N2(), 0, 1, 0, -1406.29); + final var contQ = MatBuilder.fill(Nat.N2(), Nat.N2(), 0.0025, 0, 0, 1); final var dt = 0.005; @@ -125,8 +125,8 @@ void testDiscretizeFastModelAQ() { // Test that DiscretizeR() works @Test void testDiscretizeR() { - var contR = Matrix.mat(Nat.N2(), Nat.N2()).fill(2.0, 0.0, 0.0, 1.0); - var discRTruth = Matrix.mat(Nat.N2(), Nat.N2()).fill(4.0, 0.0, 0.0, 2.0); + var contR = MatBuilder.fill(Nat.N2(), Nat.N2(), 2.0, 0.0, 0.0, 1.0); + var discRTruth = MatBuilder.fill(Nat.N2(), Nat.N2(), 4.0, 0.0, 0.0, 2.0); var discR = Discretization.discretizeR(contR, 0.5); diff --git a/wpimath/src/test/java/edu/wpi/first/math/system/LinearSystemIDTest.java b/wpimath/src/test/java/edu/wpi/first/math/system/LinearSystemIDTest.java index 3f316b3ba5e..0c2ca235f5d 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/system/LinearSystemIDTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/system/LinearSystemIDTest.java @@ -7,6 +7,7 @@ import static org.junit.jupiter.api.Assertions.assertEquals; import static org.junit.jupiter.api.Assertions.assertTrue; +import edu.wpi.first.math.MatBuilder; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.Nat; import edu.wpi.first.math.VecBuilder; @@ -23,31 +24,31 @@ void testDrivetrainVelocitySystem() { model .getA() .isEqual( - Matrix.mat(Nat.N2(), Nat.N2()).fill(-10.14132, 3.06598, 3.06598, -10.14132), + MatBuilder.fill(Nat.N2(), Nat.N2(), -10.14132, 3.06598, 3.06598, -10.14132), 0.001)); assertTrue( model .getB() .isEqual( - Matrix.mat(Nat.N2(), Nat.N2()).fill(4.2590, -1.28762, -1.2876, 4.2590), 0.001)); + MatBuilder.fill(Nat.N2(), Nat.N2(), 4.2590, -1.28762, -1.2876, 4.2590), 0.001)); assertTrue( - model.getC().isEqual(Matrix.mat(Nat.N2(), Nat.N2()).fill(1.0, 0.0, 0.0, 1.0), 0.001)); + model.getC().isEqual(MatBuilder.fill(Nat.N2(), Nat.N2(), 1.0, 0.0, 0.0, 1.0), 0.001)); assertTrue( - model.getD().isEqual(Matrix.mat(Nat.N2(), Nat.N2()).fill(0.0, 0.0, 0.0, 0.0), 0.001)); + model.getD().isEqual(MatBuilder.fill(Nat.N2(), Nat.N2(), 0.0, 0.0, 0.0, 0.0), 0.001)); } @Test void testElevatorSystem() { var model = LinearSystemId.createElevatorSystem(DCMotor.getNEO(2), 5, 0.05, 12); assertTrue( - model.getA().isEqual(Matrix.mat(Nat.N2(), Nat.N2()).fill(0, 1, 0, -99.05473), 0.001)); + model.getA().isEqual(MatBuilder.fill(Nat.N2(), Nat.N2(), 0, 1, 0, -99.05473), 0.001)); assertTrue(model.getB().isEqual(VecBuilder.fill(0, 20.8), 0.001)); - assertTrue(model.getC().isEqual(Matrix.mat(Nat.N1(), Nat.N2()).fill(1, 0), 0.001)); + assertTrue(model.getC().isEqual(MatBuilder.fill(Nat.N1(), Nat.N2(), 1, 0), 0.001)); assertTrue(model.getD().isEqual(VecBuilder.fill(0), 0.001)); } @@ -68,7 +69,7 @@ void testFlywheelSystem() { void testDCMotorSystem() { var model = LinearSystemId.createDCMotorSystem(DCMotor.getNEO(2), 0.00032, 1.0); assertTrue( - model.getA().isEqual(Matrix.mat(Nat.N2(), Nat.N2()).fill(0, 1, 0, -26.87032), 0.001)); + model.getA().isEqual(MatBuilder.fill(Nat.N2(), Nat.N2(), 0, 1, 0, -26.87032), 0.001)); assertTrue(model.getB().isEqual(VecBuilder.fill(0, 1354.166667), 0.001)); @@ -85,7 +86,7 @@ void testIdentifyPositionSystem() { var ka = 0.5; var model = LinearSystemId.identifyPositionSystem(kv, ka); - assertEquals(model.getA(), Matrix.mat(Nat.N2(), Nat.N2()).fill(0, 1, 0, -kv / ka)); + assertEquals(model.getA(), MatBuilder.fill(Nat.N2(), Nat.N2(), 0, 1, 0, -kv / ka)); assertEquals(model.getB(), VecBuilder.fill(0, 1 / ka)); } diff --git a/wpimath/src/test/java/edu/wpi/first/math/system/RungeKuttaTimeVaryingTest.java b/wpimath/src/test/java/edu/wpi/first/math/system/RungeKuttaTimeVaryingTest.java index 3db878e8e37..e0757eafe8b 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/system/RungeKuttaTimeVaryingTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/system/RungeKuttaTimeVaryingTest.java @@ -14,8 +14,8 @@ class RungeKuttaTimeVaryingTest { private static Matrix rungeKuttaTimeVaryingSolution(double t) { - return new MatBuilder<>(Nat.N1(), Nat.N1()) - .fill(12.0 * Math.exp(t) / Math.pow(Math.exp(t) + 1.0, 2.0)); + return MatBuilder.fill( + Nat.N1(), Nat.N1(), 12.0 * Math.exp(t) / Math.pow(Math.exp(t) + 1.0, 2.0)); } // Tests RK4 with a time varying solution. From @@ -32,8 +32,8 @@ void rungeKuttaTimeVaryingTest() { final var y1 = RungeKuttaTimeVarying.rungeKuttaTimeVarying( (Double t, Matrix x) -> { - return new MatBuilder<>(Nat.N1(), Nat.N1()) - .fill(x.get(0, 0) * (2.0 / (Math.exp(t) + 1.0) - 1.0)); + return MatBuilder.fill( + Nat.N1(), Nat.N1(), x.get(0, 0) * (2.0 / (Math.exp(t) + 1.0) - 1.0)); }, 5.0, y0, diff --git a/wpimath/src/test/java/edu/wpi/first/math/system/plant/proto/DCMotorProtoTest.java b/wpimath/src/test/java/edu/wpi/first/math/system/plant/proto/DCMotorProtoTest.java new file mode 100644 index 00000000000..3a6e64a0db8 --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/system/plant/proto/DCMotorProtoTest.java @@ -0,0 +1,31 @@ +// 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.math.system.plant.proto; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import edu.wpi.first.math.proto.Plant.ProtobufDCMotor; +import edu.wpi.first.math.system.plant.DCMotor; +import org.junit.jupiter.api.Test; + +class DCMotorProtoTest { + private static final DCMotor DATA = new DCMotor(1.91, 19.1, 1.74, 1.74, 22.9, 3); + + @Test + void testRoundtrip() { + ProtobufDCMotor proto = DCMotor.proto.createMessage(); + DCMotor.proto.pack(proto, DATA); + + DCMotor data = DCMotor.proto.unpack(proto); + assertEquals(DATA.nominalVoltageVolts, data.nominalVoltageVolts); + assertEquals(DATA.stallTorqueNewtonMeters, data.stallTorqueNewtonMeters); + assertEquals(DATA.stallCurrentAmps, data.stallCurrentAmps); + assertEquals(DATA.freeCurrentAmps, data.freeCurrentAmps); + assertEquals(DATA.freeSpeedRadPerSec, data.freeSpeedRadPerSec); + assertEquals(DATA.rOhms, data.rOhms); + assertEquals(DATA.KvRadPerSecPerVolt, data.KvRadPerSecPerVolt); + assertEquals(DATA.KtNMPerAmp, data.KtNMPerAmp); + } +} diff --git a/wpimath/src/test/java/edu/wpi/first/math/system/plant/struct/DCMotorStructTest.java b/wpimath/src/test/java/edu/wpi/first/math/system/plant/struct/DCMotorStructTest.java new file mode 100644 index 00000000000..806254dc23c --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/system/plant/struct/DCMotorStructTest.java @@ -0,0 +1,34 @@ +// 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.math.system.plant.struct; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import edu.wpi.first.math.system.plant.DCMotor; +import java.nio.ByteBuffer; +import java.nio.ByteOrder; +import org.junit.jupiter.api.Test; + +class DCMotorStructTest { + private static final DCMotor DATA = new DCMotor(1.91, 19.1, 1.74, 1.74, 22.9, 3); + + @Test + void testRoundtrip() { + ByteBuffer buffer = ByteBuffer.allocate(DCMotor.struct.getSize()); + buffer.order(ByteOrder.LITTLE_ENDIAN); + DCMotor.struct.pack(buffer, DATA); + buffer.rewind(); + + DCMotor data = DCMotor.struct.unpack(buffer); + assertEquals(DATA.nominalVoltageVolts, data.nominalVoltageVolts); + assertEquals(DATA.stallTorqueNewtonMeters, data.stallTorqueNewtonMeters); + assertEquals(DATA.stallCurrentAmps, data.stallCurrentAmps); + assertEquals(DATA.freeCurrentAmps, data.freeCurrentAmps); + assertEquals(DATA.freeSpeedRadPerSec, data.freeSpeedRadPerSec); + assertEquals(DATA.rOhms, data.rOhms); + assertEquals(DATA.KvRadPerSecPerVolt, data.KvRadPerSecPerVolt); + assertEquals(DATA.KtNMPerAmp, data.KtNMPerAmp); + } +} diff --git a/wpimath/src/test/java/edu/wpi/first/math/trajectory/proto/TrajectoryProtoTest.java b/wpimath/src/test/java/edu/wpi/first/math/trajectory/proto/TrajectoryProtoTest.java new file mode 100644 index 00000000000..0477bb7dcfe --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/trajectory/proto/TrajectoryProtoTest.java @@ -0,0 +1,40 @@ +// 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.math.trajectory.proto; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.proto.Trajectory.ProtobufTrajectory; +import edu.wpi.first.math.trajectory.Trajectory; +import java.util.List; +import org.junit.jupiter.api.Test; + +class TrajectoryProtoTest { + private static final Trajectory DATA = + new Trajectory( + List.of( + new Trajectory.State( + 1.1, 2.2, 3.3, new Pose2d(new Translation2d(1.1, 2.2), new Rotation2d(2.2)), 6.6), + new Trajectory.State( + 2.1, 2.2, 3.3, new Pose2d(new Translation2d(2.1, 2.2), new Rotation2d(2.2)), 6.6), + new Trajectory.State( + 3.1, + 2.2, + 3.3, + new Pose2d(new Translation2d(3.1, 2.2), new Rotation2d(2.2)), + 6.6))); + + @Test + void testRoundtrip() { + ProtobufTrajectory proto = Trajectory.proto.createMessage(); + Trajectory.proto.pack(proto, DATA); + + Trajectory data = Trajectory.proto.unpack(proto); + assertEquals(DATA.getStates(), data.getStates()); + } +} diff --git a/wpimath/src/test/java/edu/wpi/first/math/trajectory/proto/TrajectoryStateProtoTest.java b/wpimath/src/test/java/edu/wpi/first/math/trajectory/proto/TrajectoryStateProtoTest.java new file mode 100644 index 00000000000..8a9fdae5ac9 --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/trajectory/proto/TrajectoryStateProtoTest.java @@ -0,0 +1,33 @@ +// 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.math.trajectory.proto; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.proto.Trajectory.ProtobufTrajectoryState; +import edu.wpi.first.math.trajectory.Trajectory; +import org.junit.jupiter.api.Test; + +class TrajectoryStateProtoTest { + private static final Trajectory.State DATA = + new Trajectory.State( + 1.91, 4.4, 17.4, new Pose2d(new Translation2d(1.74, 19.1), new Rotation2d(22.9)), 0.174); + + @Test + void testRoundtrip() { + ProtobufTrajectoryState proto = Trajectory.State.proto.createMessage(); + Trajectory.State.proto.pack(proto, DATA); + + Trajectory.State data = Trajectory.State.proto.unpack(proto); + assertEquals(DATA.timeSeconds, data.timeSeconds); + assertEquals(DATA.velocityMetersPerSecond, data.velocityMetersPerSecond); + assertEquals(DATA.accelerationMetersPerSecondSq, data.accelerationMetersPerSecondSq); + assertEquals(DATA.poseMeters, data.poseMeters); + assertEquals(DATA.curvatureRadPerMeter, data.curvatureRadPerMeter); + } +} diff --git a/wpimath/src/test/native/cpp/controller/proto/ArmFeedforwardProtoTest.cpp b/wpimath/src/test/native/cpp/controller/proto/ArmFeedforwardProtoTest.cpp new file mode 100644 index 00000000000..b0e57c78d29 --- /dev/null +++ b/wpimath/src/test/native/cpp/controller/proto/ArmFeedforwardProtoTest.cpp @@ -0,0 +1,33 @@ +// 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. + +#include + +#include "controller.pb.h" +#include "frc/controller/ArmFeedforward.h" + +using namespace frc; + +namespace { + +using ProtoType = wpi::Protobuf; + +static constexpr auto Ks = 1.91_V; +static constexpr auto Kg = 2.29_V; +static constexpr auto Kv = 35.04_V * 1_s / 1_rad; +static constexpr auto Ka = 1.74_V * 1_s * 1_s / 1_rad; +const ArmFeedforward kExpectedData{Ks, Kg, Kv, Ka}; +} // namespace + +TEST(ArmFeedforwardProtoTest, Roundtrip) { + google::protobuf::Arena arena; + google::protobuf::Message* proto = ProtoType::New(&arena); + ProtoType::Pack(proto, kExpectedData); + + ArmFeedforward unpacked_data = ProtoType::Unpack(*proto); + EXPECT_EQ(kExpectedData.kS.value(), unpacked_data.kS.value()); + EXPECT_EQ(kExpectedData.kG.value(), unpacked_data.kG.value()); + EXPECT_EQ(kExpectedData.kV.value(), unpacked_data.kV.value()); + EXPECT_EQ(kExpectedData.kA.value(), unpacked_data.kA.value()); +} diff --git a/wpimath/src/test/native/cpp/controller/proto/DifferentialDriveWheelVoltagesProtoTest.cpp b/wpimath/src/test/native/cpp/controller/proto/DifferentialDriveWheelVoltagesProtoTest.cpp new file mode 100644 index 00000000000..1adad90fd76 --- /dev/null +++ b/wpimath/src/test/native/cpp/controller/proto/DifferentialDriveWheelVoltagesProtoTest.cpp @@ -0,0 +1,28 @@ +// 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. + +#include + +#include "controller.pb.h" +#include "frc/controller/DifferentialDriveWheelVoltages.h" + +using namespace frc; + +namespace { + +using ProtoType = wpi::Protobuf; + +const DifferentialDriveWheelVoltages kExpectedData = + DifferentialDriveWheelVoltages{0.174_V, 0.191_V}; +} // namespace + +TEST(DifferentialDriveWheelVoltagesProtoTest, Roundtrip) { + google::protobuf::Arena arena; + google::protobuf::Message* proto = ProtoType::New(&arena); + ProtoType::Pack(proto, kExpectedData); + + DifferentialDriveWheelVoltages unpacked_data = ProtoType::Unpack(*proto); + EXPECT_EQ(kExpectedData.left.value(), unpacked_data.left.value()); + EXPECT_EQ(kExpectedData.right.value(), unpacked_data.right.value()); +} diff --git a/wpimath/src/test/native/cpp/controller/proto/ElevatorFeedforwardProtoTest.cpp b/wpimath/src/test/native/cpp/controller/proto/ElevatorFeedforwardProtoTest.cpp new file mode 100644 index 00000000000..115a3dcf8b6 --- /dev/null +++ b/wpimath/src/test/native/cpp/controller/proto/ElevatorFeedforwardProtoTest.cpp @@ -0,0 +1,34 @@ +// 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. + +#include + +#include "controller.pb.h" +#include "frc/controller/ElevatorFeedforward.h" + +using namespace frc; + +namespace { + +using ProtoType = wpi::Protobuf; + +static constexpr auto Ks = 1.91_V; +static constexpr auto Kg = 2.29_V; +static constexpr auto Kv = 35.04_V * 1_s / 1_m; +static constexpr auto Ka = 1.74_V * 1_s * 1_s / 1_m; + +constexpr ElevatorFeedforward kExpectedData{Ks, Kg, Kv, Ka}; +} // namespace + +TEST(ElevatorFeedforwardProtoTest, Roundtrip) { + google::protobuf::Arena arena; + google::protobuf::Message* proto = ProtoType::New(&arena); + ProtoType::Pack(proto, kExpectedData); + + ElevatorFeedforward unpacked_data = ProtoType::Unpack(*proto); + EXPECT_EQ(kExpectedData.kS.value(), unpacked_data.kS.value()); + EXPECT_EQ(kExpectedData.kG.value(), unpacked_data.kG.value()); + EXPECT_EQ(kExpectedData.kV.value(), unpacked_data.kV.value()); + EXPECT_EQ(kExpectedData.kA.value(), unpacked_data.kA.value()); +} diff --git a/wpimath/src/test/native/cpp/controller/struct/ArmFeedforwardStructTest.cpp b/wpimath/src/test/native/cpp/controller/struct/ArmFeedforwardStructTest.cpp new file mode 100644 index 00000000000..453061c56e3 --- /dev/null +++ b/wpimath/src/test/native/cpp/controller/struct/ArmFeedforwardStructTest.cpp @@ -0,0 +1,33 @@ +// 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. + +#include + +#include "frc/controller/ArmFeedforward.h" + +using namespace frc; + +namespace { + +using StructType = wpi::Struct; + +static constexpr auto Ks = 1.91_V; +static constexpr auto Kg = 2.29_V; +static constexpr auto Kv = 35.04_V * 1_s / 1_rad; +static constexpr auto Ka = 1.74_V * 1_s * 1_s / 1_rad; +const ArmFeedforward kExpectedData{Ks, Kg, Kv, Ka}; +} // namespace + +TEST(ArmFeedforwardStructTest, Roundtrip) { + uint8_t buffer[StructType::kSize]; + std::memset(buffer, 0, StructType::kSize); + StructType::Pack(buffer, kExpectedData); + + ArmFeedforward unpacked_data = StructType::Unpack(buffer); + + EXPECT_EQ(kExpectedData.kS.value(), unpacked_data.kS.value()); + EXPECT_EQ(kExpectedData.kG.value(), unpacked_data.kG.value()); + EXPECT_EQ(kExpectedData.kV.value(), unpacked_data.kV.value()); + EXPECT_EQ(kExpectedData.kA.value(), unpacked_data.kA.value()); +} diff --git a/wpimath/src/test/native/cpp/controller/struct/DifferentialDriveWheelVoltagesStructTest.cpp b/wpimath/src/test/native/cpp/controller/struct/DifferentialDriveWheelVoltagesStructTest.cpp new file mode 100644 index 00000000000..2b47e5a5383 --- /dev/null +++ b/wpimath/src/test/native/cpp/controller/struct/DifferentialDriveWheelVoltagesStructTest.cpp @@ -0,0 +1,27 @@ +// 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. + +#include + +#include "frc/controller/DifferentialDriveWheelVoltages.h" + +using namespace frc; + +namespace { + +using StructType = wpi::Struct; +const DifferentialDriveWheelVoltages kExpectedData{ + DifferentialDriveWheelVoltages{0.174_V, 0.191_V}}; +} // namespace + +TEST(DifferentialDriveWheelVoltagesStructTest, Roundtrip) { + uint8_t buffer[StructType::kSize]; + std::memset(buffer, 0, StructType::kSize); + StructType::Pack(buffer, kExpectedData); + + DifferentialDriveWheelVoltages unpacked_data = StructType::Unpack(buffer); + + EXPECT_EQ(kExpectedData.left.value(), unpacked_data.left.value()); + EXPECT_EQ(kExpectedData.right.value(), unpacked_data.right.value()); +} diff --git a/wpimath/src/test/native/cpp/controller/struct/ElevatorFeedforwardStructTest.cpp b/wpimath/src/test/native/cpp/controller/struct/ElevatorFeedforwardStructTest.cpp new file mode 100644 index 00000000000..299d39a34be --- /dev/null +++ b/wpimath/src/test/native/cpp/controller/struct/ElevatorFeedforwardStructTest.cpp @@ -0,0 +1,34 @@ +// 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. + +#include + +#include "frc/controller/ElevatorFeedforward.h" + +using namespace frc; + +namespace { + +using StructType = wpi::Struct; + +static constexpr auto Ks = 1.91_V; +static constexpr auto Kg = 2.29_V; +static constexpr auto Kv = 35.04_V * 1_s / 1_m; +static constexpr auto Ka = 1.74_V * 1_s * 1_s / 1_m; + +constexpr ElevatorFeedforward kExpectedData{Ks, Kg, Kv, Ka}; +} // namespace + +TEST(ElevatorFeedforwardStructTest, Roundtrip) { + uint8_t buffer[StructType::kSize]; + std::memset(buffer, 0, StructType::kSize); + StructType::Pack(buffer, kExpectedData); + + ElevatorFeedforward unpacked_data = StructType::Unpack(buffer); + + EXPECT_EQ(kExpectedData.kS.value(), unpacked_data.kS.value()); + EXPECT_EQ(kExpectedData.kG.value(), unpacked_data.kG.value()); + EXPECT_EQ(kExpectedData.kV.value(), unpacked_data.kV.value()); + EXPECT_EQ(kExpectedData.kA.value(), unpacked_data.kA.value()); +} diff --git a/wpimath/src/test/native/cpp/geometry/proto/Pose2dProtoTest.cpp b/wpimath/src/test/native/cpp/geometry/proto/Pose2dProtoTest.cpp new file mode 100644 index 00000000000..c6e9d49ec5c --- /dev/null +++ b/wpimath/src/test/native/cpp/geometry/proto/Pose2dProtoTest.cpp @@ -0,0 +1,28 @@ +// 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. + +#include + +#include "frc/geometry/Pose2d.h" +#include "geometry2d.pb.h" + +using namespace frc; + +namespace { + +using ProtoType = wpi::Protobuf; + +const Pose2d kExpectedData = + Pose2d{Translation2d{0.191_m, 2.2_m}, Rotation2d{22.9_rad}}; +} // namespace + +TEST(Pose2dProtoTest, Roundtrip) { + google::protobuf::Arena arena; + google::protobuf::Message* proto = ProtoType::New(&arena); + ProtoType::Pack(proto, kExpectedData); + + Pose2d unpacked_data = ProtoType::Unpack(*proto); + EXPECT_EQ(kExpectedData.Translation(), unpacked_data.Translation()); + EXPECT_EQ(kExpectedData.Rotation(), unpacked_data.Rotation()); +} diff --git a/wpimath/src/test/native/cpp/geometry/proto/Pose3dProtoTest.cpp b/wpimath/src/test/native/cpp/geometry/proto/Pose3dProtoTest.cpp new file mode 100644 index 00000000000..d8847cab88e --- /dev/null +++ b/wpimath/src/test/native/cpp/geometry/proto/Pose3dProtoTest.cpp @@ -0,0 +1,29 @@ +// 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. + +#include + +#include "frc/geometry/Pose3d.h" +#include "geometry3d.pb.h" + +using namespace frc; + +namespace { + +using ProtoType = wpi::Protobuf; + +const Pose3d kExpectedData = + Pose3d{Translation3d{1.1_m, 2.2_m, 1.1_m}, + Rotation3d{Quaternion{1.91, 0.3504, 3.3, 1.74}}}; +} // namespace + +TEST(Pose3dProtoTest, Roundtrip) { + google::protobuf::Arena arena; + google::protobuf::Message* proto = ProtoType::New(&arena); + ProtoType::Pack(proto, kExpectedData); + + Pose3d unpacked_data = ProtoType::Unpack(*proto); + EXPECT_EQ(kExpectedData.Translation(), unpacked_data.Translation()); + EXPECT_EQ(kExpectedData.Rotation(), unpacked_data.Rotation()); +} diff --git a/wpimath/src/test/native/cpp/geometry/proto/QuaternionProtoTest.cpp b/wpimath/src/test/native/cpp/geometry/proto/QuaternionProtoTest.cpp new file mode 100644 index 00000000000..92338df8327 --- /dev/null +++ b/wpimath/src/test/native/cpp/geometry/proto/QuaternionProtoTest.cpp @@ -0,0 +1,29 @@ +// 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. + +#include + +#include "frc/geometry/Quaternion.h" +#include "geometry3d.pb.h" + +using namespace frc; + +namespace { + +using ProtoType = wpi::Protobuf; + +const Quaternion kExpectedData = Quaternion{1.1, 0.191, 35.04, 19.1}; +} // namespace + +TEST(QuaternionProtoTest, Roundtrip) { + google::protobuf::Arena arena; + google::protobuf::Message* proto = ProtoType::New(&arena); + ProtoType::Pack(proto, kExpectedData); + + Quaternion unpacked_data = ProtoType::Unpack(*proto); + EXPECT_EQ(kExpectedData.W(), unpacked_data.W()); + EXPECT_EQ(kExpectedData.X(), unpacked_data.X()); + EXPECT_EQ(kExpectedData.Y(), unpacked_data.Y()); + EXPECT_EQ(kExpectedData.Z(), unpacked_data.Z()); +} diff --git a/wpimath/src/test/native/cpp/geometry/proto/Rotation2dProtoTest.cpp b/wpimath/src/test/native/cpp/geometry/proto/Rotation2dProtoTest.cpp new file mode 100644 index 00000000000..3ab9e1bb26f --- /dev/null +++ b/wpimath/src/test/native/cpp/geometry/proto/Rotation2dProtoTest.cpp @@ -0,0 +1,26 @@ +// 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. + +#include + +#include "frc/geometry/Rotation2d.h" +#include "geometry2d.pb.h" + +using namespace frc; + +namespace { + +using ProtoType = wpi::Protobuf; + +const Rotation2d kExpectedData = Rotation2d{1.91_rad}; +} // namespace + +TEST(Rotation2dProtoTest, Roundtrip) { + google::protobuf::Arena arena; + google::protobuf::Message* proto = ProtoType::New(&arena); + ProtoType::Pack(proto, kExpectedData); + + Rotation2d unpacked_data = ProtoType::Unpack(*proto); + EXPECT_EQ(kExpectedData.Radians().value(), unpacked_data.Radians().value()); +} diff --git a/wpimath/src/test/native/cpp/geometry/proto/Rotation3dProtoTest.cpp b/wpimath/src/test/native/cpp/geometry/proto/Rotation3dProtoTest.cpp new file mode 100644 index 00000000000..a83b78c2a08 --- /dev/null +++ b/wpimath/src/test/native/cpp/geometry/proto/Rotation3dProtoTest.cpp @@ -0,0 +1,27 @@ +// 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. + +#include + +#include "frc/geometry/Rotation3d.h" +#include "geometry3d.pb.h" + +using namespace frc; + +namespace { + +using ProtoType = wpi::Protobuf; + +const Rotation3d kExpectedData = + Rotation3d{Quaternion{2.29, 0.191, 0.191, 17.4}}; +} // namespace + +TEST(Rotation3dProtoTest, Roundtrip) { + google::protobuf::Arena arena; + google::protobuf::Message* proto = ProtoType::New(&arena); + ProtoType::Pack(proto, kExpectedData); + + Rotation3d unpacked_data = ProtoType::Unpack(*proto); + EXPECT_EQ(kExpectedData.GetQuaternion(), unpacked_data.GetQuaternion()); +} diff --git a/wpimath/src/test/native/cpp/geometry/proto/Transform2dProtoTest.cpp b/wpimath/src/test/native/cpp/geometry/proto/Transform2dProtoTest.cpp new file mode 100644 index 00000000000..4b975f3e142 --- /dev/null +++ b/wpimath/src/test/native/cpp/geometry/proto/Transform2dProtoTest.cpp @@ -0,0 +1,28 @@ +// 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. + +#include + +#include "frc/geometry/Transform2d.h" +#include "geometry2d.pb.h" + +using namespace frc; + +namespace { + +using ProtoType = wpi::Protobuf; + +const Transform2d kExpectedData = + Transform2d{Translation2d{0.191_m, 2.2_m}, Rotation2d{4.4_rad}}; +} // namespace + +TEST(Transform2dProtoTest, Roundtrip) { + google::protobuf::Arena arena; + google::protobuf::Message* proto = ProtoType::New(&arena); + ProtoType::Pack(proto, kExpectedData); + + Transform2d unpacked_data = ProtoType::Unpack(*proto); + EXPECT_EQ(kExpectedData.Translation(), unpacked_data.Translation()); + EXPECT_EQ(kExpectedData.Rotation(), unpacked_data.Rotation()); +} diff --git a/wpimath/src/test/native/cpp/geometry/proto/Transform3dProtoTest.cpp b/wpimath/src/test/native/cpp/geometry/proto/Transform3dProtoTest.cpp new file mode 100644 index 00000000000..3a86421cfa3 --- /dev/null +++ b/wpimath/src/test/native/cpp/geometry/proto/Transform3dProtoTest.cpp @@ -0,0 +1,29 @@ +// 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. + +#include + +#include "frc/geometry/Transform3d.h" +#include "geometry3d.pb.h" + +using namespace frc; + +namespace { + +using ProtoType = wpi::Protobuf; + +const Transform3d kExpectedData = + Transform3d{Translation3d{0.3504_m, 22.9_m, 3.504_m}, + Rotation3d{Quaternion{0.3504, 35.04, 2.29, 0.3504}}}; +} // namespace + +TEST(Transform3dProtoTest, Roundtrip) { + google::protobuf::Arena arena; + google::protobuf::Message* proto = ProtoType::New(&arena); + ProtoType::Pack(proto, kExpectedData); + + Transform3d unpacked_data = ProtoType::Unpack(*proto); + EXPECT_EQ(kExpectedData.Translation(), unpacked_data.Translation()); + EXPECT_EQ(kExpectedData.Rotation(), unpacked_data.Rotation()); +} diff --git a/wpimath/src/test/native/cpp/geometry/proto/Translation2dProtoTest.cpp b/wpimath/src/test/native/cpp/geometry/proto/Translation2dProtoTest.cpp new file mode 100644 index 00000000000..e6a895969dd --- /dev/null +++ b/wpimath/src/test/native/cpp/geometry/proto/Translation2dProtoTest.cpp @@ -0,0 +1,27 @@ +// 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. + +#include + +#include "frc/geometry/Translation2d.h" +#include "geometry2d.pb.h" + +using namespace frc; + +namespace { + +using ProtoType = wpi::Protobuf; + +const Translation2d kExpectedData = Translation2d{3.504_m, 22.9_m}; +} // namespace + +TEST(Translation2dProtoTest, Roundtrip) { + google::protobuf::Arena arena; + google::protobuf::Message* proto = ProtoType::New(&arena); + ProtoType::Pack(proto, kExpectedData); + + Translation2d unpacked_data = ProtoType::Unpack(*proto); + EXPECT_EQ(kExpectedData.X().value(), unpacked_data.X().value()); + EXPECT_EQ(kExpectedData.Y().value(), unpacked_data.Y().value()); +} diff --git a/wpimath/src/test/native/cpp/geometry/proto/Translation3dProtoTest.cpp b/wpimath/src/test/native/cpp/geometry/proto/Translation3dProtoTest.cpp new file mode 100644 index 00000000000..3f6d59f9cfb --- /dev/null +++ b/wpimath/src/test/native/cpp/geometry/proto/Translation3dProtoTest.cpp @@ -0,0 +1,28 @@ +// 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. + +#include + +#include "frc/geometry/Translation3d.h" +#include "geometry3d.pb.h" + +using namespace frc; + +namespace { + +using ProtoType = wpi::Protobuf; + +const Translation3d kExpectedData = Translation3d{35.04_m, 22.9_m, 3.504_m}; +} // namespace + +TEST(Translation3dProtoTest, Roundtrip) { + google::protobuf::Arena arena; + google::protobuf::Message* proto = ProtoType::New(&arena); + ProtoType::Pack(proto, kExpectedData); + + Translation3d unpacked_data = ProtoType::Unpack(*proto); + EXPECT_EQ(kExpectedData.X(), unpacked_data.X()); + EXPECT_EQ(kExpectedData.Y(), unpacked_data.Y()); + EXPECT_EQ(kExpectedData.Z(), unpacked_data.Z()); +} diff --git a/wpimath/src/test/native/cpp/geometry/proto/Twist2dProtoTest.cpp b/wpimath/src/test/native/cpp/geometry/proto/Twist2dProtoTest.cpp new file mode 100644 index 00000000000..d9f4faeb213 --- /dev/null +++ b/wpimath/src/test/native/cpp/geometry/proto/Twist2dProtoTest.cpp @@ -0,0 +1,28 @@ +// 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. + +#include + +#include "frc/geometry/Twist2d.h" +#include "geometry2d.pb.h" + +using namespace frc; + +namespace { + +using ProtoType = wpi::Protobuf; + +const Twist2d kExpectedData = Twist2d{2.29_m, 35.04_m, 35.04_rad}; +} // namespace + +TEST(Twist2dProtoTest, Roundtrip) { + google::protobuf::Arena arena; + google::protobuf::Message* proto = ProtoType::New(&arena); + ProtoType::Pack(proto, kExpectedData); + + Twist2d unpacked_data = ProtoType::Unpack(*proto); + EXPECT_EQ(kExpectedData.dx.value(), unpacked_data.dx.value()); + EXPECT_EQ(kExpectedData.dy.value(), unpacked_data.dy.value()); + EXPECT_EQ(kExpectedData.dtheta.value(), unpacked_data.dtheta.value()); +} diff --git a/wpimath/src/test/native/cpp/geometry/proto/Twist3dProtoTest.cpp b/wpimath/src/test/native/cpp/geometry/proto/Twist3dProtoTest.cpp new file mode 100644 index 00000000000..b4e7bf0730b --- /dev/null +++ b/wpimath/src/test/native/cpp/geometry/proto/Twist3dProtoTest.cpp @@ -0,0 +1,32 @@ +// 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. + +#include + +#include "frc/geometry/Twist3d.h" +#include "geometry3d.pb.h" + +using namespace frc; + +namespace { + +using ProtoType = wpi::Protobuf; + +const Twist3d kExpectedData = + Twist3d{1.1_m, 2.29_m, 35.04_m, 0.174_rad, 19.1_rad, 4.4_rad}; +} // namespace + +TEST(Twist3dProtoTest, Roundtrip) { + google::protobuf::Arena arena; + google::protobuf::Message* proto = ProtoType::New(&arena); + ProtoType::Pack(proto, kExpectedData); + + Twist3d unpacked_data = ProtoType::Unpack(*proto); + EXPECT_EQ(kExpectedData.dx.value(), unpacked_data.dx.value()); + EXPECT_EQ(kExpectedData.dy.value(), unpacked_data.dy.value()); + EXPECT_EQ(kExpectedData.dz.value(), unpacked_data.dz.value()); + EXPECT_EQ(kExpectedData.rx.value(), unpacked_data.rx.value()); + EXPECT_EQ(kExpectedData.ry.value(), unpacked_data.ry.value()); + EXPECT_EQ(kExpectedData.rz.value(), unpacked_data.rz.value()); +} diff --git a/wpimath/src/test/native/cpp/geometry/struct/Pose2dStructTest.cpp b/wpimath/src/test/native/cpp/geometry/struct/Pose2dStructTest.cpp new file mode 100644 index 00000000000..369a6aae079 --- /dev/null +++ b/wpimath/src/test/native/cpp/geometry/struct/Pose2dStructTest.cpp @@ -0,0 +1,27 @@ +// 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. + +#include + +#include "frc/geometry/Pose2d.h" + +using namespace frc; + +namespace { + +using StructType = wpi::Struct; +const Pose2d kExpectedData{ + Pose2d{Translation2d{0.191_m, 2.2_m}, Rotation2d{22.9_rad}}}; +} // namespace + +TEST(Pose2dStructTest, Roundtrip) { + uint8_t buffer[StructType::kSize]; + std::memset(buffer, 0, StructType::kSize); + StructType::Pack(buffer, kExpectedData); + + Pose2d unpacked_data = StructType::Unpack(buffer); + + EXPECT_EQ(kExpectedData.Translation(), unpacked_data.Translation()); + EXPECT_EQ(kExpectedData.Rotation(), unpacked_data.Rotation()); +} diff --git a/wpimath/src/test/native/cpp/geometry/struct/Pose3dStructTest.cpp b/wpimath/src/test/native/cpp/geometry/struct/Pose3dStructTest.cpp new file mode 100644 index 00000000000..5eb1bc2d4e9 --- /dev/null +++ b/wpimath/src/test/native/cpp/geometry/struct/Pose3dStructTest.cpp @@ -0,0 +1,28 @@ +// 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. + +#include + +#include "frc/geometry/Pose3d.h" + +using namespace frc; + +namespace { + +using StructType = wpi::Struct; +const Pose3d kExpectedData{ + Pose3d{Translation3d{1.1_m, 2.2_m, 1.1_m}, + Rotation3d{Quaternion{1.91, 0.3504, 3.3, 1.74}}}}; +} // namespace + +TEST(Pose3dStructTest, Roundtrip) { + uint8_t buffer[StructType::kSize]; + std::memset(buffer, 0, StructType::kSize); + StructType::Pack(buffer, kExpectedData); + + Pose3d unpacked_data = StructType::Unpack(buffer); + + EXPECT_EQ(kExpectedData.Translation(), unpacked_data.Translation()); + EXPECT_EQ(kExpectedData.Rotation(), unpacked_data.Rotation()); +} diff --git a/wpimath/src/test/native/cpp/geometry/struct/QuaternionStructTest.cpp b/wpimath/src/test/native/cpp/geometry/struct/QuaternionStructTest.cpp new file mode 100644 index 00000000000..6eede3c6a6f --- /dev/null +++ b/wpimath/src/test/native/cpp/geometry/struct/QuaternionStructTest.cpp @@ -0,0 +1,28 @@ +// 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. + +#include + +#include "frc/geometry/Quaternion.h" + +using namespace frc; + +namespace { + +using StructType = wpi::Struct; +const Quaternion kExpectedData{Quaternion{1.1, 0.191, 35.04, 19.1}}; +} // namespace + +TEST(QuaternionStructTest, Roundtrip) { + uint8_t buffer[StructType::kSize]; + std::memset(buffer, 0, StructType::kSize); + StructType::Pack(buffer, kExpectedData); + + Quaternion unpacked_data = StructType::Unpack(buffer); + + EXPECT_EQ(kExpectedData.W(), unpacked_data.W()); + EXPECT_EQ(kExpectedData.X(), unpacked_data.X()); + EXPECT_EQ(kExpectedData.Y(), unpacked_data.Y()); + EXPECT_EQ(kExpectedData.Z(), unpacked_data.Z()); +} diff --git a/wpimath/src/test/native/cpp/geometry/struct/Rotation2dStructTest.cpp b/wpimath/src/test/native/cpp/geometry/struct/Rotation2dStructTest.cpp new file mode 100644 index 00000000000..90074632096 --- /dev/null +++ b/wpimath/src/test/native/cpp/geometry/struct/Rotation2dStructTest.cpp @@ -0,0 +1,25 @@ +// 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. + +#include + +#include "frc/geometry/Rotation2d.h" + +using namespace frc; + +namespace { + +using StructType = wpi::Struct; +const Rotation2d kExpectedData{Rotation2d{1.91_rad}}; +} // namespace + +TEST(Rotation2dStructTest, Roundtrip) { + uint8_t buffer[StructType::kSize]; + std::memset(buffer, 0, StructType::kSize); + StructType::Pack(buffer, kExpectedData); + + Rotation2d unpacked_data = StructType::Unpack(buffer); + + EXPECT_EQ(kExpectedData.Radians(), unpacked_data.Radians()); +} diff --git a/wpimath/src/test/native/cpp/geometry/struct/Rotation3dStructTest.cpp b/wpimath/src/test/native/cpp/geometry/struct/Rotation3dStructTest.cpp new file mode 100644 index 00000000000..992eb45e6c2 --- /dev/null +++ b/wpimath/src/test/native/cpp/geometry/struct/Rotation3dStructTest.cpp @@ -0,0 +1,26 @@ +// 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. + +#include + +#include "frc/geometry/Rotation3d.h" + +using namespace frc; + +namespace { + +using StructType = wpi::Struct; +const Rotation3d kExpectedData{ + Rotation3d{Quaternion{2.29, 0.191, 0.191, 17.4}}}; +} // namespace + +TEST(Rotation3dStructTest, Roundtrip) { + uint8_t buffer[StructType::kSize]; + std::memset(buffer, 0, StructType::kSize); + StructType::Pack(buffer, kExpectedData); + + Rotation3d unpacked_data = StructType::Unpack(buffer); + + EXPECT_EQ(kExpectedData.GetQuaternion(), unpacked_data.GetQuaternion()); +} diff --git a/wpimath/src/test/native/cpp/geometry/struct/Transform2dStructTest.cpp b/wpimath/src/test/native/cpp/geometry/struct/Transform2dStructTest.cpp new file mode 100644 index 00000000000..b28aeaabf64 --- /dev/null +++ b/wpimath/src/test/native/cpp/geometry/struct/Transform2dStructTest.cpp @@ -0,0 +1,27 @@ +// 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. + +#include + +#include "frc/geometry/Transform2d.h" + +using namespace frc; + +namespace { + +using StructType = wpi::Struct; +const Transform2d kExpectedData{ + Transform2d{Translation2d{0.191_m, 2.2_m}, Rotation2d{4.4_rad}}}; +} // namespace + +TEST(Transform2dStructTest, Roundtrip) { + uint8_t buffer[StructType::kSize]; + std::memset(buffer, 0, StructType::kSize); + StructType::Pack(buffer, kExpectedData); + + Transform2d unpacked_data = StructType::Unpack(buffer); + + EXPECT_EQ(kExpectedData.Translation(), unpacked_data.Translation()); + EXPECT_EQ(kExpectedData.Rotation(), unpacked_data.Rotation()); +} diff --git a/wpimath/src/test/native/cpp/geometry/struct/Transform3dStructTest.cpp b/wpimath/src/test/native/cpp/geometry/struct/Transform3dStructTest.cpp new file mode 100644 index 00000000000..d32433183b5 --- /dev/null +++ b/wpimath/src/test/native/cpp/geometry/struct/Transform3dStructTest.cpp @@ -0,0 +1,28 @@ +// 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. + +#include + +#include "frc/geometry/Transform3d.h" + +using namespace frc; + +namespace { + +using StructType = wpi::Struct; +const Transform3d kExpectedData{ + Transform3d{Translation3d{0.3504_m, 22.9_m, 3.504_m}, + Rotation3d{Quaternion{0.3504, 35.04, 2.29, 0.3504}}}}; +} // namespace + +TEST(Transform3dStructTest, Roundtrip) { + uint8_t buffer[StructType::kSize]; + std::memset(buffer, 0, StructType::kSize); + StructType::Pack(buffer, kExpectedData); + + Transform3d unpacked_data = StructType::Unpack(buffer); + + EXPECT_EQ(kExpectedData.Translation(), unpacked_data.Translation()); + EXPECT_EQ(kExpectedData.Rotation(), unpacked_data.Rotation()); +} diff --git a/wpimath/src/test/native/cpp/geometry/struct/Translation2dStructTest.cpp b/wpimath/src/test/native/cpp/geometry/struct/Translation2dStructTest.cpp new file mode 100644 index 00000000000..6f87420cdee --- /dev/null +++ b/wpimath/src/test/native/cpp/geometry/struct/Translation2dStructTest.cpp @@ -0,0 +1,26 @@ +// 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. + +#include + +#include "frc/geometry/Translation2d.h" + +using namespace frc; + +namespace { + +using StructType = wpi::Struct; +const Translation2d kExpectedData{Translation2d{3.504_m, 22.9_m}}; +} // namespace + +TEST(Translation2dStructTest, Roundtrip) { + uint8_t buffer[StructType::kSize]; + std::memset(buffer, 0, StructType::kSize); + StructType::Pack(buffer, kExpectedData); + + Translation2d unpacked_data = StructType::Unpack(buffer); + + EXPECT_EQ(kExpectedData.X(), unpacked_data.X()); + EXPECT_EQ(kExpectedData.Y(), unpacked_data.Y()); +} diff --git a/wpimath/src/test/native/cpp/geometry/struct/Translation3dStructTest.cpp b/wpimath/src/test/native/cpp/geometry/struct/Translation3dStructTest.cpp new file mode 100644 index 00000000000..1caa8a327fc --- /dev/null +++ b/wpimath/src/test/native/cpp/geometry/struct/Translation3dStructTest.cpp @@ -0,0 +1,27 @@ +// 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. + +#include + +#include "frc/geometry/Translation3d.h" + +using namespace frc; + +namespace { + +using StructType = wpi::Struct; +const Translation3d kExpectedData{Translation3d{35.04_m, 22.9_m, 3.504_m}}; +} // namespace + +TEST(Translation3dStructTest, Roundtrip) { + uint8_t buffer[StructType::kSize]; + std::memset(buffer, 0, StructType::kSize); + StructType::Pack(buffer, kExpectedData); + + Translation3d unpacked_data = StructType::Unpack(buffer); + + EXPECT_EQ(kExpectedData.X(), unpacked_data.X()); + EXPECT_EQ(kExpectedData.Y(), unpacked_data.Y()); + EXPECT_EQ(kExpectedData.Z(), unpacked_data.Z()); +} diff --git a/wpimath/src/test/native/cpp/geometry/struct/Twist2dStructTest.cpp b/wpimath/src/test/native/cpp/geometry/struct/Twist2dStructTest.cpp new file mode 100644 index 00000000000..a3d1370d4b2 --- /dev/null +++ b/wpimath/src/test/native/cpp/geometry/struct/Twist2dStructTest.cpp @@ -0,0 +1,27 @@ +// 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. + +#include + +#include "frc/geometry/Twist2d.h" + +using namespace frc; + +namespace { + +using StructType = wpi::Struct; +const Twist2d kExpectedData{Twist2d{2.29_m, 35.04_m, 35.04_rad}}; +} // namespace + +TEST(Twist2dStructTest, Roundtrip) { + uint8_t buffer[StructType::kSize]; + std::memset(buffer, 0, StructType::kSize); + StructType::Pack(buffer, kExpectedData); + + Twist2d unpacked_data = StructType::Unpack(buffer); + + EXPECT_EQ(kExpectedData.dx.value(), unpacked_data.dx.value()); + EXPECT_EQ(kExpectedData.dy.value(), unpacked_data.dy.value()); + EXPECT_EQ(kExpectedData.dtheta.value(), unpacked_data.dtheta.value()); +} diff --git a/wpimath/src/test/native/cpp/geometry/struct/Twist3dStructTest.cpp b/wpimath/src/test/native/cpp/geometry/struct/Twist3dStructTest.cpp new file mode 100644 index 00000000000..b753a3b263d --- /dev/null +++ b/wpimath/src/test/native/cpp/geometry/struct/Twist3dStructTest.cpp @@ -0,0 +1,31 @@ +// 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. + +#include + +#include "frc/geometry/Twist3d.h" + +using namespace frc; + +namespace { + +using StructType = wpi::Struct; +const Twist3d kExpectedData{ + Twist3d{1.1_m, 2.29_m, 35.04_m, 0.174_rad, 19.1_rad, 4.4_rad}}; +} // namespace + +TEST(Twist3dStructTest, Roundtrip) { + uint8_t buffer[StructType::kSize]; + std::memset(buffer, 0, StructType::kSize); + StructType::Pack(buffer, kExpectedData); + + Twist3d unpacked_data = StructType::Unpack(buffer); + + EXPECT_EQ(kExpectedData.dx.value(), unpacked_data.dx.value()); + EXPECT_EQ(kExpectedData.dy.value(), unpacked_data.dy.value()); + EXPECT_EQ(kExpectedData.dz.value(), unpacked_data.dz.value()); + EXPECT_EQ(kExpectedData.rx.value(), unpacked_data.rx.value()); + EXPECT_EQ(kExpectedData.ry.value(), unpacked_data.ry.value()); + EXPECT_EQ(kExpectedData.rz.value(), unpacked_data.rz.value()); +} diff --git a/wpimath/src/test/native/cpp/kinematics/proto/ChassisSpeedsProtoTest.cpp b/wpimath/src/test/native/cpp/kinematics/proto/ChassisSpeedsProtoTest.cpp new file mode 100644 index 00000000000..375909a330c --- /dev/null +++ b/wpimath/src/test/native/cpp/kinematics/proto/ChassisSpeedsProtoTest.cpp @@ -0,0 +1,29 @@ +// 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. + +#include + +#include "frc/kinematics/ChassisSpeeds.h" +#include "kinematics.pb.h" + +using namespace frc; + +namespace { + +using ProtoType = wpi::Protobuf; + +const ChassisSpeeds kExpectedData = + ChassisSpeeds{2.29_mps, 2.2_mps, 0.3504_rad_per_s}; +} // namespace + +TEST(ChassisSpeedsProtoTest, Roundtrip) { + google::protobuf::Arena arena; + google::protobuf::Message* proto = ProtoType::New(&arena); + ProtoType::Pack(proto, kExpectedData); + + ChassisSpeeds unpacked_data = ProtoType::Unpack(*proto); + EXPECT_EQ(kExpectedData.vx.value(), unpacked_data.vx.value()); + EXPECT_EQ(kExpectedData.vy.value(), unpacked_data.vy.value()); + EXPECT_EQ(kExpectedData.omega.value(), unpacked_data.omega.value()); +} diff --git a/wpimath/src/test/native/cpp/kinematics/proto/DifferentialDriveKinematicsProtoTest.cpp b/wpimath/src/test/native/cpp/kinematics/proto/DifferentialDriveKinematicsProtoTest.cpp new file mode 100644 index 00000000000..4d57108833e --- /dev/null +++ b/wpimath/src/test/native/cpp/kinematics/proto/DifferentialDriveKinematicsProtoTest.cpp @@ -0,0 +1,27 @@ +// 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. + +#include + +#include "frc/kinematics/DifferentialDriveKinematics.h" +#include "kinematics.pb.h" + +using namespace frc; + +namespace { + +using ProtoType = wpi::Protobuf; + +const DifferentialDriveKinematics kExpectedData = + DifferentialDriveKinematics{1.74_m}; +} // namespace + +TEST(DifferentialDriveKinematicsProtoTest, Roundtrip) { + google::protobuf::Arena arena; + google::protobuf::Message* proto = ProtoType::New(&arena); + ProtoType::Pack(proto, kExpectedData); + + DifferentialDriveKinematics unpacked_data = ProtoType::Unpack(*proto); + EXPECT_EQ(kExpectedData.trackWidth.value(), unpacked_data.trackWidth.value()); +} diff --git a/wpimath/src/test/native/cpp/kinematics/proto/DifferentialDriveWheelSpeedsProtoTest.cpp b/wpimath/src/test/native/cpp/kinematics/proto/DifferentialDriveWheelSpeedsProtoTest.cpp new file mode 100644 index 00000000000..83e317f2060 --- /dev/null +++ b/wpimath/src/test/native/cpp/kinematics/proto/DifferentialDriveWheelSpeedsProtoTest.cpp @@ -0,0 +1,28 @@ +// 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. + +#include + +#include "frc/kinematics/DifferentialDriveWheelSpeeds.h" +#include "kinematics.pb.h" + +using namespace frc; + +namespace { + +using ProtoType = wpi::Protobuf; + +const DifferentialDriveWheelSpeeds kExpectedData = + DifferentialDriveWheelSpeeds{1.74_mps, 35.04_mps}; +} // namespace + +TEST(DifferentialDriveWheelSpeedsProtoTest, Roundtrip) { + google::protobuf::Arena arena; + google::protobuf::Message* proto = ProtoType::New(&arena); + ProtoType::Pack(proto, kExpectedData); + + DifferentialDriveWheelSpeeds unpacked_data = ProtoType::Unpack(*proto); + EXPECT_EQ(kExpectedData.left.value(), unpacked_data.left.value()); + EXPECT_EQ(kExpectedData.right.value(), unpacked_data.right.value()); +} diff --git a/wpimath/src/test/native/cpp/kinematics/proto/MecanumDriveKinematicsProtoTest.cpp b/wpimath/src/test/native/cpp/kinematics/proto/MecanumDriveKinematicsProtoTest.cpp new file mode 100644 index 00000000000..7051ca1a0c4 --- /dev/null +++ b/wpimath/src/test/native/cpp/kinematics/proto/MecanumDriveKinematicsProtoTest.cpp @@ -0,0 +1,34 @@ +// 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. + +#include + +#include "frc/kinematics/MecanumDriveKinematics.h" +#include "kinematics.pb.h" + +using namespace frc; + +namespace { + +using ProtoType = wpi::Protobuf; + +const MecanumDriveKinematics kExpectedData = MecanumDriveKinematics{ + Translation2d{19.1_m, 2.2_m}, Translation2d{35.04_m, 1.91_m}, + Translation2d{1.74_m, 3.504_m}, Translation2d{3.504_m, 1.91_m}}; +} // namespace + +TEST(MecanumDriveKinematicsProtoTest, Roundtrip) { + google::protobuf::Arena arena; + google::protobuf::Message* proto = ProtoType::New(&arena); + ProtoType::Pack(proto, kExpectedData); + + MecanumDriveKinematics unpacked_data = ProtoType::Unpack(*proto); + EXPECT_EQ(kExpectedData.GetFrontLeftWheel(), + unpacked_data.GetFrontLeftWheel()); + EXPECT_EQ(kExpectedData.GetFrontRightWheel(), + unpacked_data.GetFrontRightWheel()); + EXPECT_EQ(kExpectedData.GetRearLeftWheel(), unpacked_data.GetRearLeftWheel()); + EXPECT_EQ(kExpectedData.GetRearRightWheel(), + unpacked_data.GetRearRightWheel()); +} diff --git a/wpimath/src/test/native/cpp/kinematics/proto/MecanumDriveWheelPositionsProtoTest.cpp b/wpimath/src/test/native/cpp/kinematics/proto/MecanumDriveWheelPositionsProtoTest.cpp new file mode 100644 index 00000000000..e63ffbeadd6 --- /dev/null +++ b/wpimath/src/test/native/cpp/kinematics/proto/MecanumDriveWheelPositionsProtoTest.cpp @@ -0,0 +1,30 @@ +// 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. + +#include + +#include "frc/kinematics/MecanumDriveWheelPositions.h" +#include "kinematics.pb.h" + +using namespace frc; + +namespace { + +using ProtoType = wpi::Protobuf; + +const MecanumDriveWheelPositions kExpectedData = + MecanumDriveWheelPositions{17.4_m, 2.29_m, 22.9_m, 1.74_m}; +} // namespace + +TEST(MecanumDriveWheelPositionsProtoTest, Roundtrip) { + google::protobuf::Arena arena; + google::protobuf::Message* proto = ProtoType::New(&arena); + ProtoType::Pack(proto, kExpectedData); + + MecanumDriveWheelPositions unpacked_data = ProtoType::Unpack(*proto); + EXPECT_EQ(kExpectedData.frontLeft.value(), unpacked_data.frontLeft.value()); + EXPECT_EQ(kExpectedData.frontRight.value(), unpacked_data.frontRight.value()); + EXPECT_EQ(kExpectedData.rearLeft.value(), unpacked_data.rearLeft.value()); + EXPECT_EQ(kExpectedData.rearRight.value(), unpacked_data.rearRight.value()); +} diff --git a/wpimath/src/test/native/cpp/kinematics/proto/MecanumDriveWheelSpeedsProtoTest.cpp b/wpimath/src/test/native/cpp/kinematics/proto/MecanumDriveWheelSpeedsProtoTest.cpp new file mode 100644 index 00000000000..ae91c3a1eb5 --- /dev/null +++ b/wpimath/src/test/native/cpp/kinematics/proto/MecanumDriveWheelSpeedsProtoTest.cpp @@ -0,0 +1,30 @@ +// 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. + +#include + +#include "frc/kinematics/MecanumDriveWheelSpeeds.h" +#include "kinematics.pb.h" + +using namespace frc; + +namespace { + +using ProtoType = wpi::Protobuf; + +const MecanumDriveWheelSpeeds kExpectedData = + MecanumDriveWheelSpeeds{2.29_mps, 17.4_mps, 4.4_mps, 0.229_mps}; +} // namespace + +TEST(MecanumDriveWheelSpeedsProtoTest, Roundtrip) { + google::protobuf::Arena arena; + google::protobuf::Message* proto = ProtoType::New(&arena); + ProtoType::Pack(proto, kExpectedData); + + MecanumDriveWheelSpeeds unpacked_data = ProtoType::Unpack(*proto); + EXPECT_EQ(kExpectedData.frontLeft.value(), unpacked_data.frontLeft.value()); + EXPECT_EQ(kExpectedData.frontRight.value(), unpacked_data.frontRight.value()); + EXPECT_EQ(kExpectedData.rearLeft.value(), unpacked_data.rearLeft.value()); + EXPECT_EQ(kExpectedData.rearRight.value(), unpacked_data.rearRight.value()); +} diff --git a/wpimath/src/test/native/cpp/kinematics/proto/SwerveModulePositionProtoTest.cpp b/wpimath/src/test/native/cpp/kinematics/proto/SwerveModulePositionProtoTest.cpp new file mode 100644 index 00000000000..c511b9f840a --- /dev/null +++ b/wpimath/src/test/native/cpp/kinematics/proto/SwerveModulePositionProtoTest.cpp @@ -0,0 +1,28 @@ +// 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. + +#include + +#include "frc/kinematics/SwerveModulePosition.h" +#include "kinematics.pb.h" + +using namespace frc; + +namespace { + +using ProtoType = wpi::Protobuf; + +const SwerveModulePosition kExpectedData = + SwerveModulePosition{3.504_m, Rotation2d{17.4_rad}}; +} // namespace + +TEST(SwerveModulePositionProtoTest, Roundtrip) { + google::protobuf::Arena arena; + google::protobuf::Message* proto = ProtoType::New(&arena); + ProtoType::Pack(proto, kExpectedData); + + SwerveModulePosition unpacked_data = ProtoType::Unpack(*proto); + EXPECT_EQ(kExpectedData.distance.value(), unpacked_data.distance.value()); + EXPECT_EQ(kExpectedData.angle, unpacked_data.angle); +} diff --git a/wpimath/src/test/native/cpp/kinematics/proto/SwerveModuleStateProtoTest.cpp b/wpimath/src/test/native/cpp/kinematics/proto/SwerveModuleStateProtoTest.cpp new file mode 100644 index 00000000000..e210db94790 --- /dev/null +++ b/wpimath/src/test/native/cpp/kinematics/proto/SwerveModuleStateProtoTest.cpp @@ -0,0 +1,28 @@ +// 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. + +#include + +#include "frc/kinematics/SwerveModuleState.h" +#include "kinematics.pb.h" + +using namespace frc; + +namespace { + +using ProtoType = wpi::Protobuf; + +const SwerveModuleState kExpectedData = + SwerveModuleState{22.9_mps, Rotation2d{3.3_rad}}; +} // namespace + +TEST(SwerveModuleStateProtoTest, Roundtrip) { + google::protobuf::Arena arena; + google::protobuf::Message* proto = ProtoType::New(&arena); + ProtoType::Pack(proto, kExpectedData); + + SwerveModuleState unpacked_data = ProtoType::Unpack(*proto); + EXPECT_EQ(kExpectedData.speed.value(), unpacked_data.speed.value()); + EXPECT_EQ(kExpectedData.angle, unpacked_data.angle); +} diff --git a/wpimath/src/test/native/cpp/kinematics/struct/ChassisSpeedsStructTest.cpp b/wpimath/src/test/native/cpp/kinematics/struct/ChassisSpeedsStructTest.cpp new file mode 100644 index 00000000000..6729aa0b9d1 --- /dev/null +++ b/wpimath/src/test/native/cpp/kinematics/struct/ChassisSpeedsStructTest.cpp @@ -0,0 +1,28 @@ +// 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. + +#include + +#include "frc/kinematics/ChassisSpeeds.h" + +using namespace frc; + +namespace { + +using StructType = wpi::Struct; +const ChassisSpeeds kExpectedData{ + ChassisSpeeds{2.29_mps, 2.2_mps, 0.3504_rad_per_s}}; +} // namespace + +TEST(ChassisSpeedsStructTest, Roundtrip) { + uint8_t buffer[StructType::kSize]; + std::memset(buffer, 0, StructType::kSize); + StructType::Pack(buffer, kExpectedData); + + ChassisSpeeds unpacked_data = StructType::Unpack(buffer); + + EXPECT_EQ(kExpectedData.vx.value(), unpacked_data.vx.value()); + EXPECT_EQ(kExpectedData.vy.value(), unpacked_data.vy.value()); + EXPECT_EQ(kExpectedData.omega.value(), unpacked_data.omega.value()); +} diff --git a/wpimath/src/test/native/cpp/kinematics/struct/DifferentialDriveKinematicsStructTest.cpp b/wpimath/src/test/native/cpp/kinematics/struct/DifferentialDriveKinematicsStructTest.cpp new file mode 100644 index 00000000000..ecf10ad5f78 --- /dev/null +++ b/wpimath/src/test/native/cpp/kinematics/struct/DifferentialDriveKinematicsStructTest.cpp @@ -0,0 +1,26 @@ +// 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. + +#include + +#include "frc/kinematics/DifferentialDriveKinematics.h" + +using namespace frc; + +namespace { + +using StructType = wpi::Struct; +const DifferentialDriveKinematics kExpectedData{ + DifferentialDriveKinematics{1.74_m}}; +} // namespace + +TEST(DifferentialDriveKinematicsStructTest, Roundtrip) { + uint8_t buffer[StructType::kSize]; + std::memset(buffer, 0, StructType::kSize); + StructType::Pack(buffer, kExpectedData); + + DifferentialDriveKinematics unpacked_data = StructType::Unpack(buffer); + + EXPECT_EQ(kExpectedData.trackWidth.value(), unpacked_data.trackWidth.value()); +} diff --git a/wpimath/src/test/native/cpp/kinematics/struct/DifferentialDriveWheelSpeedsStructTest.cpp b/wpimath/src/test/native/cpp/kinematics/struct/DifferentialDriveWheelSpeedsStructTest.cpp new file mode 100644 index 00000000000..33499f5d4e2 --- /dev/null +++ b/wpimath/src/test/native/cpp/kinematics/struct/DifferentialDriveWheelSpeedsStructTest.cpp @@ -0,0 +1,27 @@ +// 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. + +#include + +#include "frc/kinematics/DifferentialDriveWheelSpeeds.h" + +using namespace frc; + +namespace { + +using StructType = wpi::Struct; +const DifferentialDriveWheelSpeeds kExpectedData{ + DifferentialDriveWheelSpeeds{1.74_mps, 35.04_mps}}; +} // namespace + +TEST(DifferentialDriveWheelSpeedsStructTest, Roundtrip) { + uint8_t buffer[StructType::kSize]; + std::memset(buffer, 0, StructType::kSize); + StructType::Pack(buffer, kExpectedData); + + DifferentialDriveWheelSpeeds unpacked_data = StructType::Unpack(buffer); + + EXPECT_EQ(kExpectedData.left.value(), unpacked_data.left.value()); + EXPECT_EQ(kExpectedData.right.value(), unpacked_data.right.value()); +} diff --git a/wpimath/src/test/native/cpp/kinematics/struct/MecanumDriveKinematicsStructTest.cpp b/wpimath/src/test/native/cpp/kinematics/struct/MecanumDriveKinematicsStructTest.cpp new file mode 100644 index 00000000000..8fe0817e279 --- /dev/null +++ b/wpimath/src/test/native/cpp/kinematics/struct/MecanumDriveKinematicsStructTest.cpp @@ -0,0 +1,33 @@ +// 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. + +#include + +#include "frc/kinematics/MecanumDriveKinematics.h" + +using namespace frc; + +namespace { + +using StructType = wpi::Struct; +const MecanumDriveKinematics kExpectedData{MecanumDriveKinematics{ + Translation2d{19.1_m, 2.2_m}, Translation2d{35.04_m, 1.91_m}, + Translation2d{1.74_m, 3.504_m}, Translation2d{3.504_m, 1.91_m}}}; +} // namespace + +TEST(MecanumDriveKinematicsStructTest, Roundtrip) { + uint8_t buffer[StructType::kSize]; + std::memset(buffer, 0, StructType::kSize); + StructType::Pack(buffer, kExpectedData); + + MecanumDriveKinematics unpacked_data = StructType::Unpack(buffer); + + EXPECT_EQ(kExpectedData.GetFrontLeftWheel(), + unpacked_data.GetFrontLeftWheel()); + EXPECT_EQ(kExpectedData.GetFrontRightWheel(), + unpacked_data.GetFrontRightWheel()); + EXPECT_EQ(kExpectedData.GetRearLeftWheel(), unpacked_data.GetRearLeftWheel()); + EXPECT_EQ(kExpectedData.GetRearRightWheel(), + unpacked_data.GetRearRightWheel()); +} diff --git a/wpimath/src/test/native/cpp/kinematics/struct/MecanumDriveWheelPositionsStructTest.cpp b/wpimath/src/test/native/cpp/kinematics/struct/MecanumDriveWheelPositionsStructTest.cpp new file mode 100644 index 00000000000..39190a776f5 --- /dev/null +++ b/wpimath/src/test/native/cpp/kinematics/struct/MecanumDriveWheelPositionsStructTest.cpp @@ -0,0 +1,29 @@ +// 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. + +#include + +#include "frc/kinematics/MecanumDriveWheelPositions.h" + +using namespace frc; + +namespace { + +using StructType = wpi::Struct; +const MecanumDriveWheelPositions kExpectedData{ + MecanumDriveWheelPositions{17.4_m, 2.29_m, 22.9_m, 1.74_m}}; +} // namespace + +TEST(MecanumDriveWheelPositionsStructTest, Roundtrip) { + uint8_t buffer[StructType::kSize]; + std::memset(buffer, 0, StructType::kSize); + StructType::Pack(buffer, kExpectedData); + + MecanumDriveWheelPositions unpacked_data = StructType::Unpack(buffer); + + EXPECT_EQ(kExpectedData.frontLeft.value(), unpacked_data.frontLeft.value()); + EXPECT_EQ(kExpectedData.frontRight.value(), unpacked_data.frontRight.value()); + EXPECT_EQ(kExpectedData.rearLeft.value(), unpacked_data.rearLeft.value()); + EXPECT_EQ(kExpectedData.rearRight.value(), unpacked_data.rearRight.value()); +} diff --git a/wpimath/src/test/native/cpp/kinematics/struct/MecanumDriveWheelSpeedsStructTest.cpp b/wpimath/src/test/native/cpp/kinematics/struct/MecanumDriveWheelSpeedsStructTest.cpp new file mode 100644 index 00000000000..bbde80cdde4 --- /dev/null +++ b/wpimath/src/test/native/cpp/kinematics/struct/MecanumDriveWheelSpeedsStructTest.cpp @@ -0,0 +1,29 @@ +// 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. + +#include + +#include "frc/kinematics/MecanumDriveWheelSpeeds.h" + +using namespace frc; + +namespace { + +using StructType = wpi::Struct; +const MecanumDriveWheelSpeeds kExpectedData{ + MecanumDriveWheelSpeeds{2.29_mps, 17.4_mps, 4.4_mps, 0.229_mps}}; +} // namespace + +TEST(MecanumDriveWheelSpeedsStructTest, Roundtrip) { + uint8_t buffer[StructType::kSize]; + std::memset(buffer, 0, StructType::kSize); + StructType::Pack(buffer, kExpectedData); + + MecanumDriveWheelSpeeds unpacked_data = StructType::Unpack(buffer); + + EXPECT_EQ(kExpectedData.frontLeft.value(), unpacked_data.frontLeft.value()); + EXPECT_EQ(kExpectedData.frontRight.value(), unpacked_data.frontRight.value()); + EXPECT_EQ(kExpectedData.rearLeft.value(), unpacked_data.rearLeft.value()); + EXPECT_EQ(kExpectedData.rearRight.value(), unpacked_data.rearRight.value()); +} diff --git a/wpimath/src/test/native/cpp/kinematics/struct/SwerveModulePositionStructTest.cpp b/wpimath/src/test/native/cpp/kinematics/struct/SwerveModulePositionStructTest.cpp new file mode 100644 index 00000000000..a942efe6623 --- /dev/null +++ b/wpimath/src/test/native/cpp/kinematics/struct/SwerveModulePositionStructTest.cpp @@ -0,0 +1,27 @@ +// 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. + +#include + +#include "frc/kinematics/SwerveModulePosition.h" + +using namespace frc; + +namespace { + +using StructType = wpi::Struct; +const SwerveModulePosition kExpectedData{ + SwerveModulePosition{3.504_m, Rotation2d{17.4_rad}}}; +} // namespace + +TEST(SwerveModulePositionStructTest, Roundtrip) { + uint8_t buffer[StructType::kSize]; + std::memset(buffer, 0, StructType::kSize); + StructType::Pack(buffer, kExpectedData); + + SwerveModulePosition unpacked_data = StructType::Unpack(buffer); + + EXPECT_EQ(kExpectedData.distance.value(), unpacked_data.distance.value()); + EXPECT_EQ(kExpectedData.angle, unpacked_data.angle); +} diff --git a/wpimath/src/test/native/cpp/kinematics/struct/SwerveModuleStateStructTest.cpp b/wpimath/src/test/native/cpp/kinematics/struct/SwerveModuleStateStructTest.cpp new file mode 100644 index 00000000000..40ca305b8e3 --- /dev/null +++ b/wpimath/src/test/native/cpp/kinematics/struct/SwerveModuleStateStructTest.cpp @@ -0,0 +1,27 @@ +// 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. + +#include + +#include "frc/kinematics/SwerveModuleState.h" + +using namespace frc; + +namespace { + +using StructType = wpi::Struct; +const SwerveModuleState kExpectedData{ + SwerveModuleState{22.9_mps, Rotation2d{3.3_rad}}}; +} // namespace + +TEST(SwerveModuleStateStructTest, Roundtrip) { + uint8_t buffer[StructType::kSize]; + std::memset(buffer, 0, StructType::kSize); + StructType::Pack(buffer, kExpectedData); + + SwerveModuleState unpacked_data = StructType::Unpack(buffer); + + EXPECT_EQ(kExpectedData.speed.value(), unpacked_data.speed.value()); + EXPECT_EQ(kExpectedData.angle, unpacked_data.angle); +} diff --git a/wpimath/src/test/native/cpp/system/plant/proto/DCMotorProtoTest.cpp b/wpimath/src/test/native/cpp/system/plant/proto/DCMotorProtoTest.cpp new file mode 100644 index 00000000000..b664befd762 --- /dev/null +++ b/wpimath/src/test/native/cpp/system/plant/proto/DCMotorProtoTest.cpp @@ -0,0 +1,42 @@ +// 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. + +#include + +#include "frc/system/plant/DCMotor.h" +#include "plant.pb.h" + +using namespace frc; + +namespace { + +using ProtoType = wpi::Protobuf; + +const DCMotor kExpectedData = DCMotor{units::volt_t{1.91}, + units::newton_meter_t{19.1}, + units::ampere_t{1.74}, + units::ampere_t{2.29}, + units::radians_per_second_t{2.2}, + 2}; +} // namespace + +TEST(DCMotorProtoTest, Roundtrip) { + google::protobuf::Arena arena; + google::protobuf::Message* proto = ProtoType::New(&arena); + ProtoType::Pack(proto, kExpectedData); + + DCMotor unpacked_data = ProtoType::Unpack(*proto); + EXPECT_EQ(kExpectedData.nominalVoltage.value(), + unpacked_data.nominalVoltage.value()); + EXPECT_EQ(kExpectedData.stallTorque.value(), + unpacked_data.stallTorque.value()); + EXPECT_EQ(kExpectedData.stallCurrent.value(), + unpacked_data.stallCurrent.value()); + EXPECT_EQ(kExpectedData.freeCurrent.value(), + unpacked_data.freeCurrent.value()); + EXPECT_EQ(kExpectedData.freeSpeed.value(), unpacked_data.freeSpeed.value()); + EXPECT_EQ(kExpectedData.R.value(), unpacked_data.R.value()); + EXPECT_EQ(kExpectedData.Kv.value(), unpacked_data.Kv.value()); + EXPECT_EQ(kExpectedData.Kt.value(), unpacked_data.Kt.value()); +} diff --git a/wpimath/src/test/native/cpp/system/plant/struct/DCMotorStructTest.cpp b/wpimath/src/test/native/cpp/system/plant/struct/DCMotorStructTest.cpp new file mode 100644 index 00000000000..30306d2d5f5 --- /dev/null +++ b/wpimath/src/test/native/cpp/system/plant/struct/DCMotorStructTest.cpp @@ -0,0 +1,41 @@ +// 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. + +#include + +#include "frc/system/plant/DCMotor.h" + +using namespace frc; + +namespace { + +using StructType = wpi::Struct; +const DCMotor kExpectedData = DCMotor{units::volt_t{1.91}, + units::newton_meter_t{19.1}, + units::ampere_t{1.74}, + units::ampere_t{2.29}, + units::radians_per_second_t{2.2}, + 2}; +} // namespace + +TEST(DCMotorStructTest, Roundtrip) { + uint8_t buffer[StructType::kSize]; + std::memset(buffer, 0, StructType::kSize); + StructType::Pack(buffer, kExpectedData); + + DCMotor unpacked_data = StructType::Unpack(buffer); + + EXPECT_EQ(kExpectedData.nominalVoltage.value(), + unpacked_data.nominalVoltage.value()); + EXPECT_EQ(kExpectedData.stallTorque.value(), + unpacked_data.stallTorque.value()); + EXPECT_EQ(kExpectedData.stallCurrent.value(), + unpacked_data.stallCurrent.value()); + EXPECT_EQ(kExpectedData.freeCurrent.value(), + unpacked_data.freeCurrent.value()); + EXPECT_EQ(kExpectedData.freeSpeed.value(), unpacked_data.freeSpeed.value()); + EXPECT_EQ(kExpectedData.R.value(), unpacked_data.R.value()); + EXPECT_EQ(kExpectedData.Kv.value(), unpacked_data.Kv.value()); + EXPECT_EQ(kExpectedData.Kt.value(), unpacked_data.Kt.value()); +} diff --git a/wpimath/src/test/native/cpp/trajectory/proto/TrajectoryProtoTest.cpp b/wpimath/src/test/native/cpp/trajectory/proto/TrajectoryProtoTest.cpp new file mode 100644 index 00000000000..400689bf847 --- /dev/null +++ b/wpimath/src/test/native/cpp/trajectory/proto/TrajectoryProtoTest.cpp @@ -0,0 +1,35 @@ +// 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. + +#include + +#include "frc/trajectory/Trajectory.h" +#include "trajectory.pb.h" + +using namespace frc; + +namespace { + +using ProtoType = wpi::Protobuf; + +const Trajectory kExpectedData = Trajectory{std::vector{ + Trajectory::State{1.1_s, 2.2_mps, 3.3_mps_sq, + Pose2d(Translation2d(1.1_m, 2.2_m), Rotation2d(2.2_rad)), + units::curvature_t{6.6}}, + Trajectory::State{2.1_s, 2.2_mps, 3.3_mps_sq, + Pose2d(Translation2d(2.1_m, 2.2_m), Rotation2d(2.2_rad)), + units::curvature_t{6.6}}, + Trajectory::State{3.1_s, 2.2_mps, 3.3_mps_sq, + Pose2d(Translation2d(3.1_m, 2.2_m), Rotation2d(2.2_rad)), + units::curvature_t{6.6}}}}; +} // namespace + +TEST(TrajectoryProtoTest, Roundtrip) { + google::protobuf::Arena arena; + google::protobuf::Message* proto = ProtoType::New(&arena); + ProtoType::Pack(proto, kExpectedData); + + Trajectory unpacked_data = ProtoType::Unpack(*proto); + EXPECT_EQ(kExpectedData.States(), unpacked_data.States()); +} diff --git a/wpimath/src/test/native/cpp/trajectory/proto/TrajectoryStateProtoTest.cpp b/wpimath/src/test/native/cpp/trajectory/proto/TrajectoryStateProtoTest.cpp new file mode 100644 index 00000000000..1d0b0497de9 --- /dev/null +++ b/wpimath/src/test/native/cpp/trajectory/proto/TrajectoryStateProtoTest.cpp @@ -0,0 +1,34 @@ +// 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. + +#include + +#include "frc/trajectory/Trajectory.h" +#include "trajectory.pb.h" + +using namespace frc; + +namespace { + +using ProtoType = wpi::Protobuf; + +const Trajectory::State kExpectedData = Trajectory::State{ + 1.91_s, 4.4_mps, 17.4_mps_sq, + Pose2d{Translation2d{1.74_m, 19.1_m}, Rotation2d{22.9_rad}}, + units::curvature_t{0.174}}; +} // namespace + +TEST(TrajectoryStateProtoTest, Roundtrip) { + google::protobuf::Arena arena; + google::protobuf::Message* proto = ProtoType::New(&arena); + ProtoType::Pack(proto, kExpectedData); + + Trajectory::State unpacked_data = ProtoType::Unpack(*proto); + EXPECT_EQ(kExpectedData.t.value(), unpacked_data.t.value()); + EXPECT_EQ(kExpectedData.velocity.value(), unpacked_data.velocity.value()); + EXPECT_EQ(kExpectedData.acceleration.value(), + unpacked_data.acceleration.value()); + EXPECT_EQ(kExpectedData.pose, unpacked_data.pose); + EXPECT_EQ(kExpectedData.curvature.value(), unpacked_data.curvature.value()); +} diff --git a/wpimath/wpimath-config.cmake.in b/wpimath/wpimath-config.cmake.in index 9100d7943dc..8697dc1836e 100644 --- a/wpimath/wpimath-config.cmake.in +++ b/wpimath/wpimath-config.cmake.in @@ -1,6 +1,7 @@ include(CMakeFindDependencyMacro) @FILENAME_DEP_REPLACE@ @WPIUTIL_DEP_REPLACE@ +@WPIUNITS_DEP_REPLACE@ if(@USE_SYSTEM_EIGEN@) find_dependency(Eigen3) diff --git a/wpinet/CMakeLists.txt b/wpinet/CMakeLists.txt index f1eda760e32..9729334a264 100644 --- a/wpinet/CMakeLists.txt +++ b/wpinet/CMakeLists.txt @@ -173,15 +173,9 @@ target_include_directories(wpinet PUBLIC install(TARGETS wpinet EXPORT wpinet) -if (WITH_FLAT_INSTALL) - set (wpinet_config_dir ${wpilib_dest}) -else() - set (wpinet_config_dir share/wpinet) -endif() - configure_file(wpinet-config.cmake.in ${WPILIB_BINARY_DIR}/wpinet-config.cmake ) -install(FILES ${WPILIB_BINARY_DIR}/wpinet-config.cmake DESTINATION ${wpinet_config_dir}) -install(EXPORT wpinet DESTINATION ${wpinet_config_dir}) +install(FILES ${WPILIB_BINARY_DIR}/wpinet-config.cmake DESTINATION share/wpinet) +install(EXPORT wpinet DESTINATION share/wpinet) subdir_list(wpinet_examples "${CMAKE_CURRENT_SOURCE_DIR}/examples") foreach(example ${wpinet_examples}) diff --git a/wpinet/src/main/native/thirdparty/libuv/include/uv.h b/wpinet/src/main/native/thirdparty/libuv/include/uv.h index d5342b0d522..aff06cb7f43 100644 --- a/wpinet/src/main/native/thirdparty/libuv/include/uv.h +++ b/wpinet/src/main/native/thirdparty/libuv/include/uv.h @@ -1876,6 +1876,18 @@ struct uv_loop_s { UV_EXTERN void* uv_loop_get_data(const uv_loop_t*); UV_EXTERN void uv_loop_set_data(uv_loop_t*, void* data); +/* String utilities needed internally for dealing with Windows. */ +size_t uv_utf16_length_as_wtf8(const uint16_t* utf16, + ssize_t utf16_len); +int uv_utf16_to_wtf8(const uint16_t* utf16, + ssize_t utf16_len, + char** wtf8_ptr, + size_t* wtf8_len_ptr); +ssize_t uv_wtf8_length_as_utf16(const char* wtf8); +void uv_wtf8_to_utf16(const char* wtf8, + uint16_t* utf16, + size_t utf16_len); + /* Don't export the private CPP symbols. */ #undef UV_HANDLE_TYPE_PRIVATE #undef UV_REQ_TYPE_PRIVATE diff --git a/wpinet/src/main/native/thirdparty/libuv/include/uv/version.h b/wpinet/src/main/native/thirdparty/libuv/include/uv/version.h index 24fac8d8aa7..b17220fcf90 100644 --- a/wpinet/src/main/native/thirdparty/libuv/include/uv/version.h +++ b/wpinet/src/main/native/thirdparty/libuv/include/uv/version.h @@ -31,7 +31,7 @@ */ #define UV_VERSION_MAJOR 1 -#define UV_VERSION_MINOR 46 +#define UV_VERSION_MINOR 47 #define UV_VERSION_PATCH 0 #define UV_VERSION_IS_RELEASE 1 #define UV_VERSION_SUFFIX "" diff --git a/wpinet/src/main/native/thirdparty/libuv/src/idna.cpp b/wpinet/src/main/native/thirdparty/libuv/src/idna.cpp index 36a39a08901..fe60fb59bea 100644 --- a/wpinet/src/main/native/thirdparty/libuv/src/idna.cpp +++ b/wpinet/src/main/native/thirdparty/libuv/src/idna.cpp @@ -1,4 +1,4 @@ -/* Copyright (c) 2011, 2018 Ben Noordhuis +/* Copyright libuv contributors. All rights reserved. * * Permission to use, copy, modify, and/or distribute this software for any * purpose with or without fee is hereby granted, provided that the above @@ -18,11 +18,56 @@ */ #include "uv.h" +#include "uv-common.h" #include "idna.h" #include #include #include /* UINT_MAX */ + +static int32_t uv__wtf8_decode1(const char** input) { + uint32_t code_point; + uint8_t b1; + uint8_t b2; + uint8_t b3; + uint8_t b4; + + b1 = **input; + if (b1 <= 0x7F) + return b1; /* ASCII code point */ + if (b1 < 0xC2) + return -1; /* invalid: continuation byte */ + code_point = b1; + + b2 = *++*input; + if ((b2 & 0xC0) != 0x80) + return -1; /* invalid: not a continuation byte */ + code_point = (code_point << 6) | (b2 & 0x3F); + if (b1 <= 0xDF) + return 0x7FF & code_point; /* two-byte character */ + + b3 = *++*input; + if ((b3 & 0xC0) != 0x80) + return -1; /* invalid: not a continuation byte */ + code_point = (code_point << 6) | (b3 & 0x3F); + if (b1 <= 0xEF) + return 0xFFFF & code_point; /* three-byte character */ + + b4 = *++*input; + if ((b4 & 0xC0) != 0x80) + return -1; /* invalid: not a continuation byte */ + code_point = (code_point << 6) | (b4 & 0x3F); + if (b1 <= 0xF4) { + code_point &= 0x1FFFFF; + if (code_point <= 0x10FFFF) + return code_point; /* four-byte character */ + } + + /* code point too large */ + return -1; +} + + static unsigned uv__utf8_decode1_slow(const char** p, const char* pe, unsigned a) { @@ -89,6 +134,7 @@ static unsigned uv__utf8_decode1_slow(const char** p, return a; } + unsigned uv__utf8_decode1(const char** p, const char* pe) { unsigned a; @@ -102,6 +148,7 @@ unsigned uv__utf8_decode1(const char** p, const char* pe) { return uv__utf8_decode1_slow(p, pe, a); } + static int uv__idna_toascii_label(const char* s, const char* se, char** d, char* de) { static const char alphabet[] = "abcdefghijklmnopqrstuvwxyz0123456789"; @@ -267,7 +314,8 @@ static int uv__idna_toascii_label(const char* s, const char* se, return 0; } -long uv__idna_toascii(const char* s, const char* se, char* d, char* de) { + +ssize_t uv__idna_toascii(const char* s, const char* se, char* d, char* de) { const char* si; const char* st; unsigned c; @@ -313,3 +361,195 @@ long uv__idna_toascii(const char* s, const char* se, char* d, char* de) { return d - ds; /* Number of bytes written. */ } + + +ssize_t uv_wtf8_length_as_utf16(const char* source_ptr) { + size_t w_target_len = 0; + int32_t code_point; + + do { + code_point = uv__wtf8_decode1(&source_ptr); + if (code_point < 0) + return -1; + if (code_point > 0xFFFF) + w_target_len++; + w_target_len++; + } while (*source_ptr++); + + return w_target_len; +} + + +void uv_wtf8_to_utf16(const char* source_ptr, + uint16_t* w_target, + [[maybe_unused]] size_t w_target_len) { + int32_t code_point; + + do { + code_point = uv__wtf8_decode1(&source_ptr); + /* uv_wtf8_length_as_utf16 should have been called and checked first. */ + assert(code_point >= 0); + if (code_point > 0x10000) { + assert(code_point < 0x10FFFF); + *w_target++ = (((code_point - 0x10000) >> 10) + 0xD800); + *w_target++ = ((code_point - 0x10000) & 0x3FF) + 0xDC00; + w_target_len -= 2; + } else { + *w_target++ = code_point; + w_target_len -= 1; + } + } while (*source_ptr++); + + assert(w_target_len == 0); +} + + +static int32_t uv__get_surrogate_value(const uint16_t* w_source_ptr, + ssize_t w_source_len) { + uint16_t u; + uint16_t next; + + u = w_source_ptr[0]; + if (u >= 0xD800 && u <= 0xDBFF && w_source_len != 1) { + next = w_source_ptr[1]; + if (next >= 0xDC00 && next <= 0xDFFF) + return 0x10000 + ((u - 0xD800) << 10) + (next - 0xDC00); + } + return u; +} + + +size_t uv_utf16_length_as_wtf8(const uint16_t* w_source_ptr, + ssize_t w_source_len) { + size_t target_len; + int32_t code_point; + + target_len = 0; + while (w_source_len) { + code_point = uv__get_surrogate_value(w_source_ptr, w_source_len); + /* Can be invalid UTF-8 but must be valid WTF-8. */ + assert(code_point >= 0); + if (w_source_len < 0 && code_point == 0) + break; + if (code_point < 0x80) + target_len += 1; + else if (code_point < 0x800) + target_len += 2; + else if (code_point < 0x10000) + target_len += 3; + else { + target_len += 4; + w_source_ptr++; + if (w_source_len > 0) + w_source_len--; + } + w_source_ptr++; + if (w_source_len > 0) + w_source_len--; + } + + return target_len; +} + + +int uv_utf16_to_wtf8(const uint16_t* w_source_ptr, + ssize_t w_source_len, + char** target_ptr, + size_t* target_len_ptr) { + size_t target_len; + char* target; + char* target_end; + int32_t code_point; + + /* If *target_ptr is provided, then *target_len_ptr must be its length + * (excluding space for NUL), otherwise we will compute the target_len_ptr + * length and may return a new allocation in *target_ptr if target_ptr is + * provided. */ + if (target_ptr == NULL || *target_ptr == NULL) { + target_len = uv_utf16_length_as_wtf8(w_source_ptr, w_source_len); + if (target_len_ptr != NULL) + *target_len_ptr = target_len; + } else { + target_len = *target_len_ptr; + } + + if (target_ptr == NULL) + return 0; + + if (*target_ptr == NULL) { + target = (char*)uv__malloc(target_len + 1); + if (target == NULL) { + return UV_ENOMEM; + } + *target_ptr = target; + } else { + target = *target_ptr; + } + + target_end = target + target_len; + + while (target != target_end && w_source_len) { + code_point = uv__get_surrogate_value(w_source_ptr, w_source_len); + /* Can be invalid UTF-8 but must be valid WTF-8. */ + assert(code_point >= 0); + if (w_source_len < 0 && code_point == 0) { + w_source_len = 0; + break; + } + if (code_point < 0x80) { + *target++ = code_point; + } else if (code_point < 0x800) { + *target++ = 0xC0 | (code_point >> 6); + if (target == target_end) + break; + *target++ = 0x80 | (code_point & 0x3F); + } else if (code_point < 0x10000) { + *target++ = 0xE0 | (code_point >> 12); + if (target == target_end) + break; + *target++ = 0x80 | ((code_point >> 6) & 0x3F); + if (target == target_end) + break; + *target++ = 0x80 | (code_point & 0x3F); + } else { + *target++ = 0xF0 | (code_point >> 18); + if (target == target_end) + break; + *target++ = 0x80 | ((code_point >> 12) & 0x3F); + if (target == target_end) + break; + *target++ = 0x80 | ((code_point >> 6) & 0x3F); + if (target == target_end) + break; + *target++ = 0x80 | (code_point & 0x3F); + /* uv__get_surrogate_value consumed 2 input characters */ + w_source_ptr++; + if (w_source_len > 0) + w_source_len--; + } + target_len = target - *target_ptr; + w_source_ptr++; + if (w_source_len > 0) + w_source_len--; + } + + if (target != target_end && target_len_ptr != NULL) + /* Did not fill all of the provided buffer, so update the target_len_ptr + * output with the space used. */ + *target_len_ptr = target - *target_ptr; + + /* Check if input fit into target exactly. */ + if (w_source_len < 0 && target == target_end && w_source_ptr[0] == 0) + w_source_len = 0; + + *target++ = '\0'; + + /* Characters remained after filling the buffer, compute the remaining length now. */ + if (w_source_len) { + if (target_len_ptr != NULL) + *target_len_ptr = target_len + uv_utf16_length_as_wtf8(w_source_ptr, w_source_len); + return UV_ENOBUFS; + } + + return 0; +} diff --git a/wpinet/src/main/native/thirdparty/libuv/src/idna.h b/wpinet/src/main/native/thirdparty/libuv/src/idna.h index 8e0c592fe13..ea6b4df9671 100644 --- a/wpinet/src/main/native/thirdparty/libuv/src/idna.h +++ b/wpinet/src/main/native/thirdparty/libuv/src/idna.h @@ -1,4 +1,4 @@ -/* Copyright (c) 2011, 2018 Ben Noordhuis +/* Copyright libuv contributors. All rights reserved. * * Permission to use, copy, modify, and/or distribute this software for any * purpose with or without fee is hereby granted, provided that the above @@ -26,6 +26,6 @@ unsigned uv__utf8_decode1(const char** p, const char* pe); * is the number of bytes written to |d|, including the trailing nul byte. * A return value < 0 is a libuv error code. |s| and |d| can not overlap. */ -long uv__idna_toascii(const char* s, const char* se, char* d, char* de); +ssize_t uv__idna_toascii(const char* s, const char* se, char* d, char* de); #endif /* UV_SRC_IDNA_H_ */ diff --git a/wpinet/src/main/native/thirdparty/libuv/src/unix/darwin.cpp b/wpinet/src/main/native/thirdparty/libuv/src/unix/darwin.cpp index 9ee5cd8eb9d..dc93d236b6b 100644 --- a/wpinet/src/main/native/thirdparty/libuv/src/unix/darwin.cpp +++ b/wpinet/src/main/native/thirdparty/libuv/src/unix/darwin.cpp @@ -209,7 +209,7 @@ int uv_cpu_info(uv_cpu_info_t** cpu_infos, int* count) { if (cpuspeed == 0) /* If sysctl hw.cputype == CPU_TYPE_ARM64, the correct value is unavailable * from Apple, but we can hard-code it here to a plausible value. */ - cpuspeed = 2400000000; + cpuspeed = 2400000000U; if (host_processor_info(mach_host_self(), PROCESSOR_CPU_LOAD_INFO, &numcpus, (processor_info_array_t*)&info, @@ -235,7 +235,7 @@ int uv_cpu_info(uv_cpu_info_t** cpu_infos, int* count) { cpu_info->cpu_times.irq = 0; cpu_info->model = uv__strdup(model); - cpu_info->speed = cpuspeed/1000000; + cpu_info->speed = (int)(cpuspeed / 1000000); } vm_deallocate(mach_task_self(), (vm_address_t)info, msg_type); diff --git a/wpinet/src/main/native/thirdparty/libuv/src/unix/fs.cpp b/wpinet/src/main/native/thirdparty/libuv/src/unix/fs.cpp index aba190a9c02..afdf0c6e592 100644 --- a/wpinet/src/main/native/thirdparty/libuv/src/unix/fs.cpp +++ b/wpinet/src/main/native/thirdparty/libuv/src/unix/fs.cpp @@ -41,27 +41,12 @@ #include #include #include -#include #include #include #include #include -#if defined(__DragonFly__) || \ - defined(__FreeBSD__) || \ - defined(__OpenBSD__) || \ - defined(__NetBSD__) -# define HAVE_PREADV 1 -#else -# define HAVE_PREADV 0 -#endif - -/* preadv() and pwritev() were added in Android N (level 24) */ -#if defined(__linux__) && !(defined(__ANDROID__) && __ANDROID_API__ < 24) -# define TRY_PREADV 1 -#endif - #if defined(__linux__) # include #endif @@ -99,6 +84,15 @@ # include #endif +#if defined(__CYGWIN__) || \ + (defined(__HAIKU__) && B_HAIKU_VERSION < B_HAIKU_VERSION_1_PRE_BETA_5) || \ + (defined(__sun) && !defined(__illumos__)) +#define preadv(fd, bufs, nbufs, off) \ + pread(fd, (bufs)->iov_base, (bufs)->iov_len, off) +#define pwritev(fd, bufs, nbufs, off) \ + pwrite(fd, (bufs)->iov_base, (bufs)->iov_len, off) +#endif + #if defined(_AIX) && _XOPEN_SOURCE <= 600 extern char *mkdtemp(char *template); /* See issue #740 on AIX < 7 */ #endif @@ -412,123 +406,57 @@ static ssize_t uv__fs_open(uv_fs_t* req) { } -#if !HAVE_PREADV -static ssize_t uv__fs_preadv(uv_file fd, - uv_buf_t* bufs, - unsigned int nbufs, - off_t off) { - uv_buf_t* buf; - uv_buf_t* end; - ssize_t result; - ssize_t rc; - size_t pos; - - assert(nbufs > 0); - - result = 0; - pos = 0; - buf = bufs + 0; - end = bufs + nbufs; - - for (;;) { - do - rc = pread(fd, buf->base + pos, buf->len - pos, off + result); - while (rc == -1 && errno == EINTR); - - if (rc == 0) - break; - - if (rc == -1 && result == 0) - return UV__ERR(errno); - - if (rc == -1) - break; /* We read some data so return that, ignore the error. */ - - pos += rc; - result += rc; - - if (pos < buf->len) - continue; - - pos = 0; - buf += 1; - - if (buf == end) - break; - } - - return result; -} -#endif - - static ssize_t uv__fs_read(uv_fs_t* req) { -#if TRY_PREADV - static std::atomic no_preadv; -#endif + const struct iovec* bufs; unsigned int iovmax; - ssize_t result; + size_t nbufs; + ssize_t r; + off_t off; + int fd; + + fd = req->file; + off = req->off; + bufs = (const struct iovec*) req->bufs; + nbufs = req->nbufs; iovmax = uv__getiovmax(); - if (req->nbufs > iovmax) - req->nbufs = iovmax; - - if (req->off < 0) { - if (req->nbufs == 1) - result = read(req->file, req->bufs[0].base, req->bufs[0].len); - else - result = readv(req->file, (struct iovec*) req->bufs, req->nbufs); + if (nbufs > iovmax) + nbufs = iovmax; + + r = 0; + if (off < 0) { + if (nbufs == 1) + r = read(fd, bufs->iov_base, bufs->iov_len); + else if (nbufs > 1) + r = readv(fd, bufs, nbufs); } else { - if (req->nbufs == 1) { - result = pread(req->file, req->bufs[0].base, req->bufs[0].len, req->off); - goto done; - } - -#if HAVE_PREADV - result = preadv(req->file, (struct iovec*) req->bufs, req->nbufs, req->off); -#else -# if TRY_PREADV - if (atomic_load_explicit(&no_preadv, std::memory_order_relaxed)) retry: -# endif - { - result = uv__fs_preadv(req->file, req->bufs, req->nbufs, req->off); - } -# if TRY_PREADV - else { - result = preadv(req->file, - (struct iovec*) req->bufs, - req->nbufs, - req->off); - if (result == -1 && errno == ENOSYS) { - atomic_store_explicit(&no_preadv, 1, std::memory_order_relaxed); - goto retry; - } - } -# endif -#endif + if (nbufs == 1) + r = pread(fd, bufs->iov_base, bufs->iov_len, off); + else if (nbufs > 1) + r = preadv(fd, bufs, nbufs, off); } -done: - /* Early cleanup of bufs allocation, since we're done with it. */ - if (req->bufs != req->bufsml) - uv__free(req->bufs); - - req->bufs = NULL; - req->nbufs = 0; - #ifdef __PASE__ /* PASE returns EOPNOTSUPP when reading a directory, convert to EISDIR */ - if (result == -1 && errno == EOPNOTSUPP) { + if (r == -1 && errno == EOPNOTSUPP) { struct stat buf; ssize_t rc; - rc = uv__fstat(req->file, &buf); + rc = uv__fstat(fd, &buf); if (rc == 0 && S_ISDIR(buf.st_mode)) { errno = EISDIR; } } #endif - return result; + /* We don't own the buffer list in the synchronous case. */ + if (req->cb != NULL) + if (req->bufs != req->bufsml) + uv__free(req->bufs); + + req->bufs = NULL; + req->nbufs = 0; + + return r; } @@ -1163,65 +1091,34 @@ static ssize_t uv__fs_lutime(uv_fs_t* req) { static ssize_t uv__fs_write(uv_fs_t* req) { -#if TRY_PREADV - static std::atomic no_pwritev; -#endif + const struct iovec* bufs; + size_t nbufs; ssize_t r; + off_t off; + int fd; - /* Serialize writes on OS X, concurrent write() and pwrite() calls result in - * data loss. We can't use a per-file descriptor lock, the descriptor may be - * a dup(). - */ -#if defined(__APPLE__) - static pthread_mutex_t lock = PTHREAD_MUTEX_INITIALIZER; - - if (pthread_mutex_lock(&lock)) - abort(); -#endif + fd = req->file; + off = req->off; + bufs = (const struct iovec*) req->bufs; + nbufs = req->nbufs; - if (req->off < 0) { - if (req->nbufs == 1) - r = write(req->file, req->bufs[0].base, req->bufs[0].len); - else - r = writev(req->file, (struct iovec*) req->bufs, req->nbufs); + r = 0; + if (off < 0) { + if (nbufs == 1) + r = write(fd, bufs->iov_base, bufs->iov_len); + else if (nbufs > 1) + r = writev(fd, bufs, nbufs); } else { - if (req->nbufs == 1) { - r = pwrite(req->file, req->bufs[0].base, req->bufs[0].len, req->off); - goto done; - } -#if HAVE_PREADV - r = pwritev(req->file, (struct iovec*) req->bufs, req->nbufs, req->off); -#else -# if TRY_PREADV - if (atomic_load_explicit(&no_pwritev, std::memory_order_relaxed)) retry: -# endif - { - r = pwrite(req->file, req->bufs[0].base, req->bufs[0].len, req->off); - } -# if TRY_PREADV - else { - r = pwritev(req->file, - (struct iovec*) req->bufs, - req->nbufs, - req->off); - if (r == -1 && errno == ENOSYS) { - atomic_store_explicit(&no_pwritev, 1, std::memory_order_relaxed); - goto retry; - } - } -# endif -#endif + if (nbufs == 1) + r = pwrite(fd, bufs->iov_base, bufs->iov_len, off); + else if (nbufs > 1) + r = pwritev(fd, bufs, nbufs, off); } -done: -#if defined(__APPLE__) - if (pthread_mutex_unlock(&lock)) - abort(); -#endif - return r; } + static ssize_t uv__fs_copyfile(uv_fs_t* req) { uv_fs_t fs_req; uv_file srcfd; @@ -1981,9 +1878,14 @@ int uv_fs_read(uv_loop_t* loop, uv_fs_t* req, if (bufs == NULL || nbufs == 0) return UV_EINVAL; + req->off = off; req->file = file; - + req->bufs = (uv_buf_t*) bufs; /* Safe, doesn't mutate |bufs| */ req->nbufs = nbufs; + + if (cb == NULL) + goto post; + req->bufs = req->bufsml; if (nbufs > ARRAY_SIZE(req->bufsml)) req->bufs = (uv_buf_t*)uv__malloc(nbufs * sizeof(*bufs)); @@ -1993,12 +1895,10 @@ int uv_fs_read(uv_loop_t* loop, uv_fs_t* req, memcpy(req->bufs, bufs, nbufs * sizeof(*bufs)); - req->off = off; - - if (cb != NULL) - if (uv__iou_fs_read_or_write(loop, req, /* is_read */ 1)) - return 0; + if (uv__iou_fs_read_or_write(loop, req, /* is_read */ 1)) + return 0; +post: POST; } diff --git a/wpinet/src/main/native/thirdparty/libuv/src/unix/kqueue.cpp b/wpinet/src/main/native/thirdparty/libuv/src/unix/kqueue.cpp index ffe0f9191cc..939a4269677 100644 --- a/wpinet/src/main/native/thirdparty/libuv/src/unix/kqueue.cpp +++ b/wpinet/src/main/native/thirdparty/libuv/src/unix/kqueue.cpp @@ -30,6 +30,9 @@ #include #include #include +#if defined(__FreeBSD__) +#include +#endif #include #include #include @@ -264,6 +267,9 @@ void uv__io_poll(uv_loop_t* loop, int timeout) { if (nfds == -1) assert(errno == EINTR); + else if (nfds == 0) + /* Unlimited timeout should only return with events or signal. */ + assert(timeout != -1); if (pset != NULL) pthread_sigmask(SIG_UNBLOCK, pset, NULL); @@ -288,8 +294,6 @@ void uv__io_poll(uv_loop_t* loop, int timeout) { timeout = user_timeout; reset_timeout = 0; } else if (nfds == 0) { - /* Reached the user timeout value. */ - assert(timeout != -1); return; } @@ -481,6 +485,16 @@ static void uv__fs_event(uv_loop_t* loop, uv__io_t* w, unsigned int fflags) { */ if (fcntl(handle->event_watcher.fd, F_GETPATH, pathbuf) == 0) path = uv__basename_r(pathbuf); +#elif defined(F_KINFO) + /* We try to get the file info reference from the file descriptor. + * the struct's kf_structsize must be initialised beforehand + * whether with the KINFO_FILE_SIZE constant or this way. + */ + struct kinfo_file kf; + kf.kf_structsize = sizeof(kf); + + if (fcntl(handle->event_watcher.fd, F_KINFO, &kf) == 0) + path = uv__basename_r(kf.kf_path); #endif handle->cb(handle, path, events, 0); diff --git a/wpinet/src/main/native/thirdparty/libuv/src/unix/linux.cpp b/wpinet/src/main/native/thirdparty/libuv/src/unix/linux.cpp index d365b623a0a..95a2fe2b428 100644 --- a/wpinet/src/main/native/thirdparty/libuv/src/unix/linux.cpp +++ b/wpinet/src/main/native/thirdparty/libuv/src/unix/linux.cpp @@ -78,6 +78,8 @@ # define __NR_copy_file_range 379 # elif defined(__arc__) # define __NR_copy_file_range 285 +# elif defined(__riscv) +# define __NR_copy_file_range 285 # endif #endif /* __NR_copy_file_range */ @@ -94,6 +96,8 @@ # define __NR_statx 383 # elif defined(__s390__) # define __NR_statx 379 +# elif defined(__riscv) +# define __NR_statx 291 # endif #endif /* __NR_statx */ @@ -110,6 +114,8 @@ # define __NR_getrandom 359 # elif defined(__s390__) # define __NR_getrandom 349 +# elif defined(__riscv) +# define __NR_getrandom 278 # endif #endif /* __NR_getrandom */ @@ -318,17 +324,64 @@ unsigned uv__kernel_version(void) { unsigned major; unsigned minor; unsigned patch; + char v_sig[256]; + char* needle; version = std::atomic_load_explicit(&cached_version, std::memory_order_relaxed); if (version != 0) return version; + /* Check /proc/version_signature first as it's the way to get the mainline + * kernel version in Ubuntu. The format is: + * Ubuntu ubuntu_kernel_version mainline_kernel_version + * For example: + * Ubuntu 5.15.0-79.86-generic 5.15.111 + */ + if (0 == uv__slurp("/proc/version_signature", v_sig, sizeof(v_sig))) + if (3 == sscanf(v_sig, "Ubuntu %*s %u.%u.%u", &major, &minor, &patch)) + goto calculate_version; + if (-1 == uname(&u)) return 0; + /* In Debian we need to check `version` instead of `release` to extract the + * mainline kernel version. This is an example of how it looks like: + * #1 SMP Debian 5.10.46-4 (2021-08-03) + */ + needle = strstr(u.version, "Debian "); + if (needle != NULL) + if (3 == sscanf(needle, "Debian %u.%u.%u", &major, &minor, &patch)) + goto calculate_version; + if (3 != sscanf(u.release, "%u.%u.%u", &major, &minor, &patch)) return 0; + /* Handle it when the process runs under the UNAME26 personality: + * + * - kernels >= 3.x identify as 2.6.40+x + * - kernels >= 4.x identify as 2.6.60+x + * + * UNAME26 is a poorly conceived hack that doesn't let us distinguish + * between 4.x kernels and 5.x/6.x kernels so we conservatively assume + * that 2.6.60+x means 4.x. + * + * Fun fact of the day: it's technically possible to observe the actual + * kernel version for a brief moment because uname() first copies out the + * real release string before overwriting it with the backcompat string. + */ + if (major == 2 && minor == 6) { + if (patch >= 60) { + major = 4; + minor = patch - 60; + patch = 0; + } else if (patch >= 40) { + major = 3; + minor = patch - 40; + patch = 0; + } + } + +calculate_version: version = major * 65536 + minor * 256 + patch; std::atomic_store_explicit(&cached_version, version, std::memory_order_relaxed); @@ -423,6 +476,9 @@ int uv__io_uring_register(int fd, unsigned opcode, void* arg, unsigned nargs) { static int uv__use_io_uring(void) { #if defined(__ANDROID_API__) return 0; /* Possibly available but blocked by seccomp. */ +#elif defined(__arm__) && __SIZEOF_POINTER__ == 4 + /* See https://github.com/libuv/libuv/issues/4158. */ + return 0; /* All 32 bits kernels appear buggy. */ #else /* Ternary: unknown=0, yes=1, no=-1 */ static std::atomic use_io_uring; @@ -432,8 +488,14 @@ static int uv__use_io_uring(void) { use = std::atomic_load_explicit(&use_io_uring, std::memory_order_relaxed); if (use == 0) { + /* Older kernels have a bug where the sqpoll thread uses 100% CPU. */ + use = uv__kernel_version() >= /* 5.10.186 */ 0x050ABA ? 1 : -1; + + /* But users can still enable it if they so desire. */ val = getenv("UV_USE_IO_URING"); - use = val == NULL || atoi(val) ? 1 : -1; + if (val != NULL) + use = atoi(val) ? 1 : -1; + std::atomic_store_explicit(&use_io_uring, use, std::memory_order_relaxed); } @@ -757,7 +819,9 @@ static void uv__iou_submit(struct uv__iou* iou) { int uv__iou_fs_close(uv_loop_t* loop, uv_fs_t* req) { struct uv__io_uring_sqe* sqe; struct uv__iou* iou; + int kv; + kv = uv__kernel_version(); /* Work around a poorly understood bug in older kernels where closing a file * descriptor pointing to /foo/bar results in ETXTBSY errors when trying to * execve("/foo/bar") later on. The bug seems to have been fixed somewhere @@ -765,10 +829,17 @@ int uv__iou_fs_close(uv_loop_t* loop, uv_fs_t* req) { * but good candidates are the several data race fixes. Interestingly, it * seems to manifest only when running under Docker so the possibility of * a Docker bug can't be completely ruled out either. Yay, computers. + * Also, disable on non-longterm versions between 5.16.0 (non-longterm) and + * 6.1.0 (longterm). Starting with longterm 6.1.x, the issue seems to be + * solved. */ - if (uv__kernel_version() < /* 5.15.90 */ 0x050F5A) + if (kv < /* 5.15.90 */ 0x050F5A) + return 0; + + if (kv >= /* 5.16.0 */ 0x050A00 && kv < /* 6.1.0 */ 0x060100) return 0; + iou = &uv__get_internal_fields(loop)->iou; sqe = uv__iou_get_sqe(iou, loop, req); @@ -1365,41 +1436,20 @@ void uv__io_poll(uv_loop_t* loop, int timeout) { */ SAVE_ERRNO(uv__update_time(loop)); - if (nfds == 0) { + if (nfds == -1) + assert(errno == EINTR); + else if (nfds == 0) + /* Unlimited timeout should only return with events or signal. */ assert(timeout != -1); + if (nfds == 0 || nfds == -1) { if (reset_timeout != 0) { timeout = user_timeout; reset_timeout = 0; + } else if (nfds == 0) { + return; } - if (timeout == -1) - continue; - - if (timeout == 0) - break; - - /* We may have been inside the system call for longer than |timeout| - * milliseconds so we need to update the timestamp to avoid drift. - */ - goto update_timeout; - } - - if (nfds == -1) { - if (errno != EINTR) - abort(); - - if (reset_timeout != 0) { - timeout = user_timeout; - reset_timeout = 0; - } - - if (timeout == -1) - continue; - - if (timeout == 0) - break; - /* Interrupted by a signal. Update timeout and poll again. */ goto update_timeout; } @@ -1510,13 +1560,13 @@ void uv__io_poll(uv_loop_t* loop, int timeout) { break; } +update_timeout: if (timeout == 0) break; if (timeout == -1) continue; -update_timeout: assert(timeout > 0); real_timeout -= (loop->time - base); @@ -1719,11 +1769,8 @@ int uv_cpu_info(uv_cpu_info_t** ci, int* count) { return UV__ERR(errno); } - /* Skip first line. */ - if (!fgets(buf, sizeof(buf), fp)) { - uv__free(cpus); - return UV__ERR(errno); - } + if (NULL == fgets(buf, sizeof(buf), fp)) + abort(); for (;;) { memset(&t, 0, sizeof(t)); @@ -1734,10 +1781,8 @@ int uv_cpu_info(uv_cpu_info_t** ci, int* count) { if (n != 7) break; - /* Skip rest of line. */ - if (!fgets(buf, sizeof(buf), fp)) { - break; - } + if (NULL == fgets(buf, sizeof(buf), fp)) + abort(); if (cpu >= ARRAY_SIZE(*cpus)) continue; @@ -1817,9 +1862,8 @@ int uv_cpu_info(uv_cpu_info_t** ci, int* count) { if (fp == NULL) continue; - if (0 > fscanf(fp, "%llu", &(*cpus)[cpu].freq)) { - (*cpus)[cpu].freq = 0llu; - } + if (1 != fscanf(fp, "%llu", &(*cpus)[cpu].freq)) + abort(); fclose(fp); fp = NULL; } diff --git a/wpinet/src/main/native/thirdparty/libuv/src/unix/signal.cpp b/wpinet/src/main/native/thirdparty/libuv/src/unix/signal.cpp index 63aba5a60e0..bc4206e6d86 100644 --- a/wpinet/src/main/native/thirdparty/libuv/src/unix/signal.cpp +++ b/wpinet/src/main/native/thirdparty/libuv/src/unix/signal.cpp @@ -279,6 +279,8 @@ static int uv__signal_loop_once_init(uv_loop_t* loop) { int uv__signal_loop_fork(uv_loop_t* loop) { + struct uv__queue* q; + if (loop->signal_pipefd[0] == -1) return 0; uv__io_stop(loop, &loop->signal_io_watcher, POLLIN); @@ -286,6 +288,19 @@ int uv__signal_loop_fork(uv_loop_t* loop) { uv__close(loop->signal_pipefd[1]); loop->signal_pipefd[0] = -1; loop->signal_pipefd[1] = -1; + + uv__queue_foreach(q, &loop->handle_queue) { + uv_handle_t* handle = uv__queue_data(q, uv_handle_t, handle_queue); + uv_signal_t* sh; + + if (handle->type != UV_SIGNAL) + continue; + + sh = (uv_signal_t*) handle; + sh->caught_signals = 0; + sh->dispatched_signals = 0; + } + return uv__signal_loop_once_init(loop); } diff --git a/wpinet/src/main/native/thirdparty/libuv/src/unix/tcp.cpp b/wpinet/src/main/native/thirdparty/libuv/src/unix/tcp.cpp index d6c848f4610..4c4213a4241 100644 --- a/wpinet/src/main/native/thirdparty/libuv/src/unix/tcp.cpp +++ b/wpinet/src/main/native/thirdparty/libuv/src/unix/tcp.cpp @@ -27,6 +27,17 @@ #include #include +#include +#include + +#if defined(__PASE__) +#include +#define ifaddrs ifaddrs_pase +#define getifaddrs Qp2getifaddrs +#define freeifaddrs Qp2freeifaddrs +#else +#include +#endif static int maybe_bind_socket(int fd) { union uv__sockaddr s; @@ -198,11 +209,50 @@ int uv__tcp_bind(uv_tcp_t* tcp, } +static int uv__is_ipv6_link_local(const struct sockaddr* addr) { + const struct sockaddr_in6* a6; + uint8_t b[2]; + + if (addr->sa_family != AF_INET6) + return 0; + + a6 = (const struct sockaddr_in6*) addr; + memcpy(b, &a6->sin6_addr, sizeof(b)); + + return b[0] == 0xFE && b[1] == 0x80; +} + + +static int uv__ipv6_link_local_scope_id(void) { + struct sockaddr_in6* a6; + struct ifaddrs* ifa; + struct ifaddrs* p; + int rv; + + if (getifaddrs(&ifa)) + return 0; + + for (p = ifa; p != NULL; p = p->ifa_next) + if (uv__is_ipv6_link_local(p->ifa_addr)) + break; + + rv = 0; + if (p != NULL) { + a6 = (struct sockaddr_in6*) p->ifa_addr; + rv = a6->sin6_scope_id; + } + + freeifaddrs(ifa); + return rv; +} + + int uv__tcp_connect(uv_connect_t* req, uv_tcp_t* handle, const struct sockaddr* addr, unsigned int addrlen, uv_connect_cb cb) { + struct sockaddr_in6 tmp6; int err; int r; @@ -220,6 +270,14 @@ int uv__tcp_connect(uv_connect_t* req, if (err) return err; + if (uv__is_ipv6_link_local(addr)) { + memcpy(&tmp6, addr, sizeof(tmp6)); + if (tmp6.sin6_scope_id == 0) { + tmp6.sin6_scope_id = uv__ipv6_link_local_scope_id(); + addr = (const struct sockaddr*) &tmp6; + } + } + do { errno = 0; r = connect(uv__stream_fd(handle), addr, addrlen); @@ -374,28 +432,39 @@ int uv__tcp_nodelay(int fd, int on) { int uv__tcp_keepalive(int fd, int on, unsigned int delay) { + int intvl; + int cnt; + + (void) &intvl; + (void) &cnt; + if (setsockopt(fd, SOL_SOCKET, SO_KEEPALIVE, &on, sizeof(on))) return UV__ERR(errno); + if (!on) + return 0; + #ifdef TCP_KEEPIDLE - if (on) { - int intvl = 1; /* 1 second; same as default on Win32 */ - int cnt = 10; /* 10 retries; same as hardcoded on Win32 */ - if (setsockopt(fd, IPPROTO_TCP, TCP_KEEPIDLE, &delay, sizeof(delay))) - return UV__ERR(errno); - if (setsockopt(fd, IPPROTO_TCP, TCP_KEEPINTVL, &intvl, sizeof(intvl))) - return UV__ERR(errno); - if (setsockopt(fd, IPPROTO_TCP, TCP_KEEPCNT, &cnt, sizeof(cnt))) - return UV__ERR(errno); - } + if (setsockopt(fd, IPPROTO_TCP, TCP_KEEPIDLE, &delay, sizeof(delay))) + return UV__ERR(errno); +/* Solaris/SmartOS, if you don't support keep-alive, + * then don't advertise it in your system headers... + */ +/* FIXME(bnoordhuis) That's possibly because sizeof(delay) should be 1. */ +#elif defined(TCP_KEEPALIVE) && !defined(__sun) + if (setsockopt(fd, IPPROTO_TCP, TCP_KEEPALIVE, &delay, sizeof(delay))) + return UV__ERR(errno); #endif - /* Solaris/SmartOS, if you don't support keep-alive, - * then don't advertise it in your system headers... - */ - /* FIXME(bnoordhuis) That's possibly because sizeof(delay) should be 1. */ -#if defined(TCP_KEEPALIVE) && !defined(__sun) - if (on && setsockopt(fd, IPPROTO_TCP, TCP_KEEPALIVE, &delay, sizeof(delay))) +#ifdef TCP_KEEPINTVL + intvl = 1; /* 1 second; same as default on Win32 */ + if (setsockopt(fd, IPPROTO_TCP, TCP_KEEPINTVL, &intvl, sizeof(intvl))) + return UV__ERR(errno); +#endif + +#ifdef TCP_KEEPCNT + cnt = 10; /* 10 retries; same as hardcoded on Win32 */ + if (setsockopt(fd, IPPROTO_TCP, TCP_KEEPCNT, &cnt, sizeof(cnt))) return UV__ERR(errno); #endif diff --git a/wpinet/src/main/native/thirdparty/libuv/src/unix/thread.cpp b/wpinet/src/main/native/thirdparty/libuv/src/unix/thread.cpp index f8600947e3e..688c042e1ae 100644 --- a/wpinet/src/main/native/thirdparty/libuv/src/unix/thread.cpp +++ b/wpinet/src/main/native/thirdparty/libuv/src/unix/thread.cpp @@ -782,11 +782,33 @@ void uv_cond_broadcast(uv_cond_t* cond) { abort(); } +#if defined(__APPLE__) && defined(__MACH__) + +void uv_cond_wait(uv_cond_t* cond, uv_mutex_t* mutex) { + int r; + + errno = 0; + r = pthread_cond_wait(cond, mutex); + + /* Workaround for a bug in OS X at least up to 13.6 + * See https://github.com/libuv/libuv/issues/4165 + */ + if (r == EINVAL) + if (errno == EBUSY) + return; + + if (r) + abort(); +} + +#else /* !(defined(__APPLE__) && defined(__MACH__)) */ + void uv_cond_wait(uv_cond_t* cond, uv_mutex_t* mutex) { if (pthread_cond_wait(cond, mutex)) abort(); } +#endif int uv_cond_timedwait(uv_cond_t* cond, uv_mutex_t* mutex, uint64_t timeout) { int r; diff --git a/wpinet/src/main/native/thirdparty/libuv/src/uv-common.cpp b/wpinet/src/main/native/thirdparty/libuv/src/uv-common.cpp index 5c6d8415540..569c6906b7e 100644 --- a/wpinet/src/main/native/thirdparty/libuv/src/uv-common.cpp +++ b/wpinet/src/main/native/thirdparty/libuv/src/uv-common.cpp @@ -559,6 +559,9 @@ static void uv__print_handles(uv_loop_t* loop, int only_active, FILE* stream) { if (loop == NULL) loop = uv_default_loop(); + if (stream == NULL) + stream = stderr; + uv__queue_foreach(q, &loop->handle_queue) { h = uv__queue_data(q, uv_handle_t, handle_queue); diff --git a/wpinet/src/main/native/thirdparty/libuv/src/win/dl.cpp b/wpinet/src/main/native/thirdparty/libuv/src/win/dl.cpp index 676be4dc7b5..d88400f0e81 100644 --- a/wpinet/src/main/native/thirdparty/libuv/src/win/dl.cpp +++ b/wpinet/src/main/native/thirdparty/libuv/src/win/dl.cpp @@ -27,18 +27,17 @@ static int uv__dlerror(uv_lib_t* lib, const char* filename, DWORD errorno); int uv_dlopen(const char* filename, uv_lib_t* lib) { WCHAR filename_w[32768]; + ssize_t r; lib->handle = NULL; lib->errmsg = NULL; - if (!MultiByteToWideChar(CP_UTF8, - 0, - filename, - -1, - filename_w, - ARRAY_SIZE(filename_w))) { - return uv__dlerror(lib, filename, GetLastError()); - } + r = uv_wtf8_length_as_utf16(filename); + if (r < 0) + return uv__dlerror(lib, filename, ERROR_NO_UNICODE_TRANSLATION); + if ((size_t) r > ARRAY_SIZE(filename_w)) + return uv__dlerror(lib, filename, ERROR_INSUFFICIENT_BUFFER); + uv_wtf8_to_utf16(filename, (uint16_t*)filename_w, r); lib->handle = LoadLibraryExW(filename_w, NULL, LOAD_WITH_ALTERED_SEARCH_PATH); if (lib->handle == NULL) { diff --git a/wpinet/src/main/native/thirdparty/libuv/src/win/fs-event.cpp b/wpinet/src/main/native/thirdparty/libuv/src/win/fs-event.cpp index 3244a4e4320..381220bf241 100644 --- a/wpinet/src/main/native/thirdparty/libuv/src/win/fs-event.cpp +++ b/wpinet/src/main/native/thirdparty/libuv/src/win/fs-event.cpp @@ -159,7 +159,8 @@ int uv_fs_event_start(uv_fs_event_t* handle, uv_fs_event_cb cb, const char* path, unsigned int flags) { - int name_size, is_path_dir, size; + int is_path_dir; + size_t size; DWORD attr, last_error; WCHAR* dir = NULL, *dir_to_watch, *pathw = NULL; DWORD short_path_buffer_len; @@ -178,23 +179,9 @@ int uv_fs_event_start(uv_fs_event_t* handle, uv__handle_start(handle); - /* Convert name to UTF16. */ - - name_size = MultiByteToWideChar(CP_UTF8, 0, path, -1, NULL, 0) * - sizeof(WCHAR); - pathw = (WCHAR*)uv__malloc(name_size); - if (!pathw) { - uv_fatal_error(ERROR_OUTOFMEMORY, "uv__malloc"); - } - - if (!MultiByteToWideChar(CP_UTF8, - 0, - path, - -1, - pathw, - name_size / sizeof(WCHAR))) { - return uv_translate_sys_error(GetLastError()); - } + last_error = uv__convert_utf8_to_utf16(path, &pathw); + if (last_error) + goto error_uv; /* Determine whether path is a file or a directory. */ attr = GetFileAttributesW(pathw); @@ -335,6 +322,9 @@ int uv_fs_event_start(uv_fs_event_t* handle, return 0; error: + last_error = uv_translate_sys_error(last_error); + +error_uv: if (handle->path) { uv__free(handle->path); handle->path = NULL; @@ -367,7 +357,7 @@ int uv_fs_event_start(uv_fs_event_t* handle, uv__free(short_path); - return uv_translate_sys_error(last_error); + return last_error; } diff --git a/wpinet/src/main/native/thirdparty/libuv/src/win/fs.cpp b/wpinet/src/main/native/thirdparty/libuv/src/win/fs.cpp index f415ddc2c39..26ba5399367 100644 --- a/wpinet/src/main/native/thirdparty/libuv/src/win/fs.cpp +++ b/wpinet/src/main/native/thirdparty/libuv/src/win/fs.cpp @@ -33,13 +33,16 @@ #include #include "uv.h" + +/* requires , included via "uv.h" above, but needs to + be included before our "winapi.h", included via "internal.h" below. */ +#include + #include "internal.h" #include "req-inl.h" #include "handle-inl.h" #include "fs-fd-hash-inl.h" -#include - #pragma comment(lib, "Advapi32.lib") #define UV_FS_FREE_PATHS 0x0002 @@ -147,279 +150,6 @@ void uv__fs_init(void) { } -static int32_t fs__decode_wtf8_char(const char** input) { - uint32_t code_point; - uint8_t b1; - uint8_t b2; - uint8_t b3; - uint8_t b4; - - b1 = **input; - if (b1 <= 0x7F) - return b1; /* ASCII code point */ - if (b1 < 0xC2) - return -1; /* invalid: continuation byte */ - code_point = b1; - - b2 = *++*input; - if ((b2 & 0xC0) != 0x80) - return -1; /* invalid: not a continuation byte */ - code_point = (code_point << 6) | (b2 & 0x3F); - if (b1 <= 0xDF) - return 0x7FF & code_point; /* two-byte character */ - - b3 = *++*input; - if ((b3 & 0xC0) != 0x80) - return -1; /* invalid: not a continuation byte */ - code_point = (code_point << 6) | (b3 & 0x3F); - if (b1 <= 0xEF) - return 0xFFFF & code_point; /* three-byte character */ - - b4 = *++*input; - if ((b4 & 0xC0) != 0x80) - return -1; /* invalid: not a continuation byte */ - code_point = (code_point << 6) | (b4 & 0x3F); - if (b1 <= 0xF4) - if (code_point <= 0x10FFFF) - return code_point; /* four-byte character */ - - /* code point too large */ - return -1; -} - - -static ssize_t fs__get_length_wtf8(const char* source_ptr) { - size_t w_target_len = 0; - int32_t code_point; - - do { - code_point = fs__decode_wtf8_char(&source_ptr); - if (code_point < 0) - return -1; - if (code_point > 0xFFFF) - w_target_len++; - w_target_len++; - } while (*source_ptr++); - return w_target_len; -} - - -static void fs__wtf8_to_wide(const char* source_ptr, WCHAR* w_target) { - int32_t code_point; - - do { - code_point = fs__decode_wtf8_char(&source_ptr); - /* fs__get_length_wtf8 should have been called and checked first. */ - assert(code_point >= 0); - if (code_point > 0x10000) { - assert(code_point < 0x10FFFF); - *w_target++ = (((code_point - 0x10000) >> 10) + 0xD800); - *w_target++ = ((code_point - 0x10000) & 0x3FF) + 0xDC00; - } else { - *w_target++ = code_point; - } - } while (*source_ptr++); -} - - -INLINE static int fs__capture_path(uv_fs_t* req, const char* path, - const char* new_path, const int copy_path) { - WCHAR* buf; - WCHAR* pos; - size_t buf_sz = 0; - size_t path_len = 0; - ssize_t pathw_len = 0; - ssize_t new_pathw_len = 0; - - /* new_path can only be set if path is also set. */ - assert(new_path == NULL || path != NULL); - - if (path != NULL) { - pathw_len = fs__get_length_wtf8(path); - if (pathw_len < 0) - return ERROR_INVALID_NAME; - buf_sz += pathw_len * sizeof(WCHAR); - } - - if (path != NULL && copy_path) { - path_len = 1 + strlen(path); - buf_sz += path_len; - } - - if (new_path != NULL) { - new_pathw_len = fs__get_length_wtf8(new_path); - if (new_pathw_len < 0) - return ERROR_INVALID_NAME; - buf_sz += new_pathw_len * sizeof(WCHAR); - } - - - if (buf_sz == 0) { - req->file.pathw = NULL; - req->fs.info.new_pathw = NULL; - req->path = NULL; - return 0; - } - - buf = (WCHAR *)uv__malloc(buf_sz); - if (buf == NULL) { - return ERROR_OUTOFMEMORY; - } - - pos = buf; - - if (path != NULL) { - fs__wtf8_to_wide(path, pos); - req->file.pathw = pos; - pos += pathw_len; - } else { - req->file.pathw = NULL; - } - - if (new_path != NULL) { - fs__wtf8_to_wide(new_path, pos); - req->fs.info.new_pathw = pos; - pos += new_pathw_len; - } else { - req->fs.info.new_pathw = NULL; - } - - req->path = path; - if (path != NULL && copy_path) { - memcpy(pos, path, path_len); - assert(path_len == buf_sz - (pos - buf) * sizeof(WCHAR)); - req->path = (char*) pos; - } - - req->flags |= UV_FS_FREE_PATHS; - - return 0; -} - - - -INLINE static void uv__fs_req_init(uv_loop_t* loop, uv_fs_t* req, - uv_fs_type fs_type, const uv_fs_cb cb) { - uv__once_init(); - UV_REQ_INIT(req, UV_FS); - req->loop = loop; - req->flags = 0; - req->fs_type = fs_type; - req->sys_errno_ = 0; - req->result = 0; - req->ptr = NULL; - req->path = NULL; - req->cb = cb; - memset(&req->fs, 0, sizeof(req->fs)); -} - - -static int32_t fs__get_surrogate_value(const WCHAR* w_source_ptr, - size_t w_source_len) { - WCHAR u; - WCHAR next; - - u = w_source_ptr[0]; - if (u >= 0xD800 && u <= 0xDBFF && w_source_len > 1) { - next = w_source_ptr[1]; - if (next >= 0xDC00 && next <= 0xDFFF) - return 0x10000 + ((u - 0xD800) << 10) + (next - 0xDC00); - } - return u; -} - - -static size_t fs__get_length_wide(const WCHAR* w_source_ptr, - size_t w_source_len) { - size_t target_len; - int32_t code_point; - - target_len = 0; - for (; w_source_len; w_source_len--, w_source_ptr++) { - code_point = fs__get_surrogate_value(w_source_ptr, w_source_len); - /* Can be invalid UTF-8 but must be valid WTF-8. */ - assert(code_point >= 0); - if (code_point < 0x80) - target_len += 1; - else if (code_point < 0x800) - target_len += 2; - else if (code_point < 0x10000) - target_len += 3; - else { - target_len += 4; - w_source_ptr++; - w_source_len--; - } - } - return target_len; -} - - -static int fs__wide_to_wtf8(WCHAR* w_source_ptr, - size_t w_source_len, - char** target_ptr, - size_t* target_len_ptr) { - size_t target_len; - char* target; - int32_t code_point; - - /* If *target_ptr is provided, then *target_len_ptr must be its length - * (excluding space for null), otherwise we will compute the target_len_ptr - * length and may return a new allocation in *target_ptr if target_ptr is - * provided. */ - if (target_ptr == NULL || *target_ptr == NULL) { - target_len = fs__get_length_wide(w_source_ptr, w_source_len); - if (target_len_ptr != NULL) - *target_len_ptr = target_len; - } else { - target_len = *target_len_ptr; - } - - if (target_ptr == NULL) - return 0; - - if (*target_ptr == NULL) { - target = (char *)uv__malloc(target_len + 1); - if (target == NULL) { - SetLastError(ERROR_OUTOFMEMORY); - return -1; - } - *target_ptr = target; - } else { - target = *target_ptr; - } - - for (; w_source_len; w_source_len--, w_source_ptr++) { - code_point = fs__get_surrogate_value(w_source_ptr, w_source_len); - /* Can be invalid UTF-8 but must be valid WTF-8. */ - assert(code_point >= 0); - - if (code_point < 0x80) { - *target++ = code_point; - } else if (code_point < 0x800) { - *target++ = 0xC0 | (code_point >> 6); - *target++ = 0x80 | (code_point & 0x3F); - } else if (code_point < 0x10000) { - *target++ = 0xE0 | (code_point >> 12); - *target++ = 0x80 | ((code_point >> 6) & 0x3F); - *target++ = 0x80 | (code_point & 0x3F); - } else { - *target++ = 0xF0 | (code_point >> 18); - *target++ = 0x80 | ((code_point >> 12) & 0x3F); - *target++ = 0x80 | ((code_point >> 6) & 0x3F); - *target++ = 0x80 | (code_point & 0x3F); - w_source_ptr++; - w_source_len--; - } - } - assert((size_t) (target - *target_ptr) == target_len); - - *target++ = '\0'; - - return 0; -} - - INLINE static int fs__readlink_handle(HANDLE handle, char** target_ptr, size_t* target_len_ptr) { @@ -553,7 +283,98 @@ INLINE static int fs__readlink_handle(HANDLE handle, } assert(target_ptr == NULL || *target_ptr == NULL); - return fs__wide_to_wtf8(w_target, w_target_len, target_ptr, target_len_ptr); + return uv_utf16_to_wtf8((const uint16_t*)w_target, w_target_len, target_ptr, target_len_ptr); +} + + +INLINE static int fs__capture_path(uv_fs_t* req, const char* path, + const char* new_path, const int copy_path) { + WCHAR* buf; + WCHAR* pos; + size_t buf_sz = 0; + size_t path_len = 0; + ssize_t pathw_len = 0; + ssize_t new_pathw_len = 0; + + /* new_path can only be set if path is also set. */ + assert(new_path == NULL || path != NULL); + + if (path != NULL) { + pathw_len = uv_wtf8_length_as_utf16(path); + if (pathw_len < 0) + return ERROR_INVALID_NAME; + buf_sz += pathw_len * sizeof(WCHAR); + } + + if (path != NULL && copy_path) { + path_len = 1 + strlen(path); + buf_sz += path_len; + } + + if (new_path != NULL) { + new_pathw_len = uv_wtf8_length_as_utf16(new_path); + if (new_pathw_len < 0) + return ERROR_INVALID_NAME; + buf_sz += new_pathw_len * sizeof(WCHAR); + } + + + if (buf_sz == 0) { + req->file.pathw = NULL; + req->fs.info.new_pathw = NULL; + req->path = NULL; + return 0; + } + + buf = (WCHAR*)uv__malloc(buf_sz); + if (buf == NULL) { + return ERROR_OUTOFMEMORY; + } + + pos = buf; + + if (path != NULL) { + uv_wtf8_to_utf16(path, (uint16_t*)pos, pathw_len); + req->file.pathw = pos; + pos += pathw_len; + } else { + req->file.pathw = NULL; + } + + if (new_path != NULL) { + uv_wtf8_to_utf16(new_path, (uint16_t*)pos, new_pathw_len); + req->fs.info.new_pathw = pos; + pos += new_pathw_len; + } else { + req->fs.info.new_pathw = NULL; + } + + req->path = path; + if (path != NULL && copy_path) { + memcpy(pos, path, path_len); + assert(path_len == buf_sz - (pos - buf) * sizeof(WCHAR)); + req->path = (char*) pos; + } + + req->flags |= UV_FS_FREE_PATHS; + + return 0; +} + + +INLINE static void uv__fs_req_init(uv_loop_t* loop, uv_fs_t* req, + uv_fs_type fs_type, const uv_fs_cb cb) { + uv__once_init(); + UV_REQ_INIT(req, UV_FS); + req->loop = loop; + req->flags = 0; + req->fs_type = fs_type; + req->sys_errno_ = 0; + req->result = 0; + req->ptr = NULL; + req->path = NULL; + req->cb = cb; + memset(&req->fs, 0, sizeof(req->fs)); } @@ -1572,7 +1393,7 @@ void fs__scandir(uv_fs_t* req) { continue; /* Compute the space required to store the filename as WTF-8. */ - wtf8_len = fs__get_length_wide(&info->FileName[0], wchar_len); + wtf8_len = uv_utf16_length_as_wtf8((const uint16_t*)&info->FileName[0], wchar_len); /* Resize the dirent array if needed. */ if (dirents_used >= dirents_size) { @@ -1600,8 +1421,8 @@ void fs__scandir(uv_fs_t* req) { /* Convert file name to UTF-8. */ wtf8 = &dirent->d_name[0]; - if (fs__wide_to_wtf8(&info->FileName[0], wchar_len, &wtf8, &wtf8_len) == -1) - goto win32_error; + if (uv_utf16_to_wtf8((const uint16_t*)&info->FileName[0], wchar_len, &wtf8, &wtf8_len) != 0) + goto out_of_memory_error; /* Fill out the type field. */ if (info->FileAttributes & FILE_ATTRIBUTE_DEVICE) @@ -2827,7 +2648,7 @@ static ssize_t fs__realpath_handle(HANDLE handle, char** realpath_ptr) { } assert(*realpath_ptr == NULL); - r = fs__wide_to_wtf8(w_realpath_ptr, w_realpath_len, realpath_ptr, NULL); + r = uv_utf16_to_wtf8((const uint16_t*)w_realpath_ptr, w_realpath_len, realpath_ptr, NULL); uv__free(w_realpath_buf); return r; } diff --git a/wpinet/src/main/native/thirdparty/libuv/src/win/getaddrinfo.cpp b/wpinet/src/main/native/thirdparty/libuv/src/win/getaddrinfo.cpp index dfab860a735..5bc63d8e194 100644 --- a/wpinet/src/main/native/thirdparty/libuv/src/win/getaddrinfo.cpp +++ b/wpinet/src/main/native/thirdparty/libuv/src/win/getaddrinfo.cpp @@ -104,13 +104,14 @@ static void uv__getaddrinfo_work(struct uv__work* w) { */ static void uv__getaddrinfo_done(struct uv__work* w, int status) { uv_getaddrinfo_t* req; - int addrinfo_len = 0; - int name_len = 0; + size_t addrinfo_len = 0; + ssize_t name_len = 0; size_t addrinfo_struct_len = ALIGNED_SIZE(sizeof(struct addrinfo)); struct addrinfoW* addrinfow_ptr; struct addrinfo* addrinfo_ptr; char* alloc_ptr = NULL; char* cur_ptr = NULL; + int r; req = container_of(w, uv_getaddrinfo_t, work_req); @@ -131,19 +132,12 @@ static void uv__getaddrinfo_done(struct uv__work* w, int status) { addrinfo_len += addrinfo_struct_len + ALIGNED_SIZE(addrinfow_ptr->ai_addrlen); if (addrinfow_ptr->ai_canonname != NULL) { - name_len = WideCharToMultiByte(CP_UTF8, - 0, - addrinfow_ptr->ai_canonname, - -1, - NULL, - 0, - NULL, - NULL); - if (name_len == 0) { - req->retcode = uv_translate_sys_error(GetLastError()); + name_len = uv_utf16_length_as_wtf8((const uint16_t*)addrinfow_ptr->ai_canonname, -1); + if (name_len < 0) { + req->retcode = name_len; goto complete; } - addrinfo_len += ALIGNED_SIZE(name_len); + addrinfo_len += ALIGNED_SIZE(name_len + 1); } addrinfow_ptr = addrinfow_ptr->ai_next; } @@ -182,27 +176,14 @@ static void uv__getaddrinfo_done(struct uv__work* w, int status) { /* convert canonical name to UTF-8 */ if (addrinfow_ptr->ai_canonname != NULL) { - name_len = WideCharToMultiByte(CP_UTF8, - 0, - addrinfow_ptr->ai_canonname, - -1, - NULL, - 0, - NULL, - NULL); - assert(name_len > 0); - assert(cur_ptr + name_len <= alloc_ptr + addrinfo_len); - name_len = WideCharToMultiByte(CP_UTF8, - 0, - addrinfow_ptr->ai_canonname, - -1, - cur_ptr, - name_len, - NULL, - NULL); - assert(name_len > 0); + name_len = alloc_ptr + addrinfo_len - cur_ptr; + r = uv__copy_utf16_to_utf8(addrinfow_ptr->ai_canonname, + -1, + cur_ptr, + (size_t*)&name_len); + assert(r == 0); addrinfo_ptr->ai_canonname = cur_ptr; - cur_ptr += ALIGNED_SIZE(name_len); + cur_ptr += ALIGNED_SIZE(name_len + 1); } assert(cur_ptr <= alloc_ptr + addrinfo_len); @@ -261,12 +242,11 @@ int uv_getaddrinfo(uv_loop_t* loop, const char* service, const struct addrinfo* hints) { char hostname_ascii[256]; - int nodesize = 0; - int servicesize = 0; - int hintssize = 0; + size_t nodesize = 0; + size_t servicesize = 0; + size_t hintssize = 0; char* alloc_ptr = NULL; - int err; - long rc; + ssize_t rc; if (req == NULL || (node == NULL && service == NULL)) { return UV_EINVAL; @@ -286,56 +266,36 @@ int uv_getaddrinfo(uv_loop_t* loop, hostname_ascii + sizeof(hostname_ascii)); if (rc < 0) return rc; - nodesize = ALIGNED_SIZE(MultiByteToWideChar(CP_UTF8, 0, hostname_ascii, - -1, NULL, 0) * sizeof(WCHAR)); - if (nodesize == 0) { - err = GetLastError(); - goto error; - } + nodesize = strlen(hostname_ascii) + 1; node = hostname_ascii; } if (service != NULL) { - servicesize = ALIGNED_SIZE(MultiByteToWideChar(CP_UTF8, - 0, - service, - -1, - NULL, - 0) * - sizeof(WCHAR)); - if (servicesize == 0) { - err = GetLastError(); - goto error; - } + rc = uv_wtf8_length_as_utf16(service); + if (rc < 0) + return rc; + servicesize = rc; } if (hints != NULL) { hintssize = ALIGNED_SIZE(sizeof(struct addrinfoW)); } /* allocate memory for inputs, and partition it as needed */ - alloc_ptr = (char*)uv__malloc(nodesize + servicesize + hintssize); - if (!alloc_ptr) { - err = WSAENOBUFS; - goto error; - } + alloc_ptr = (char*)uv__malloc(ALIGNED_SIZE(nodesize * sizeof(WCHAR)) + + ALIGNED_SIZE(servicesize * sizeof(WCHAR)) + + hintssize); + if (!alloc_ptr) + return UV_ENOMEM; /* save alloc_ptr now so we can free if error */ - req->alloc = (void*)alloc_ptr; + req->alloc = (void*) alloc_ptr; /* Convert node string to UTF16 into allocated memory and save pointer in the - * request. */ + * request. The node here has been converted to ascii. */ if (node != NULL) { - req->node = (WCHAR*)alloc_ptr; - if (MultiByteToWideChar(CP_UTF8, - 0, - node, - -1, - (WCHAR*) alloc_ptr, - nodesize / sizeof(WCHAR)) == 0) { - err = GetLastError(); - goto error; - } - alloc_ptr += nodesize; + req->node = (WCHAR*) alloc_ptr; + uv_wtf8_to_utf16(node, (uint16_t*) alloc_ptr, nodesize); + alloc_ptr += ALIGNED_SIZE(nodesize * sizeof(WCHAR)); } else { req->node = NULL; } @@ -343,24 +303,16 @@ int uv_getaddrinfo(uv_loop_t* loop, /* Convert service string to UTF16 into allocated memory and save pointer in * the req. */ if (service != NULL) { - req->service = (WCHAR*)alloc_ptr; - if (MultiByteToWideChar(CP_UTF8, - 0, - service, - -1, - (WCHAR*) alloc_ptr, - servicesize / sizeof(WCHAR)) == 0) { - err = GetLastError(); - goto error; - } - alloc_ptr += servicesize; + req->service = (WCHAR*) alloc_ptr; + uv_wtf8_to_utf16(service, (uint16_t*) alloc_ptr, servicesize); + alloc_ptr += ALIGNED_SIZE(servicesize * sizeof(WCHAR)); } else { req->service = NULL; } /* copy hints to allocated memory and save pointer in req */ if (hints != NULL) { - req->addrinfow = (struct addrinfoW*)alloc_ptr; + req->addrinfow = (struct addrinfoW*) alloc_ptr; req->addrinfow->ai_family = hints->ai_family; req->addrinfow->ai_socktype = hints->ai_socktype; req->addrinfow->ai_protocol = hints->ai_protocol; @@ -387,19 +339,11 @@ int uv_getaddrinfo(uv_loop_t* loop, uv__getaddrinfo_done(&req->work_req, 0); return req->retcode; } - -error: - if (req != NULL) { - uv__free(req->alloc); - req->alloc = NULL; - } - return uv_translate_sys_error(err); } int uv_if_indextoname(unsigned int ifindex, char* buffer, size_t* size) { NET_LUID luid; wchar_t wname[NDIS_IF_MAX_STRING_SIZE + 1]; /* Add one for the NUL. */ - DWORD bufsize; int r; if (buffer == NULL || size == NULL || *size == 0) @@ -415,31 +359,7 @@ int uv_if_indextoname(unsigned int ifindex, char* buffer, size_t* size) { if (r != 0) return uv_translate_sys_error(r); - /* Check how much space we need */ - bufsize = WideCharToMultiByte(CP_UTF8, 0, wname, -1, NULL, 0, NULL, NULL); - - if (bufsize == 0) { - return uv_translate_sys_error(GetLastError()); - } else if (bufsize > *size) { - *size = bufsize; - return UV_ENOBUFS; - } - - /* Convert to UTF-8 */ - bufsize = WideCharToMultiByte(CP_UTF8, - 0, - wname, - -1, - buffer, - *size, - NULL, - NULL); - - if (bufsize == 0) - return uv_translate_sys_error(GetLastError()); - - *size = bufsize - 1; - return 0; + return uv__copy_utf16_to_utf8(wname, -1, buffer, size); } int uv_if_indextoiid(unsigned int ifindex, char* buffer, size_t* size) { diff --git a/wpinet/src/main/native/thirdparty/libuv/src/win/getnameinfo.cpp b/wpinet/src/main/native/thirdparty/libuv/src/win/getnameinfo.cpp index b3773380c21..32863176ef6 100644 --- a/wpinet/src/main/native/thirdparty/libuv/src/win/getnameinfo.cpp +++ b/wpinet/src/main/native/thirdparty/libuv/src/win/getnameinfo.cpp @@ -42,6 +42,7 @@ static void uv__getnameinfo_work(struct uv__work* w) { uv_getnameinfo_t* req; WCHAR host[NI_MAXHOST]; WCHAR service[NI_MAXSERV]; + size_t size; int ret; req = container_of(w, uv_getnameinfo_t, work_req); @@ -57,29 +58,17 @@ static void uv__getnameinfo_work(struct uv__work* w) { return; } - ret = WideCharToMultiByte(CP_UTF8, - 0, - host, - -1, - req->host, - sizeof(req->host), - NULL, - NULL); - if (ret == 0) { - req->retcode = uv_translate_sys_error(GetLastError()); + size = sizeof(req->host); + ret = uv__copy_utf16_to_utf8(host, -1, req->host, &size); + if (ret < 0) { + req->retcode = ret; return; } - ret = WideCharToMultiByte(CP_UTF8, - 0, - service, - -1, - req->service, - sizeof(req->service), - NULL, - NULL); - if (ret == 0) { - req->retcode = uv_translate_sys_error(GetLastError()); + size = sizeof(req->service); + ret = uv__copy_utf16_to_utf8(service, -1, req->service, &size); + if (ret < 0) { + req->retcode = ret; } } diff --git a/wpinet/src/main/native/thirdparty/libuv/src/win/internal.h b/wpinet/src/main/native/thirdparty/libuv/src/win/internal.h index 9672fbc6826..867dea5e0ed 100644 --- a/wpinet/src/main/native/thirdparty/libuv/src/win/internal.h +++ b/wpinet/src/main/native/thirdparty/libuv/src/win/internal.h @@ -257,8 +257,9 @@ void uv__util_init(void); uint64_t uv__hrtime(unsigned int scale); __declspec(noreturn) void uv_fatal_error(const int errorno, const char* syscall); -int uv__convert_utf16_to_utf8(const WCHAR* utf16, int utf16len, char** utf8); -int uv__convert_utf8_to_utf16(const char* utf8, int utf8len, WCHAR** utf16); +int uv__convert_utf16_to_utf8(const WCHAR* utf16, size_t utf16len, char** utf8); +int uv__copy_utf16_to_utf8(const WCHAR* utf16, size_t utf16len, char* utf8, size_t *size); +int uv__convert_utf8_to_utf16(const char* utf8, WCHAR** utf16); typedef int (WINAPI *uv__peersockfunc)(SOCKET, struct sockaddr*, int*); diff --git a/wpinet/src/main/native/thirdparty/libuv/src/win/pipe.cpp b/wpinet/src/main/native/thirdparty/libuv/src/win/pipe.cpp index 258d6a684c6..b2348bd0430 100644 --- a/wpinet/src/main/native/thirdparty/libuv/src/win/pipe.cpp +++ b/wpinet/src/main/native/thirdparty/libuv/src/win/pipe.cpp @@ -51,7 +51,7 @@ static const int default_pending_pipe_instances = 4; /* Pipe prefix */ static char pipe_prefix[] = "\\\\?\\pipe"; -static const int pipe_prefix_len = sizeof(pipe_prefix) - 1; +static const size_t pipe_prefix_len = sizeof(pipe_prefix) - 1; /* IPC incoming xfer queue item. */ typedef struct { @@ -705,7 +705,7 @@ int uv_pipe_bind2(uv_pipe_t* handle, size_t namelen, unsigned int flags) { uv_loop_t* loop = handle->loop; - int i, err, nameSize; + int i, err; uv_pipe_accept_t* req; if (flags & ~UV_PIPE_NO_TRUNCATE) { @@ -744,9 +744,8 @@ int uv_pipe_bind2(uv_pipe_t* handle, handle->pipe.serv.accept_reqs = (uv_pipe_accept_t*) uv__malloc(sizeof(uv_pipe_accept_t) * handle->pipe.serv.pending_instances); - if (!handle->pipe.serv.accept_reqs) { - uv_fatal_error(ERROR_OUTOFMEMORY, "uv__malloc"); - } + if (!handle->pipe.serv.accept_reqs) + return UV_ENOMEM; for (i = 0; i < handle->pipe.serv.pending_instances; i++) { req = &handle->pipe.serv.accept_reqs[i]; @@ -756,22 +755,9 @@ int uv_pipe_bind2(uv_pipe_t* handle, req->next_pending = NULL; } - /* Convert name to UTF16. */ - nameSize = MultiByteToWideChar(CP_UTF8, 0, name, -1, NULL, 0) * sizeof(WCHAR); - handle->name = (WCHAR*)uv__malloc(nameSize); - if (!handle->name) { - uv_fatal_error(ERROR_OUTOFMEMORY, "uv__malloc"); - } - - if (!MultiByteToWideChar(CP_UTF8, - 0, - name, - -1, - handle->name, - nameSize / sizeof(WCHAR))) { - err = GetLastError(); - goto error; - } + err = uv__convert_utf8_to_utf16(name, &handle->name); + if (err) + return err; /* * Attempt to create the first pipe with FILE_FLAG_FIRST_PIPE_INSTANCE. @@ -797,10 +783,8 @@ int uv_pipe_bind2(uv_pipe_t* handle, return 0; error: - if (handle->name) { - uv__free(handle->name); - handle->name = NULL; - } + uv__free(handle->name); + handle->name = NULL; return uv_translate_sys_error(err); } @@ -863,7 +847,8 @@ int uv_pipe_connect2(uv_connect_t* req, unsigned int flags, uv_connect_cb cb) { uv_loop_t* loop = handle->loop; - int err, nameSize; + int err; + size_t nameSize; HANDLE pipeHandle = INVALID_HANDLE_VALUE; DWORD duplex_flags; @@ -906,26 +891,16 @@ int uv_pipe_connect2(uv_connect_t* req, } uv__pipe_connection_init(handle); - /* Convert name to UTF16. */ - nameSize = MultiByteToWideChar(CP_UTF8, 0, name, -1, NULL, 0) * sizeof(WCHAR); - handle->name = (WCHAR*)uv__malloc(nameSize); - if (!handle->name) { - uv_fatal_error(ERROR_OUTOFMEMORY, "uv__malloc"); - } - - if (!MultiByteToWideChar(CP_UTF8, - 0, - name, - -1, - handle->name, - nameSize / sizeof(WCHAR))) { - err = GetLastError(); + err = uv__convert_utf8_to_utf16(name, &handle->name); + if (err) { + err = ERROR_NO_UNICODE_TRANSLATION; goto error; } pipeHandle = open_named_pipe(handle->name, &duplex_flags); if (pipeHandle == INVALID_HANDLE_VALUE) { if (GetLastError() == ERROR_PIPE_BUSY) { + nameSize = (wcslen(handle->name) + 1) * sizeof(WCHAR); req->u.connect.name = (WCHAR *)uv__malloc(nameSize); if (!req->u.connect.name) { uv_fatal_error(ERROR_OUTOFMEMORY, "uv__malloc"); @@ -2441,7 +2416,6 @@ static int uv__pipe_getname(const uv_pipe_t* handle, char* buffer, size_t* size) FILE_NAME_INFORMATION tmp_name_info; FILE_NAME_INFORMATION* name_info; WCHAR* name_buf; - unsigned int addrlen; unsigned int name_size; unsigned int name_len; int err; @@ -2452,46 +2426,7 @@ static int uv__pipe_getname(const uv_pipe_t* handle, char* buffer, size_t* size) if (handle->name != NULL) { /* The user might try to query the name before we are connected, * and this is just easier to return the cached value if we have it. */ - name_buf = handle->name; - name_len = wcslen(name_buf); - - /* check how much space we need */ - addrlen = WideCharToMultiByte(CP_UTF8, - 0, - name_buf, - name_len, - NULL, - 0, - NULL, - NULL); - if (!addrlen) { - *size = 0; - err = uv_translate_sys_error(GetLastError()); - return err; - } else if (addrlen >= *size) { - *size = addrlen + 1; - err = UV_ENOBUFS; - goto error; - } - - addrlen = WideCharToMultiByte(CP_UTF8, - 0, - name_buf, - name_len, - buffer, - addrlen, - NULL, - NULL); - if (!addrlen) { - *size = 0; - err = uv_translate_sys_error(GetLastError()); - return err; - } - - *size = addrlen; - buffer[addrlen] = '\0'; - - return 0; + return uv__copy_utf16_to_utf8(handle->name, -1, buffer, size); } if (handle->handle == INVALID_HANDLE_VALUE) { @@ -2519,8 +2454,7 @@ static int uv__pipe_getname(const uv_pipe_t* handle, char* buffer, size_t* size) name_info = (FILE_NAME_INFORMATION*)uv__malloc(name_size); if (!name_info) { *size = 0; - err = UV_ENOMEM; - goto cleanup; + return UV_ENOMEM; } nt_status = pNtQueryInformationFile(handle->handle, @@ -2553,51 +2487,19 @@ static int uv__pipe_getname(const uv_pipe_t* handle, char* buffer, size_t* size) name_len /= sizeof(WCHAR); - /* check how much space we need */ - addrlen = WideCharToMultiByte(CP_UTF8, - 0, - name_buf, - name_len, - NULL, - 0, - NULL, - NULL); - if (!addrlen) { + /* "\\\\.\\pipe" + name */ + if (*size < pipe_prefix_len) { *size = 0; - err = uv_translate_sys_error(GetLastError()); - goto error; - } else if (pipe_prefix_len + addrlen >= *size) { - /* "\\\\.\\pipe" + name */ - *size = pipe_prefix_len + addrlen + 1; - err = UV_ENOBUFS; - goto error; } - - memcpy(buffer, pipe_prefix, pipe_prefix_len); - addrlen = WideCharToMultiByte(CP_UTF8, - 0, - name_buf, - name_len, - buffer+pipe_prefix_len, - *size-pipe_prefix_len, - NULL, - NULL); - if (!addrlen) { - *size = 0; - err = uv_translate_sys_error(GetLastError()); - goto error; + else { + memcpy(buffer, pipe_prefix, pipe_prefix_len); + *size -= pipe_prefix_len; } - - addrlen += pipe_prefix_len; - *size = addrlen; - buffer[addrlen] = '\0'; - - err = 0; + err = uv__copy_utf16_to_utf8(name_buf, name_len, buffer+pipe_prefix_len, size); + *size += pipe_prefix_len; error: uv__free(name_info); - -cleanup: return err; } diff --git a/wpinet/src/main/native/thirdparty/libuv/src/win/process.cpp b/wpinet/src/main/native/thirdparty/libuv/src/win/process.cpp index 18816d3b1e8..69f94b913f0 100644 --- a/wpinet/src/main/native/thirdparty/libuv/src/win/process.cpp +++ b/wpinet/src/main/native/thirdparty/libuv/src/win/process.cpp @@ -103,38 +103,26 @@ static void uv__init_global_job_handle(void) { &info, sizeof info)) uv_fatal_error(GetLastError(), "SetInformationJobObject"); -} - -static int uv__utf8_to_utf16_alloc(const char* s, WCHAR** ws_ptr) { - int ws_len, r; - WCHAR* ws; - - ws_len = MultiByteToWideChar(CP_UTF8, - 0, - s, - -1, - NULL, - 0); - if (ws_len <= 0) { - return GetLastError(); - } - ws = (WCHAR*) uv__malloc(ws_len * sizeof(WCHAR)); - if (ws == NULL) { - return ERROR_OUTOFMEMORY; + if (!AssignProcessToJobObject(uv_global_job_handle_, GetCurrentProcess())) { + /* Make sure this handle is functional. The Windows kernel has a bug that + * if the first use of AssignProcessToJobObject is for a Windows Store + * program, subsequent attempts to use the handle with fail with + * INVALID_PARAMETER (87). This is possibly because all uses of the handle + * must be for the same Terminal Services session. We can ensure it is tied + * to our current session now by adding ourself to it. We could remove + * ourself afterwards, but there doesn't seem to be a reason to. + */ + DWORD err = GetLastError(); + if (err != ERROR_ACCESS_DENIED) + uv_fatal_error(err, "AssignProcessToJobObject"); } +} - r = MultiByteToWideChar(CP_UTF8, - 0, - s, - -1, - ws, - ws_len); - assert(r == ws_len); - *ws_ptr = ws; - return 0; +static int uv__utf8_to_utf16_alloc(const char* s, WCHAR** ws_ptr) { + return uv__convert_utf8_to_utf16(s, ws_ptr); } @@ -394,7 +382,7 @@ static WCHAR* search_path(const WCHAR *file, name_has_ext); while (result == NULL) { - if (*dir_end == L'\0') { + if (dir_end == NULL || *dir_end == L'\0') { break; } @@ -537,21 +525,15 @@ int make_program_args(char** args, int verbatim_arguments, WCHAR** dst_ptr) { /* Count the required size. */ for (arg = args; *arg; arg++) { - DWORD arg_len; - - arg_len = MultiByteToWideChar(CP_UTF8, - 0, - *arg, - -1, - NULL, - 0); - if (arg_len == 0) { - return GetLastError(); - } + ssize_t arg_len; + + arg_len = uv_wtf8_length_as_utf16(*arg); + if (arg_len < 0) + return arg_len; dst_len += arg_len; - if (arg_len > temp_buffer_len) + if ((size_t) arg_len > temp_buffer_len) temp_buffer_len = arg_len; arg_count++; @@ -562,34 +544,28 @@ int make_program_args(char** args, int verbatim_arguments, WCHAR** dst_ptr) { dst_len = dst_len * 2 + arg_count * 2; /* Allocate buffer for the final command line. */ - dst = (WCHAR*) uv__malloc(dst_len * sizeof(WCHAR)); + dst = (WCHAR*)uv__malloc(dst_len * sizeof(WCHAR)); if (dst == NULL) { - err = ERROR_OUTOFMEMORY; + err = UV_ENOMEM; goto error; } /* Allocate temporary working buffer. */ - temp_buffer = (WCHAR*) uv__malloc(temp_buffer_len * sizeof(WCHAR)); + temp_buffer = (WCHAR*)uv__malloc(temp_buffer_len * sizeof(WCHAR)); if (temp_buffer == NULL) { - err = ERROR_OUTOFMEMORY; + err = UV_ENOMEM; goto error; } pos = dst; for (arg = args; *arg; arg++) { - DWORD arg_len; + ssize_t arg_len; /* Convert argument to wide char. */ - arg_len = MultiByteToWideChar(CP_UTF8, - 0, - *arg, - -1, - temp_buffer, - (int) (dst + dst_len - pos)); - if (arg_len == 0) { - err = GetLastError(); - goto error; - } + arg_len = uv_wtf8_length_as_utf16(*arg); + assert(arg_len > 0); + assert(temp_buffer_len >= (size_t) arg_len); + uv_wtf8_to_utf16(*arg, (uint16_t*)temp_buffer, arg_len); if (verbatim_arguments) { /* Copy verbatim. */ @@ -601,6 +577,7 @@ int make_program_args(char** args, int verbatim_arguments, WCHAR** dst_ptr) { } *pos++ = *(arg + 1) ? L' ' : L'\0'; + assert(pos <= dst + dst_len); } uv__free(temp_buffer); @@ -686,28 +663,22 @@ int make_program_env(char* env_block[], WCHAR** dst_ptr) { WCHAR* ptr; char** env; size_t env_len = 0; - int len; + size_t len; size_t i; - DWORD var_size; + size_t var_size; size_t env_block_count = 1; /* 1 for null-terminator */ WCHAR* dst_copy; WCHAR** ptr_copy; WCHAR** env_copy; - DWORD required_vars_value_len[ARRAY_SIZE(required_vars)]; + size_t required_vars_value_len[ARRAY_SIZE(required_vars)]; /* first pass: determine size in UTF-16 */ for (env = env_block; *env; env++) { - int len; + ssize_t len; if (strchr(*env, '=')) { - len = MultiByteToWideChar(CP_UTF8, - 0, - *env, - -1, - NULL, - 0); - if (len <= 0) { - return GetLastError(); - } + len = uv_wtf8_length_as_utf16(*env); + if (len < 0) + return len; env_len += len; env_block_count++; } @@ -716,25 +687,19 @@ int make_program_env(char* env_block[], WCHAR** dst_ptr) { /* second pass: copy to UTF-16 environment block */ dst_copy = (WCHAR*)uv__malloc(env_len * sizeof(WCHAR)); if (dst_copy == NULL && env_len > 0) { - return ERROR_OUTOFMEMORY; + return UV_ENOMEM; } env_copy = (WCHAR**)alloca(env_block_count * sizeof(WCHAR*)); ptr = dst_copy; ptr_copy = env_copy; for (env = env_block; *env; env++) { + ssize_t len; if (strchr(*env, '=')) { - len = MultiByteToWideChar(CP_UTF8, - 0, - *env, - -1, - ptr, - (int) (env_len - (ptr - dst_copy))); - if (len <= 0) { - DWORD err = GetLastError(); - uv__free(dst_copy); - return err; - } + len = uv_wtf8_length_as_utf16(*env); + assert(len > 0); + assert((size_t) len <= env_len - (ptr - dst_copy)); + uv_wtf8_to_utf16(*env, (uint16_t*)ptr, len); *ptr_copy++ = ptr; ptr += len; } @@ -752,7 +717,7 @@ int make_program_env(char* env_block[], WCHAR** dst_ptr) { cmp = -1; } else { cmp = env_strncmp(required_vars[i].wide_eq, - required_vars[i].len, + required_vars[i].len, *ptr_copy); } if (cmp < 0) { @@ -775,7 +740,7 @@ int make_program_env(char* env_block[], WCHAR** dst_ptr) { dst = (WCHAR*)uv__malloc((1+env_len) * sizeof(WCHAR)); if (!dst) { uv__free(dst_copy); - return ERROR_OUTOFMEMORY; + return UV_ENOMEM; } for (ptr = dst, ptr_copy = env_copy, i = 0; @@ -973,26 +938,26 @@ int uv_spawn(uv_loop_t* loop, err = uv__utf8_to_utf16_alloc(options->file, &application); if (err) - goto done; + goto done_uv; err = make_program_args( options->args, options->flags & UV_PROCESS_WINDOWS_VERBATIM_ARGUMENTS, &arguments); if (err) - goto done; + goto done_uv; if (options->env) { err = make_program_env(options->env, &env); if (err) - goto done; + goto done_uv; } if (options->cwd) { /* Explicit cwd */ err = uv__utf8_to_utf16_alloc(options->cwd, &cwd); if (err) - goto done; + goto done_uv; } else { /* Inherit cwd */ @@ -1023,22 +988,19 @@ int uv_spawn(uv_loop_t* loop, DWORD path_len, r; path_len = GetEnvironmentVariableW(L"PATH", NULL, 0); - if (path_len == 0) { - err = GetLastError(); - goto done; - } - - alloc_path = (WCHAR*) uv__malloc(path_len * sizeof(WCHAR)); - if (alloc_path == NULL) { - err = ERROR_OUTOFMEMORY; - goto done; - } - path = alloc_path; + if (path_len != 0) { + alloc_path = (WCHAR*) uv__malloc(path_len * sizeof(WCHAR)); + if (alloc_path == NULL) { + err = ERROR_OUTOFMEMORY; + goto done; + } + path = alloc_path; - r = GetEnvironmentVariableW(L"PATH", path, path_len); - if (r == 0 || r >= path_len) { - err = GetLastError(); - goto done; + r = GetEnvironmentVariableW(L"PATH", path, path_len); + if (r == 0 || r >= path_len) { + err = GetLastError(); + goto done; + } } } @@ -1100,6 +1062,7 @@ int uv_spawn(uv_loop_t* loop, * breakaway. */ process_flags |= DETACHED_PROCESS | CREATE_NEW_PROCESS_GROUP; + process_flags |= CREATE_SUSPENDED; } if (!CreateProcessW(application_path, @@ -1117,11 +1080,6 @@ int uv_spawn(uv_loop_t* loop, goto done; } - /* Spawn succeeded. Beyond this point, failure is reported asynchronously. */ - - process->process_handle = info.hProcess; - process->pid = info.dwProcessId; - /* If the process isn't spawned as detached, assign to the global job object * so windows will kill it when the parent process dies. */ if (!(options->flags & UV_PROCESS_DETACHED)) { @@ -1144,6 +1102,19 @@ int uv_spawn(uv_loop_t* loop, } } + if (process_flags & CREATE_SUSPENDED) { + if (ResumeThread(info.hThread) == ((DWORD)-1)) { + err = GetLastError(); + TerminateProcess(info.hProcess, 1); + goto done; + } + } + + /* Spawn succeeded. Beyond this point, failure is reported asynchronously. */ + + process->process_handle = info.hProcess; + process->pid = info.dwProcessId; + /* Set IPC pid to all IPC pipes. */ for (i = 0; i < options->stdio_count; i++) { const uv_stdio_container_t* fdopt = &options->stdio[i]; @@ -1171,8 +1142,13 @@ int uv_spawn(uv_loop_t* loop, * made or the handle is closed, whichever happens first. */ uv__handle_start(process); + goto done_uv; + /* Cleanup, whether we succeeded or failed. */ done: + err = uv_translate_sys_error(err); + + done_uv: uv__free(application); uv__free(application_path); uv__free(arguments); @@ -1186,7 +1162,7 @@ int uv_spawn(uv_loop_t* loop, child_stdio_buffer = NULL; } - return uv_translate_sys_error(err); + return err; } diff --git a/wpinet/src/main/native/thirdparty/libuv/src/win/tty.cpp b/wpinet/src/main/native/thirdparty/libuv/src/win/tty.cpp index 9bb3d9e830c..7adf3cd3ba8 100644 --- a/wpinet/src/main/native/thirdparty/libuv/src/win/tty.cpp +++ b/wpinet/src/main/native/thirdparty/libuv/src/win/tty.cpp @@ -486,9 +486,11 @@ static DWORD CALLBACK uv_tty_line_read_thread(void* data) { uv_loop_t* loop; uv_tty_t* handle; uv_req_t* req; - DWORD bytes, read_bytes; + DWORD bytes; + size_t read_bytes; WCHAR utf16[MAX_INPUT_BUFFER_LENGTH / 3]; - DWORD chars, read_chars; + DWORD chars; + DWORD read_chars; LONG status; COORD pos; BOOL read_console_success; @@ -529,16 +531,13 @@ static DWORD CALLBACK uv_tty_line_read_thread(void* data) { NULL); if (read_console_success) { - read_bytes = WideCharToMultiByte(CP_UTF8, - 0, - utf16, - read_chars, - handle->tty.rd.read_line_buffer.base, - bytes, - NULL, - NULL); + read_bytes = bytes; + uv_utf16_to_wtf8((const uint16_t*)utf16, + read_chars, + &handle->tty.rd.read_line_buffer.base, + &read_bytes); SET_REQ_SUCCESS(req); - req->u.io.overlapped.InternalHigh = read_bytes; + req->u.io.overlapped.InternalHigh = (DWORD) read_bytes; } else { SET_REQ_ERROR(req, GetLastError()); } @@ -802,7 +801,9 @@ void uv_process_tty_read_raw_req(uv_loop_t* loop, uv_tty_t* handle, } if (KEV.uChar.UnicodeChar != 0) { - int prefix_len, char_len; + int prefix_len; + size_t char_len; + char* last_key_buf; /* Character key pressed */ if (KEV.uChar.UnicodeChar >= 0xD800 && @@ -823,38 +824,31 @@ void uv_process_tty_read_raw_req(uv_loop_t* loop, uv_tty_t* handle, prefix_len = 0; } - if (KEV.uChar.UnicodeChar >= 0xDC00 && - KEV.uChar.UnicodeChar < 0xE000) { + char_len = sizeof handle->tty.rd.last_key; + last_key_buf = &handle->tty.rd.last_key[prefix_len]; + if (handle->tty.rd.last_utf16_high_surrogate) { /* UTF-16 surrogate pair */ WCHAR utf16_buffer[2]; utf16_buffer[0] = handle->tty.rd.last_utf16_high_surrogate; utf16_buffer[1] = KEV.uChar.UnicodeChar; - char_len = WideCharToMultiByte(CP_UTF8, - 0, - utf16_buffer, - 2, - &handle->tty.rd.last_key[prefix_len], - sizeof handle->tty.rd.last_key, - NULL, - NULL); + if (uv_utf16_to_wtf8((const uint16_t*)utf16_buffer, + 2, + &last_key_buf, + &char_len)) + char_len = 0; + handle->tty.rd.last_utf16_high_surrogate = 0; } else { /* Single UTF-16 character */ - char_len = WideCharToMultiByte(CP_UTF8, - 0, - &KEV.uChar.UnicodeChar, - 1, - &handle->tty.rd.last_key[prefix_len], - sizeof handle->tty.rd.last_key, - NULL, - NULL); + if (uv_utf16_to_wtf8((const uint16_t*)&KEV.uChar.UnicodeChar, + 1, + &last_key_buf, + &char_len)) + char_len = 0; } - /* Whatever happened, the last character wasn't a high surrogate. */ - handle->tty.rd.last_utf16_high_surrogate = 0; - /* If the utf16 character(s) couldn't be converted something must be * wrong. */ - if (!char_len) { + if (char_len == 0) { handle->flags &= ~UV_HANDLE_READING; DECREASE_ACTIVE_COUNT(loop, handle); handle->read_cb((uv_stream_t*) handle, diff --git a/wpinet/src/main/native/thirdparty/libuv/src/win/util.cpp b/wpinet/src/main/native/thirdparty/libuv/src/win/util.cpp index 4b76417fcba..14295c41342 100644 --- a/wpinet/src/main/native/thirdparty/libuv/src/win/util.cpp +++ b/wpinet/src/main/native/thirdparty/libuv/src/win/util.cpp @@ -105,7 +105,7 @@ void uv__util_init(void) { int uv_exepath(char* buffer, size_t* size_ptr) { - int utf8_len, utf16_buffer_len, utf16_len; + size_t utf8_len, utf16_buffer_len, utf16_len; WCHAR* utf16_buffer; int err; @@ -133,25 +133,17 @@ int uv_exepath(char* buffer, size_t* size_ptr) { } /* Convert to UTF-8 */ - utf8_len = WideCharToMultiByte(CP_UTF8, - 0, - utf16_buffer, - -1, - buffer, - (int) *size_ptr, - NULL, - NULL); - if (utf8_len == 0) { - err = GetLastError(); - goto error; + utf8_len = *size_ptr - 1; /* Reserve space for NUL */ + err = uv_utf16_to_wtf8((const uint16_t*)utf16_buffer, utf16_len, &buffer, &utf8_len); + if (err == UV_ENOBUFS) { + utf8_len = *size_ptr - 1; + err = 0; } + *size_ptr = utf8_len; uv__free(utf16_buffer); - /* utf8_len *does* include the terminating null at this point, but the - * returned size shouldn't. */ - *size_ptr = utf8_len - 1; - return 0; + return err; error: uv__free(utf16_buffer); @@ -214,45 +206,14 @@ int uv_cwd(char* buffer, size_t* size) { } r = uv__cwd(&utf16_buffer, &utf16_len); - if (r < 0) { + if (r < 0) return r; - } - /* Check how much space we need */ - r = WideCharToMultiByte(CP_UTF8, - 0, - utf16_buffer, - -1, - NULL, - 0, - NULL, - NULL); - if (r == 0) { - uv__free(utf16_buffer); - return uv_translate_sys_error(GetLastError()); - } else if (r > (int) *size) { - uv__free(utf16_buffer); - *size = r; - return UV_ENOBUFS; - } + r = uv__copy_utf16_to_utf8(utf16_buffer, utf16_len, buffer, size); - /* Convert to UTF-8 */ - r = WideCharToMultiByte(CP_UTF8, - 0, - utf16_buffer, - -1, - buffer, - *size > INT_MAX ? INT_MAX : (int) *size, - NULL, - NULL); uv__free(utf16_buffer); - if (r == 0) { - return uv_translate_sys_error(GetLastError()); - } - - *size = r - 1; - return 0; + return r; } @@ -262,33 +223,10 @@ int uv_chdir(const char* dir) { WCHAR drive_letter, env_var[4]; int r; - if (dir == NULL) { - return UV_EINVAL; - } - - utf16_len = MultiByteToWideChar(CP_UTF8, - 0, - dir, - -1, - NULL, - 0); - if (utf16_len == 0) { - return uv_translate_sys_error(GetLastError()); - } - utf16_buffer = (WCHAR*)uv__malloc(utf16_len * sizeof(WCHAR)); - if (utf16_buffer == NULL) { - return UV_ENOMEM; - } - - if (MultiByteToWideChar(CP_UTF8, - 0, - dir, - -1, - utf16_buffer, - utf16_len) == 0) { - uv__free(utf16_buffer); - return uv_translate_sys_error(GetLastError()); - } + /* Convert to UTF-16 */ + r = uv__convert_utf8_to_utf16(dir, &utf16_buffer); + if (r) + return r; if (!SetCurrentDirectoryW(utf16_buffer)) { uv__free(utf16_buffer); @@ -426,29 +364,14 @@ int uv_set_process_title(const char* title) { uv__once_init(); - /* Find out how big the buffer for the wide-char title must be */ - length = MultiByteToWideChar(CP_UTF8, 0, title, -1, NULL, 0); - if (!length) { - err = GetLastError(); - goto done; - } - - /* Convert to wide-char string */ - title_w = (WCHAR*)uv__malloc(sizeof(WCHAR) * length); - if (!title_w) { - uv_fatal_error(ERROR_OUTOFMEMORY, "uv__malloc"); - } - - length = MultiByteToWideChar(CP_UTF8, 0, title, -1, title_w, length); - if (!length) { - err = GetLastError(); - goto done; - } + err = uv__convert_utf8_to_utf16(title, &title_w); + if (err) + return err; /* If the title must be truncated insert a \0 terminator there */ - if (length > MAX_TITLE_LENGTH) { + length = wcslen(title_w); + if (length >= MAX_TITLE_LENGTH) title_w[MAX_TITLE_LENGTH - 1] = L'\0'; - } if (!SetConsoleTitleW(title_w)) { err = GetLastError(); @@ -470,20 +393,19 @@ int uv_set_process_title(const char* title) { static int uv__get_process_title(void) { WCHAR title_w[MAX_TITLE_LENGTH]; + DWORD wlen; - if (!GetConsoleTitleW(title_w, sizeof(title_w) / sizeof(WCHAR))) { - return -1; - } - - if (uv__convert_utf16_to_utf8(title_w, -1, &process_title) != 0) - return -1; + wlen = GetConsoleTitleW(title_w, sizeof(title_w) / sizeof(WCHAR)); + if (wlen == 0) + return uv_translate_sys_error(GetLastError()); - return 0; + return uv__convert_utf16_to_utf8(title_w, wlen, &process_title); } int uv_get_process_title(char* buffer, size_t size) { size_t len; + int r; if (buffer == NULL || size == 0) return UV_EINVAL; @@ -495,9 +417,12 @@ int uv_get_process_title(char* buffer, size_t size) { * If the process_title was never read before nor explicitly set, * we must query it with getConsoleTitleW */ - if (!process_title && uv__get_process_title() == -1) { - LeaveCriticalSection(&process_title_lock); - return uv_translate_sys_error(GetLastError()); + if (process_title == NULL) { + r = uv__get_process_title(); + if (r) { + LeaveCriticalSection(&process_title_lock); + return r; + } } assert(process_title); @@ -844,19 +769,8 @@ int uv_interface_addresses(uv_interface_address_t** addresses_ptr, continue; /* Compute the size of the interface name. */ - name_size = WideCharToMultiByte(CP_UTF8, - 0, - adapter->FriendlyName, - -1, - NULL, - 0, - NULL, - FALSE); - if (name_size <= 0) { - uv__free(win_address_buf); - return uv_translate_sys_error(GetLastError()); - } - uv_address_buf_size += name_size; + name_size = uv_utf16_length_as_wtf8((const uint16_t*)adapter->FriendlyName, -1); + uv_address_buf_size += name_size + 1; /* Count the number of addresses associated with this interface, and * compute the size. */ @@ -886,30 +800,25 @@ int uv_interface_addresses(uv_interface_address_t** addresses_ptr, adapter != NULL; adapter = adapter->Next) { IP_ADAPTER_UNICAST_ADDRESS* unicast_address; - int name_size; - size_t max_name_size; + size_t name_size; + int r; if (adapter->OperStatus != IfOperStatusUp || adapter->FirstUnicastAddress == NULL) continue; /* Convert the interface name to UTF8. */ - max_name_size = (char*) uv_address_buf + uv_address_buf_size - name_buf; - if (max_name_size > (size_t) INT_MAX) - max_name_size = INT_MAX; - name_size = WideCharToMultiByte(CP_UTF8, - 0, - adapter->FriendlyName, - -1, - name_buf, - (int) max_name_size, - NULL, - FALSE); - if (name_size <= 0) { + name_size = (char*) uv_address_buf + uv_address_buf_size - name_buf; + r = uv__copy_utf16_to_utf8(adapter->FriendlyName, + -1, + name_buf, + &name_size); + if (r) { uv__free(win_address_buf); uv__free(uv_address_buf); - return uv_translate_sys_error(GetLastError()); + return r; } + name_size += 1; /* Add NUL byte. */ /* Add an uv_interface_address_t element for every unicast address. */ for (unicast_address = (IP_ADAPTER_UNICAST_ADDRESS*) @@ -1072,7 +981,6 @@ int uv_os_homedir(char* buffer, size_t* size) { int uv_os_tmpdir(char* buffer, size_t* size) { wchar_t *path; - DWORD bufsize; size_t len; if (buffer == NULL || size == NULL || *size == 0) @@ -1089,7 +997,7 @@ int uv_os_tmpdir(char* buffer, size_t* size) { if (path == NULL) { return UV_ENOMEM; } - len = GetTempPathW(len, path); + len = GetTempPathW(len, path); if (len == 0) { uv__free(path); @@ -1104,34 +1012,7 @@ int uv_os_tmpdir(char* buffer, size_t* size) { path[len] = L'\0'; } - /* Check how much space we need */ - bufsize = WideCharToMultiByte(CP_UTF8, 0, path, -1, NULL, 0, NULL, NULL); - - if (bufsize == 0) { - uv__free(path); - return uv_translate_sys_error(GetLastError()); - } else if (bufsize > *size) { - uv__free(path); - *size = bufsize; - return UV_ENOBUFS; - } - - /* Convert to UTF-8 */ - bufsize = WideCharToMultiByte(CP_UTF8, - 0, - path, - -1, - buffer, - *size, - NULL, - NULL); - uv__free(path); - - if (bufsize == 0) - return uv_translate_sys_error(GetLastError()); - - *size = bufsize - 1; - return 0; + return uv__copy_utf16_to_utf8(path, len, buffer, size); } @@ -1142,95 +1023,71 @@ int uv_os_tmpdir(char* buffer, size_t* size) { * If utf16 is null terminated, utf16len can be set to -1, otherwise it must * be specified. */ -int uv__convert_utf16_to_utf8(const WCHAR* utf16, int utf16len, char** utf8) { - DWORD bufsize; +int uv__convert_utf16_to_utf8(const WCHAR* utf16, size_t utf16len, char** utf8) { + size_t utf8_len = 0; if (utf16 == NULL) return UV_EINVAL; - /* Check how much space we need */ - bufsize = WideCharToMultiByte(CP_UTF8, - 0, - utf16, - utf16len, - NULL, - 0, - NULL, - NULL); - - if (bufsize == 0) - return uv_translate_sys_error(GetLastError()); - - /* Allocate the destination buffer adding an extra byte for the terminating - * NULL. If utf16len is not -1 WideCharToMultiByte will not add it, so - * we do it ourselves always, just in case. */ - *utf8 = (char*)uv__malloc(bufsize + 1); - - if (*utf8 == NULL) - return UV_ENOMEM; - - /* Convert to UTF-8 */ - bufsize = WideCharToMultiByte(CP_UTF8, - 0, - utf16, - utf16len, - *utf8, - bufsize, - NULL, - NULL); - - if (bufsize == 0) { - uv__free(*utf8); - *utf8 = NULL; - return uv_translate_sys_error(GetLastError()); - } - - (*utf8)[bufsize] = '\0'; - return 0; + *utf8 = NULL; + return uv_utf16_to_wtf8((const uint16_t*)utf16, utf16len, utf8, &utf8_len); } /* * Converts a UTF-8 string into a UTF-16 one. The resulting string is * null-terminated. - * - * If utf8 is null terminated, utf8len can be set to -1, otherwise it must - * be specified. */ -int uv__convert_utf8_to_utf16(const char* utf8, int utf8len, WCHAR** utf16) { +int uv__convert_utf8_to_utf16(const char* utf8, WCHAR** utf16) { int bufsize; if (utf8 == NULL) return UV_EINVAL; - /* Check how much space we need */ - bufsize = MultiByteToWideChar(CP_UTF8, 0, utf8, utf8len, NULL, 0); - - if (bufsize == 0) - return uv_translate_sys_error(GetLastError()); + /* Check how much space we need (including NUL). */ + bufsize = uv_wtf8_length_as_utf16(utf8); + if (bufsize < 0) + return UV__EINVAL; - /* Allocate the destination buffer adding an extra byte for the terminating - * NULL. If utf8len is not -1 MultiByteToWideChar will not add it, so - * we do it ourselves always, just in case. */ - *utf16 = (WCHAR*)uv__malloc(sizeof(WCHAR) * (bufsize + 1)); + /* Allocate the destination buffer. */ + *utf16 = (WCHAR*)uv__malloc(sizeof(WCHAR) * bufsize); if (*utf16 == NULL) return UV_ENOMEM; /* Convert to UTF-16 */ - bufsize = MultiByteToWideChar(CP_UTF8, 0, utf8, utf8len, *utf16, bufsize); + uv_wtf8_to_utf16(utf8, (uint16_t*)*utf16, bufsize); - if (bufsize == 0) { - uv__free(*utf16); - *utf16 = NULL; - return uv_translate_sys_error(GetLastError()); - } - - (*utf16)[bufsize] = L'\0'; return 0; } +/* + * Converts a UTF-16 string into a UTF-8 one in an existing buffer. The + * resulting string is null-terminated. + * + * If utf16 is null terminated, utf16len can be set to -1, otherwise it must + * be specified. + */ +int uv__copy_utf16_to_utf8(const WCHAR* utf16buffer, size_t utf16len, char* utf8, size_t *size) { + int r; + + if (utf8 == NULL || size == NULL) + return UV_EINVAL; + + if (*size == 0) { + *size = uv_utf16_length_as_wtf8((const uint16_t*)utf16buffer, utf16len); + r = UV_ENOBUFS; + } else { + *size -= 1; /* Reserve space for NUL. */ + r = uv_utf16_to_wtf8((const uint16_t*)utf16buffer, utf16len, &utf8, size); + } + if (r == UV_ENOBUFS) + *size += 1; /* Add space for NUL. */ + return r; +} + + static int uv__getpwuid_r(uv_passwd_t* pwd) { HANDLE token; wchar_t username[UNLEN + 1]; @@ -1395,14 +1252,13 @@ int uv_os_getenv(const char* name, char* buffer, size_t* size) { wchar_t* var; DWORD varlen; wchar_t* name_w; - DWORD bufsize; size_t len; int r; if (name == NULL || buffer == NULL || size == NULL || *size == 0) return UV_EINVAL; - r = uv__convert_utf8_to_utf16(name, -1, &name_w); + r = uv__convert_utf8_to_utf16(name, &name_w); if (r != 0) return r; @@ -1443,35 +1299,7 @@ int uv_os_getenv(const char* name, char* buffer, size_t* size) { } } - /* Check how much space we need */ - bufsize = WideCharToMultiByte(CP_UTF8, 0, var, -1, NULL, 0, NULL, NULL); - - if (bufsize == 0) { - r = uv_translate_sys_error(GetLastError()); - goto fail; - } else if (bufsize > *size) { - *size = bufsize; - r = UV_ENOBUFS; - goto fail; - } - - /* Convert to UTF-8 */ - bufsize = WideCharToMultiByte(CP_UTF8, - 0, - var, - -1, - buffer, - *size, - NULL, - NULL); - - if (bufsize == 0) { - r = uv_translate_sys_error(GetLastError()); - goto fail; - } - - *size = bufsize - 1; - r = 0; + r = uv__copy_utf16_to_utf8(var, len, buffer, size); fail: @@ -1493,12 +1321,12 @@ int uv_os_setenv(const char* name, const char* value) { if (name == NULL || value == NULL) return UV_EINVAL; - r = uv__convert_utf8_to_utf16(name, -1, &name_w); + r = uv__convert_utf8_to_utf16(name, &name_w); if (r != 0) return r; - r = uv__convert_utf8_to_utf16(value, -1, &value_w); + r = uv__convert_utf8_to_utf16(value, &value_w); if (r != 0) { uv__free(name_w); @@ -1523,7 +1351,7 @@ int uv_os_unsetenv(const char* name) { if (name == NULL) return UV_EINVAL; - r = uv__convert_utf8_to_utf16(name, -1, &name_w); + r = uv__convert_utf8_to_utf16(name, &name_w); if (r != 0) return r; @@ -1540,9 +1368,6 @@ int uv_os_unsetenv(const char* name) { int uv_os_gethostname(char* buffer, size_t* size) { WCHAR buf[UV_MAXHOSTNAMESIZE]; - size_t len; - char* utf8_str; - int convert_result; if (buffer == NULL || size == NULL || *size == 0) return UV_EINVAL; @@ -1555,22 +1380,7 @@ int uv_os_gethostname(char* buffer, size_t* size) { if (pGetHostNameW(buf, UV_MAXHOSTNAMESIZE) != 0) return uv_translate_sys_error(WSAGetLastError()); - convert_result = uv__convert_utf16_to_utf8(buf, -1, &utf8_str); - - if (convert_result != 0) - return convert_result; - - len = strlen(utf8_str); - if (len >= *size) { - *size = len + 1; - uv__free(utf8_str); - return UV_ENOBUFS; - } - - memcpy(buffer, utf8_str, len + 1); - uv__free(utf8_str); - *size = len; - return 0; + return uv__copy_utf16_to_utf8(buf, -1, buffer, size); } @@ -1676,7 +1486,7 @@ int uv_os_uname(uv_utsname_t* buffer) { HKEY registry_key; WCHAR product_name_w[256]; DWORD product_name_w_size; - int version_size; + size_t version_size; int processor_level; int r; @@ -1707,7 +1517,7 @@ int uv_os_uname(uv_utsname_t* buffer) { r = RegOpenKeyExW(HKEY_LOCAL_MACHINE, L"SOFTWARE\\Microsoft\\Windows NT\\CurrentVersion", 0, - KEY_QUERY_VALUE, + KEY_QUERY_VALUE | KEY_WOW64_64KEY, ®istry_key); if (r == ERROR_SUCCESS) { @@ -1738,37 +1548,29 @@ int uv_os_uname(uv_utsname_t* buffer) { } } - version_size = WideCharToMultiByte(CP_UTF8, - 0, - product_name_w, - -1, - buffer->version, - sizeof(buffer->version), - NULL, - NULL); - if (version_size == 0) { - r = uv_translate_sys_error(GetLastError()); + version_size = sizeof(buffer->version); + r = uv__copy_utf16_to_utf8(product_name_w, + -1, + buffer->version, + &version_size); + if (r) goto error; - } } } /* Append service pack information to the version if present. */ if (os_info.szCSDVersion[0] != L'\0') { if (version_size > 0) - buffer->version[version_size - 1] = ' '; - - if (WideCharToMultiByte(CP_UTF8, - 0, - os_info.szCSDVersion, - -1, - buffer->version + version_size, - sizeof(buffer->version) - version_size, - NULL, - NULL) == 0) { - r = uv_translate_sys_error(GetLastError()); + buffer->version[version_size++] = ' '; + + version_size = sizeof(buffer->version) - version_size; + r = uv__copy_utf16_to_utf8(os_info.szCSDVersion, + -1, + buffer->version + + sizeof(buffer->version) - version_size, + &version_size); + if (r) goto error; - } } /* Populate the sysname field. */ diff --git a/wpiunits/CMakeLists.txt b/wpiunits/CMakeLists.txt new file mode 100644 index 00000000000..bb7cb696a2f --- /dev/null +++ b/wpiunits/CMakeLists.txt @@ -0,0 +1,19 @@ +project (wpiunits) + +# Java bindings +if (WITH_JAVA) + find_package(Java REQUIRED) + include(UseJava) + set(CMAKE_JAVA_COMPILE_FLAGS "-encoding" "UTF8" "-Xlint:unchecked") + + file(GLOB_RECURSE JAVA_SOURCES src/main/java/*.java) + + add_jar(wpiunits_jar ${JAVA_SOURCES} OUTPUT_NAME wpiunits) + + get_property(WPIUNITS_JAR_FILE TARGET wpiunits_jar PROPERTY JAR_FILE) + install(FILES ${WPIUNITS_JAR_FILE} DESTINATION "${java_lib_dest}") + + set_property(TARGET wpiunits_jar PROPERTY FOLDER "java") + + install(FILES wpiunits-config.cmake DESTINATION share/wpiunits) +endif() diff --git a/wpiunits/src/main/java/edu/wpi/first/units/BaseUnits.java b/wpiunits/src/main/java/edu/wpi/first/units/BaseUnits.java index 5e342da121a..facf02540d6 100644 --- a/wpiunits/src/main/java/edu/wpi/first/units/BaseUnits.java +++ b/wpiunits/src/main/java/edu/wpi/first/units/BaseUnits.java @@ -20,11 +20,11 @@ private BaseUnits() { public static final Velocity Velocity = new Velocity<>(Distance, Time, "Meter per Second", "m/s"); - /** The standard unit of mass, grams. */ + /** The standard unit of mass, kilograms. */ public static final Mass Mass = new Mass(1, "Kilogram", "Kg"); - /** The standard unit of angles, revolutions. */ - public static final Angle Angle = new Angle(1, "Revolution", "R"); + /** The standard unit of angles, radians. */ + public static final Angle Angle = new Angle(1, "Radian", "rad"); /** The standard "unitless" unit. */ public static final Dimensionless Value = new Dimensionless(1, "", ""); diff --git a/wpiunits/src/main/java/edu/wpi/first/units/Units.java b/wpiunits/src/main/java/edu/wpi/first/units/Units.java index ecf0768a186..a04b54cc08a 100644 --- a/wpiunits/src/main/java/edu/wpi/first/units/Units.java +++ b/wpiunits/src/main/java/edu/wpi/first/units/Units.java @@ -38,10 +38,9 @@ private Units() { public static final Time Minute = Minutes; // singularized alias // Angle - public static final Angle Revolutions = BaseUnits.Angle; - public static final Angle Rotations = new Angle(1, "Rotation", "R"); // alias - public static final Angle Radians = - derive(Revolutions).splitInto(2 * Math.PI).named("Radian").symbol("rad").make(); + public static final Angle Radians = BaseUnits.Angle; + public static final Angle Revolutions = new Angle(2 * Math.PI, "Revolution", "R"); + public static final Angle Rotations = new Angle(2 * Math.PI, "Rotation", "R"); // alias revolution public static final Angle Degrees = derive(Revolutions).splitInto(360).named("Degree").symbol("°").make(); diff --git a/wpiunits/src/test/java/edu/wpi/first/units/UnitsTest.java b/wpiunits/src/test/java/edu/wpi/first/units/UnitsTest.java index 46591118ab7..c428cad8acd 100644 --- a/wpiunits/src/test/java/edu/wpi/first/units/UnitsTest.java +++ b/wpiunits/src/test/java/edu/wpi/first/units/UnitsTest.java @@ -203,13 +203,14 @@ void testOunces() { @Test void testRevolutions() { - testBaseUnit(Revolutions); + assertEquals(1, Revolutions.convertFrom(2 * Math.PI, Radians), thresh); assertEquals("Revolution", Revolutions.name()); assertEquals("R", Revolutions.symbol()); } @Test void testRadians() { + testBaseUnit(Radians); assertEquals(2 * Math.PI, Radians.convertFrom(1, Revolutions), thresh); assertEquals(2 * Math.PI, Radians.convertFrom(360, Degrees), thresh); assertEquals("Radian", Radians.name()); diff --git a/wpiunits/wpiunits-config.cmake b/wpiunits/wpiunits-config.cmake new file mode 100644 index 00000000000..e04a6bc9de1 --- /dev/null +++ b/wpiunits/wpiunits-config.cmake @@ -0,0 +1,2 @@ +get_filename_component(SELF_DIR "${CMAKE_CURRENT_LIST_FILE}" PATH) +include(${SELF_DIR}/wpiunits.cmake) diff --git a/wpiutil/CMakeLists.txt b/wpiutil/CMakeLists.txt index a31929330e9..783dffa3895 100644 --- a/wpiutil/CMakeLists.txt +++ b/wpiutil/CMakeLists.txt @@ -205,15 +205,9 @@ target_include_directories(wpiutil PUBLIC install(TARGETS wpiutil EXPORT wpiutil) -if (WITH_FLAT_INSTALL) - set (wpiutil_config_dir ${wpilib_dest}) -else() - set (wpiutil_config_dir share/wpiutil) -endif() - configure_file(wpiutil-config.cmake.in ${WPILIB_BINARY_DIR}/wpiutil-config.cmake ) -install(FILES ${WPILIB_BINARY_DIR}/wpiutil-config.cmake DESTINATION ${wpiutil_config_dir}) -install(EXPORT wpiutil DESTINATION ${wpiutil_config_dir}) +install(FILES ${WPILIB_BINARY_DIR}/wpiutil-config.cmake DESTINATION share/wpiutil) +install(EXPORT wpiutil DESTINATION share/wpiutil) subdir_list(wpiutil_examples "${CMAKE_CURRENT_SOURCE_DIR}/examples") foreach(example ${wpiutil_examples}) diff --git a/wpiutil/src/main/java/edu/wpi/first/util/protobuf/ProtobufBuffer.java b/wpiutil/src/main/java/edu/wpi/first/util/protobuf/ProtobufBuffer.java index af3e4664112..334cb161c9c 100644 --- a/wpiutil/src/main/java/edu/wpi/first/util/protobuf/ProtobufBuffer.java +++ b/wpiutil/src/main/java/edu/wpi/first/util/protobuf/ProtobufBuffer.java @@ -61,7 +61,7 @@ public ByteBuffer write(T value) throws IOException { m_msg.clearQuick(); m_proto.pack(m_msg, value); int size = m_msg.getSerializedSize(); - if (size < m_buf.capacity()) { + if (size > m_buf.capacity()) { m_buf = ByteBuffer.allocateDirect(size * 2); m_sink.setOutput(m_buf); } diff --git a/wpiutil/src/main/native/cpp/timestamp.cpp b/wpiutil/src/main/native/cpp/timestamp.cpp index c7e2fa92d10..44d15faf42d 100644 --- a/wpiutil/src/main/native/cpp/timestamp.cpp +++ b/wpiutil/src/main/native/cpp/timestamp.cpp @@ -50,14 +50,15 @@ using NiFpga_OpenHmbFunc = NiFpga_Status (*)(const NiFpga_Session session, const char* memoryName, size_t* memorySize, void** virtualAddress); +static std::atomic_flag hmbInitialized = ATOMIC_FLAG_INIT; struct HMBHolder { ~HMBHolder() { + hmbInitialized.clear(); if (hmb) { closeHmb(hmb->getSystemInterface()->getHandle(), hmbName); dlclose(niFpga); } } - explicit operator bool() const { return hmb != nullptr; } void Configure() { nFPGA::nRoboRIO_FPGANamespace::g_currentTargetClass = nLoadOut::getTargetClass(); @@ -91,8 +92,10 @@ struct HMBHolder { auto cfg = hmb->readConfig(&status); cfg.Enables_Timestamp = 1; hmb->writeConfig(cfg, &status); + hmbInitialized.test_and_set(); } void Reset() { + hmbInitialized.clear(); if (hmb) { std::unique_ptr oldHmb; oldHmb.swap(hmb); @@ -188,7 +191,7 @@ static std::atomic now_impl{wpi::NowDefault}; void wpi::impl::SetupNowRio() { #ifdef __FRC_ROBORIO__ - if (!hmb) { + if (!hmbInitialized.test()) { hmb.Configure(); } #endif @@ -207,12 +210,12 @@ void wpi::SetNowImpl(uint64_t (*func)(void)) { uint64_t wpi::Now() { #ifdef __FRC_ROBORIO__ // Same code as HAL_GetFPGATime() - if (!hmb) { + if (!hmbInitialized.test()) { std::fputs( "FPGA not yet configured in wpi::Now(). Time will not be correct", stderr); std::fflush(stderr); - return 0; + return 1; } asm("dmb"); diff --git a/wpiutil/src/main/native/include/wpi/Algorithm.h b/wpiutil/src/main/native/include/wpi/Algorithm.h index 1fd2502c294..112bfc34852 100644 --- a/wpiutil/src/main/native/include/wpi/Algorithm.h +++ b/wpiutil/src/main/native/include/wpi/Algorithm.h @@ -5,6 +5,8 @@ #pragma once #include +#include +#include #include namespace wpi { @@ -15,4 +17,19 @@ typename std::vector::iterator insert_sorted(std::vector& vec, T const& item) { return vec.insert(std::upper_bound(vec.begin(), vec.end(), item), item); } + +/** + * Calls f(i, elem) for each element of elems where i is the index of the + * element in elems and elem is the element. + * + * @param f The callback. + * @param elems The elements. + */ +template +constexpr void for_each(F&& f, Ts&&... elems) { + [&](std::index_sequence) { + (f(Is, elems), ...); + }(std::index_sequence_for{}); +} + } // namespace wpi diff --git a/wpiutil/src/main/native/thirdparty/fmtlib/include/fmt/core.h b/wpiutil/src/main/native/thirdparty/fmtlib/include/fmt/core.h index 1fe13888a00..915d8952d5f 100644 --- a/wpiutil/src/main/native/thirdparty/fmtlib/include/fmt/core.h +++ b/wpiutil/src/main/native/thirdparty/fmtlib/include/fmt/core.h @@ -1435,7 +1435,7 @@ template struct arg_mapper { // Only map owning types because mapping views can be unsafe. template , FMT_ENABLE_IF(std::is_arithmetic::value)> - FMT_CONSTEXPR FMT_INLINE auto map(const T& val) -> decltype(this->map(U())) { + FMT_CONSTEXPR FMT_INLINE auto map(const T& val) -> decltype(map(U())) { return map(format_as(val)); } @@ -1459,13 +1459,13 @@ template struct arg_mapper { !is_string::value && !is_char::value && !is_named_arg::value && !std::is_arithmetic>::value)> - FMT_CONSTEXPR FMT_INLINE auto map(T& val) -> decltype(this->do_map(val)) { + FMT_CONSTEXPR FMT_INLINE auto map(T& val) -> decltype(do_map(val)) { return do_map(val); } template ::value)> FMT_CONSTEXPR FMT_INLINE auto map(const T& named_arg) - -> decltype(this->map(named_arg.value)) { + -> decltype(map(named_arg.value)) { return map(named_arg.value); } diff --git a/xrpVendordep/CMakeLists.txt b/xrpVendordep/CMakeLists.txt index 40d40a71313..039d90718cc 100644 --- a/xrpVendordep/CMakeLists.txt +++ b/xrpVendordep/CMakeLists.txt @@ -10,18 +10,12 @@ if (WITH_JAVA) set(CMAKE_JAVA_COMPILE_FLAGS "-encoding" "UTF8" "-Xlint:unchecked") file(GLOB_RECURSE JAVA_SOURCES src/main/java/*.java) - add_jar(xrpVendordep_jar ${JAVA_SOURCES} INCLUDE_JARS hal_jar ntcore_jar cscore_jar cameraserver_jar wpimath_jar wpiutil_jar wpilibj_jar OUTPUT_NAME xrpVendordep) + add_jar(xrpVendordep_jar ${JAVA_SOURCES} INCLUDE_JARS hal_jar ntcore_jar cscore_jar cameraserver_jar wpimath_jar wpiunits_jar wpiutil_jar wpilibj_jar OUTPUT_NAME xrpVendordep) get_property(xrpVendordep_JAR_FILE TARGET xrpVendordep_jar PROPERTY JAR_FILE) install(FILES ${xrpVendordep_JAR_FILE} DESTINATION "${java_lib_dest}") set_property(TARGET xrpVendordep_jar PROPERTY FOLDER "java") - - if (WITH_FLAT_INSTALL) - set (xrpVendordep_config_dir ${wpilib_dest}) - else() - set (xrpVendordep_config_dir share/xrpVendordep) - endif() endif() if (WITH_JAVA_SOURCE) @@ -54,15 +48,9 @@ target_include_directories(xrpVendordep PUBLIC install(TARGETS xrpVendordep EXPORT xrpVendordep DESTINATION "${main_lib_dest}") install(DIRECTORY src/main/native/include/ DESTINATION "${include_dest}/xrpVendordep") -if (FLAT_INSTALL_WPILIB) - set(xrpVendordep_config_dir ${wpilib_dest}) - else() - set(xrpVendordep_config_dir share/xrpVendordep) - endif() - - configure_file(xrpVendordep-config.cmake.in ${WPILIB_BINARY_DIR}/xrpVendordep-config.cmake) - install(FILES ${WPILIB_BINARY_DIR}/xrpVendordep-config.cmake DESTINATION ${xrpVendordep_config_dir}) - install(EXPORT xrpVendordep DESTINATION ${xrpVendordep_config_dir}) +configure_file(xrpVendordep-config.cmake.in ${WPILIB_BINARY_DIR}/xrpVendordep-config.cmake) +install(FILES ${WPILIB_BINARY_DIR}/xrpVendordep-config.cmake DESTINATION share/xrpVendordep) +install(EXPORT xrpVendordep DESTINATION share/xrpVendordep) if (WITH_TESTS) wpilib_add_test(xrpVendordep src/test/native/cpp)