mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-24 01:31:46 +00:00
[wpimath] Upgrade to Drake v1.15.0 (#5310)
This commit is contained in:
Tyler Veness
164
wpimath/src/test/native/include/drake/common/fmt.h
Normal file
164
wpimath/src/test/native/include/drake/common/fmt.h
Normal file
@@ -0,0 +1,164 @@
|
||||
#pragma once
|
||||
|
||||
#include <string_view>
|
||||
|
||||
#include <fmt/format.h>
|
||||
|
||||
// This file contains the essentials of fmt support in Drake, mainly
|
||||
// compatibility code to inter-operate with different versions of fmt.
|
||||
|
||||
namespace drake {
|
||||
|
||||
#if FMT_VERSION >= 80000 || defined(DRAKE_DOXYGEN_CXX)
|
||||
/** When using fmt >= 8, this is an alias for
|
||||
<a href="https://fmt.dev/latest/api.html#compile-time-format-string-checks">fmt::runtime</a>.
|
||||
When using fmt < 8, this is a no-op. */
|
||||
inline auto fmt_runtime(std::string_view s) {
|
||||
return fmt::runtime(s);
|
||||
}
|
||||
/** When using fmt >= 8, this is defined to be `const` to indicate that the
|
||||
`fmt::formatter<T>::format(...)` function should be object-const.
|
||||
When using fmt < 8, the function signature was incorrect (lacking the const),
|
||||
so this macro will be empty. */
|
||||
#define DRAKE_FMT8_CONST const
|
||||
#else // FMT_VERSION
|
||||
inline auto fmt_runtime(std::string_view s) {
|
||||
return s;
|
||||
}
|
||||
#define DRAKE_FMT8_CONST
|
||||
#endif // FMT_VERSION
|
||||
|
||||
namespace internal::formatter_as {
|
||||
|
||||
/* The DRAKE_FORMATTER_AS macro specializes this for it's format_as types.
|
||||
This struct is a functor that performs a conversion. See below for details.
|
||||
@tparam T is the TYPE to be formatted. */
|
||||
template <typename T>
|
||||
struct Converter;
|
||||
|
||||
/* Provides convenient type shortcuts for the TYPE in DRAKE_FORMATTER_AS. */
|
||||
template <typename T>
|
||||
struct Traits {
|
||||
/* The type being formatted. */
|
||||
using InputType = T;
|
||||
/* The format_as functor that provides the alternative object to format. */
|
||||
using Functor = Converter<T>;
|
||||
/* The type of the alternative object. */
|
||||
using OutputType = decltype(Functor::call(std::declval<InputType>()));
|
||||
/* The fmt::formatter<> for the alternative object. */
|
||||
using OutputTypeFormatter = fmt::formatter<OutputType>;
|
||||
};
|
||||
|
||||
/* Implements fmt::formatter<TYPE> for DRAKE_FORMATER_AS types. The macro
|
||||
specializes this for it's format_as types. See below for details.
|
||||
@tparam T is the TYPE to be formatted. */
|
||||
template <typename T>
|
||||
struct Formatter;
|
||||
|
||||
} // namespace internal::formatter_as
|
||||
|
||||
} // namespace drake
|
||||
|
||||
/** Adds a `fmt::formatter<NAMESPACE::TYPE>` template specialization that
|
||||
formats the `TYPE` by delegating the formatting using a transformed expression,
|
||||
as if a conversion function like this were interposed during formatting:
|
||||
|
||||
@code{cpp}
|
||||
template <TEMPLATE_ARGS>
|
||||
auto format_as(const NAMESPACE::TYPE& ARG) {
|
||||
return EXPR;
|
||||
}
|
||||
@endcode
|
||||
|
||||
For example, this declaration ...
|
||||
|
||||
@code{cpp}
|
||||
DRAKE_FORMATTER_AS(, my_namespace, MyType, x, x.to_string())
|
||||
@endcode
|
||||
|
||||
... changes this code ...
|
||||
|
||||
@code{cpp}
|
||||
MyType foo;
|
||||
fmt::print(foo);
|
||||
@endcode
|
||||
|
||||
... to be equivalent to ...
|
||||
|
||||
@code{cpp}
|
||||
MyType foo;
|
||||
fmt::print(foo.to_string());
|
||||
@endcode
|
||||
|
||||
... allowing user to format `my_namespace::MyType` objects without manually
|
||||
adding the `to_string` call every time.
|
||||
|
||||
This provides a convenient mechanism to add formatters for classes that already
|
||||
have a `to_string` function, or classes that are just thin wrappers over simple
|
||||
types (ints, enums, etc.).
|
||||
|
||||
Always use this macro in the global namespace, and do not use a semicolon (`;`)
|
||||
afterward.
|
||||
|
||||
@param TEMPLATE_ARGS The optional first argument `TEMPLATE_ARGS` can be used in
|
||||
case the `TYPE` is templated, e.g., it might commonly be set to `typename T`. It
|
||||
should be left empty when the TYPE is not templated. In case `TYPE` has multiple
|
||||
template arguments, note that macros will fight with commas so you should use
|
||||
`typename... Ts` instead of writing them all out.
|
||||
|
||||
@param NAMESPACE The namespace that encloses the `TYPE` being formatted. Cannot
|
||||
be empty. For nested namespaces, use intemediate colons, e.g., `%drake::common`.
|
||||
Do not place _leading_ colons on the `NAMESPACE`.
|
||||
|
||||
@param TYPE The class name (or struct name, or enum name, etc.) being formatted.
|
||||
Do not place _leading_ double-colons on the `TYPE`. If the type is templated,
|
||||
use the template arguments here, e.g., `MyOptional<T>` if `TEMPLATE_ARGS` was
|
||||
chosen as `typename T`.
|
||||
|
||||
@param ARG A placeholder variable name to use for the value (i.e., object)
|
||||
being formatted within the `EXPR` expression.
|
||||
|
||||
@param EXPR An expression to `return` from the format_as function; it can
|
||||
refer to the given `ARG` name which will be of type `const TYPE& ARG`.
|
||||
|
||||
@note In future versions of fmt (perhaps fmt >= 10) there might be an ADL
|
||||
`format_as` customization point with this feature built-in. If so, then we can
|
||||
update this macro to use that spelling, and eventually deprecate the macro once
|
||||
Drake drops support for earlier version of fmt. */
|
||||
#define DRAKE_FORMATTER_AS(TEMPLATE_ARGS, NAMESPACE, TYPE, ARG, EXPR) \
|
||||
/* Specializes the Converter<> class template for our TYPE. */ \
|
||||
namespace drake::internal::formatter_as { \
|
||||
template <TEMPLATE_ARGS> \
|
||||
struct Converter<NAMESPACE::TYPE> { \
|
||||
using InputType = NAMESPACE::TYPE; \
|
||||
static auto call(const InputType& ARG) { return EXPR; } \
|
||||
}; \
|
||||
\
|
||||
/* Provides the fmt::formatter<TYPE> implementation. */ \
|
||||
template <TEMPLATE_ARGS> \
|
||||
struct Formatter<NAMESPACE::TYPE> \
|
||||
: fmt::formatter<typename Traits<NAMESPACE::TYPE>::OutputType> { \
|
||||
using MyTraits = Traits<NAMESPACE::TYPE>; \
|
||||
/* Shadow our base class member function template of the same name. */ \
|
||||
template <typename FormatContext> \
|
||||
auto format(const typename MyTraits::InputType& x, \
|
||||
FormatContext& ctx) DRAKE_FMT8_CONST { \
|
||||
/* Call the base class member function after laundering the object */ \
|
||||
/* through the user's provided format_as function. Older versions of */ \
|
||||
/* fmt have const-correctness bugs, which we can fix with some good */ \
|
||||
/* old fashioned const_cast-ing here. */ \
|
||||
using Base = typename MyTraits::OutputTypeFormatter; \
|
||||
const Base* const self = this; \
|
||||
return const_cast<Base*>(self)->format( \
|
||||
MyTraits::Functor::call(x), \
|
||||
ctx); \
|
||||
} \
|
||||
}; \
|
||||
} /* namespace drake::internal::formatter_as */ \
|
||||
\
|
||||
/* Specializes the fmt::formatter<> class template for TYPE. */ \
|
||||
namespace fmt { \
|
||||
template <TEMPLATE_ARGS> \
|
||||
struct formatter<NAMESPACE::TYPE> \
|
||||
: drake::internal::formatter_as::Formatter<NAMESPACE::TYPE> {}; \
|
||||
} /* namespace fmt */
|
||||
87
wpimath/src/test/native/include/drake/common/fmt_eigen.h
Normal file
87
wpimath/src/test/native/include/drake/common/fmt_eigen.h
Normal file
@@ -0,0 +1,87 @@
|
||||
#pragma once
|
||||
|
||||
#include <string>
|
||||
#include <string_view>
|
||||
|
||||
#include <Eigen/Core>
|
||||
|
||||
#include "drake/common/fmt.h"
|
||||
|
||||
namespace drake {
|
||||
namespace internal {
|
||||
|
||||
/* A tag type to be used in fmt::format("{}", fmt_eigen(...)) calls.
|
||||
Below we'll add a fmt::formatter<> specialization for this tag. */
|
||||
template <typename Derived>
|
||||
struct fmt_eigen_ref {
|
||||
const Eigen::MatrixBase<Derived>& matrix;
|
||||
};
|
||||
|
||||
/* Returns the string formatting of the given matrix.
|
||||
@tparam T must be either double, float, or string */
|
||||
template <typename T>
|
||||
std::string FormatEigenMatrix(
|
||||
const Eigen::Ref<const Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>>&
|
||||
matrix);
|
||||
|
||||
} // namespace internal
|
||||
|
||||
/** When passing an Eigen::Matrix to fmt, use this wrapper function to instruct
|
||||
fmt to use Drake's custom formatter for Eigen types.
|
||||
|
||||
Within Drake, when formatting an Eigen matrix into a string you must wrap the
|
||||
Eigen object as `fmt_eigen(M)`. This holds true whether it be for logging, error
|
||||
messages, debugging, or etc.
|
||||
|
||||
For example:
|
||||
@code
|
||||
if (!CheckValid(M)) {
|
||||
throw std::logic_error(fmt::format("Invalid M = {}", fmt_eigen(M)));
|
||||
}
|
||||
@endcode
|
||||
|
||||
@warning The return value of this function should only ever be used as a
|
||||
temporary object, i.e., in a fmt argument list or a logging statement argument
|
||||
list. Never store it as a local variable, member field, etc.
|
||||
|
||||
@note To ensure floating-point data is formatted without losing any digits,
|
||||
Drake's code is compiled using -DEIGEN_NO_IO, which enforces that nothing within
|
||||
Drake is allowed to use Eigen's `operator<<`. Downstream code that calls into
|
||||
Drake is not required to use that option; it is only enforced by Drake's build
|
||||
system, not by Drake's headers. */
|
||||
template <typename Derived>
|
||||
internal::fmt_eigen_ref<Derived> fmt_eigen(
|
||||
const Eigen::MatrixBase<Derived>& matrix) {
|
||||
return {matrix};
|
||||
}
|
||||
|
||||
} // namespace drake
|
||||
|
||||
#ifndef DRAKE_DOXYGEN_CXX
|
||||
// Formatter specialization for drake::fmt_eigen.
|
||||
namespace fmt {
|
||||
template <typename Derived>
|
||||
struct formatter<drake::internal::fmt_eigen_ref<Derived>>
|
||||
: formatter<std::string_view> {
|
||||
template <typename FormatContext>
|
||||
auto format(const drake::internal::fmt_eigen_ref<Derived>& ref,
|
||||
// NOLINTNEXTLINE(runtime/references) To match fmt API.
|
||||
FormatContext& ctx) DRAKE_FMT8_CONST -> decltype(ctx.out()) {
|
||||
using Scalar = typename Derived::Scalar;
|
||||
const auto& matrix = ref.matrix;
|
||||
if constexpr (std::is_same_v<Scalar, double> ||
|
||||
std::is_same_v<Scalar, float>) {
|
||||
return formatter<std::string_view>{}.format(
|
||||
drake::internal::FormatEigenMatrix<Scalar>(matrix), ctx);
|
||||
} else {
|
||||
return formatter<std::string_view>{}.format(
|
||||
drake::internal::FormatEigenMatrix<std::string>(
|
||||
matrix.unaryExpr([](const auto& element) -> std::string {
|
||||
return fmt::to_string(element);
|
||||
})),
|
||||
ctx);
|
||||
}
|
||||
}
|
||||
};
|
||||
} // namespace fmt
|
||||
#endif
|
||||
@@ -3,10 +3,12 @@
|
||||
#include <algorithm>
|
||||
#include <cmath>
|
||||
#include <limits>
|
||||
#include <string>
|
||||
|
||||
#include <Eigen/Core>
|
||||
#include <gtest/gtest.h>
|
||||
|
||||
#include "drake/common/fmt_eigen.h"
|
||||
// #include "drake/common/text_logging.h"
|
||||
|
||||
namespace drake {
|
||||
@@ -33,9 +35,10 @@ template <typename DerivedA, typename DerivedB>
|
||||
const Eigen::MatrixBase<DerivedB>& m2, double tolerance = 0.0,
|
||||
MatrixCompareType compare_type = MatrixCompareType::absolute) {
|
||||
if (m1.rows() != m2.rows() || m1.cols() != m2.cols()) {
|
||||
return ::testing::AssertionFailure()
|
||||
<< "Matrix size mismatch: (" << m1.rows() << " x " << m1.cols()
|
||||
<< " vs. " << m2.rows() << " x " << m2.cols() << ")";
|
||||
const std::string message =
|
||||
fmt::format("Matrix size mismatch: ({} x {} vs. {} x {})", m1.rows(),
|
||||
m1.cols(), m2.rows(), m2.cols());
|
||||
return ::testing::AssertionFailure() << message;
|
||||
}
|
||||
|
||||
for (int ii = 0; ii < m1.rows(); ii++) {
|
||||
@@ -59,10 +62,10 @@ template <typename DerivedA, typename DerivedB>
|
||||
// Check for case where one value is NaN and the other is not
|
||||
if ((isnan(m1(ii, jj)) && !isnan(m2(ii, jj))) ||
|
||||
(!isnan(m1(ii, jj)) && isnan(m2(ii, jj)))) {
|
||||
return ::testing::AssertionFailure() << "NaN mismatch at (" << ii
|
||||
<< ", " << jj << "):\nm1 =\n"
|
||||
<< m1 << "\nm2 =\n"
|
||||
<< m2;
|
||||
const std::string message =
|
||||
fmt::format("NaN mismatch at ({}, {}):\nm1 =\n{}\nm2 =\n{}", ii, jj,
|
||||
fmt_eigen(m1), fmt_eigen(m2));
|
||||
return ::testing::AssertionFailure() << message;
|
||||
}
|
||||
|
||||
// Determine whether the difference between the two matrices is less than
|
||||
@@ -72,16 +75,13 @@ template <typename DerivedA, typename DerivedB>
|
||||
|
||||
if (compare_type == MatrixCompareType::absolute) {
|
||||
// Perform comparison using absolute tolerance.
|
||||
|
||||
if (delta > tolerance) {
|
||||
return ::testing::AssertionFailure()
|
||||
<< "Values at (" << ii << ", " << jj
|
||||
<< ") exceed tolerance: " << m1(ii, jj) << " vs. "
|
||||
<< m2(ii, jj) << ", diff = " << delta
|
||||
<< ", tolerance = " << tolerance << "\nm1 =\n"
|
||||
<< m1 << "\nm2 =\n"
|
||||
<< m2 << "\ndelta=\n"
|
||||
<< (m1 - m2);
|
||||
const std::string message = fmt::format(
|
||||
"Values at ({}, {}) exceed tolerance: {} vs. {}, diff = {}, "
|
||||
"tolerance = {}\nm1 =\n{}\nm2 =\n{}\ndelta=\n{}",
|
||||
ii, jj, m1(ii, jj), m2(ii, jj), delta, tolerance, fmt_eigen(m1),
|
||||
fmt_eigen(m2), fmt_eigen(m1 - m2));
|
||||
return ::testing::AssertionFailure() << message;
|
||||
}
|
||||
} else {
|
||||
// Perform comparison using relative tolerance, see:
|
||||
@@ -90,27 +90,24 @@ template <typename DerivedA, typename DerivedB>
|
||||
const auto max_value = max(abs(m1(ii, jj)), abs(m2(ii, jj)));
|
||||
const auto relative_tolerance =
|
||||
tolerance * max(decltype(max_value){1}, max_value);
|
||||
|
||||
if (delta > relative_tolerance) {
|
||||
return ::testing::AssertionFailure()
|
||||
<< "Values at (" << ii << ", " << jj
|
||||
<< ") exceed tolerance: " << m1(ii, jj) << " vs. "
|
||||
<< m2(ii, jj) << ", diff = " << delta
|
||||
<< ", tolerance = " << tolerance
|
||||
<< ", relative tolerance = " << relative_tolerance
|
||||
<< "\nm1 =\n"
|
||||
<< m1 << "\nm2 =\n"
|
||||
<< m2 << "\ndelta=\n"
|
||||
<< (m1 - m2);
|
||||
const std::string message = fmt::format(
|
||||
"Values at ({}, {}) exceed tolerance: {} vs. {}, diff = {}, "
|
||||
"tolerance = {}, relative tolerance = {}\nm1 =\n{}\nm2 "
|
||||
"=\n{}\ndelta=\n{}",
|
||||
ii, jj, m1(ii, jj), m2(ii, jj), delta, tolerance,
|
||||
relative_tolerance, fmt_eigen(m1), fmt_eigen(m2),
|
||||
fmt_eigen(m1 - m2));
|
||||
return ::testing::AssertionFailure() << message;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return ::testing::AssertionSuccess() << "m1 =\n"
|
||||
<< m1
|
||||
<< "\nis approximately equal to m2 =\n"
|
||||
<< m2;
|
||||
const std::string message =
|
||||
fmt::format("m1 =\n{}\nis approximately equal to m2 =\n{}", fmt_eigen(m1),
|
||||
fmt_eigen(m2));
|
||||
return ::testing::AssertionSuccess() << message;
|
||||
}
|
||||
|
||||
} // namespace drake
|
||||
|
||||
Reference in New Issue
Block a user