robot-rust 0.2.1

The robot-rust project, bindings for wpilib, ctre, and rev in rust.
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71

// 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 <stdexcept>

#include <fmt/format.h>
#include <wpi/protobuf/Protobuf.h>
#include <wpi/protobuf/ProtobufCallbacks.h>

#include "frc/EigenCore.h"
#include "wpimath/protobuf/wpimath.npb.h"

template <int Rows, int Cols, int Options, int MaxRows, int MaxCols>
  requires(Cols != 1)
struct wpi::Protobuf<frc::Matrixd<Rows, Cols, Options, MaxRows, MaxCols>> {
  using MessageStruct = wpi_proto_ProtobufMatrix;
  using InputStream = wpi::ProtoInputStream<
      frc::Matrixd<Rows, Cols, Options, MaxRows, MaxCols>>;
  using OutputStream = wpi::ProtoOutputStream<
      frc::Matrixd<Rows, Cols, Options, MaxRows, MaxCols>>;

  static std::optional<frc::Matrixd<Rows, Cols, Options, MaxRows, MaxCols>>
  Unpack(InputStream& stream) {
    constexpr bool isSmall = Rows * Cols * sizeof(double) < 256;
    using UnpackType =
        std::conditional_t<isSmall, wpi::UnpackCallback<double, Rows * Cols>,
                           wpi::StdVectorUnpackCallback<double, Rows * Cols>>;
    UnpackType data;
    data.Vec().reserve(Rows * Cols);
    data.SetLimits(wpi::DecodeLimits::Fail);
    wpi_proto_ProtobufMatrix msg;
    msg.data = data.Callback();
    if (!stream.Decode(msg)) {
      return {};
    }

    if (msg.num_rows != Rows || msg.num_cols != Cols) {
      return {};
    }

    auto items = data.Items();

    if (items.size() != Rows * Cols) {
      return {};
    }

    frc::Matrixd<Rows, Cols, Options, MaxRows, MaxCols> mat;
    for (int i = 0; i < Rows * Cols; i++) {
      mat(i) = items[i];
    }

    return mat;
  }

  static bool Pack(
      OutputStream& stream,
      const frc::Matrixd<Rows, Cols, Options, MaxRows, MaxCols>& value) {
    std::span<const double> dataSpan{value.data(),
                                     static_cast<size_t>(Rows * Cols)};
    wpi::PackCallback<double> data{dataSpan};
    wpi_proto_ProtobufMatrix msg{
        .num_rows = Rows,
        .num_cols = Cols,
        .data = data.Callback(),
    };
    return stream.Encode(msg);
  }
};